fast_mesh_query/
rigid.rs

1//! Rigid transforms (rotation plus translation) between two meshes' local
2//! coordinates.
3
4use fast_mesh_bvh::Aabb;
5
6use crate::{QueryError, Result};
7
8/// How far the rotation may be from orthonormal with determinant +1.
9const TOLERANCE: f64 = 1e-6;
10
11/// Maps mesh B's local coordinates into mesh A's local coordinates:
12/// `p_a = rotation * p_b + translation`.
13///
14/// `rotation` is row-major (`rotation[i][j]` is row `i`, column `j`) and must
15/// be a proper rotation: orthonormal with determinant +1, within `1e-6` per
16/// entry. Scale and reflection are not supported, so distances measured in A's
17/// space are distances between the meshes. Queries given anything else, or
18/// non-finite values, fail with [`QueryError::InvalidInput`].
19///
20/// B's vertices are transformed in `f64` when they are tested and never stored.
21/// "Exact" intersection results are exact for those transformed `f64`
22/// coordinates.
23#[derive(Debug, Clone, Copy, PartialEq)]
24pub struct Rigid {
25    /// The rotation, row-major.
26    pub rotation: [[f64; 3]; 3],
27    /// The translation, applied after the rotation.
28    pub translation: [f64; 3],
29}
30
31impl Rigid {
32    /// The transform that leaves every point where it is.
33    pub const IDENTITY: Rigid = Rigid {
34        rotation: [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]],
35        translation: [0.0; 3],
36    };
37
38    /// The transform that maps A's coordinates back into B's: the transposed
39    /// rotation and the translation `-rotationᵀ * translation`.
40    pub fn inverse(&self) -> Rigid {
41        let r = &self.rotation;
42        let rotation: [[f64; 3]; 3] = std::array::from_fn(|i| std::array::from_fn(|j| r[j][i]));
43        let t = self.translation;
44        let translation = std::array::from_fn(|i| {
45            -(rotation[i][0] * t[0] + rotation[i][1] * t[1] + rotation[i][2] * t[2])
46        });
47        Rigid {
48            rotation,
49            translation,
50        }
51    }
52
53    /// Checks that every value is finite and that the rotation is a proper
54    /// rotation (orthonormal with determinant +1) within `1e-6` per entry: the
55    /// check every mesh-vs-mesh query runs first.
56    ///
57    /// # Errors
58    /// [`QueryError::InvalidInput`] for a non-finite value, a rotation that is
59    /// not orthonormal, or a reflection.
60    pub fn validate(&self) -> Result<()> {
61        let r = &self.rotation;
62        if !r
63            .iter()
64            .flatten()
65            .chain(&self.translation)
66            .all(|v| v.is_finite())
67        {
68            return Err(QueryError::InvalidInput("transform has a non-finite value"));
69        }
70        for i in 0..3 {
71            for j in 0..3 {
72                let dot = r[0][i] * r[0][j] + r[1][i] * r[1][j] + r[2][i] * r[2][j];
73                let want = if i == j { 1.0 } else { 0.0 };
74                if (dot - want).abs() > TOLERANCE {
75                    return Err(QueryError::InvalidInput(
76                        "transform rotation is not orthonormal",
77                    ));
78                }
79            }
80        }
81        let det = r[0][0] * (r[1][1] * r[2][2] - r[1][2] * r[2][1])
82            - r[0][1] * (r[1][0] * r[2][2] - r[1][2] * r[2][0])
83            + r[0][2] * (r[1][0] * r[2][1] - r[1][1] * r[2][0]);
84        if (det - 1.0).abs() > TOLERANCE {
85            return Err(QueryError::InvalidInput(
86                "transform rotation is a reflection",
87            ));
88        }
89        Ok(())
90    }
91
92    /// Maps a point of B into A's space: `rotation * p + translation`, in
93    /// `f64`.
94    ///
95    /// This is the exact arithmetic (same expression, same order) the queries
96    /// use to move B's vertices, so points transformed with it agree bit for
97    /// bit with what the queries tested. To map a result point such as
98    /// [`MeshDistance::point_b`](crate::MeshDistance::point_b) back into B's
99    /// space, use `apply` on [`Rigid::inverse`].
100    ///
101    /// ```
102    /// use fast_mesh_query::Rigid;
103    ///
104    /// // A quarter turn about z, then a step along x.
105    /// let xf = Rigid {
106    ///     rotation: [[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]],
107    ///     translation: [1.0, 0.0, 0.0],
108    /// };
109    /// assert_eq!(xf.validate(), Ok(()));
110    /// let in_a = xf.apply([1.0, 0.0, 0.0]);
111    /// assert_eq!(in_a, [1.0, 1.0, 0.0]);
112    /// let back = xf.inverse().apply(in_a.map(|v| v as f32));
113    /// assert_eq!(back, [1.0, 0.0, 0.0]);
114    /// ```
115    pub fn apply(&self, p: [f32; 3]) -> [f64; 3] {
116        self.apply_f64(p.map(f64::from))
117    }
118
119    /// [`Rigid::apply`] for an `f64` point. `transform_box` relies on this
120    /// exact expression and evaluation order.
121    pub(crate) fn apply_f64(&self, p: [f64; 3]) -> [f64; 3] {
122        let r = &self.rotation;
123        let t = self.translation;
124        std::array::from_fn(|i| r[i][0] * p[0] + r[i][1] * p[1] + r[i][2] * p[2] + t[i])
125    }
126
127    /// A box in A's space holding every point of `b` (a box in B's space) as
128    /// mapped by [`Rigid::apply`], rounded outward to `f32`. An empty box stays
129    /// empty.
130    ///
131    /// Each output bound is `apply` evaluated at the corner of `b` that
132    /// minimizes (or maximizes) that coordinate: the corner picks `min` or `max`
133    /// on each axis by the sign of the rotation entry. Because rounding is
134    /// monotone, `apply` of any point in `b` lands inside these bounds as
135    /// computed, not just in real arithmetic. The result is the exact image box
136    /// `center ± |rotation| * half_extent` up to rounding.
137    pub(crate) fn transform_box(&self, b: &Aabb) -> Aabb {
138        if b.is_empty() {
139            return Aabb::EMPTY;
140        }
141        let lo = b.min.map(f64::from);
142        let hi = b.max.map(f64::from);
143        let r = &self.rotation;
144        let t = self.translation;
145        let mut out = Aabb::EMPTY;
146        for i in 0..3 {
147            let low = |j: usize| if r[i][j] >= 0.0 { lo[j] } else { hi[j] };
148            let high = |j: usize| if r[i][j] >= 0.0 { hi[j] } else { lo[j] };
149            let min = r[i][0] * low(0) + r[i][1] * low(1) + r[i][2] * low(2) + t[i];
150            let max = r[i][0] * high(0) + r[i][1] * high(1) + r[i][2] * high(2) + t[i];
151            out.min[i] = round_down(min);
152            out.max[i] = round_up(max);
153        }
154        out
155    }
156}
157
158/// The largest `f32` at or below `x`.
159fn round_down(x: f64) -> f32 {
160    let f = x as f32;
161    if f64::from(f) > x { f.next_down() } else { f }
162}
163
164/// The smallest `f32` at or above `x`.
165fn round_up(x: f64) -> f32 {
166    let f = x as f32;
167    if f64::from(f) < x { f.next_up() } else { f }
168}
169
170#[cfg(test)]
171mod tests {
172    use super::*;
173    use crate::test_util::{Lcg, random_rotation};
174
175    fn rot_z(deg: f64) -> [[f64; 3]; 3] {
176        let (s, c) = deg.to_radians().sin_cos();
177        [[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]]
178    }
179
180    fn rigid(rotation: [[f64; 3]; 3], translation: [f64; 3]) -> Rigid {
181        Rigid {
182            rotation,
183            translation,
184        }
185    }
186
187    fn is_invalid(r: Rigid) -> bool {
188        matches!(r.validate(), Err(QueryError::InvalidInput(_)))
189    }
190
191    #[test]
192    fn rigid_validation() {
193        assert_eq!(Rigid::IDENTITY.validate(), Ok(()));
194        let r30 = rot_z(30.0);
195        assert_eq!(rigid(r30, [1.0, 2.0, 3.0]).validate(), Ok(()));
196        let r30_f32 = r30.map(|row| row.map(|v| f64::from(v as f32)));
197        assert_ne!(r30_f32, r30);
198        assert_eq!(rigid(r30_f32, [0.0; 3]).validate(), Ok(()));
199
200        let mut off = r30;
201        off[0][0] += 1e-3;
202        assert!(is_invalid(rigid(off, [0.0; 3])));
203        let reflection = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, -1.0]];
204        assert!(is_invalid(rigid(reflection, [0.0; 3])));
205        assert!(is_invalid(rigid(r30, [0.0, f64::NAN, 0.0])));
206        let mut inf = r30;
207        inf[2][1] = f64::INFINITY;
208        assert!(is_invalid(rigid(inf, [0.0; 3])));
209    }
210
211    #[test]
212    fn inverse_round_trips() {
213        let r = rigid(rot_z(30.0), [1.5, -2.0, 0.25]);
214        let inv = r.inverse();
215        assert_eq!(inv.validate(), Ok(()));
216        for p in [[0.0, 0.0, 0.0], [1.0, 2.0, 3.0], [-7.5, 0.125, 4.0]] {
217            let back = inv.apply_f64(r.apply(p));
218            for k in 0..3 {
219                assert!(
220                    (back[k] - f64::from(p[k])).abs() <= 1e-12,
221                    "{back:?} vs {p:?}"
222                );
223            }
224        }
225        // `inverse` of a point stored as f32 round-trips through `apply` too.
226        let p = [0.5f32, -1.0, 2.0];
227        let q = inv.apply(p);
228        let q32 = q.map(|v| v as f32);
229        let back = r.apply(q32);
230        for k in 0..3 {
231            assert!((back[k] - f64::from(p[k])).abs() <= 1e-6);
232        }
233    }
234
235    /// Asserts that `apply` puts every corner of `b`, and each of `extra`, inside `t`.
236    fn assert_inside(r: &Rigid, b: &Aabb, t: &Aabb, extra: &[[f32; 3]]) {
237        let corners = (0..8).map(|c: u32| -> [f32; 3] {
238            std::array::from_fn(|k| if c >> k & 1 == 0 { b.min[k] } else { b.max[k] })
239        });
240        for p in corners.chain(extra.iter().copied()) {
241            let q = r.apply(p);
242            let inside = q
243                .iter()
244                .zip(t.min.iter().zip(&t.max))
245                .all(|(&v, (&lo, &hi))| f64::from(lo) <= v && v <= f64::from(hi));
246            assert!(inside, "{p:?} -> {q:?} outside {t:?}");
247        }
248    }
249
250    #[test]
251    fn transform_box_is_conservative() {
252        let mut rng = Lcg(42);
253        for _ in 0..1000 {
254            let r = rigid(
255                random_rotation(&mut rng),
256                std::array::from_fn(|_| f64::from(rng.next()) * 200.0 - 100.0),
257            );
258            assert_eq!(r.validate(), Ok(()));
259            let lo: [f32; 3] = std::array::from_fn(|_| rng.next() * 20.0 - 10.0);
260            let size: [f32; 3] = std::array::from_fn(|_| rng.next() * 5.0);
261            let b = Aabb::new(lo, std::array::from_fn(|k| lo[k] + size[k]));
262            let inner: Vec<[f32; 3]> = (0..4)
263                .map(|_| std::array::from_fn(|k| b.min[k] + rng.next() * size[k]))
264                .filter(|p: &[f32; 3]| (0..3).all(|k| p[k] <= b.max[k]))
265                .collect();
266            assert_inside(&r, &b, &r.transform_box(&b), &inner);
267            assert_eq!(Rigid::IDENTITY.transform_box(&b), b);
268        }
269        assert!(Rigid::IDENTITY.transform_box(&Aabb::EMPTY).is_empty());
270        let r = rigid(rot_z(30.0), [1.0, 2.0, 3.0]);
271        assert!(r.transform_box(&Aabb::EMPTY).is_empty());
272    }
273
274    #[test]
275    fn transform_box_rounds_outward() {
276        // A 90 degree turn about z built from f64 sin/cos: entries like 6e-17
277        // instead of 0 move the corners off the f32 grid, so the box must widen.
278        let r = rigid(rot_z(90.0), [0.1, 0.0, 0.0]);
279        let b = Aabb::new([1.0, 1.0, 1.0], [2.0, 2.0, 2.0]);
280        let t = r.transform_box(&b);
281        assert_inside(&r, &b, &t, &[]);
282        // Still tight: within an f32 ulp or so of the exact box.
283        assert!((t.min[0] - (0.1 - 2.0)).abs() < 1e-5);
284        assert!((t.max[1] - 2.0).abs() < 1e-5);
285    }
286}
fast_mesh_query/rigid.rs

fast_mesh_query/
rigid.rs
