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 { origin, x, y, z: forward }
40    }
41
42    /// Build a frame from origin, forward (+Z), and a suggested up vector.
43    ///
44    /// `up_hint` need not be perpendicular to `forward`; it is projected to
45    /// produce the Y axis.  Falls back to the arbitrary-perp method if `up_hint`
46    /// is nearly parallel to `forward`.
47    pub fn from_origin_z_up(origin: Point3, forward: UnitVec3, up_hint: Vec3) -> Self {
48        let up_proj = up_hint - forward.as_vec() * forward.dot_vec(up_hint);
49        let y = match UnitVec3::try_from_vec(up_proj) {
50            Some(u) => u,
51            None    => forward.perp_basis().1,  // fallback
52        };
53        let x = UnitVec3::try_from_vec(y.as_vec().cross(forward.as_vec()))
54            .unwrap_or_else(|| forward.perp_basis().0);
55        Self { origin, x, y, z: forward }
56    }
57
58    /// Express a world-space point in local coordinates.
59    #[inline]
60    pub fn to_local_point(self, p: Point3) -> Point3 {
61        let d = p - self.origin;
62        Point3::new(
63            self.x.dot_vec(d),
64            self.y.dot_vec(d),
65            self.z.dot_vec(d),
66        )
67    }
68
69    /// Express a local-space point in world coordinates.
70    #[inline]
71    pub fn to_world_point(self, p: Point3) -> Point3 {
72        self.origin + self.x * p.x + self.y * p.y + self.z * p.z
73    }
74
75    /// Express a world-space vector in local coordinates.
76    #[inline]
77    pub fn to_local_vec(self, v: Vec3) -> Vec3 {
78        Vec3::new(self.x.dot_vec(v), self.y.dot_vec(v), self.z.dot_vec(v))
79    }
80
81    /// Express a local-space vector in world coordinates.
82    #[inline]
83    pub fn to_world_vec(self, v: Vec3) -> Vec3 {
84        self.x * v.x + self.y * v.y + self.z * v.z
85    }
86
87    /// Rotation matrix that maps world axes to local axes.
88    #[inline]
89    pub fn rotation(self) -> Mat3 {
90        Mat3::from_axes(self.x, self.y, self.z)
91    }
92
93    /// Move the frame origin by `delta` (in world space).
94    #[inline]
95    pub fn translate(self, delta: Vec3) -> Self {
96        Self { origin: self.origin + delta, ..self }
97    }
98}
99
100#[cfg(test)]
101mod tests {
102    use super::*;
103
104    #[test]
105    fn round_trip_point() {
106        let frame = Frame3::from_origin_z(
107            Point3::new(1.0, 2.0, 3.0),
108            UnitVec3::try_from_vec(Vec3::new(1.0, 1.0, 1.0).normalize()).unwrap(),
109        );
110        let world_pt = Point3::new(5.0, 7.0, -2.0);
111        let local_pt = frame.to_local_point(world_pt);
112        let back     = frame.to_world_point(local_pt);
113        assert!((world_pt - back).length() < 1e-10);
114    }
115}
cadcore_math/frame.rs

cadcore_math/
frame.rs
