1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
//! Orthonormal reference frame in 3D space.
//!
//! [`Frame3`] bundles an origin with three mutually perpendicular unit axes.
//! It replaces the 14+ hand-rolled "pick a candidate, cross twice" patterns
//! scattered across surfaces, curves, and intersection code.
use crate::MathError;
use crate::vec::{Point3, Vec3};
/// An orthonormal reference frame: origin + three mutually perpendicular unit
/// axes (`x`, `y`, `z`).
///
/// The `z` axis is the *primary* direction (surface normal, curve axis, etc.).
/// `x` and `y` span the plane perpendicular to `z`.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Frame3 {
/// Frame origin.
pub origin: Point3,
/// First axis in the perpendicular plane.
pub x: Vec3,
/// Second axis in the perpendicular plane.
pub y: Vec3,
/// Primary axis / normal.
pub z: Vec3,
}
impl Frame3 {
/// Build an orthonormal frame from an origin and a primary axis (normal).
///
/// `x` and `y` are chosen arbitrarily in the plane perpendicular to `z`.
/// The input `normal` is normalized internally.
///
/// # Errors
///
/// Returns [`MathError::ZeroVector`] if `normal` is zero-length.
pub fn from_normal(origin: Point3, normal: Vec3) -> Result<Self, MathError> {
let z = normal.normalize()?;
let (x, y) = perpendicular_pair(z)?;
Ok(Self { origin, x, y, z })
}
/// Build an orthonormal frame from an origin, a primary axis, and a
/// preferred reference direction for `x`.
///
/// `ref_dir` is projected onto the plane perpendicular to `z` to produce
/// `x`. If `ref_dir` is (nearly) parallel to `z`, the frame falls back to
/// an arbitrary perpendicular choice.
///
/// # Errors
///
/// Returns [`MathError::ZeroVector`] if `normal` is zero-length.
pub fn from_normal_and_ref(
origin: Point3,
normal: Vec3,
ref_dir: Vec3,
) -> Result<Self, MathError> {
let z = normal.normalize()?;
let ref_proj = ref_dir - z * ref_dir.dot(z);
let x = if let Ok(v) = ref_proj.normalize() {
v
} else {
// ref_dir is parallel to z — fall back to arbitrary choice.
let (arb_x, _) = perpendicular_pair(z)?;
arb_x
};
let y = z.cross(x);
Ok(Self { origin, x, y, z })
}
}
/// Given a unit vector `z`, return two unit vectors `(x, y)` forming an
/// orthonormal basis where `x = z × candidate` and `y = z × x`.
fn perpendicular_pair(z: Vec3) -> Result<(Vec3, Vec3), MathError> {
let candidate = if z.x().abs() < 0.9 {
Vec3::new(1.0, 0.0, 0.0)
} else {
Vec3::new(0.0, 1.0, 0.0)
};
let x = z.cross(candidate).normalize()?;
let y = z.cross(x);
Ok((x, y))
}
#[cfg(test)]
#[allow(clippy::unwrap_used, clippy::expect_used)]
mod tests {
use super::*;
use proptest::prelude::*;
#[test]
fn frame_from_z_axis() {
let f = Frame3::from_normal(Point3::new(0.0, 0.0, 0.0), Vec3::new(0.0, 0.0, 5.0))
.expect("non-zero");
assert!((f.z.z() - 1.0).abs() < 1e-14);
assert!(f.x.dot(f.y).abs() < 1e-14);
assert!(f.x.dot(f.z).abs() < 1e-14);
assert!(f.y.dot(f.z).abs() < 1e-14);
}
#[test]
fn frame_from_x_axis() {
// Tests the branch where x_component > 0.9
let f = Frame3::from_normal(Point3::new(0.0, 0.0, 0.0), Vec3::new(1.0, 0.0, 0.0))
.expect("non-zero");
assert!((f.z.x() - 1.0).abs() < 1e-14);
assert!(f.x.dot(f.z).abs() < 1e-14);
}
#[test]
fn frame_with_ref_dir() {
let f = Frame3::from_normal_and_ref(
Point3::new(0.0, 0.0, 0.0),
Vec3::new(0.0, 0.0, 1.0),
Vec3::new(1.0, 0.0, 0.0),
)
.expect("non-zero");
// x should align with the reference direction
assert!((f.x.x() - 1.0).abs() < 1e-14);
assert!(f.x.y().abs() < 1e-14);
}
#[test]
fn frame_with_parallel_ref_falls_back() {
// ref_dir parallel to normal — should still produce valid frame
let f = Frame3::from_normal_and_ref(
Point3::new(0.0, 0.0, 0.0),
Vec3::new(0.0, 0.0, 1.0),
Vec3::new(0.0, 0.0, 1.0),
)
.expect("non-zero");
assert!(f.x.dot(f.z).abs() < 1e-14);
assert!((f.x.length() - 1.0).abs() < 1e-14);
}
#[test]
fn zero_normal_errors() {
let r = Frame3::from_normal(Point3::new(0.0, 0.0, 0.0), Vec3::new(0.0, 0.0, 0.0));
assert!(r.is_err());
}
proptest! {
#[test]
fn prop_frame_orthonormal(
nx in -10.0f64..10.0, ny in -10.0f64..10.0, nz in -10.0f64..10.0,
) {
let n = Vec3::new(nx, ny, nz);
if let Ok(f) = Frame3::from_normal(Point3::new(0.0, 0.0, 0.0), n) {
// All axes are unit length
prop_assert!((f.x.length() - 1.0).abs() < 1e-12);
prop_assert!((f.y.length() - 1.0).abs() < 1e-12);
prop_assert!((f.z.length() - 1.0).abs() < 1e-12);
// Mutually perpendicular
prop_assert!(f.x.dot(f.y).abs() < 1e-12);
prop_assert!(f.x.dot(f.z).abs() < 1e-12);
prop_assert!(f.y.dot(f.z).abs() < 1e-12);
// Right-handed: x × y ≈ z
let cross = f.x.cross(f.y);
prop_assert!((cross.x() - f.z.x()).abs() < 1e-12);
prop_assert!((cross.y() - f.z.y()).abs() < 1e-12);
prop_assert!((cross.z() - f.z.z()).abs() < 1e-12);
}
}
}
}