Skip to main content

vibe/animation/
squad.rs

1//! Spherical and Quadrangle (SQUAD) Quaternion Spline Interpolation (Zero-Heap).
2//!
3//! Provides C¹-continuous orientation interpolation across multi-keyframe paths,
4//! eliminating instantaneous angular acceleration spikes characteristic of linear SLERP.
5
6/// A 4D Unit Quaternion representing 3D spatial orientation [w, x, y, z].
7#[derive(Debug, Clone, Copy, PartialEq)]
8pub struct Quat {
9    pub w: f64,
10    pub x: f64,
11    pub y: f64,
12    pub z: f64,
13}
14
15impl Quat {
16    /// Identity quaternion/transform.
17    pub const fn identity() -> Self {
18        Self {
19            w: 1.0,
20            x: 0.0,
21            y: 0.0,
22            z: 0.0,
23        }
24    }
25
26    /// Creates a new `Quat`.
27    pub const fn new(w: f64, x: f64, y: f64, z: f64) -> Self {
28        Self { w, x, y, z }
29    }
30
31    #[inline]
32    /// Dot product.
33    pub fn dot(&self, other: &Self) -> f64 {
34        self.w * other.w + self.x * other.x + self.y * other.y + self.z * other.z
35    }
36
37    /// Normalizes.
38    pub fn normalize(&self) -> Self {
39        let mag_sq = self.dot(self);
40        if mag_sq < 1e-12 {
41            return Self::identity();
42        }
43        let inv_mag = 1.0 / mag_sq.sqrt();
44        Self {
45            w: self.w * inv_mag,
46            x: self.x * inv_mag,
47            y: self.y * inv_mag,
48            z: self.z * inv_mag,
49        }
50    }
51
52    /// Quaternion conjugate.
53    pub fn conjugate(&self) -> Self {
54        Self {
55            w: self.w,
56            x: -self.x,
57            y: -self.y,
58            z: -self.z,
59        }
60    }
61
62    /// Multiplies.
63    pub fn mul(&self, other: &Self) -> Self {
64        Self {
65            w: self.w * other.w - self.x * other.x - self.y * other.y - self.z * other.z,
66            x: self.w * other.x + self.x * other.w + self.y * other.z - self.z * other.y,
67            y: self.w * other.y - self.x * other.z + self.y * other.w + self.z * other.x,
68            z: self.w * other.z + self.x * other.y - self.y * other.x + self.z * other.w,
69        }
70    }
71
72    /// Quaternion logarithm: ln(q) -> imaginary vector [x, y, z].
73    pub fn log(&self) -> [f64; 3] {
74        let v_len_sq = self.x * self.x + self.y * self.y + self.z * self.z;
75        if v_len_sq < 1e-12 {
76            return [0.0, 0.0, 0.0];
77        }
78        let v_len = v_len_sq.sqrt();
79        let theta = v_len.atan2(self.w);
80        let scale = theta / v_len;
81        [self.x * scale, self.y * scale, self.z * scale]
82    }
83
84    /// Quaternion exponential: exp([x, y, z]) -> unit quaternion.
85    pub fn exp(v: [f64; 3]) -> Self {
86        let theta_sq = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
87        if theta_sq < 1e-12 {
88            return Self::identity();
89        }
90        let theta = theta_sq.sqrt();
91        let sin_theta = theta.sin();
92        let scale = sin_theta / theta;
93        Self {
94            w: theta.cos(),
95            x: v[0] * scale,
96            y: v[1] * scale,
97            z: v[2] * scale,
98        }
99    }
100
101    /// Standard Spherical Linear Interpolation (SLERP) along the shortest arc.
102    pub fn slerp(q0: &Self, q1: &Self, t: f64) -> Self {
103        if t <= 0.0 {
104            return *q0;
105        }
106        if t >= 1.0 {
107            return *q1;
108        }
109
110        let mut dot = q0.dot(q1);
111        let mut target = *q1;
112
113        // Take the shortest path on S³
114        if dot < 0.0 {
115            dot = -dot;
116            target = Self {
117                w: -target.w,
118                x: -target.x,
119                y: -target.y,
120                z: -target.z,
121            };
122        }
123
124        if dot > 0.9995 {
125            // Linear interpolation fallback when quaternions are nearly parallel
126            return Self {
127                w: q0.w + t * (target.w - q0.w),
128                x: q0.x + t * (target.x - q0.x),
129                y: q0.y + t * (target.y - q0.y),
130                z: q0.z + t * (target.z - q0.z),
131            }
132            .normalize();
133        }
134
135        let theta = dot.clamp(-1.0, 1.0).acos();
136        let sin_theta = theta.sin();
137        let s0 = ((1.0 - t) * theta).sin() / sin_theta;
138        let s1 = (t * theta).sin() / sin_theta;
139
140        Self {
141            w: s0 * q0.w + s1 * target.w,
142            x: s0 * q0.x + s1 * target.x,
143            y: s0 * q0.y + s1 * target.y,
144            z: s0 * q0.z + s1 * target.z,
145        }
146    }
147
148    /// Compute intermediate control point `a_i` for SQUAD given three consecutive orientations (q_{i-1}, q_i, q_{i+1}).
149    pub fn compute_inner_control_point(q_prev: &Self, q_curr: &Self, q_next: &Self) -> Self {
150        let q_curr_inv = q_curr.conjugate();
151        let log_next = q_curr_inv.mul(q_next).log();
152        let log_prev = q_curr_inv.mul(q_prev).log();
153
154        let mut sum = [0.0; 3];
155        for k in 0..3 {
156            sum[k] = -(log_next[k] + log_prev[k]) * 0.25;
157        }
158
159        let exp_term = Self::exp(sum);
160        q_curr.mul(&exp_term).normalize()
161    }
162
163    /// Spherical and Quadrangle Spline (SQUAD) interpolation between `q_i` and `q_{i+1}`.
164    /// Uses inner control points `a_i` and `b_{i+1}` for C¹ continuity.
165    pub fn squad(q_i: &Self, q_next: &Self, a_i: &Self, b_next: &Self, t: f64) -> Self {
166        let slerp_q = Self::slerp(q_i, q_next, t);
167        let slerp_ab = Self::slerp(a_i, b_next, t);
168        let factor = 2.0 * t * (1.0 - t);
169        Self::slerp(&slerp_q, &slerp_ab, factor)
170    }
171}
172
173#[cfg(test)]
174mod tests {
175    use super::*;
176
177    #[test]
178    fn slerp_endpoints() {
179        let q0 = Quat::identity();
180        let q1 = Quat::new(0.0, 1.0, 0.0, 0.0);
181        let mid = Quat::slerp(&q0, &q1, 0.5);
182        assert!((mid.w - 0.707106).abs() < 1e-4);
183        assert!((mid.x - 0.707106).abs() < 1e-4);
184    }
185
186    #[test]
187    fn squad_endpoints_match() {
188        let q0 = Quat::identity();
189        let q1 = Quat::new(0.7071, 0.7071, 0.0, 0.0);
190        let q2 = Quat::new(0.0, 1.0, 0.0, 0.0);
191
192        let a0 = Quat::compute_inner_control_point(&q0, &q0, &q1);
193        let b1 = Quat::compute_inner_control_point(&q0, &q1, &q2);
194
195        let start = Quat::squad(&q0, &q1, &a0, &b1, 0.0);
196        let end = Quat::squad(&q0, &q1, &a0, &b1, 1.0);
197
198        assert!((start.dot(&q0).abs() - 1.0).abs() < 1e-4);
199        assert!((end.dot(&q1).abs() - 1.0).abs() < 1e-4);
200    }
201}