loam_math/
spherical_embedded.rs

1use glam::Vec4;
2
3use crate::rasterizable::{Projection, RasterizableSpace};
4use crate::space::{IsometryGroup, Space};
5use crate::spherical::Iso4;
6use crate::EuclideanR4;
7
8const GEODESIC_DIRECTION_MIN: f32 = 1e-7;
9
10const TRANSPORT_DENOM_MIN: f32 = 1e-7;
11
12const UNIT_POINT_TOLERANCE: f32 = 1e-4;
13
14/// S³ with unit Vec4 points and ambient tangents; methods assume unit points.
15#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)]
16pub struct SphericalS3Embedded;
17
18impl Space for SphericalS3Embedded {
19    type Point = Vec4;
20
21    type Vector = Vec4;
22
23    type Frame = Iso4;
24
25    fn frame_at(&self, at: Vec4) -> Iso4 {
26        Iso4::left_translation(at)
27    }
28
29    fn distance(&self, a: Vec4, b: Vec4) -> f32 {
30        let half_chord = (a - b).length() * 0.5;
31        2.0 * half_chord.clamp(0.0, 1.0).asin()
32    }
33
34    fn exp(&self, at: Vec4, v: Vec4) -> Vec4 {
35        let v_tan = v - v.dot(at) * at;
36        let theta = v_tan.length();
37        if theta < GEODESIC_DIRECTION_MIN {
38            return at;
39        }
40        at * theta.cos() + v_tan * (theta.sin() / theta)
41    }
42
43    fn log(&self, from: Vec4, to: Vec4) -> Vec4 {
44        let d = self.distance(from, to);
45
46        let dot = from.dot(to).clamp(-1.0, 1.0);
47        let perp = to - dot * from;
48        let n = perp.length();
49        if n < GEODESIC_DIRECTION_MIN {
50            return Vec4::ZERO;
51        }
52        perp * (d / n)
53    }
54
55    fn parallel_transport(&self, from: Vec4, to: Vec4, v: Vec4) -> Vec4 {
56        // do Carmo, Riemannian Geometry, ch. 2.
57        let sum = from + to;
58        let denom = (sum.length_squared() * 0.5).max(TRANSPORT_DENOM_MIN);
59        v - (v.dot(to) / denom) * sum
60    }
61
62    fn chart_envelope(&self) -> f32 {
63        std::f32::consts::PI
64    }
65
66    fn valid_point(&self, p: Vec4) -> bool {
67        p.is_finite() && (p.length_squared() - 1.0).abs() <= UNIT_POINT_TOLERANCE
68    }
69}
70
71impl IsometryGroup for SphericalS3Embedded {
72    type Iso = Iso4;
73
74    fn iso_identity(&self) -> Iso4 {
75        Iso4::IDENTITY
76    }
77
78    fn iso_compose(&self, a: Iso4, b: Iso4) -> Iso4 {
79        Iso4 {
80            matrix: a.matrix * b.matrix,
81        }
82    }
83
84    fn iso_inverse(&self, a: Iso4) -> Iso4 {
85        Iso4 {
86            matrix: a.matrix.transpose(),
87        }
88    }
89
90    fn iso_apply(&self, iso: Iso4, p: Vec4) -> Vec4 {
91        (iso.matrix * p).normalize()
92    }
93
94    fn iso_transport(&self, iso: Iso4, _at: Vec4, v: Vec4) -> Vec4 {
95        iso.matrix * v
96    }
97}
98
99impl RasterizableSpace<4> for SphericalS3Embedded {
100    fn point_to_array(p: Vec4) -> [f32; 4] {
101        p.to_array()
102    }
103
104    fn array_to_point(arr: [f32; 4]) -> Vec4 {
105        Vec4::from_array(arr).normalize()
106    }
107
108    fn project_point(point: Vec4, projection: &Projection<4>) -> Option<glam::Vec3> {
109        match projection {
110            Projection::Stereographic { pole } => {
111                Some(crate::rasterizable::stereographic_to_r3(point, *pole))
112            }
113
114            Projection::Identity
115            | Projection::Orthographic { .. }
116            | Projection::Perspective4D { .. }
117            | Projection::Schlegel { .. } => {
118                <EuclideanR4 as RasterizableSpace<4>>::project_point(point, projection)
119            }
120        }
121    }
122
123    fn tessellate_segment(p0: Vec4, p1: Vec4, samples: usize, mut emit: impl FnMut(Vec4)) {
124        // Absil, Mahony and Sepulchre, Optimization Algorithms on Matrix Manifolds, 2008, §3.6.
125        // Shoemake, Animating Rotation with Quaternion Curves, 1985.
126        let dot = p0.dot(p1).clamp(-1.0, 1.0);
127        let half_chord = (p0 - p1).length() * 0.5;
128        let omega = 2.0 * half_chord.clamp(0.0, 1.0).asin();
129
130        let perp = p1 - dot * p0;
131        let n = perp.length();
132        let dir = if n > GEODESIC_DIRECTION_MIN {
133            perp / n
134        } else {
135            deterministic_perp(p0)
136        };
137        emit(p0);
138        for i in 1..samples {
139            let ang = i as f32 / samples as f32 * omega;
140            emit(ang.cos() * p0 + ang.sin() * dir);
141        }
142        emit(p1);
143    }
144}
145
146// do Carmo, Differential Geometry of Curves and Surfaces, 1976, §1.4.
147fn deterministic_perp(p0: Vec4) -> Vec4 {
148    let a = p0.abs();
149
150    let mut min_idx = 0usize;
151    let mut min_v = a.x;
152    if a.y < min_v {
153        min_v = a.y;
154        min_idx = 1;
155    }
156    if a.z < min_v {
157        min_v = a.z;
158        min_idx = 2;
159    }
160    if a.w < min_v {
161        min_idx = 3;
162    }
163    let axis = match min_idx {
164        0 => Vec4::X,
165        1 => Vec4::Y,
166        2 => Vec4::Z,
167        _ => Vec4::W,
168    };
169    (axis - axis.dot(p0) * p0).normalize()
170}
171
172#[cfg(test)]
173mod tests {
174    use super::*;
175    use approx::assert_relative_eq;
176    use std::f32::consts::PI;
177
178    fn s3() -> SphericalS3Embedded {
179        SphericalS3Embedded
180    }
181
182    #[test]
183    fn distance_orthonormal_is_quarter_circle() {
184        let s = s3();
185        assert_relative_eq!(s.distance(Vec4::X, Vec4::Y), PI / 2.0, epsilon = 1e-6);
186        assert_relative_eq!(s.distance(Vec4::X, Vec4::W), PI / 2.0, epsilon = 1e-6);
187    }
188
189    #[test]
190    fn distance_at_antipode_is_pi() {
191        let s = s3();
192        let a = Vec4::new(0.5, 0.5, 0.5, 0.5);
193        assert_relative_eq!(s.distance(a, -a), PI, epsilon = 1e-5);
194    }
195
196    #[test]
197    fn log_magnitude_is_distance_and_is_tangent() {
198        let s = s3();
199        let from = Vec4::new(0.3, 0.2, 0.1, 0.9).normalize();
200        let to = Vec4::new(-0.2, 0.5, 0.0, 0.8).normalize();
201        let v = s.log(from, to);
202        assert_relative_eq!(v.length(), s.distance(from, to), epsilon = 1e-5);
203        assert_relative_eq!(v.dot(from), 0.0, epsilon = 1e-6);
204    }
205
206    #[test]
207    fn exp_stays_on_sphere_with_non_tangent_input() {
208        let s = s3();
209        let at = Vec4::new(0.0, 0.0, 0.0, 1.0);
210        let v = Vec4::new(0.4, 0.2, 0.0, 0.7);
211        let moved = s.exp(at, v);
212        assert_relative_eq!(moved.length(), 1.0, epsilon = 1e-6);
213    }
214
215    #[test]
216    fn parallel_transport_preserves_norm_and_tangency() {
217        let s = s3();
218        let from = Vec4::new(0.0, 0.0, 0.0, 1.0);
219        let to = Vec4::new(0.6, 0.0, 0.0, 0.8);
220        let v = Vec4::new(0.5, 0.0, 0.0, 0.0);
221        let vt = s.parallel_transport(from, to, v);
222        assert_relative_eq!(vt.length(), v.length(), epsilon = 1e-5);
223        assert_relative_eq!(vt.dot(to), 0.0, epsilon = 1e-5);
224        assert!((vt - v).length() > 1e-3, "in-plane vector should rotate");
225    }
226
227    #[test]
228    fn parallel_transport_preserves_norm_near_antipode() {
229        let s = s3();
230        let from = Vec4::X;
231        for delta in [3e-3_f32, 2e-3, 1e-3] {
232            let omega = PI - delta;
233            let to = Vec4::new(omega.cos(), omega.sin(), 0.0, 0.0).normalize();
234
235            let v = Vec4::Y;
236            let vt = s.parallel_transport(from, to, v);
237            assert_relative_eq!(vt.length(), v.length(), epsilon = 1e-3);
238            assert_relative_eq!(vt.dot(to), 0.0, epsilon = 1e-3);
239        }
240    }
241
242    #[test]
243    fn iso_transport_keeps_tangency_and_norm() {
244        let s = s3();
245        let iso = Iso4::from_translation(glam::Vec3::new(0.2, -0.1, 0.15));
246        let at = Vec4::new(0.0, 0.0, 0.0, 1.0);
247        let v = Vec4::new(0.3, 0.2, 0.0, 0.0);
248        let moved_at = s.iso_apply(iso, at);
249        let moved_v = s.iso_transport(iso, at, v);
250        assert_relative_eq!(moved_v.length(), v.length(), epsilon = 1e-5);
251        assert_relative_eq!(moved_v.dot(moved_at), 0.0, epsilon = 1e-5);
252    }
253
254    #[test]
255    fn slerp_endpoints_exact_and_count() {
256        let p0 = Vec4::X;
257        let p1 = Vec4::Y;
258        let mut out = Vec::new();
259        <SphericalS3Embedded as RasterizableSpace<4>>::tessellate_segment(p0, p1, 4, |p| {
260            out.push(p)
261        });
262        assert_eq!(out.len(), 5);
263        assert_relative_eq!(out[0].x, p0.x, epsilon = 1e-6);
264        assert_relative_eq!(out[4].y, p1.y, epsilon = 1e-6);
265    }
266
267    #[test]
268    fn slerp_midpoint_is_on_great_circle_not_chord() {
269        let s = s3();
270        let p0 = Vec4::new(1.0, 0.0, 0.0, 0.0);
271        let p1 = Vec4::new(0.0, 0.0, 0.0, 1.0);
272        let mut out = Vec::new();
273        <SphericalS3Embedded as RasterizableSpace<4>>::tessellate_segment(p0, p1, 2, |p| {
274            out.push(p)
275        });
276        let mid = out[1];
277        assert_relative_eq!(s.distance(mid, p0), s.distance(mid, p1), epsilon = 1e-6);
278
279        let c = (PI / 4.0).cos();
280        assert_relative_eq!(mid.x, c, epsilon = 1e-5);
281        assert_relative_eq!(mid.w, c, epsilon = 1e-5);
282        assert!(mid.x > 0.5, "slerp midpoint must bulge off the chord");
283    }
284
285    #[test]
286    fn slerp_antipode_produces_finite_unit_samples() {
287        let p0 = Vec4::X;
288        let p1 = -Vec4::X;
289        let mut out = Vec::new();
290        <SphericalS3Embedded as RasterizableSpace<4>>::tessellate_segment(p0, p1, 16, |p| {
291            out.push(p)
292        });
293        assert_eq!(out.len(), 17);
294        for p in &out {
295            assert!(
296                p.is_finite(),
297                "antipodal slerp sample must be finite: {p:?}"
298            );
299            assert_relative_eq!(p.length(), 1.0, epsilon = 1e-6);
300        }
301    }
302
303    #[test]
304    fn slerp_near_antipode_samples_stay_on_sphere() {
305        let p0 = Vec4::X;
306
307        for delta in [1e-3_f32, 1e-5, GEODESIC_DIRECTION_MIN * 2.0] {
308            let omega = PI - delta;
309            let p1 = Vec4::new(omega.cos(), omega.sin(), 0.0, 0.0).normalize();
310            let mut out = Vec::new();
311            <SphericalS3Embedded as RasterizableSpace<4>>::tessellate_segment(p0, p1, 16, |p| {
312                out.push(p)
313            });
314            for p in &out[1..out.len() - 1] {
315                assert!(p.is_finite(), "near-antipode sample must be finite: {p:?}");
316                assert!(
317                    (p.length() - 1.0).abs() < 1e-4,
318                    "near-antipode (omega = PI - {delta:e}) sample off-sphere: |p| = {}",
319                    p.length()
320                );
321            }
322        }
323    }
324
325    #[test]
326    fn slerp_consecutive_arc_sum_equals_total() {
327        let s = s3();
328        let p0 = Vec4::new(0.2, 0.1, -0.3, 0.9).normalize();
329        let p1 = Vec4::new(-0.1, 0.4, 0.2, 0.8).normalize();
330        let total = s.distance(p0, p1);
331        for samples in [2usize, 3, 8, 17] {
332            let mut out = Vec::new();
333            <SphericalS3Embedded as RasterizableSpace<4>>::tessellate_segment(
334                p0,
335                p1,
336                samples,
337                |p| out.push(p),
338            );
339            let arc_sum: f32 = out.windows(2).map(|w| s.distance(w[0], w[1])).sum();
340            assert_relative_eq!(arc_sum, total, epsilon = 1e-6);
341        }
342    }
343
344    #[test]
345    fn slerp_near_coincident_falls_back_on_sphere() {
346        let p0 = Vec4::new(0.1, -0.2, 0.3, 0.9).normalize();
347        let nudge = GEODESIC_DIRECTION_MIN * 0.01;
348        let p1 = (p0 + Vec4::new(nudge, 0.0, -nudge, 0.0)).normalize();
349        let mut out = Vec::new();
350        <SphericalS3Embedded as RasterizableSpace<4>>::tessellate_segment(p0, p1, 8, |p| {
351            out.push(p)
352        });
353        for p in &out {
354            assert!(p.is_finite(), "coincident sample must be finite: {p:?}");
355            assert_relative_eq!(p.length(), 1.0, epsilon = 1e-6);
356            assert!(
357                s3().distance(*p, p0) < 1e-4,
358                "coincident-arc sample should stay at p0, dist {}",
359                s3().distance(*p, p0)
360            );
361        }
362    }
363
364    #[test]
365    fn project_point_stereographic_is_conformal_map_not_drop_w() {
366        let p = Vec4::new(0.5, 0.5, 0.5, 0.5);
367        let proj = Projection::Stereographic { pole: Vec4::W };
368        let got = <SphericalS3Embedded as RasterizableSpace<4>>::project_point(p, &proj)
369            .expect("stereographic should accept an off-pole point");
370        let want = glam::Vec3::new(p.x, p.y, p.z) / (1.0 - p.w);
371        assert_relative_eq!(got.x, want.x, epsilon = 1e-6);
372        assert_relative_eq!(got.y, want.y, epsilon = 1e-6);
373        assert_relative_eq!(got.z, want.z, epsilon = 1e-6);
374        let drop_w = glam::Vec3::new(p.x, p.y, p.z);
375        assert!(
376            (got - drop_w).length() > 1e-3,
377            "stereographic must scale by 1/(1-w), not pass through drop-w; got {got:?}"
378        );
379    }
380}
loam_math/spherical_embedded.rs

loam_math/
spherical_embedded.rs
