1#[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 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 pub const fn new(w: f64, x: f64, y: f64, z: f64) -> Self {
28 Self { w, x, y, z }
29 }
30
31 #[inline]
32 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 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 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 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 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 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 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 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 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 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 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}