cadcore_math/
frame.rs

1//! Right-handed coordinate frame (`Frame3`).
2//!
3//! A `Frame3` is an orthonormal basis + origin — a local coordinate system
4//! embedded in world space.  It is the fundamental "placement" primitive used
5//! throughout the CAD kernel (surface parameterisation, joint construction, etc.)
6
7use crate::{Mat3, Point3, UnitVec3, Vec3};
8
9/// Right-handed orthonormal frame: origin + three unit axes.
10///
11/// `x`, `y`, `z` form a right-handed basis: `x.cross(y) == z`.
12#[derive(Clone, Copy, Debug, PartialEq)]
13pub struct Frame3 {
14    /// Frame origin in world space.
15    pub origin: Point3,
16    /// Local X axis.
17    pub x: UnitVec3,
18    /// Local Y axis.
19    pub y: UnitVec3,
20    /// Local Z axis (= x × y).
21    pub z: UnitVec3,
22}
23
24impl Frame3 {
25    /// World frame: origin at (0,0,0), axes aligned with world axes.
26    pub const WORLD: Self = Self {
27        origin: Point3::ORIGIN,
28        x: UnitVec3::X,
29        y: UnitVec3::Y,
30        z: UnitVec3::Z,
31    };
32
33    /// Build a frame from an origin and a forward (+Z) direction.
34    ///
35    /// The X axis is chosen to be perpendicular to `forward` (arbitrary but
36    /// consistent).  Y is derived as `z × x` (right-handed).
37    pub fn from_origin_z(origin: Point3, forward: UnitVec3) -> Self {
38        let (x, y) = forward.perp_basis();
39        Self {
40            origin,
41            x,
42            y,
43            z: forward,
44        }
45    }
46
47    /// Build a frame from origin, forward (+Z), and a suggested up vector.
48    ///
49    /// `up_hint` need not be perpendicular to `forward`; it is projected to
50    /// produce the Y axis.  Falls back to the arbitrary-perp method if `up_hint`
51    /// is nearly parallel to `forward`.
52    pub fn from_origin_z_up(origin: Point3, forward: UnitVec3, up_hint: Vec3) -> Self {
53        let up_proj = up_hint - forward.as_vec() * forward.dot_vec(up_hint);
54        let y = match UnitVec3::try_from_vec(up_proj) {
55            Some(u) => u,
56            None => forward.perp_basis().1, // fallback
57        };
58        let x = UnitVec3::try_from_vec(y.as_vec().cross(forward.as_vec()))
59            .unwrap_or_else(|| forward.perp_basis().0);
60        Self {
61            origin,
62            x,
63            y,
64            z: forward,
65        }
66    }
67
68    /// Express a world-space point in local coordinates.
69    #[inline]
70    pub fn to_local_point(self, p: Point3) -> Point3 {
71        let d = p - self.origin;
72        Point3::new(self.x.dot_vec(d), self.y.dot_vec(d), self.z.dot_vec(d))
73    }
74
75    /// Express a local-space point in world coordinates.
76    #[inline]
77    pub fn to_world_point(self, p: Point3) -> Point3 {
78        self.origin + self.x * p.x + self.y * p.y + self.z * p.z
79    }
80
81    /// Express a world-space vector in local coordinates.
82    #[inline]
83    pub fn to_local_vec(self, v: Vec3) -> Vec3 {
84        Vec3::new(self.x.dot_vec(v), self.y.dot_vec(v), self.z.dot_vec(v))
85    }
86
87    /// Express a local-space vector in world coordinates.
88    #[inline]
89    pub fn to_world_vec(self, v: Vec3) -> Vec3 {
90        self.x * v.x + self.y * v.y + self.z * v.z
91    }
92
93    /// Rotation matrix that maps world axes to local axes.
94    #[inline]
95    pub fn rotation(self) -> Mat3 {
96        Mat3::from_axes(self.x, self.y, self.z)
97    }
98
99    /// Move the frame origin by `delta` (in world space).
100    #[inline]
101    pub fn translate(self, delta: Vec3) -> Self {
102        Self {
103            origin: self.origin + delta,
104            ..self
105        }
106    }
107}
108
109#[cfg(test)]
110mod tests {
111    use super::*;
112
113    #[test]
114    fn round_trip_point() {
115        let frame = Frame3::from_origin_z(
116            Point3::new(1.0, 2.0, 3.0),
117            UnitVec3::try_from_vec(Vec3::new(1.0, 1.0, 1.0).normalize()).unwrap(),
118        );
119        let world_pt = Point3::new(5.0, 7.0, -2.0);
120        let local_pt = frame.to_local_point(world_pt);
121        let back = frame.to_world_point(local_pt);
122        assert!((world_pt - back).length() < 1e-10);
123    }
124}
cadcore_math/frame.rs

cadcore_math/
frame.rs
