sphereql_core/
interpolation.rs

1use crate::conversions::{cartesian_to_spherical, spherical_to_cartesian};
2use crate::distance::angular_distance;
3use crate::types::{CartesianPoint, SphericalPoint};
4
5/// Spherical linear interpolation between two unit-sphere points.
6///
7/// The parameter `t` is clamped to [0, 1]. At t=0 returns `a`, at t=1 returns `b`.
8///
9/// At antipodal (or near-antipodal) inputs the geodesic is not unique — every
10/// great circle through the two points is equally valid. To stay numerically
11/// stable and deterministic we pick a fixed orthogonal tangent to `a` and
12/// trace `cos(ωt)·a + sin(ωt)·tangent` along that great circle.
13///
14/// ```
15/// use sphereql_core::{SphericalPoint, slerp, angular_distance};
16/// use std::f64::consts::FRAC_PI_2;
17///
18/// let a = SphericalPoint::new_unchecked(1.0, 0.0, FRAC_PI_2);
19/// let b = SphericalPoint::new_unchecked(1.0, FRAC_PI_2, FRAC_PI_2);
20/// let mid = slerp(&a, &b, 0.5);
21/// let da = angular_distance(&mid, &a);
22/// let db = angular_distance(&mid, &b);
23/// assert!((da - db).abs() < 1e-10);
24/// ```
25pub fn slerp(a: &SphericalPoint, b: &SphericalPoint, t: f64) -> SphericalPoint {
26    let t = t.clamp(0.0, 1.0);
27    let a_unit = SphericalPoint::new_unchecked(1.0, a.theta, a.phi);
28    let b_unit = SphericalPoint::new_unchecked(1.0, b.theta, b.phi);
29
30    let omega = angular_distance(&a_unit, &b_unit);
31
32    if omega.abs() < 1e-10 {
33        return a_unit;
34    }
35
36    let ac = spherical_to_cartesian(&a_unit);
37    let bc = spherical_to_cartesian(&b_unit);
38
39    let sin_omega = omega.sin();
40    if sin_omega.abs() < 1e-10 {
41        // Antipodal (or near-antipodal): the geodesic is degenerate.
42        // Pick a deterministic orthogonal tangent to `a` and trace
43        // cos(ωt)·a + sin(ωt)·tangent along that great circle.
44        let basis = if ac.z.abs() < 0.9 {
45            [0.0, 0.0, 1.0]
46        } else {
47            [1.0, 0.0, 0.0]
48        };
49        let proj = basis[0] * ac.x + basis[1] * ac.y + basis[2] * ac.z;
50        let mut ux = basis[0] - proj * ac.x;
51        let mut uy = basis[1] - proj * ac.y;
52        let mut uz = basis[2] - proj * ac.z;
53        let u_norm = (ux * ux + uy * uy + uz * uz).sqrt();
54        ux /= u_norm;
55        uy /= u_norm;
56        uz /= u_norm;
57        let (ca, sa) = ((omega * t).cos(), (omega * t).sin());
58        let result = CartesianPoint::new(
59            ca * ac.x + sa * ux,
60            ca * ac.y + sa * uy,
61            ca * ac.z + sa * uz,
62        );
63        return cartesian_to_spherical(&result);
64    }
65
66    let factor_a = ((1.0 - t) * omega).sin() / sin_omega;
67    let factor_b = (t * omega).sin() / sin_omega;
68
69    let result = CartesianPoint::new(
70        factor_a * ac.x + factor_b * bc.x,
71        factor_a * ac.y + factor_b * bc.y,
72        factor_a * ac.z + factor_b * bc.z,
73    );
74
75    cartesian_to_spherical(&result)
76}
77
78/// Normalized linear interpolation between two unit-sphere points.
79///
80/// Linearly interpolates in Cartesian space, then normalizes back to the
81/// unit sphere. Faster than [`slerp`] but does not produce constant-speed
82/// traversal along the great circle arc.
83///
84/// The parameter `t` is clamped to [0, 1].
85pub fn nlerp(a: &SphericalPoint, b: &SphericalPoint, t: f64) -> SphericalPoint {
86    let t = t.clamp(0.0, 1.0);
87    let a_unit = SphericalPoint::new_unchecked(1.0, a.theta, a.phi);
88    let b_unit = SphericalPoint::new_unchecked(1.0, b.theta, b.phi);
89
90    let ac = spherical_to_cartesian(&a_unit);
91    let bc = spherical_to_cartesian(&b_unit);
92
93    let lerped = CartesianPoint::new(
94        ac.x + t * (bc.x - ac.x),
95        ac.y + t * (bc.y - ac.y),
96        ac.z + t * (bc.z - ac.z),
97    );
98
99    if lerped.magnitude() < f64::EPSILON {
100        return SphericalPoint::new_unchecked(1.0, a.theta, a.phi);
101    }
102
103    cartesian_to_spherical(&lerped.normalize())
104}
105
106/// Full spherical linear interpolation including radius.
107///
108/// Interpolates both direction (via [`slerp`]) and radial distance
109/// (linearly between `a.r` and `b.r`). The parameter `t` is clamped
110/// to [0, 1].
111pub fn full_slerp(a: &SphericalPoint, b: &SphericalPoint, t: f64) -> SphericalPoint {
112    let t = t.clamp(0.0, 1.0);
113    let direction = slerp(a, b, t);
114    let r = a.r + t * (b.r - a.r);
115    SphericalPoint::new_unchecked(r, direction.theta, direction.phi)
116}
117
118#[cfg(test)]
119mod tests {
120    use super::*;
121    use approx::assert_relative_eq;
122    use std::f64::consts::FRAC_PI_2;
123
124    fn unit_point(theta: f64, phi: f64) -> SphericalPoint {
125        SphericalPoint::new_unchecked(1.0, theta, phi)
126    }
127
128    #[test]
129    fn slerp_at_t0_returns_a() {
130        let a = unit_point(0.0, FRAC_PI_2);
131        let b = unit_point(FRAC_PI_2, FRAC_PI_2);
132        let result = slerp(&a, &b, 0.0);
133        assert_relative_eq!(result.theta, a.theta, epsilon = 1e-12);
134        assert_relative_eq!(result.phi, a.phi, epsilon = 1e-12);
135    }
136
137    #[test]
138    fn slerp_at_t1_returns_b() {
139        let a = unit_point(0.0, FRAC_PI_2);
140        let b = unit_point(FRAC_PI_2, FRAC_PI_2);
141        let result = slerp(&a, &b, 1.0);
142        assert_relative_eq!(result.theta, b.theta, epsilon = 1e-12);
143        assert_relative_eq!(result.phi, b.phi, epsilon = 1e-12);
144    }
145
146    #[test]
147    fn slerp_midpoint_is_on_great_circle() {
148        let a = unit_point(0.0, FRAC_PI_2);
149        let b = unit_point(FRAC_PI_2, FRAC_PI_2);
150        let mid = slerp(&a, &b, 0.5);
151
152        let dist_a = angular_distance(&unit_point(mid.theta, mid.phi), &a);
153        let dist_b = angular_distance(&unit_point(mid.theta, mid.phi), &b);
154        assert_relative_eq!(dist_a, dist_b, epsilon = 1e-12);
155
156        let total = angular_distance(&a, &b);
157        assert_relative_eq!(dist_a, total / 2.0, epsilon = 1e-12);
158    }
159
160    #[test]
161    fn slerp_clamps_t() {
162        let a = unit_point(0.0, FRAC_PI_2);
163        let b = unit_point(FRAC_PI_2, FRAC_PI_2);
164
165        let below = slerp(&a, &b, -0.5);
166        let at_zero = slerp(&a, &b, 0.0);
167        assert_relative_eq!(below.theta, at_zero.theta, epsilon = 1e-12);
168        assert_relative_eq!(below.phi, at_zero.phi, epsilon = 1e-12);
169
170        let above = slerp(&a, &b, 1.5);
171        let at_one = slerp(&a, &b, 1.0);
172        assert_relative_eq!(above.theta, at_one.theta, epsilon = 1e-12);
173        assert_relative_eq!(above.phi, at_one.phi, epsilon = 1e-12);
174    }
175
176    #[test]
177    fn nlerp_endpoints_match_slerp() {
178        let a = unit_point(0.3, 1.0);
179        let b = unit_point(1.5, 0.8);
180
181        let s0 = slerp(&a, &b, 0.0);
182        let n0 = nlerp(&a, &b, 0.0);
183        assert_relative_eq!(s0.theta, n0.theta, epsilon = 1e-10);
184        assert_relative_eq!(s0.phi, n0.phi, epsilon = 1e-10);
185
186        let s1 = slerp(&a, &b, 1.0);
187        let n1 = nlerp(&a, &b, 1.0);
188        assert_relative_eq!(s1.theta, n1.theta, epsilon = 1e-10);
189        assert_relative_eq!(s1.phi, n1.phi, epsilon = 1e-10);
190    }
191
192    #[test]
193    fn full_slerp_interpolates_radius() {
194        let a = SphericalPoint::new_unchecked(2.0, 0.0, FRAC_PI_2);
195        let b = SphericalPoint::new_unchecked(8.0, FRAC_PI_2, FRAC_PI_2);
196
197        let mid = full_slerp(&a, &b, 0.5);
198        assert_relative_eq!(mid.r, 5.0, epsilon = 1e-12);
199
200        let quarter = full_slerp(&a, &b, 0.25);
201        assert_relative_eq!(quarter.r, 3.5, epsilon = 1e-12);
202    }
203
204    #[test]
205    fn full_slerp_endpoints() {
206        let a = SphericalPoint::new_unchecked(3.0, 0.5, 1.0);
207        let b = SphericalPoint::new_unchecked(7.0, 1.5, 0.8);
208
209        let at_a = full_slerp(&a, &b, 0.0);
210        assert_relative_eq!(at_a.r, a.r, epsilon = 1e-12);
211        assert_relative_eq!(at_a.theta, a.theta, epsilon = 1e-12);
212        assert_relative_eq!(at_a.phi, a.phi, epsilon = 1e-12);
213
214        let at_b = full_slerp(&a, &b, 1.0);
215        assert_relative_eq!(at_b.r, b.r, epsilon = 1e-12);
216        assert_relative_eq!(at_b.theta, b.theta, epsilon = 1e-12);
217        assert_relative_eq!(at_b.phi, b.phi, epsilon = 1e-12);
218    }
219
220    #[test]
221    fn slerp_antipodal_is_finite_and_on_sphere() {
222        // Antipodal pair on the equator — sin(ω) = 0, the naive formula
223        // produces NaN. The branch must return a finite unit-sphere point.
224        let a = unit_point(0.0, FRAC_PI_2);
225        let b = unit_point(std::f64::consts::PI, FRAC_PI_2);
226
227        let mid = slerp(&a, &b, 0.5);
228        assert!(mid.theta.is_finite());
229        assert!(mid.phi.is_finite());
230
231        // Midpoint must be exactly π/2 from both endpoints (great-circle
232        // midpoint of an antipodal pair).
233        let half_pi = FRAC_PI_2;
234        let da = angular_distance(&mid, &a);
235        let db = angular_distance(&mid, &b);
236        assert_relative_eq!(da, half_pi, epsilon = 1e-10);
237        assert_relative_eq!(db, half_pi, epsilon = 1e-10);
238    }
239
240    #[test]
241    fn slerp_antipodal_endpoints_match() {
242        let a = unit_point(0.0, FRAC_PI_2);
243        let b = unit_point(std::f64::consts::PI, FRAC_PI_2);
244
245        let at_zero = slerp(&a, &b, 0.0);
246        assert_relative_eq!(at_zero.theta, a.theta, epsilon = 1e-10);
247        assert_relative_eq!(at_zero.phi, a.phi, epsilon = 1e-10);
248    }
249
250    #[test]
251    fn interpolation_between_identical_points() {
252        let p = unit_point(1.0, 0.5);
253
254        let s = slerp(&p, &p, 0.5);
255        assert_relative_eq!(s.theta, p.theta, epsilon = 1e-12);
256        assert_relative_eq!(s.phi, p.phi, epsilon = 1e-12);
257
258        let n = nlerp(&p, &p, 0.5);
259        assert_relative_eq!(n.theta, p.theta, epsilon = 1e-12);
260        assert_relative_eq!(n.phi, p.phi, epsilon = 1e-12);
261
262        let f = full_slerp(&p, &p, 0.5);
263        assert_relative_eq!(f.r, p.r, epsilon = 1e-12);
264        assert_relative_eq!(f.theta, p.theta, epsilon = 1e-12);
265        assert_relative_eq!(f.phi, p.phi, epsilon = 1e-12);
266    }
267}
sphereql_core/interpolation.rs

sphereql_core/
interpolation.rs
