geop_core_math/primitives/
coordiante_system.rs

1use crate::{
2    geop_error::{GeopError, GeopResult},
3    scalars::Scalar,
4    vector::{Vector2, Vector3},
5};
6use core::fmt::Display;
7
8/// A local coordinate system: an `origin` point plus 3 linearly independent
9/// (not necessarily orthogonal or unit-length) basis vectors `u`, `v`, `w`,
10/// letting a caller convert a point's coordinates between this frame
11/// (`uvw` space, relative to `origin`) and the ambient `xyz` space it was
12/// itself expressed in.
13#[derive(Clone, Debug)]
14pub struct CoordinateSystem<S: Scalar> {
15    origin: Vector3<S>,
16    u: Vector3<S>,
17    v: Vector3<S>,
18    w: Vector3<S>,
19    /// The reciprocal basis (`u*`, `v*`, `w*`), precomputed so `to_uvw` is
20    /// just 3 dot products: `u* = (v × w) / det`, and cyclically for `v*`,
21    /// `w*`, where `det = u . (v × w)` is the basis's signed volume.
22    u_star: Vector3<S>,
23    v_star: Vector3<S>,
24    w_star: Vector3<S>,
25}
26
27impl<S: Scalar> CoordinateSystem<S> {
28    /// Build the coordinate system from its `origin` and 3 basis vectors.
29    /// Fails if `u`, `v`, `w` could be linearly dependent (zero signed
30    /// volume `u . (v × w)`), since that basis can't be inverted into a
31    /// reciprocal one.
32    pub fn try_new(
33        origin: Vector3<S>,
34        u: Vector3<S>,
35        v: Vector3<S>,
36        w: Vector3<S>,
37    ) -> GeopResult<Self> {
38        let det = u.prod_dot(&v.prod_cross(&w));
39        if det.could_be_equal(S::ZERO) {
40            return Err(GeopError::new(
41                "CoordinateSystem::try_new: u, v, w could be linearly dependent (zero volume)",
42            ));
43        }
44        let inv_det = S::ONE.div(det)?;
45        let u_star = v.prod_cross(&w).prod_scalar(inv_det);
46        let v_star = w.prod_cross(&u).prod_scalar(inv_det);
47        let w_star = u.prod_cross(&v).prod_scalar(inv_det);
48        Ok(Self {
49            origin,
50            u,
51            v,
52            w,
53            u_star,
54            v_star,
55            w_star,
56        })
57    }
58
59    pub fn origin(&self) -> &Vector3<S> {
60        &self.origin
61    }
62    pub fn u(&self) -> &Vector3<S> {
63        &self.u
64    }
65    pub fn v(&self) -> &Vector3<S> {
66        &self.v
67    }
68    pub fn w(&self) -> &Vector3<S> {
69        &self.w
70    }
71
72    /// Convert a point expressed in this frame (`uvw` coordinates, relative
73    /// to `origin`) to ambient `xyz` space: `origin + p_uvw[0] * u +
74    /// p_uvw[1] * v + p_uvw[2] * w`.
75    pub fn to_xyz(&self, p_uvw: &Vector3<S>) -> Vector3<S> {
76        self.origin
77            .add(&self.u.prod_scalar(p_uvw[0]))
78            .add(&self.v.prod_scalar(p_uvw[1]))
79            .add(&self.w.prod_scalar(p_uvw[2]))
80    }
81
82    /// Convert a point expressed in this frame's `u`/`v` plane (`w = 0`) to
83    /// ambient `xyz` space: `origin + p_uv[0] * u + p_uv[1] * v`.
84    pub fn uv_to_xyz(&self, p_uv: &Vector2<S>) -> Vector3<S> {
85        self.origin
86            .add(&self.u.prod_scalar(p_uv[0]))
87            .add(&self.v.prod_scalar(p_uv[1]))
88    }
89
90    /// Convert an ambient `xyz` point into this frame's `uvw` coordinates
91    /// (relative to `origin`), via the precomputed reciprocal basis (`u* .
92    /// (p - origin)`, `v* . (p - origin)`, `w* . (p - origin)`).
93    pub fn to_uvw(&self, p_xyz: &Vector3<S>) -> Vector3<S> {
94        let p = p_xyz.sub(&self.origin);
95        Vector3::from_array([
96            p.prod_dot(&self.u_star),
97            p.prod_dot(&self.v_star),
98            p.prod_dot(&self.w_star),
99        ])
100    }
101}
102
103impl<S: Scalar> Display for CoordinateSystem<S> {
104    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
105        write!(
106            f,
107            "CoordinateSystem(origin={}, u={}, v={}, w={})",
108            self.origin, self.u, self.v, self.w
109        )
110    }
111}
112
113#[cfg(test)]
114mod tests {
115    use super::CoordinateSystem;
116    use crate::{
117        for_all_scalars,
118        scalars::Scalar,
119        vector::{Vector2, Vector3},
120    };
121
122    fn check_orthonormal_round_trip<S: Scalar>() {
123        let origin = Vector3::from_array([S::ZERO; 3]);
124        let u = Vector3::from_array([S::ONE, S::ZERO, S::ZERO]);
125        let v = Vector3::from_array([S::ZERO, S::ONE, S::ZERO]);
126        let w = Vector3::from_array([S::ZERO, S::ZERO, S::ONE]);
127        let cs = CoordinateSystem::try_new(origin, u, v, w).unwrap();
128
129        let p = Vector3::from_array([S::from_f64(2.0), S::from_f64(-3.0), S::from_f64(5.0)]);
130        assert!(cs.to_uvw(&p).could_be_equal(&p));
131        assert!(cs.to_xyz(&p).could_be_equal(&p));
132    }
133    #[test]
134    fn orthonormal_round_trip() {
135        for_all_scalars!(check_orthonormal_round_trip);
136    }
137
138    fn check_skewed_basis_round_trip<S: Scalar>() {
139        let origin = Vector3::from_array([S::ZERO; 3]);
140        let u = Vector3::from_array([S::from_f64(1.0), S::from_f64(0.5), S::ZERO]);
141        let v = Vector3::from_array([S::ZERO, S::from_f64(2.0), S::from_f64(0.3)]);
142        let w = Vector3::from_array([S::from_f64(0.2), S::ZERO, S::from_f64(1.5)]);
143        let cs = CoordinateSystem::try_new(origin, u, v, w).unwrap();
144
145        let p_uvw = Vector3::from_array([S::from_f64(1.3), S::from_f64(-0.7), S::from_f64(2.1)]);
146        let p_xyz = cs.to_xyz(&p_uvw);
147        let round_tripped = cs.to_uvw(&p_xyz);
148        assert!(round_tripped.could_be_equal(&p_uvw));
149
150        // u, v, w themselves must map to the standard basis vectors.
151        assert!(
152            cs.to_uvw(&u)
153                .could_be_equal(&Vector3::from_array([S::ONE, S::ZERO, S::ZERO]))
154        );
155        assert!(
156            cs.to_uvw(&v)
157                .could_be_equal(&Vector3::from_array([S::ZERO, S::ONE, S::ZERO]))
158        );
159        assert!(
160            cs.to_uvw(&w)
161                .could_be_equal(&Vector3::from_array([S::ZERO, S::ZERO, S::ONE]))
162        );
163    }
164    #[test]
165    fn skewed_basis_round_trip() {
166        for_all_scalars!(check_skewed_basis_round_trip);
167    }
168
169    fn check_offset_origin_round_trip<S: Scalar>() {
170        let origin = Vector3::from_array([S::from_f64(10.0), S::from_f64(-4.0), S::from_f64(2.0)]);
171        let u = Vector3::from_array([S::ONE, S::ZERO, S::ZERO]);
172        let v = Vector3::from_array([S::ZERO, S::ONE, S::ZERO]);
173        let w = Vector3::from_array([S::ZERO, S::ZERO, S::ONE]);
174        let cs = CoordinateSystem::try_new(origin, u, v, w).unwrap();
175
176        // The origin itself is `(0, 0, 0)` in `uvw` space.
177        assert!(
178            cs.to_uvw(&origin)
179                .could_be_equal(&Vector3::from_array([S::ZERO; 3]))
180        );
181        assert!(
182            cs.to_xyz(&Vector3::from_array([S::ZERO; 3]))
183                .could_be_equal(&origin)
184        );
185
186        let p_uvw = Vector3::from_array([S::from_f64(1.0), S::from_f64(2.0), S::from_f64(3.0)]);
187        let p_xyz = cs.to_xyz(&p_uvw);
188        assert!(p_xyz.could_be_equal(&origin.add(&p_uvw)));
189        assert!(cs.to_uvw(&p_xyz).could_be_equal(&p_uvw));
190    }
191    #[test]
192    fn offset_origin_round_trip() {
193        for_all_scalars!(check_offset_origin_round_trip);
194    }
195
196    fn check_uv_to_xyz_matches_to_xyz_with_zero_w<S: Scalar>() {
197        let origin = Vector3::from_array([S::from_f64(1.0), S::from_f64(2.0), S::from_f64(3.0)]);
198        let u = Vector3::from_array([S::from_f64(1.0), S::from_f64(0.5), S::ZERO]);
199        let v = Vector3::from_array([S::ZERO, S::from_f64(2.0), S::from_f64(0.3)]);
200        let w = Vector3::from_array([S::from_f64(0.2), S::ZERO, S::from_f64(1.5)]);
201        let cs = CoordinateSystem::try_new(origin, u, v, w).unwrap();
202
203        let p_uv = Vector2::from_array([S::from_f64(1.3), S::from_f64(-0.7)]);
204        let p_uvw = Vector3::from_array([p_uv[0], p_uv[1], S::ZERO]);
205        assert!(cs.uv_to_xyz(&p_uv).could_be_equal(&cs.to_xyz(&p_uvw)));
206    }
207    #[test]
208    fn uv_to_xyz_matches_to_xyz_with_zero_w() {
209        for_all_scalars!(check_uv_to_xyz_matches_to_xyz_with_zero_w);
210    }
211
212    fn check_degenerate_basis_fails<S: Scalar>() {
213        let origin = Vector3::from_array([S::ZERO; 3]);
214        let u = Vector3::from_array([S::ONE, S::ZERO, S::ZERO]);
215        let v = Vector3::from_array([S::ZERO, S::ONE, S::ZERO]);
216        let w = u.add(&v); // coplanar with u, v -- zero volume.
217        assert!(CoordinateSystem::try_new(origin, u, v, w).is_err());
218    }
219    #[test]
220    fn degenerate_basis_fails() {
221        for_all_scalars!(check_degenerate_basis_fails);
222    }
223}
geop_core_math/primitives/coordiante_system.rs

geop_core_math/primitives/
coordiante_system.rs
