1#![no_std]
4
5use core::ops::{Add, Mul, Sub};
6use embedded_trig_f32::sincos;
7use embedded_f32_sqrt::sqrt;
8
9#[derive(Debug, Clone, Copy, PartialEq)]
12pub struct Quaternion {
13 pub w: f32, pub x: f32, pub y: f32, pub z: f32, }
18
19impl Quaternion {
20 pub const fn new(w: f32, x: f32, y: f32, z: f32) -> Self {
22 Self { w, x, y, z }
23 }
24
25 pub const ZERO: Self = Self::new(0.0, 0.0, 0.0, 0.0);
27
28 pub const ONE: Self = Self::new(1.0, 0.0, 0.0, 0.0);
30
31 pub fn from_angle_plane(angle: f32) -> (f32, f32) {
34 sincos(angle)
35 }
36
37 pub fn rotate_xw(self, angle: f32) -> Self {
39 let (s, c) = sincos(angle);
40 Self {
41 w: self.x * s + self.w * c,
42 x: self.x * c - self.w * s,
43 y: self.y,
44 z: self.z,
45 }
46 }
47
48 pub fn rotate_yz(self, angle: f32) -> Self {
50 let (s, c) = sincos(angle);
51 Self {
52 w: self.w,
53 x: self.x,
54 y: self.y * c - self.z * s,
55 z: self.y * s + self.z * c,
56 }
57 }
58
59 pub fn conjugate(self) -> Self {
61 Self {
62 w: self.w,
63 x: -self.x,
64 y: -self.y,
65 z: -self.z,
66 }
67 }
68
69 pub fn norm_squared(self) -> f32 {
71 self.w * self.w + self.x * self.x + self.y * self.y + self.z * self.z
72 }
73
74 pub fn project_2d(
77 self,
78 center_x: f32,
79 center_y: f32,
80 dist_4d: f32,
81 dist_3d: f32,
82 ) -> (i16, i16, f32) {
83 let pers_4d = dist_4d / (dist_4d + self.w);
85 let x_3d = self.x * pers_4d;
86 let y_3d = self.y * pers_4d;
87 let z_3d = self.z * pers_4d;
88
89 let pers_3d = dist_3d / (dist_3d + z_3d);
91 let px = (center_x + x_3d * pers_3d) as i16;
92 let py = (center_y + y_3d * pers_3d) as i16;
93
94 (px, py, z_3d)
95 }
96
97 pub fn normalize(self) -> Option<Self> {
100 let norm_sq = self.norm_squared();
101 if norm_sq > f32::EPSILON {
102 if let Ok(norm) = sqrt(norm_sq) {
104 let inv_norm = 1.0 / norm;
105 Some(Self {
106 w: self.w * inv_norm,
107 x: self.x * inv_norm,
108 y: self.y * inv_norm,
109 z: self.z * inv_norm,
110 })
111 } else {
112 None
114 }
115 } else {
116 None
117 }
118 }
119
120}
121
122impl Mul for Quaternion {
125 type Output = Self;
126 fn mul(self, rhs: Self) -> Self {
127 Self {
128 w: self.w * rhs.w - self.x * rhs.x - self.y * rhs.y - self.z * rhs.z,
129 x: self.w * rhs.x + self.x * rhs.w + self.y * rhs.z - self.z * rhs.y,
130 y: self.w * rhs.y - self.x * rhs.z + self.y * rhs.w + self.z * rhs.x,
131 z: self.w * rhs.z + self.x * rhs.y - self.y * rhs.x + self.z * rhs.w,
132 }
133 }
134}
135
136impl Add for Quaternion {
138 type Output = Self;
139 fn add(self, rhs: Self) -> Self {
140 Self {
141 w: self.w + rhs.w,
142 x: self.x + rhs.x,
143 y: self.y + rhs.y,
144 z: self.z + rhs.z,
145 }
146 }
147}
148
149impl Sub for Quaternion {
151 type Output = Self;
152 fn sub(self, rhs: Self) -> Self {
153 Self {
154 w: self.w - rhs.w,
155 x: self.x - rhs.x,
156 y: self.y - rhs.y,
157 z: self.z - rhs.z,
158 }
159 }
160}
161
162impl Mul<f32> for Quaternion {
164 type Output = Self;
165 fn mul(self, scalar: f32) -> Self {
166 Self {
167 w: self.w * scalar,
168 x: self.x * scalar,
169 y: self.y * scalar,
170 z: self.z * scalar,
171 }
172 }
173}
174
175#[cfg(test)]
176mod tests {
177 use super::*;
178 use core::f32::consts::PI;
179
180 const EPS: f32 = 1e-4;
181
182 fn approx_eq(a: f32, b: f32) -> bool {
183 (a - b).abs() <= EPS
184 }
185
186 fn assert_q_approx(q: Quaternion, w: f32, x: f32, y: f32, z: f32) {
187 assert!(approx_eq(q.w, w), "w: {} != {}", q.w, w);
188 assert!(approx_eq(q.x, x), "x: {} != {}", q.x, x);
189 assert!(approx_eq(q.y, y), "y: {} != {}", q.y, y);
190 assert!(approx_eq(q.z, z), "z: {} != {}", q.z, z);
191 }
192
193 #[test]
194 fn test_new_and_constants() {
195 let q = Quaternion::new(1.0, 2.0, 3.0, 4.0);
196 assert_eq!(q.w, 1.0);
197 assert_eq!(q.x, 2.0);
198 assert_eq!(q.y, 3.0);
199 assert_eq!(q.z, 4.0);
200
201 assert_eq!(Quaternion::ZERO, Quaternion::new(0.0, 0.0, 0.0, 0.0));
202 assert_eq!(Quaternion::ONE, Quaternion::new(1.0, 0.0, 0.0, 0.0));
203 }
204
205 #[test]
206 fn test_add() {
207 let a = Quaternion::new(1.0, 2.0, 3.0, 4.0);
208 let b = Quaternion::new(4.0, 3.0, 2.0, 1.0);
209 let sum = a + b;
210 assert_q_approx(sum, 5.0, 5.0, 5.0, 5.0);
211
212 assert_eq!(a + Quaternion::ZERO, a);
214 }
215
216 #[test]
217 fn test_sub() {
218 let a = Quaternion::new(4.0, 3.0, 2.0, 1.0);
219 let b = Quaternion::new(1.0, 1.0, 1.0, 1.0);
220 let diff = a - b;
221 assert_q_approx(diff, 3.0, 2.0, 1.0, 0.0);
222 assert_eq!(a - a, Quaternion::ZERO);
223 }
224
225 #[test]
226 fn test_mul_scalar() {
227 let a = Quaternion::new(1.0, 2.0, 3.0, 4.0);
228 let scaled = a * 2.0;
229 assert_q_approx(scaled, 2.0, 4.0, 6.0, 8.0);
230
231 assert_eq!(a * 0.0, Quaternion::ZERO);
233 }
234
235 #[test]
236 fn test_conjugate() {
237 let a = Quaternion::new(1.0, 2.0, 3.0, 4.0);
238 let conj = a.conjugate();
239 assert_q_approx(conj, 1.0, -2.0, -3.0, -4.0);
240
241 assert_eq!(conj.conjugate(), a);
243 }
244
245 #[test]
246 fn test_norm_squared() {
247 let a = Quaternion::new(1.0, 2.0, 2.0, 4.0);
248 assert!(approx_eq(a.norm_squared(), 25.0));
250
251 assert!(approx_eq(Quaternion::ZERO.norm_squared(), 0.0));
252 assert!(approx_eq(Quaternion::ONE.norm_squared(), 1.0));
253 }
254
255 #[test]
256 fn test_mul_identity() {
257 let a = Quaternion::new(1.0, 2.0, 3.0, 4.0);
258 assert_q_approx(Quaternion::ONE * a, a.w, a.x, a.y, a.z);
260 assert_q_approx(a * Quaternion::ONE, a.w, a.x, a.y, a.z);
261 }
262
263 #[test]
264 fn test_hamilton_basis_products() {
265 let i = Quaternion::new(0.0, 1.0, 0.0, 0.0);
266 let j = Quaternion::new(0.0, 0.0, 1.0, 0.0);
267 let k = Quaternion::new(0.0, 0.0, 0.0, 1.0);
268
269 assert_q_approx(i * i, -1.0, 0.0, 0.0, 0.0);
271 assert_q_approx(j * j, -1.0, 0.0, 0.0, 0.0);
272 assert_q_approx(k * k, -1.0, 0.0, 0.0, 0.0);
273
274 assert_q_approx(i * j, 0.0, 0.0, 0.0, 1.0);
276 assert_q_approx(j * k, 0.0, 1.0, 0.0, 0.0);
277 assert_q_approx(k * i, 0.0, 0.0, 1.0, 0.0);
278
279 assert_q_approx(j * i, 0.0, 0.0, 0.0, -1.0);
281 }
282
283 #[test]
284 fn test_mul_not_commutative() {
285 let a = Quaternion::new(1.0, 2.0, 3.0, 4.0);
286 let b = Quaternion::new(5.0, 6.0, 7.0, 8.0);
287 assert_ne!(a * b, b * a);
288 }
289
290 #[test]
291 fn test_from_angle_plane() {
292 let (s, c) = Quaternion::from_angle_plane(0.0);
293 assert!(approx_eq(s, 0.0));
294 assert!(approx_eq(c, 1.0));
295
296 let (s, c) = Quaternion::from_angle_plane(PI / 2.0);
297 assert!(approx_eq(s, 1.0));
298 assert!(approx_eq(c, 0.0));
299 }
300
301 #[test]
302 fn test_rotate_xw_zero_angle_is_identity() {
303 let q = Quaternion::new(1.0, 2.0, 3.0, 4.0);
304 let rotated = q.rotate_xw(0.0);
305 assert_q_approx(rotated, q.w, q.x, q.y, q.z);
306 }
307
308 #[test]
309 fn test_rotate_xw_quarter_turn() {
310 let q = Quaternion::new(0.0, 1.0, 0.0, 0.0);
312 let rotated = q.rotate_xw(PI / 2.0);
313 assert_q_approx(rotated, 1.0, 0.0, 0.0, 0.0);
315 }
316
317 #[test]
318 fn test_rotate_xw_norm_matches_formula() {
319 let q = Quaternion::new(1.0, 2.0, 3.0, 4.0);
326 let angle = 0.73;
327 let (s, c) = Quaternion::from_angle_plane(angle);
328 let rotated = q.rotate_xw(angle);
329
330 let expected_plane_norm = (q.w * q.w + q.x * q.x) * (s * s + c * c);
331 let actual_plane_norm = rotated.w * rotated.w + rotated.x * rotated.x;
332 assert!(approx_eq(expected_plane_norm, actual_plane_norm));
333
334 assert!(approx_eq(rotated.y, q.y));
336 assert!(approx_eq(rotated.z, q.z));
337 }
338
339 #[test]
340 fn test_rotate_yz_norm_matches_formula() {
341 let q = Quaternion::new(1.0, 2.0, 3.0, 4.0);
342 let angle = 1.1;
343 let (s, c) = Quaternion::from_angle_plane(angle);
344 let rotated = q.rotate_yz(angle);
345
346 let expected_plane_norm = (q.y * q.y + q.z * q.z) * (s * s + c * c);
347 let actual_plane_norm = rotated.y * rotated.y + rotated.z * rotated.z;
348 assert!(approx_eq(expected_plane_norm, actual_plane_norm));
349
350 assert!(approx_eq(rotated.w, q.w));
352 assert!(approx_eq(rotated.x, q.x));
353 }
354
355 #[test]
356 fn test_sincos_approx_unit_circle() {
357 for angle in [0.0_f32, 0.3, 0.73, 1.1, PI / 2.0, PI, 2.5] {
360 let (s, c) = Quaternion::from_angle_plane(angle);
361 assert!(
362 (s * s + c * c - 1.0).abs() <= 5e-2,
363 "sin²+cos² hors tolérance pour angle={angle}: s={s}, c={c}"
364 );
365 }
366 }
367
368 #[test]
369 fn test_rotate_yz_zero_angle_is_identity() {
370 let q = Quaternion::new(1.0, 2.0, 3.0, 4.0);
371 let rotated = q.rotate_yz(0.0);
372 assert_q_approx(rotated, q.w, q.x, q.y, q.z);
373 }
374
375 #[test]
376 fn test_rotate_yz_quarter_turn() {
377 let q = Quaternion::new(0.0, 0.0, 1.0, 0.0);
379 let rotated = q.rotate_yz(PI / 2.0);
380 assert_q_approx(rotated, 0.0, 0.0, 0.0, 1.0);
382 }
383
384 #[test]
385 fn test_project_2d_origin_at_center() {
386 let q = Quaternion::ZERO;
388 let (px, py, z) = q.project_2d(160.0, 80.0, 5.0, 5.0);
389 assert_eq!(px, 160);
390 assert_eq!(py, 80);
391 assert!(approx_eq(z, 0.0));
392 }
393
394 #[test]
395 fn test_project_2d_offset_point() {
396 let q = Quaternion::new(0.0, 2.0, 0.0, 0.0);
398 let (px, py, _z) = q.project_2d(100.0, 100.0, 5.0, 5.0);
399 assert!(px > 100);
400 assert_eq!(py, 100);
401 }
402
403
404
405 #[test]
406 fn test_normalize() {
407 let q = Quaternion::new(1.0, 2.0, 3.0, 4.0);
408 let normalized = q.normalize().unwrap();
409
410 assert!(approx_eq(normalized.norm_squared(), 1.0));
412
413 let norm_sq = q.norm_squared();
415 let norm = sqrt(norm_sq).unwrap(); assert_q_approx(
417 normalized,
418 q.w / norm,
419 q.x / norm,
420 q.y / norm,
421 q.z / norm,
422 );
423 }
424
425 #[test]
426 fn test_normalize_zero() {
427 let q = Quaternion::ZERO;
428 assert!(q.normalize().is_none());
429 }
430
431 #[test]
432 fn test_normalize_unit_already() {
433 let q = Quaternion::new(0.0, 1.0, 0.0, 0.0);
434 let normalized = q.normalize().unwrap();
435 assert_q_approx(normalized, 0.0, 1.0, 0.0, 0.0);
436 }
437
438 #[test]
439 fn test_normalize_small() {
440 let q = Quaternion::new(0.1, 0.0, 0.0, 0.0);
444 assert!(q.normalize().is_some());
445 let normalized = q.normalize().unwrap();
446 assert!(approx_eq(normalized.norm_squared(), 1.0));
447
448 let q = Quaternion::ZERO;
450 assert!(q.normalize().is_none());
451
452 let q = Quaternion::new(1e-20, 0.0, 0.0, 0.0);
454 assert!(q.normalize().is_none());
455 }
456
457
458
459
460
461}