loam_math/
blended.rs

1use std::borrow::Cow;
2
3use glam::{Mat3, Vec3};
4
5use crate::space::{Space, WgslAccuracy, WgslSpace};
6
7/// `g_ij(p) = f(p)·δ_ij` in the chart `Space::Point` carries, not in some other chart.
8pub trait ConformallyFlat: Space {
9    /// May return `f32::INFINITY` at the chart boundary.
10    fn conformal_factor(&self, p: Vec3) -> f32;
11
12    /// `R = −(4/f)·[∇²φ + |∇φ|²/2]`; the default is finite-difference.
13    fn scalar_curvature(&self, p: Vec3) -> f32 {
14        const EPS: f32 = 5.0e-3;
15        let phi_at = self.conformal_log_half(p);
16        let lap = (self.conformal_log_half(p + Vec3::X * EPS)
17            + self.conformal_log_half(p - Vec3::X * EPS)
18            + self.conformal_log_half(p + Vec3::Y * EPS)
19            + self.conformal_log_half(p - Vec3::Y * EPS)
20            + self.conformal_log_half(p + Vec3::Z * EPS)
21            + self.conformal_log_half(p - Vec3::Z * EPS)
22            - 6.0 * phi_at)
23            / (EPS * EPS);
24        let grad = self.conformal_log_half_gradient(p);
25        let grad_sq = grad.length_squared();
26        let f_p = self.conformal_factor(p);
27        if !f_p.is_finite() || f_p <= 0.0 {
28            return 0.0;
29        }
30        -(4.0 / f_p) * (lap + 0.5 * grad_sq)
31    }
32
33    fn conformal_log_half(&self, p: Vec3) -> f32 {
34        0.5 * self.conformal_factor(p).ln()
35    }
36
37    fn conformal_log_half_gradient(&self, p: Vec3) -> Vec3 {
38        const EPS: f32 = 1.0e-3;
39        let dx = (self.conformal_log_half(p + Vec3::X * EPS)
40            - self.conformal_log_half(p - Vec3::X * EPS))
41            / (2.0 * EPS);
42        let dy = (self.conformal_log_half(p + Vec3::Y * EPS)
43            - self.conformal_log_half(p - Vec3::Y * EPS))
44            / (2.0 * EPS);
45        let dz = (self.conformal_log_half(p + Vec3::Z * EPS)
46            - self.conformal_log_half(p - Vec3::Z * EPS))
47            / (2.0 * EPS);
48        Vec3::new(dx, dy, dz)
49    }
50}
51
52impl ConformallyFlat for crate::EuclideanR3 {
53    fn conformal_factor(&self, _p: Vec3) -> f32 {
54        1.0
55    }
56    fn conformal_log_half(&self, _p: Vec3) -> f32 {
57        0.0
58    }
59    fn conformal_log_half_gradient(&self, _p: Vec3) -> Vec3 {
60        Vec3::ZERO
61    }
62    fn scalar_curvature(&self, _p: Vec3) -> f32 {
63        0.0
64    }
65}
66
67impl ConformallyFlat for crate::HyperbolicH3 {
68    fn conformal_factor(&self, p: Vec3) -> f32 {
69        let r2 = p.length_squared();
70        let denom = (1.0 - r2).max(0.0);
71        if denom <= 0.0 {
72            return f32::INFINITY;
73        }
74        4.0 / (denom * denom)
75    }
76    fn conformal_log_half(&self, p: Vec3) -> f32 {
77        let r2 = p.length_squared();
78        let denom = (1.0 - r2).max(0.0);
79        if denom <= 0.0 {
80            return f32::INFINITY;
81        }
82        std::f32::consts::LN_2 - denom.ln()
83    }
84    fn conformal_log_half_gradient(&self, p: Vec3) -> Vec3 {
85        let r2 = p.length_squared();
86        let denom = (1.0 - r2).max(0.0);
87        if denom <= 0.0 {
88            return Vec3::ZERO;
89        }
90        p * (2.0 / denom)
91    }
92    fn scalar_curvature(&self, _p: Vec3) -> f32 {
93        -6.0
94    }
95}
96
97/// Metric `g = (1 − α)·g_A + α·g_B`, with a spatially varying [`BlendingField`].
98pub struct BlendedSpace<A, B, F>
99where
100    A: Space<Point = Vec3, Vector = Vec3>,
101    B: Space<Point = Vec3, Vector = Vec3>,
102    F: BlendingField,
103{
104    pub a: A,
105    pub b: B,
106    pub field: F,
107}
108
109impl<A, B, F> BlendedSpace<A, B, F>
110where
111    A: Space<Point = Vec3, Vector = Vec3>,
112    B: Space<Point = Vec3, Vector = Vec3>,
113    F: BlendingField,
114{
115    pub fn new(a: A, b: B, field: F) -> Self {
116        Self { a, b, field }
117    }
118}
119
120impl<A, B, F> Space for BlendedSpace<A, B, F>
121where
122    A: Space<Point = Vec3, Vector = Vec3> + ConformallyFlat,
123    B: Space<Point = Vec3, Vector = Vec3> + ConformallyFlat,
124    F: BlendingField,
125{
126    type Point = Vec3;
127    type Vector = Vec3;
128    type Frame = Mat3;
129
130    fn frame_at(&self, at: Vec3) -> Mat3 {
131        Mat3::from_diagonal(Vec3::splat(1.0 / self.conformal_factor(at).sqrt()))
132    }
133
134    fn distance(&self, a: Vec3, b: Vec3) -> f32 {
135        let log = self.log(a, b);
136        let f_a = self.conformal_factor(a);
137        f_a.sqrt() * log.length()
138    }
139
140    fn exp(&self, at: Vec3, v: Vec3) -> Vec3 {
141        rk4_geodesic(self, at, v, GEODESIC_DEFAULT_STEPS).0
142    }
143
144    fn log(&self, from: Vec3, to: Vec3) -> Vec3 {
145        gauss_newton_log(self, from, to, GEODESIC_DEFAULT_STEPS, LOG_MAX_ITERS)
146    }
147
148    fn parallel_transport(&self, from: Vec3, to: Vec3, v: Vec3) -> Vec3 {
149        transport_along_geodesic(self, from, to, v)
150    }
151
152    fn chart_envelope(&self) -> f32 {
153        self.a.chart_envelope().min(self.b.chart_envelope())
154    }
155
156    fn valid_point(&self, p: Vec3) -> bool {
157        self.a.valid_point(p) && self.b.valid_point(p)
158    }
159}
160
161pub const GEODESIC_DEFAULT_STEPS: u32 = 32;
162
163/// Maximum accepted heuristic from metric-scaled RK4 step doubling.
164pub const GEODESIC_ERROR_BUDGET: f32 = 1.0e-3;
165
166#[derive(Clone, Copy, Debug, PartialEq)]
167pub struct GeodesicState {
168    pub point: Vec3,
169    pub velocity: Vec3,
170    pub frame: Mat3,
171    pub estimated_error: f32,
172}
173
174#[derive(Clone, Copy, Debug, PartialEq)]
175pub struct GeodesicLog {
176    pub vector: Vec3,
177    pub estimated_error: f32,
178}
179
180#[derive(Clone, Copy, Debug, PartialEq)]
181pub enum GeodesicError {
182    InvalidStepCount,
183    InvalidErrorBudget,
184    NonFinite,
185    ChartBoundary,
186    ErrorBudget { estimated: f32, budget: f32 },
187    Singular,
188    NoConvergence,
189}
190
191#[derive(Clone, Copy)]
192struct GeodesicDerivative {
193    point: Vec3,
194    velocity: Vec3,
195    frame: Mat3,
196}
197
198fn validate_geodesic_point<S: ConformallyFlat<Point = Vec3, Vector = Vec3>>(
199    space: &S,
200    point: Vec3,
201) -> Result<(), GeodesicError> {
202    if !point.is_finite() {
203        return Err(GeodesicError::NonFinite);
204    }
205    if !space.valid_point(point) {
206        return Err(GeodesicError::ChartBoundary);
207    }
208    Ok(())
209}
210
211// Wald, General Relativity, 1984, App. D.
212fn transport_rhs(gradient: Vec3, tangent: Vec3, v: Vec3) -> Vec3 {
213    -(v * gradient.dot(tangent) + tangent * gradient.dot(v) - gradient * tangent.dot(v))
214}
215
216#[inline]
217fn geodesic_derivative<S: ConformallyFlat<Point = Vec3, Vector = Vec3>>(
218    space: &S,
219    state: &GeodesicState,
220    fault: &mut Option<GeodesicError>,
221) -> GeodesicDerivative {
222    let gradient = space.conformal_log_half_gradient(state.point);
223    let velocity_squared = state.velocity.length_squared();
224    let derivative = GeodesicDerivative {
225        point: state.velocity,
226        velocity: gradient * velocity_squared
227            - state.velocity * (2.0 * gradient.dot(state.velocity)),
228        frame: Mat3::from_cols(
229            transport_rhs(gradient, state.velocity, state.frame.x_axis),
230            transport_rhs(gradient, state.velocity, state.frame.y_axis),
231            transport_rhs(gradient, state.velocity, state.frame.z_axis),
232        ),
233    };
234    if fault.is_none() {
235        *fault = match validate_geodesic_point(space, state.point) {
236            Err(found) => Some(found),
237            Ok(()) => (!state.velocity.is_finite()
238                || !state.frame.is_finite()
239                || !gradient.is_finite()
240                || !derivative.velocity.is_finite()
241                || !derivative.frame.is_finite())
242            .then_some(GeodesicError::NonFinite),
243        };
244    }
245    derivative
246}
247
248fn advance_geodesic_state(
249    state: GeodesicState,
250    derivative: GeodesicDerivative,
251    scale: f32,
252) -> GeodesicState {
253    GeodesicState {
254        point: state.point + derivative.point * scale,
255        velocity: state.velocity + derivative.velocity * scale,
256        frame: state.frame + derivative.frame * scale,
257        estimated_error: state.estimated_error,
258    }
259}
260
261fn rk4_geodesic_frame_step<S: ConformallyFlat<Point = Vec3, Vector = Vec3>>(
262    space: &S,
263    state: GeodesicState,
264    h: f32,
265) -> (GeodesicState, Option<GeodesicError>) {
266    let mut fault = None;
267    let k1 = geodesic_derivative(space, &state, &mut fault);
268    let k2 = geodesic_derivative(
269        space,
270        &advance_geodesic_state(state, k1, h * 0.5),
271        &mut fault,
272    );
273    let k3 = geodesic_derivative(
274        space,
275        &advance_geodesic_state(state, k2, h * 0.5),
276        &mut fault,
277    );
278    let k4 = geodesic_derivative(space, &advance_geodesic_state(state, k3, h), &mut fault);
279    let point = state.point + (k1.point + k2.point * 2.0 + k3.point * 2.0 + k4.point) * (h / 6.0);
280    let velocity = state.velocity
281        + (k1.velocity + k2.velocity * 2.0 + k3.velocity * 2.0 + k4.velocity) * (h / 6.0);
282    let frame = state.frame + (k1.frame + k2.frame * 2.0 + k3.frame * 2.0 + k4.frame) * (h / 6.0);
283    let next = GeodesicState {
284        point,
285        velocity,
286        frame,
287        estimated_error: state.estimated_error,
288    };
289    geodesic_derivative(space, &next, &mut fault);
290    (next, fault)
291}
292
293fn geodesic_state_error<S: ConformallyFlat<Point = Vec3, Vector = Vec3>>(
294    space: &S,
295    coarse: GeodesicState,
296    refined: GeodesicState,
297) -> Result<f32, GeodesicError> {
298    let factor = space.conformal_factor(refined.point);
299    if !factor.is_finite() || factor <= 0.0 {
300        return Err(GeodesicError::NonFinite);
301    }
302    let scale = factor.sqrt() / 15.0;
303    let point = (refined.point - coarse.point).length() * scale;
304    let velocity = (refined.velocity - coarse.velocity).length() * scale;
305    let frame = (refined.frame.x_axis - coarse.frame.x_axis)
306        .length()
307        .max((refined.frame.y_axis - coarse.frame.y_axis).length())
308        .max((refined.frame.z_axis - coarse.frame.z_axis).length())
309        * scale;
310    let error = point.max(velocity).max(frame);
311    if !error.is_finite() {
312        return Err(GeodesicError::NonFinite);
313    }
314    Ok(error)
315}
316
317fn integrate_geodesic_frame<S: ConformallyFlat<Point = Vec3, Vector = Vec3>>(
318    space: &S,
319    at: Vec3,
320    velocity: Vec3,
321    frame: Mat3,
322    n_steps: u32,
323    error_budget: Option<f32>,
324) -> (GeodesicState, Option<GeodesicError>) {
325    let mut state = GeodesicState {
326        point: at,
327        velocity,
328        frame,
329        estimated_error: 0.0,
330    };
331    if let Some(budget) = error_budget {
332        if n_steps == 0 {
333            return (state, Some(GeodesicError::InvalidStepCount));
334        }
335        if !budget.is_finite() || budget <= 0.0 {
336            return (state, Some(GeodesicError::InvalidErrorBudget));
337        }
338    }
339    let h = 1.0 / n_steps as f32;
340    for _ in 0..n_steps {
341        let (coarse, fault) = rk4_geodesic_frame_step(space, state, h);
342        let Some(budget) = error_budget else {
343            if !coarse.point.is_finite() || !coarse.velocity.is_finite() {
344                tracing::warn!(
345                    "rk4_geodesic step produced non-finite state; clamping to previous step"
346                );
347                break;
348            }
349            state = coarse;
350            continue;
351        };
352        if let Some(fault) = fault {
353            return (state, Some(fault));
354        }
355        let (half, fault) = rk4_geodesic_frame_step(space, state, h * 0.5);
356        if let Some(fault) = fault {
357            return (state, Some(fault));
358        }
359        let (refined, fault) = rk4_geodesic_frame_step(space, half, h * 0.5);
360        if let Some(fault) = fault {
361            return (state, Some(fault));
362        }
363        let estimated_error = match geodesic_state_error(space, coarse, refined) {
364            Ok(error) => state.estimated_error + error,
365            Err(fault) => return (state, Some(fault)),
366        };
367        if estimated_error > budget {
368            return (
369                state,
370                Some(GeodesicError::ErrorBudget {
371                    estimated: estimated_error,
372                    budget,
373                }),
374            );
375        }
376        state = GeodesicState {
377            estimated_error,
378            ..refined
379        };
380    }
381    (state, None)
382}
383
384pub fn integrate_geodesic_frame_checked<S: ConformallyFlat<Point = Vec3, Vector = Vec3>>(
385    space: &S,
386    at: Vec3,
387    velocity: Vec3,
388    frame: Mat3,
389    n_steps: u32,
390    error_budget: f32,
391) -> Result<GeodesicState, GeodesicError> {
392    match integrate_geodesic_frame(space, at, velocity, frame, n_steps, Some(error_budget)) {
393        (state, None) => Ok(state),
394        (_, Some(fault)) => Err(fault),
395    }
396}
397
398// Press et al., Numerical Recipes, 3rd ed., 2007, §18.1; Nocedal and Wright, Numerical Optimization, 2nd ed., 2006, ch. 10.
399fn gauss_newton_log_core<S: ConformallyFlat<Point = Vec3, Vector = Vec3>>(
400    space: &S,
401    from: Vec3,
402    to: Vec3,
403    n_steps: u32,
404    max_iters: u32,
405    error_budget: Option<f32>,
406) -> (GeodesicLog, Option<GeodesicError>) {
407    let mut log = GeodesicLog {
408        vector: Vec3::ZERO,
409        estimated_error: 0.0,
410    };
411    let mut target_scale = 0.0;
412    if let Some(budget) = error_budget {
413        if n_steps == 0 {
414            return (log, Some(GeodesicError::InvalidStepCount));
415        }
416        if !budget.is_finite() || budget <= 0.0 {
417            return (log, Some(GeodesicError::InvalidErrorBudget));
418        }
419        if let Err(fault) = validate_geodesic_point(space, from) {
420            return (log, Some(fault));
421        }
422        if let Err(fault) = validate_geodesic_point(space, to) {
423            return (log, Some(fault));
424        }
425        if from == to {
426            return (log, None);
427        }
428        let target_factor = space.conformal_factor(to);
429        if !target_factor.is_finite() || target_factor <= 0.0 {
430            return (log, Some(GeodesicError::NonFinite));
431        }
432        target_scale = target_factor.sqrt();
433    } else if from == to {
434        return (log, None);
435    }
436    log.vector = to - from;
437    for _ in 0..max_iters {
438        let (state, fault) = integrate_geodesic_frame(
439            space,
440            from,
441            log.vector,
442            Mat3::IDENTITY,
443            n_steps,
444            error_budget,
445        );
446        if let Some(fault) = fault {
447            return (log, Some(fault));
448        }
449        let residual = to - state.point;
450        log.estimated_error = state.estimated_error + residual.length() * target_scale;
451        let converged = match error_budget {
452            Some(budget) => log.estimated_error <= budget,
453            None => residual.length() < LOG_RESIDUAL_TOL,
454        };
455        if converged {
456            return (log, None);
457        }
458        let two_eps = 2.0 * LOG_JACOBIAN_EPS;
459        let mut jacobian = Mat3::ZERO;
460        for axis in 0..3 {
461            let mut delta = Vec3::ZERO;
462            delta[axis] = LOG_JACOBIAN_EPS;
463            let (plus, fault) = integrate_geodesic_frame(
464                space,
465                from,
466                log.vector + delta,
467                Mat3::IDENTITY,
468                n_steps,
469                error_budget,
470            );
471            if let Some(fault) = fault {
472                return (log, Some(fault));
473            }
474            let (minus, fault) = integrate_geodesic_frame(
475                space,
476                from,
477                log.vector - delta,
478                Mat3::IDENTITY,
479                n_steps,
480                error_budget,
481            );
482            if let Some(fault) = fault {
483                return (log, Some(fault));
484            }
485            *jacobian.col_mut(axis) = (plus.point - minus.point) / two_eps;
486        }
487        let determinant = jacobian.determinant();
488        if !determinant.is_finite() {
489            return (log, Some(GeodesicError::NonFinite));
490        }
491        if determinant.abs() < 1.0e-8 {
492            return (log, Some(GeodesicError::Singular));
493        }
494        let next = log.vector + jacobian.inverse() * residual;
495        if !next.is_finite() {
496            return (log, Some(GeodesicError::NonFinite));
497        }
498        log.vector = next;
499    }
500    (log, Some(GeodesicError::NoConvergence))
501}
502
503pub fn geodesic_log_checked<S: ConformallyFlat<Point = Vec3, Vector = Vec3>>(
504    space: &S,
505    from: Vec3,
506    to: Vec3,
507    n_steps: u32,
508    max_iters: u32,
509    error_budget: f32,
510) -> Result<GeodesicLog, GeodesicError> {
511    match gauss_newton_log_core(space, from, to, n_steps, max_iters, Some(error_budget)) {
512        (log, None) => Ok(log),
513        (_, Some(fault)) => Err(fault),
514    }
515}
516
517pub fn integrate_geodesic_frame_to_checked<S: ConformallyFlat<Point = Vec3, Vector = Vec3>>(
518    space: &S,
519    from: Vec3,
520    to: Vec3,
521    frame: Mat3,
522    n_steps: u32,
523    max_iters: u32,
524    error_budget: f32,
525) -> Result<GeodesicState, GeodesicError> {
526    let logarithm = geodesic_log_checked(space, from, to, n_steps, max_iters, error_budget)?;
527    let mut state = integrate_geodesic_frame_checked(
528        space,
529        from,
530        logarithm.vector,
531        frame,
532        n_steps,
533        error_budget,
534    )?;
535    state.estimated_error = state.estimated_error.max(logarithm.estimated_error);
536    Ok(state)
537}
538
539pub fn rk4_geodesic<S: ConformallyFlat<Point = Vec3, Vector = Vec3>>(
540    space: &S,
541    at: Vec3,
542    v: Vec3,
543    n_steps: u32,
544) -> (Vec3, Vec3) {
545    let (state, _) = integrate_geodesic_frame(space, at, v, Mat3::IDENTITY, n_steps, None);
546    (state.point, state.velocity)
547}
548
549fn transport_along_geodesic<S: ConformallyFlat<Point = Vec3, Vector = Vec3>>(
550    space: &S,
551    from: Vec3,
552    to: Vec3,
553    v: Vec3,
554) -> Vec3 {
555    let tangent = gauss_newton_log(space, from, to, GEODESIC_DEFAULT_STEPS, LOG_MAX_ITERS);
556    let (state, _) = integrate_geodesic_frame(
557        space,
558        from,
559        tangent,
560        Mat3::IDENTITY,
561        GEODESIC_DEFAULT_STEPS,
562        None,
563    );
564    state.frame * v
565}
566
567pub const LOG_MAX_ITERS: u32 = 12;
568
569/// Euclidean threshold on the residual `|to − exp_from(v)|`.
570pub const LOG_RESIDUAL_TOL: f32 = 1.0e-5;
571
572const LOG_JACOBIAN_EPS: f32 = 1.0e-3;
573
574/// Solves `exp_from(v) ≈ to`; failure returns the last finite iterate.
575pub fn gauss_newton_log<S: ConformallyFlat<Point = Vec3, Vector = Vec3>>(
576    space: &S,
577    from: Vec3,
578    to: Vec3,
579    n_steps: u32,
580    max_iters: u32,
581) -> Vec3 {
582    gauss_newton_log_checked(space, from, to, n_steps, max_iters).0
583}
584
585#[derive(Clone, Copy, Debug, PartialEq, Eq)]
586pub enum LogError {
587    Singular,
588    NoConvergence,
589}
590
591/// `gauss_newton_log` with its failure reported instead of hidden; the vector is still the last finite iterate.
592pub fn gauss_newton_log_checked<S: ConformallyFlat<Point = Vec3, Vector = Vec3>>(
593    space: &S,
594    from: Vec3,
595    to: Vec3,
596    n_steps: u32,
597    max_iters: u32,
598) -> (Vec3, Option<LogError>) {
599    let (log, fault) = gauss_newton_log_core(space, from, to, n_steps, max_iters, None);
600    match fault {
601        None => (log.vector, None),
602        Some(GeodesicError::Singular) => {
603            tracing::warn!(
604                "gauss_newton_log: singular Jacobian; returning best guess. \
605                 `to` may be in the cut locus of `from`."
606            );
607            (log.vector, Some(LogError::Singular))
608        }
609        Some(_) => {
610            tracing::warn!(
611                "gauss_newton_log: did not converge in {max_iters} iters; \
612                 residual remained > {LOG_RESIDUAL_TOL}. Returning best guess."
613            );
614            (log.vector, Some(LogError::NoConvergence))
615        }
616    }
617}
618
619impl<A, B, F> ConformallyFlat for BlendedSpace<A, B, F>
620where
621    A: Space<Point = Vec3, Vector = Vec3> + ConformallyFlat,
622    B: Space<Point = Vec3, Vector = Vec3> + ConformallyFlat,
623    F: BlendingField,
624{
625    fn conformal_factor(&self, p: Vec3) -> f32 {
626        let alpha = self.field.weight(p);
627        let f_a = self.a.conformal_factor(p);
628        let f_b = self.b.conformal_factor(p);
629
630        if alpha <= 0.0 {
631            return f_a;
632        }
633        if alpha >= 1.0 {
634            return f_b;
635        }
636        (1.0 - alpha) * f_a + alpha * f_b
637    }
638
639    fn conformal_log_half_gradient(&self, p: Vec3) -> Vec3 {
640        let alpha = self.field.weight(p);
641        if alpha <= 0.0 {
642            return self.a.conformal_log_half_gradient(p);
643        }
644        if alpha >= 1.0 {
645            return self.b.conformal_log_half_gradient(p);
646        }
647        let f_a = self.a.conformal_factor(p);
648        let f_b = self.b.conformal_factor(p);
649        let f = (1.0 - alpha) * f_a + alpha * f_b;
650
651        if !f.is_finite() || f <= 0.0 {
652            return Vec3::NAN;
653        }
654        let grad_alpha = self.field.gradient(p);
655        let grad_phi_a = self.a.conformal_log_half_gradient(p);
656        let grad_phi_b = self.b.conformal_log_half_gradient(p);
657        let grad_f = grad_alpha * (f_b - f_a)
658            + grad_phi_a * (2.0 * (1.0 - alpha) * f_a)
659            + grad_phi_b * (2.0 * alpha * f_b);
660        grad_f / (2.0 * f)
661    }
662}
663
664/// `weight` is 0 for pure A and 1 for pure B, and must be C¹: the integrator differentiates it.
665pub trait BlendingField: Copy + Send + Sync + 'static {
666    /// Implementations must clamp to `[0, 1]`.
667    fn weight(&self, p: Vec3) -> f32;
668
669    fn gradient(&self, p: Vec3) -> Vec3 {
670        const EPS: f32 = 1.0e-3;
671        let dx = (self.weight(p + Vec3::X * EPS) - self.weight(p - Vec3::X * EPS)) / (2.0 * EPS);
672        let dy = (self.weight(p + Vec3::Y * EPS) - self.weight(p - Vec3::Y * EPS)) / (2.0 * EPS);
673        let dz = (self.weight(p + Vec3::Z * EPS) - self.weight(p - Vec3::Z * EPS)) / (2.0 * EPS);
674        Vec3::new(dx, dy, dz)
675    }
676}
677
678// Perlin, Improving Noise, 2002.
679/// C² quintic smootherstep along x.
680#[derive(Copy, Clone, Debug, PartialEq)]
681pub struct LinearBlendX {
682    start: f32,
683    end: f32,
684}
685
686impl LinearBlendX {
687    /// Orders reversed bounds; rejects nonfinite bounds, widths, and peak gradients.
688    pub fn new(start: f32, end: f32) -> Option<Self> {
689        if !start.is_finite() || !end.is_finite() {
690            return None;
691        }
692        let (start, end) = (start.min(end), start.max(end));
693        let width = end - start;
694        if width <= 0.0 || !width.is_finite() || !(1.875 / width).is_finite() {
695            return None;
696        }
697        Some(Self { start, end })
698    }
699
700    fn width(&self) -> f32 {
701        self.end - self.start
702    }
703}
704
705impl BlendingField for LinearBlendX {
706    fn weight(&self, p: Vec3) -> f32 {
707        let w = self.width();
708        let t = ((p.x - self.start) / w).clamp(0.0, 1.0);
709        t * t * t * (t * (t * 6.0 - 15.0) + 10.0)
710    }
711
712    fn gradient(&self, p: Vec3) -> Vec3 {
713        let w = self.width();
714        let raw_t = (p.x - self.start) / w;
715        if !(0.0..=1.0).contains(&raw_t) {
716            return Vec3::ZERO;
717        }
718        let t = raw_t;
719        let one_minus_t = 1.0 - t;
720        let dx = 30.0 * t * t * one_minus_t * one_minus_t / w;
721        Vec3::new(dx, 0.0, 0.0)
722    }
723}
724
725fn blended_e3_h3_linearx_wgsl(field: &LinearBlendX, max_arc: f32) -> String {
726    format!(
727        r#"
728// loam-math :: BlendedSpace<EuclideanR3, HyperbolicH3, LinearBlendX> (v0 Space WGSL ABI)
729const LOAM_MAX_ARC: f32 = {max_arc:?};
730const LOAM_BLENDED_R2_MAX: f32 = 0.9999999;
731const LOAM_BLENDED_X_START: f32 = {start:?};
732const LOAM_BLENDED_X_END:   f32 = {end:?};
733const LOAM_BLENDED_X_WIDTH: f32 = {width:?};
734const LOAM_BLENDED_RK4_SUB: i32 = 16;
735
736fn loam_blended_alpha(p: vec3<f32>) -> f32 {{
737    let raw_t = (p.x - LOAM_BLENDED_X_START) / LOAM_BLENDED_X_WIDTH;
738    let t = clamp(raw_t, 0.0, 1.0);
739    return t * t * t * (t * (t * 6.0 - 15.0) + 10.0);
740}}
741
742fn loam_blended_alpha_dx(p: vec3<f32>) -> f32 {{
743    let raw_t = (p.x - LOAM_BLENDED_X_START) / LOAM_BLENDED_X_WIDTH;
744    if raw_t <= 0.0 || raw_t >= 1.0 {{ return 0.0; }}
745    let t = raw_t;
746    let one_minus_t = 1.0 - t;
747    return 30.0 * t * t * one_minus_t * one_minus_t / LOAM_BLENDED_X_WIDTH;
748}}
749
750fn loam_blended_f_h3(p: vec3<f32>) -> f32 {{
751    let r2 = min(dot(p, p), LOAM_BLENDED_R2_MAX);
752    let denom = 1.0 - r2;
753    return 4.0 / (denom * denom);
754}}
755
756fn loam_blended_f(p: vec3<f32>) -> f32 {{
757    let alpha = loam_blended_alpha(p);
758    return (1.0 - alpha) + alpha * loam_blended_f_h3(p);
759}}
760
761fn loam_blended_grad_phi(p: vec3<f32>) -> vec3<f32> {{
762    let alpha = loam_blended_alpha(p);
763    let alpha_dx = loam_blended_alpha_dx(p);
764    let f_e3 = 1.0;
765    let f_h3 = loam_blended_f_h3(p);
766
767    let r2 = min(dot(p, p), LOAM_BLENDED_R2_MAX);
768    let denom = 1.0 - r2;
769    let grad_f_h3 = (16.0 / (denom * denom * denom)) * p;
770
771    let grad_f = vec3<f32>(
772        alpha_dx * (f_h3 - f_e3) + alpha * grad_f_h3.x,
773        alpha * grad_f_h3.y,
774        alpha * grad_f_h3.z,
775    );
776
777    let f = (1.0 - alpha) * f_e3 + alpha * f_h3;
778    return grad_f / (2.0 * max(f, 1e-12));
779}}
780
781struct LoamBlendedRhs {{ dp: vec3<f32>, dv: vec3<f32> }};
782
783fn loam_blended_rhs(p: vec3<f32>, v: vec3<f32>) -> LoamBlendedRhs {{
784    let g = loam_blended_grad_phi(p);
785    let v_sq = dot(v, v);
786    let g_dot_v = dot(g, v);
787    return LoamBlendedRhs(v, g * v_sq - v * (2.0 * g_dot_v));
788}}
789
790struct LoamBlendedState {{ p: vec3<f32>, v: vec3<f32> }};
791
792fn loam_blended_rk4_step(p0: vec3<f32>, v0: vec3<f32>, h: f32) -> LoamBlendedState {{
793    let k1 = loam_blended_rhs(p0, v0);
794    let p1 = p0 + 0.5 * h * k1.dp;
795    let v1 = v0 + 0.5 * h * k1.dv;
796    let k2 = loam_blended_rhs(p1, v1);
797    let p2 = p0 + 0.5 * h * k2.dp;
798    let v2 = v0 + 0.5 * h * k2.dv;
799    let k3 = loam_blended_rhs(p2, v2);
800    let p3 = p0 + h * k3.dp;
801    let v3 = v0 + h * k3.dv;
802    let k4 = loam_blended_rhs(p3, v3);
803    let p_out = p0 + (h / 6.0) * (k1.dp + 2.0 * k2.dp + 2.0 * k3.dp + k4.dp);
804    let v_out = v0 + (h / 6.0) * (k1.dv + 2.0 * k2.dv + 2.0 * k3.dv + k4.dv);
805
806    return LoamBlendedState(p_out, v_out);
807}}
808
809fn loam_exp(at: vec3<f32>, v: vec3<f32>) -> vec3<f32> {{
810    let n2 = dot(v, v);
811    if n2 < 1e-14 {{ return at; }}
812    var p = at;
813    var vv = v;
814    let h = 1.0 / f32(LOAM_BLENDED_RK4_SUB);
815    for (var i: i32 = 0; i < LOAM_BLENDED_RK4_SUB; i = i + 1) {{
816        let s = loam_blended_rk4_step(p, vv, h);
817        p = s.p;
818        vv = s.v;
819    }}
820    return p;
821}}
822
823struct LoamGeodesicStep {{ p: vec3<f32>, v: vec3<f32> }}
824
825fn loam_geodesic_step(p: vec3<f32>, v: vec3<f32>, s: f32) -> LoamGeodesicStep {{
826    let vs = v * s;
827    if dot(vs, vs) < 1e-14 {{ return LoamGeodesicStep(p, v); }}
828    var pp = p;
829    var vv = vs;
830    let h = 1.0 / f32(LOAM_BLENDED_RK4_SUB);
831    for (var i: i32 = 0; i < LOAM_BLENDED_RK4_SUB; i = i + 1) {{
832        let st = loam_blended_rk4_step(pp, vv, h);
833        pp = st.p;
834        vv = st.v;
835    }}
836    return LoamGeodesicStep(pp, vv / s);
837}}
838
839fn loam_distance(a: vec3<f32>, b: vec3<f32>) -> f32 {{
840
841    let mid = 0.5 * (a + b);
842    let f_mid = loam_blended_f(mid);
843    return sqrt(max(f_mid, 0.0)) * length(b - a);
844}}
845
846fn loam_origin_distance(p: vec3<f32>) -> f32 {{
847    return loam_distance(vec3<f32>(0.0, 0.0, 0.0), p);
848}}
849
850fn loam_log(p_from: vec3<f32>, p_to: vec3<f32>) -> vec3<f32> {{
851
852    return p_to - p_from;
853}}
854"#,
855        start = field.start,
856        end = field.end,
857        width = (field.end - field.start).max(1e-12),
858    )
859}
860
861pub const BLENDED_E3_H3_WGSL_RESIDUAL: f32 = 1.9e-2;
862
863impl WgslSpace for BlendedSpace<crate::EuclideanR3, crate::HyperbolicH3, LinearBlendX> {
864    fn wgsl_impl(&self) -> Cow<'static, str> {
865        Cow::Owned(blended_e3_h3_linearx_wgsl(
866            &self.field,
867            self.chart_envelope(),
868        ))
869    }
870
871    fn wgsl_accuracy(&self) -> WgslAccuracy {
872        WgslAccuracy::FirstOrder {
873            residual: BLENDED_E3_H3_WGSL_RESIDUAL,
874        }
875    }
876}
877
878#[cfg(test)]
879mod tests {
880    use super::*;
881
882    #[test]
883    fn blend_interval_rejects_nonfinite_or_zero_width() {
884        for (start, end) in [
885            (0.0, 0.0),
886            (f32::NAN, 1.0),
887            (0.0, f32::INFINITY),
888            (-f32::MAX, f32::MAX),
889            (0.0, f32::from_bits(1)),
890            (0.0, 5e-39),
891        ] {
892            assert!(LinearBlendX::new(start, end).is_none());
893        }
894    }
895
896    #[test]
897    fn equal_endpoint_weights_do_not_hide_an_interior_metric_change() {
898        #[derive(Clone, Copy)]
899        struct Bump;
900        impl BlendingField for Bump {
901            fn weight(&self, p: Vec3) -> f32 {
902                0.1 * (1.0 - (p.x / 0.2).powi(2)).max(0.0).powi(2)
903            }
904            fn gradient(&self, p: Vec3) -> Vec3 {
905                Vec3::X * (-10.0 * p.x * (1.0 - (p.x / 0.2).powi(2)).max(0.0))
906            }
907        }
908        let space = BlendedSpace::new(crate::EuclideanR3, crate::HyperbolicH3, Bump);
909        let a = Vec3::X * -0.2;
910        let b = Vec3::X * 0.2;
911        let length: f32 = (0..1024)
912            .map(|i| {
913                let x = -0.2 + (i as f32 + 0.5) * (0.4 / 1024.0);
914                space.conformal_factor(Vec3::X * x).sqrt() * (0.4 / 1024.0)
915            })
916            .sum();
917        assert!(length > 0.42);
918        assert!((space.distance(a, b) - length).abs() < 1e-3);
919    }
920
921    fn close(a: f32, b: f32, tol: f32) {
922        assert!((a - b).abs() <= tol, "expected {a} ≈ {b} (tol {tol})");
923    }
924
925    #[test]
926    fn linear_blend_x_smoothstep_endpoints() {
927        let f = LinearBlendX::new(-1.0, 1.0).unwrap();
928        close(f.weight(Vec3::new(-1.0, 0.0, 0.0)), 0.0, 1e-6);
929        close(f.weight(Vec3::new(1.0, 0.0, 0.0)), 1.0, 1e-6);
930        close(f.weight(Vec3::ZERO), 0.5, 1e-6);
931    }
932
933    #[test]
934    fn linear_blend_x_is_constant_outside_zone() {
935        let f = LinearBlendX::new(-1.0, 1.0).unwrap();
936        close(f.weight(Vec3::new(-100.0, 0.0, 0.0)), 0.0, 0.0);
937        assert_eq!(f.gradient(Vec3::new(-100.0, 0.0, 0.0)), Vec3::ZERO);
938        close(f.weight(Vec3::new(100.0, 0.0, 0.0)), 1.0, 0.0);
939        assert_eq!(f.gradient(Vec3::new(100.0, 0.0, 0.0)), Vec3::ZERO);
940        close(f.gradient(Vec3::new(-1.0, 0.0, 0.0)).x, 0.0, 1e-6);
941        close(f.gradient(Vec3::new(1.0, 0.0, 0.0)).x, 0.0, 1e-6);
942    }
943
944    #[test]
945    fn linear_blend_x_gradient_is_axis_aligned() {
946        let f = LinearBlendX::new(-1.0, 1.0).unwrap();
947        let g = f.gradient(Vec3::ZERO);
948        assert!(g.x > 0.0, "midpoint gradient should be positive along +x");
949        close(g.y, 0.0, 0.0);
950        close(g.z, 0.0, 0.0);
951
952        close(g.x, 0.9375, 1e-6);
953    }
954
955    #[test]
956    fn linear_blend_x_closed_form_matches_finite_diff() {
957        let f = LinearBlendX::new(-1.0, 1.0).unwrap();
958
959        fn finite_diff_gradient<F: BlendingField>(field: &F, p: Vec3) -> Vec3 {
960            const EPS: f32 = 1.0e-3;
961            let dx =
962                (field.weight(p + Vec3::X * EPS) - field.weight(p - Vec3::X * EPS)) / (2.0 * EPS);
963            let dy =
964                (field.weight(p + Vec3::Y * EPS) - field.weight(p - Vec3::Y * EPS)) / (2.0 * EPS);
965            let dz =
966                (field.weight(p + Vec3::Z * EPS) - field.weight(p - Vec3::Z * EPS)) / (2.0 * EPS);
967            Vec3::new(dx, dy, dz)
968        }
969
970        for x in [-0.7_f32, -0.3, 0.0, 0.3, 0.7] {
971            let p = Vec3::new(x, 0.0, 0.0);
972            let analytic = f.gradient(p);
973            let numeric = finite_diff_gradient(&f, p);
974            close(analytic.x, numeric.x, 5e-4);
975            close(analytic.y, numeric.y, 1e-6);
976            close(analytic.z, numeric.z, 1e-6);
977        }
978    }
979
980    #[test]
981    fn linear_blend_x_handles_reversed_inputs() {
982        let f = LinearBlendX::new(1.0, -1.0).unwrap();
983        close(f.weight(Vec3::new(-1.0, 0.0, 0.0)), 0.0, 1e-6);
984        close(f.weight(Vec3::new(1.0, 0.0, 0.0)), 1.0, 1e-6);
985    }
986
987    #[test]
988    fn hyperbolic_h3_log_half_gradient_matches_finite_diff() {
989        use crate::HyperbolicH3;
990        let s = HyperbolicH3;
991        const EPS: f32 = 1e-3;
992        let fd = |p: Vec3| -> Vec3 {
993            let dx = (s.conformal_log_half(p + Vec3::X * EPS)
994                - s.conformal_log_half(p - Vec3::X * EPS))
995                / (2.0 * EPS);
996            let dy = (s.conformal_log_half(p + Vec3::Y * EPS)
997                - s.conformal_log_half(p - Vec3::Y * EPS))
998                / (2.0 * EPS);
999            let dz = (s.conformal_log_half(p + Vec3::Z * EPS)
1000                - s.conformal_log_half(p - Vec3::Z * EPS))
1001                / (2.0 * EPS);
1002            Vec3::new(dx, dy, dz)
1003        };
1004        for p in [
1005            Vec3::new(0.1, 0.0, 0.0),
1006            Vec3::new(0.3, -0.2, 0.1),
1007            Vec3::new(-0.4, 0.5, 0.2),
1008        ] {
1009            let analytic = s.conformal_log_half_gradient(p);
1010            let numeric = fd(p);
1011            close(analytic.x, numeric.x, 5e-3);
1012            close(analytic.y, numeric.y, 5e-3);
1013            close(analytic.z, numeric.z, 5e-3);
1014        }
1015    }
1016
1017    const METRIC_PROBE_EPS: f32 = 1.0e-3;
1018
1019    const METRIC_PROBE_SLACK: f32 = 1.0e-3;
1020
1021    fn metric_probe_directions(p: Vec3) -> [Vec3; 4] {
1022        let radial = if p.length_squared() > 0.0 {
1023            p.normalize()
1024        } else {
1025            Vec3::X
1026        };
1027        let tangential_a = radial.any_orthonormal_vector();
1028        let tangential_b = radial.cross(tangential_a).normalize();
1029        let oblique = (radial + tangential_a + tangential_b).normalize();
1030        [radial, tangential_a, tangential_b, oblique]
1031    }
1032
1033    fn assert_conformal_factor_matches_metric<S>(space: &S, samples: &[Vec3])
1034    where
1035        S: ConformallyFlat<Point = Vec3, Vector = Vec3>,
1036    {
1037        for &p in samples {
1038            let root_f = space.conformal_factor(p).sqrt();
1039
1040            let truncation = 0.5 * space.conformal_log_half_gradient(p).length() * METRIC_PROBE_EPS;
1041            let tol = root_f * (truncation + METRIC_PROBE_SLACK);
1042            for u in metric_probe_directions(p) {
1043                let measured = space.distance(p, p + u * METRIC_PROBE_EPS) / METRIC_PROBE_EPS;
1044                assert!(
1045                    (measured - root_f).abs() <= tol,
1046                    "conformal factor disagrees with the metric at p = {p:?} along \
1047                     u = {u:?}: measured {measured}, √f = {root_f} (tol {tol})"
1048                );
1049            }
1050        }
1051    }
1052
1053    #[test]
1054    fn conformal_factor_reproduces_the_space_metric_in_every_direction() {
1055        use crate::{EuclideanR3, HyperbolicH3};
1056
1057        assert_conformal_factor_matches_metric(
1058            &EuclideanR3,
1059            &[
1060                Vec3::ZERO,
1061                Vec3::new(0.25, -0.4, 0.1),
1062                Vec3::new(1.0, 0.5, -0.75),
1063            ],
1064        );
1065
1066        assert_conformal_factor_matches_metric(
1067            &HyperbolicH3,
1068            &[
1069                Vec3::ZERO,
1070                Vec3::new(0.2, 0.0, 0.0),
1071                Vec3::new(0.3, -0.2, 0.1),
1072                Vec3::new(0.4, 0.3, -0.2),
1073            ],
1074        );
1075
1076        assert_conformal_factor_matches_metric(
1077            &BlendedSpace::new(
1078                EuclideanR3,
1079                HyperbolicH3,
1080                LinearBlendX::new(-0.5, 0.5).unwrap(),
1081            ),
1082            &[Vec3::new(-0.7, 0.1, 0.0), Vec3::new(0.55, 0.15, -0.1)],
1083        );
1084    }
1085
1086    #[test]
1087    fn blended_space_log_half_gradient_matches_finite_diff() {
1088        use crate::{EuclideanR3, HyperbolicH3};
1089        let bs = BlendedSpace::new(
1090            EuclideanR3,
1091            HyperbolicH3,
1092            LinearBlendX::new(-0.5, 0.5).unwrap(),
1093        );
1094        let fd = |p: Vec3| -> Vec3 {
1095            const EPS: f32 = 1e-3;
1096            let dx = (bs.conformal_log_half(p + Vec3::X * EPS)
1097                - bs.conformal_log_half(p - Vec3::X * EPS))
1098                / (2.0 * EPS);
1099            let dy = (bs.conformal_log_half(p + Vec3::Y * EPS)
1100                - bs.conformal_log_half(p - Vec3::Y * EPS))
1101                / (2.0 * EPS);
1102            let dz = (bs.conformal_log_half(p + Vec3::Z * EPS)
1103                - bs.conformal_log_half(p - Vec3::Z * EPS))
1104                / (2.0 * EPS);
1105            Vec3::new(dx, dy, dz)
1106        };
1107
1108        for p in [
1109            Vec3::new(-0.7, 0.05, 0.0),
1110            Vec3::new(0.0, 0.1, -0.1),
1111            Vec3::new(0.6, 0.05, 0.0),
1112        ] {
1113            let analytic = bs.conformal_log_half_gradient(p);
1114            let numeric = fd(p);
1115            close(analytic.x, numeric.x, 5e-3);
1116            close(analytic.y, numeric.y, 5e-3);
1117            close(analytic.z, numeric.z, 5e-3);
1118        }
1119    }
1120
1121    #[test]
1122    fn rk4_in_pure_e3_is_straight_line() {
1123        use crate::EuclideanR3;
1124        let bs = BlendedSpace::new(
1125            EuclideanR3,
1126            EuclideanR3,
1127            LinearBlendX::new(100.0, 200.0).unwrap(),
1128        );
1129        for (p, v) in [
1130            (Vec3::ZERO, Vec3::X),
1131            (Vec3::new(1.0, 2.0, 3.0), Vec3::new(0.5, -0.3, 0.7)),
1132            (Vec3::new(-5.0, 0.0, 0.0), Vec3::new(2.0, 0.0, 0.0)),
1133        ] {
1134            let (final_p, final_v) = rk4_geodesic(&bs, p, v, GEODESIC_DEFAULT_STEPS);
1135            let expected = p + v;
1136            close((final_p - expected).length(), 0.0, 1e-5);
1137            close((final_v - v).length(), 0.0, 1e-5);
1138        }
1139    }
1140
1141    #[test]
1142    fn checked_geodesic_refuses_an_incomplete_step() {
1143        use crate::{EuclideanR3, HyperbolicH3};
1144
1145        assert_eq!(
1146            integrate_geodesic_frame_checked(
1147                &EuclideanR3,
1148                Vec3::ZERO,
1149                Vec3::splat(1.0e20),
1150                Mat3::IDENTITY,
1151                GEODESIC_DEFAULT_STEPS,
1152                GEODESIC_ERROR_BUDGET,
1153            ),
1154            Err(GeodesicError::NonFinite)
1155        );
1156        assert_eq!(
1157            integrate_geodesic_frame_checked(
1158                &HyperbolicH3,
1159                Vec3::X * 0.9,
1160                Vec3::X,
1161                Mat3::IDENTITY,
1162                GEODESIC_DEFAULT_STEPS,
1163                GEODESIC_ERROR_BUDGET,
1164            ),
1165            Err(GeodesicError::ChartBoundary)
1166        );
1167    }
1168
1169    #[test]
1170    fn checked_frame_transport_uses_the_integrated_geodesic() {
1171        use crate::HyperbolicH3;
1172
1173        let at = Vec3::new(0.25, -0.1, 0.05);
1174        let velocity = Vec3::new(0.2, 0.15, -0.04);
1175        let frame = Mat3::from_cols(velocity, Vec3::Y, Vec3::Z);
1176        let state = integrate_geodesic_frame_checked(
1177            &HyperbolicH3,
1178            at,
1179            velocity,
1180            frame,
1181            GEODESIC_DEFAULT_STEPS,
1182            GEODESIC_ERROR_BUDGET,
1183        )
1184        .expect("checked integration");
1185
1186        close((state.frame.x_axis - state.velocity).length(), 0.0, 1e-6);
1187        assert!((state.point - at - velocity).length() > 1e-4);
1188    }
1189
1190    #[test]
1191    fn rk4_in_pure_h3_matches_closed_form_at_origin() {
1192        use crate::{HyperbolicH3, Space};
1193
1194        for &mag in &[0.1_f32, 0.3, 0.5] {
1195            let v = Vec3::new(mag, 0.0, 0.0);
1196            let (final_p, _) = rk4_geodesic(&HyperbolicH3, Vec3::ZERO, v, GEODESIC_DEFAULT_STEPS);
1197
1198            let expected_radius = mag.tanh();
1199            close(final_p.x, expected_radius, 5e-3);
1200            close(final_p.y, 0.0, 1e-4);
1201            close(final_p.z, 0.0, 1e-4);
1202        }
1203
1204        let v = Vec3::new(0.4, 0.1, 0.0);
1205        let (numerical, _) = rk4_geodesic(&HyperbolicH3, Vec3::ZERO, v, GEODESIC_DEFAULT_STEPS);
1206        let closed_form = HyperbolicH3.exp(Vec3::ZERO, v);
1207        close((numerical - closed_form).length(), 0.0, 5e-3);
1208
1209        let bs = BlendedSpace::new(
1210            HyperbolicH3,
1211            HyperbolicH3,
1212            LinearBlendX::new(-100.0, -50.0).unwrap(),
1213        );
1214        let v = Vec3::new(0.5, 0.0, 0.0);
1215        let final_p_blended = bs.exp(Vec3::ZERO, v);
1216        close(final_p_blended.x, 0.5_f32.tanh(), 5e-3);
1217    }
1218
1219    #[test]
1220    fn exp_log_round_trip_in_pure_h3() {
1221        use crate::HyperbolicH3;
1222        for (from, to) in [
1223            (Vec3::ZERO, Vec3::new(0.3, 0.0, 0.0)),
1224            (Vec3::ZERO, Vec3::new(0.2, 0.1, -0.1)),
1225            (Vec3::new(0.1, 0.1, 0.0), Vec3::new(-0.1, 0.2, 0.1)),
1226        ] {
1227            let v = gauss_newton_log(
1228                &HyperbolicH3,
1229                from,
1230                to,
1231                GEODESIC_DEFAULT_STEPS,
1232                LOG_MAX_ITERS,
1233            );
1234            let endpoint = rk4_geodesic(&HyperbolicH3, from, v, GEODESIC_DEFAULT_STEPS).0;
1235            close((endpoint - to).length(), 0.0, 5e-4);
1236        }
1237    }
1238
1239    #[test]
1240    fn log_in_pure_h3_matches_closed_form() {
1241        use crate::{HyperbolicH3, Space};
1242        for (from, to) in [
1243            (Vec3::ZERO, Vec3::new(0.3, 0.0, 0.0)),
1244            (Vec3::ZERO, Vec3::new(0.2, 0.1, 0.0)),
1245            (Vec3::new(0.1, 0.0, 0.0), Vec3::new(0.3, 0.2, 0.0)),
1246        ] {
1247            let numerical = gauss_newton_log(
1248                &HyperbolicH3,
1249                from,
1250                to,
1251                GEODESIC_DEFAULT_STEPS,
1252                LOG_MAX_ITERS,
1253            );
1254            let closed_form = HyperbolicH3.log(from, to);
1255            close((numerical - closed_form).length(), 0.0, 5e-3);
1256        }
1257    }
1258
1259    #[test]
1260    fn log_of_self_is_zero() {
1261        use crate::HyperbolicH3;
1262        let p = Vec3::new(0.2, 0.1, 0.0);
1263        let v = gauss_newton_log(&HyperbolicH3, p, p, GEODESIC_DEFAULT_STEPS, LOG_MAX_ITERS);
1264        close(v.length(), 0.0, 1e-5);
1265    }
1266
1267    #[test]
1268    fn transport_along_the_geodesic_matches_the_h3_closed_form_where_the_blend_is_pure_h3() {
1269        use crate::{EuclideanR3, HyperbolicH3, Space};
1270        let bs = BlendedSpace::new(
1271            EuclideanR3,
1272            HyperbolicH3,
1273            LinearBlendX::new(-0.15, 0.15).unwrap(),
1274        );
1275        let v = Vec3::new(0.05, -0.03, 0.04);
1276
1277        let mut worst = 0.0_f32;
1278        for (a, b) in [
1279            (Vec3::new(0.3, 0.05, 0.0), Vec3::new(0.45, -0.05, 0.1)),
1280            (Vec3::new(0.25, 0.0, 0.0), Vec3::new(0.4, 0.1, -0.05)),
1281            (Vec3::new(0.5, 0.1, 0.05), Vec3::new(0.35, 0.15, 0.0)),
1282        ] {
1283            let closed = HyperbolicH3.parallel_transport(a, b, v);
1284            let integrated = bs.parallel_transport(a, b, v);
1285            worst = worst.max((integrated - closed).length());
1286        }
1287
1288        assert!(
1289            worst <= 1.0e-4,
1290            "the blended transport left the H3 closed form by {worst}"
1291        );
1292    }
1293
1294    #[test]
1295    fn parallel_transport_in_h3_has_nonzero_holonomy() {
1296        use crate::{HyperbolicH3, Space};
1297        let bs = BlendedSpace::new(
1298            HyperbolicH3,
1299            HyperbolicH3,
1300            LinearBlendX::new(-100.0, -50.0).unwrap(),
1301        );
1302        let path = [
1303            Vec3::new(0.1, 0.0, 0.0),
1304            Vec3::new(0.3, 0.0, 0.0),
1305            Vec3::new(0.2, 0.2, 0.0),
1306            Vec3::new(0.1, 0.0, 0.0),
1307        ];
1308        let v = Vec3::new(0.1, 0.0, 0.0);
1309        let transported = bs.parallel_transport_along(&path, v);
1310        assert!(transported.is_finite());
1311        let drift = (transported - v).length();
1312        assert!(
1313            drift > 1e-3,
1314            "expected non-zero holonomy in H³, got drift {drift}"
1315        );
1316        assert!(drift < 0.5, "holonomy unreasonably large: {drift}");
1317    }
1318
1319    #[test]
1320    fn finite_diff_curvature_matches_closed_form_in_h3() {
1321        use crate::HyperbolicH3;
1322
1323        struct H3FdOnly;
1324        impl crate::space::Space for H3FdOnly {
1325            type Point = Vec3;
1326            type Vector = Vec3;
1327            type Frame = Mat3;
1328            fn frame_at(&self, _at: Vec3) -> Mat3 {
1329                Mat3::IDENTITY
1330            }
1331            fn distance(&self, _: Vec3, _: Vec3) -> f32 {
1332                0.0
1333            }
1334            fn exp(&self, _: Vec3, _: Vec3) -> Vec3 {
1335                Vec3::ZERO
1336            }
1337            fn log(&self, _: Vec3, _: Vec3) -> Vec3 {
1338                Vec3::ZERO
1339            }
1340            fn parallel_transport(&self, _: Vec3, _: Vec3, v: Vec3) -> Vec3 {
1341                v
1342            }
1343            fn chart_envelope(&self) -> f32 {
1344                f32::INFINITY
1345            }
1346            fn valid_point(&self, p: Vec3) -> bool {
1347                p.is_finite()
1348            }
1349        }
1350        impl ConformallyFlat for H3FdOnly {
1351            fn conformal_factor(&self, p: Vec3) -> f32 {
1352                HyperbolicH3.conformal_factor(p)
1353            }
1354            fn conformal_log_half(&self, p: Vec3) -> f32 {
1355                HyperbolicH3.conformal_log_half(p)
1356            }
1357            fn conformal_log_half_gradient(&self, p: Vec3) -> Vec3 {
1358                HyperbolicH3.conformal_log_half_gradient(p)
1359            }
1360        }
1361        let fd = H3FdOnly;
1362
1363        for p in [Vec3::ZERO, Vec3::new(0.2, 0.1, 0.0)] {
1364            close(fd.scalar_curvature(p), -6.0, 0.5);
1365        }
1366    }
1367
1368    #[test]
1369    fn distinct_nearby_points_keep_nonzero_distance() {
1370        let space = BlendedSpace::new(
1371            crate::EuclideanR3,
1372            crate::HyperbolicH3,
1373            LinearBlendX::new(50.0, 100.0).unwrap(),
1374        );
1375        let from = Vec3::ZERO;
1376        let to = Vec3::X * (0.1 * LOG_RESIDUAL_TOL);
1377        let distance = space.distance(from, to);
1378        assert!(distance > 0.0);
1379        assert!((distance - to.x).abs() < 1e-10);
1380    }
1381
1382    #[test]
1383    fn blended_space_at_alpha_zero_is_pure_a() {
1384        use crate::{EuclideanR3, HyperbolicH3, Space};
1385        let bs = BlendedSpace::new(
1386            EuclideanR3,
1387            HyperbolicH3,
1388            LinearBlendX::new(50.0, 100.0).unwrap(),
1389        );
1390        let p = Vec3::new(1.0, 2.0, 3.0);
1391        let q = Vec3::new(4.0, 5.0, 6.0);
1392        let v = Vec3::new(0.5, -0.3, 0.7);
1393
1394        close((bs.exp(p, v) - EuclideanR3.exp(p, v)).length(), 0.0, 1e-5);
1395        close((bs.log(p, q) - EuclideanR3.log(p, q)).length(), 0.0, 1e-3);
1396        close(bs.distance(p, q), EuclideanR3.distance(p, q), 1e-3);
1397        close(
1398            (bs.parallel_transport(p, q, v) - EuclideanR3.parallel_transport(p, q, v)).length(),
1399            0.0,
1400            1e-5,
1401        );
1402        close(
1403            bs.conformal_factor(p),
1404            EuclideanR3.conformal_factor(p),
1405            1e-6,
1406        );
1407        close(bs.scalar_curvature(p), 0.0, 1e-3);
1408    }
1409
1410    #[test]
1411    fn blended_space_at_alpha_one_is_pure_b() {
1412        use crate::{EuclideanR3, HyperbolicH3, Space};
1413        let bs = BlendedSpace::new(
1414            EuclideanR3,
1415            HyperbolicH3,
1416            LinearBlendX::new(-100.0, -50.0).unwrap(),
1417        );
1418        let p = Vec3::new(0.1, 0.0, 0.0);
1419        let q = Vec3::new(0.2, 0.1, 0.0);
1420        let v = Vec3::new(0.05, 0.0, 0.0);
1421
1422        close((bs.exp(p, v) - HyperbolicH3.exp(p, v)).length(), 0.0, 5e-3);
1423        close((bs.log(p, q) - HyperbolicH3.log(p, q)).length(), 0.0, 5e-3);
1424        close(bs.distance(p, q), HyperbolicH3.distance(p, q), 5e-3);
1425        close(
1426            bs.conformal_factor(p),
1427            HyperbolicH3.conformal_factor(p),
1428            1e-3,
1429        );
1430        close(bs.scalar_curvature(p), -6.0, 0.05);
1431    }
1432
1433    #[test]
1434    fn linear_blend_x_is_monotonic() {
1435        let f = LinearBlendX::new(-1.0, 1.0).unwrap();
1436        let xs: Vec<f32> = (0..=20).map(|i| -1.0 + (i as f32) / 10.0).collect();
1437        let mut prev = f.weight(Vec3::new(xs[0], 0.0, 0.0));
1438        for &x in &xs[1..] {
1439            let curr = f.weight(Vec3::new(x, 0.0, 0.0));
1440            assert!(
1441                curr >= prev - 1e-6,
1442                "non-monotonic: at x={x}, weight={curr} < previous {prev}"
1443            );
1444            prev = curr;
1445        }
1446    }
1447}
loam_math/blended.rs

loam_math/
blended.rs
