geop_core_math/primitives/
coordinate_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, PartialEq)]
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    /// The world's own axes `x`, `y` and `z`, at `origin`.
60    pub fn world_at(origin: Vector3<S>) -> Self {
61        Self::try_new(origin, Vector3::axis(0), Vector3::axis(1), Vector3::axis(2))
62            .expect("the world's axes are independent")
63    }
64
65    pub fn origin(&self) -> &Vector3<S> {
66        &self.origin
67    }
68    pub fn u(&self) -> &Vector3<S> {
69        &self.u
70    }
71    pub fn v(&self) -> &Vector3<S> {
72        &self.v
73    }
74    pub fn w(&self) -> &Vector3<S> {
75        &self.w
76    }
77
78    /// Convert a point expressed in this frame (`uvw` coordinates, relative
79    /// to `origin`) to ambient `xyz` space: `origin + p_uvw[0] * u +
80    /// p_uvw[1] * v + p_uvw[2] * w`.
81    pub fn to_xyz(&self, p_uvw: &Vector3<S>) -> Vector3<S> {
82        self.origin
83            .add(&self.u.prod_scalar(p_uvw[0]))
84            .add(&self.v.prod_scalar(p_uvw[1]))
85            .add(&self.w.prod_scalar(p_uvw[2]))
86    }
87
88    /// Convert a point expressed in this frame's `u`/`v` plane (`w = 0`) to
89    /// ambient `xyz` space: `origin + p_uv[0] * u + p_uv[1] * v`.
90    pub fn uv_to_xyz(&self, p_uv: &Vector2<S>) -> Vector3<S> {
91        self.origin
92            .add(&self.u.prod_scalar(p_uv[0]))
93            .add(&self.v.prod_scalar(p_uv[1]))
94    }
95
96    /// Convert an ambient `xyz` point into this frame's `uvw` coordinates
97    /// (relative to `origin`), via the precomputed reciprocal basis (`u* .
98    /// (p - origin)`, `v* . (p - origin)`, `w* . (p - origin)`).
99    pub fn to_uvw(&self, p_xyz: &Vector3<S>) -> Vector3<S> {
100        let p = p_xyz.sub(&self.origin);
101        Vector3::from_array([
102            p.prod_dot(&self.u_star),
103            p.prod_dot(&self.v_star),
104            p.prod_dot(&self.w_star),
105        ])
106    }
107}
108
109/// Serializes as `{origin, u, v, w}`, for a viewer.
110impl<S: Scalar> serde::Serialize for CoordinateSystem<S> {
111    fn serialize<Ser: serde::Serializer>(&self, serializer: Ser) -> Result<Ser::Ok, Ser::Error> {
112        use serde::ser::SerializeStruct;
113        let mut s = serializer.serialize_struct("CoordinateSystem", 4)?;
114        s.serialize_field("origin", &self.origin)?;
115        s.serialize_field("u", &self.u)?;
116        s.serialize_field("v", &self.v)?;
117        s.serialize_field("w", &self.w)?;
118        s.end()
119    }
120}
121
122impl<S: Scalar> Display for CoordinateSystem<S> {
123    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
124        write!(
125            f,
126            "CoordinateSystem(origin={}, u={}, v={}, w={})",
127            self.origin, self.u, self.v, self.w
128        )
129    }
130}
131
132#[cfg(test)]
133mod tests {
134    use super::CoordinateSystem;
135    use crate::{
136        for_all_scalars,
137        scalars::Scalar,
138        vector::{Vector2, Vector3},
139    };
140
141    fn check_orthonormal_round_trip<S: Scalar>() {
142        let origin = Vector3::from_array([S::ZERO; 3]);
143        let u = Vector3::from_array([S::ONE, S::ZERO, S::ZERO]);
144        let v = Vector3::from_array([S::ZERO, S::ONE, S::ZERO]);
145        let w = Vector3::from_array([S::ZERO, S::ZERO, S::ONE]);
146        let cs = CoordinateSystem::try_new(origin, u, v, w).unwrap();
147
148        let p = Vector3::from_array([S::from_f64(2.0), S::from_f64(-3.0), S::from_f64(5.0)]);
149        assert!(cs.to_uvw(&p).could_be_equal(&p));
150        assert!(cs.to_xyz(&p).could_be_equal(&p));
151    }
152    #[test]
153    fn orthonormal_round_trip() {
154        for_all_scalars!(check_orthonormal_round_trip);
155    }
156
157    fn check_skewed_basis_round_trip<S: Scalar>() {
158        let origin = Vector3::from_array([S::ZERO; 3]);
159        let u = Vector3::from_array([S::from_f64(1.0), S::from_f64(0.5), S::ZERO]);
160        let v = Vector3::from_array([S::ZERO, S::from_f64(2.0), S::from_f64(0.3)]);
161        let w = Vector3::from_array([S::from_f64(0.2), S::ZERO, S::from_f64(1.5)]);
162        let cs = CoordinateSystem::try_new(origin, u, v, w).unwrap();
163
164        let p_uvw = Vector3::from_array([S::from_f64(1.3), S::from_f64(-0.7), S::from_f64(2.1)]);
165        let p_xyz = cs.to_xyz(&p_uvw);
166        let round_tripped = cs.to_uvw(&p_xyz);
167        assert!(round_tripped.could_be_equal(&p_uvw));
168
169        // u, v, w themselves must map to the standard basis vectors.
170        assert!(
171            cs.to_uvw(&u)
172                .could_be_equal(&Vector3::from_array([S::ONE, S::ZERO, S::ZERO]))
173        );
174        assert!(
175            cs.to_uvw(&v)
176                .could_be_equal(&Vector3::from_array([S::ZERO, S::ONE, S::ZERO]))
177        );
178        assert!(
179            cs.to_uvw(&w)
180                .could_be_equal(&Vector3::from_array([S::ZERO, S::ZERO, S::ONE]))
181        );
182    }
183    #[test]
184    fn skewed_basis_round_trip() {
185        for_all_scalars!(check_skewed_basis_round_trip);
186    }
187
188    fn check_offset_origin_round_trip<S: Scalar>() {
189        let origin = Vector3::from_array([S::from_f64(10.0), S::from_f64(-4.0), S::from_f64(2.0)]);
190        let u = Vector3::from_array([S::ONE, S::ZERO, S::ZERO]);
191        let v = Vector3::from_array([S::ZERO, S::ONE, S::ZERO]);
192        let w = Vector3::from_array([S::ZERO, S::ZERO, S::ONE]);
193        let cs = CoordinateSystem::try_new(origin, u, v, w).unwrap();
194
195        // The origin itself is `(0, 0, 0)` in `uvw` space.
196        assert!(
197            cs.to_uvw(&origin)
198                .could_be_equal(&Vector3::from_array([S::ZERO; 3]))
199        );
200        assert!(
201            cs.to_xyz(&Vector3::from_array([S::ZERO; 3]))
202                .could_be_equal(&origin)
203        );
204
205        let p_uvw = Vector3::from_array([S::from_f64(1.0), S::from_f64(2.0), S::from_f64(3.0)]);
206        let p_xyz = cs.to_xyz(&p_uvw);
207        assert!(p_xyz.could_be_equal(&origin.add(&p_uvw)));
208        assert!(cs.to_uvw(&p_xyz).could_be_equal(&p_uvw));
209    }
210    #[test]
211    fn offset_origin_round_trip() {
212        for_all_scalars!(check_offset_origin_round_trip);
213    }
214
215    fn check_uv_to_xyz_matches_to_xyz_with_zero_w<S: Scalar>() {
216        let origin = Vector3::from_array([S::from_f64(1.0), S::from_f64(2.0), S::from_f64(3.0)]);
217        let u = Vector3::from_array([S::from_f64(1.0), S::from_f64(0.5), S::ZERO]);
218        let v = Vector3::from_array([S::ZERO, S::from_f64(2.0), S::from_f64(0.3)]);
219        let w = Vector3::from_array([S::from_f64(0.2), S::ZERO, S::from_f64(1.5)]);
220        let cs = CoordinateSystem::try_new(origin, u, v, w).unwrap();
221
222        let p_uv = Vector2::from_array([S::from_f64(1.3), S::from_f64(-0.7)]);
223        let p_uvw = Vector3::from_array([p_uv[0], p_uv[1], S::ZERO]);
224        assert!(cs.uv_to_xyz(&p_uv).could_be_equal(&cs.to_xyz(&p_uvw)));
225    }
226    #[test]
227    fn uv_to_xyz_matches_to_xyz_with_zero_w() {
228        for_all_scalars!(check_uv_to_xyz_matches_to_xyz_with_zero_w);
229    }
230
231    fn check_degenerate_basis_fails<S: Scalar>() {
232        let origin = Vector3::from_array([S::ZERO; 3]);
233        let u = Vector3::from_array([S::ONE, S::ZERO, S::ZERO]);
234        let v = Vector3::from_array([S::ZERO, S::ONE, S::ZERO]);
235        let w = u.add(&v); // coplanar with u, v -- zero volume.
236        assert!(CoordinateSystem::try_new(origin, u, v, w).is_err());
237    }
238    #[test]
239    fn degenerate_basis_fails() {
240        for_all_scalars!(check_degenerate_basis_fails);
241    }
242}
geop_core_math/primitives/coordinate_system.rs

geop_core_math/primitives/
coordinate_system.rs
