Skip to main content

embedded_quaternion_f32/
lib.rs

1// Copyright (C) 2026 Jorge Andre Castro
2// SPDX-License-Identifier: GPL-2.0-or-later
3#![no_std]
4
5use core::ops::{Add, Mul, Sub};
6use embedded_trig_f32::sincos;
7use embedded_f32_sqrt::sqrt; 
8
9/// Représente un quaternion q = w + x*i + y*j + z*k
10/// ou un point/vecteur dans l'espace 4D.
11#[derive(Debug, Clone, Copy, PartialEq)]
12pub struct Quaternion {
13    pub w: f32, // Composante scalaire (ou 4ème dimension)
14    pub x: f32, // Composante i
15    pub y: f32, // Composante j
16    pub z: f32, // Composante k
17}
18
19impl Quaternion {
20    /// Construit un quaternion
21    pub const fn new(w: f32, x: f32, y: f32, z: f32) -> Self {
22        Self { w, x, y, z }
23    }
24
25    /// Quaternion nul / Point à l'origine
26    pub const ZERO: Self = Self::new(0.0, 0.0, 0.0, 0.0);
27
28    /// Unité de Hamilton (1)
29    pub const ONE: Self = Self::new(1.0, 0.0, 0.0, 0.0);
30
31    /// Crée un quaternion de rotation 4D (Rotor de Hamilton)
32    /// dans le plan spécifié (ex: XW ou YZ).
33    pub fn from_angle_plane(angle: f32) -> (f32, f32) {
34        sincos(angle)
35    }
36
37    /// Applique une rotation 4D dans le plan X-W (Propre aux Quaternions)
38    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    /// Applique une rotation 4D dans le plan Y-Z
49    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    /// Conjugué du quaternion : q* = w - xi - yj - zk
60    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    /// Norme au carré (évite le sqrt en embedded)
70    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    /// Projection perspective 4D -> 3D -> 2D
75    /// Retourne (screen_x, screen_y, depth_z)
76    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        // 1. Perspective 4D (w) -> 3D
84        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        // 2. Perspective 3D (z) -> 2D (écran)
90        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    /// Normalise le quaternion (le rend unitaire)
98    /// Retourne None si la norme est trop proche de zéro
99    pub fn normalize(self) -> Option<Self> {
100        let norm_sq = self.norm_squared();
101        if norm_sq > f32::EPSILON {
102            // Utilisation de embedded-f32-sqrt au lieu de f32::sqrt()
103            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                // sqrt() a retourné une erreur (normalement impossible avec norm_sq > EPSILON)
113                None
114            }
115        } else {
116            None
117        }
118    }
119
120}
121
122// --- Produit de Hamilton (Multiplication de 2 quaternions) ---
123// i² = j² = k² = ijk = -1
124impl 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
136// --- Addition de Quaternions ---
137impl 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
149// --- Soustraction de Quaternions ---
150impl 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
162// --- Multiplication par un scalaire f32 ---
163impl 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        // Élément neutre
213        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        // Multiplier par 0 donne le quaternion nul
232        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        // Le conjugué du conjugué redonne l'original
242        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        // 1 + 4 + 4 + 16 = 25
249        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        // 1 * q = q et q * 1 = q
259        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        // i² = j² = k² = -1
270        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        // i*j = k, j*k = i, k*i = j
275        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        // j*i = -k (non-commutatif)
280        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        // Point pur sur l'axe X (w=0, x=1)
311        let q = Quaternion::new(0.0, 1.0, 0.0, 0.0);
312        let rotated = q.rotate_xw(PI / 2.0);
313        // Après rotation de 90°, x -> w
314        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        // La conservation de la norme dépend de sin²+cos² = 1, ce qui n'est
320        // vrai qu'approximativement avec embedded-trig-f32 (~2e-3 de tolérance,
321        // cf. sa documentation). On vérifie donc la formule elle-même plutôt
322        // que de supposer une identité trigonométrique parfaite : la norme
323        // du plan tourné doit valoir (w²+x²) * (s²+c²), calculé avec les
324        // mêmes s/c que rotate_xw utilise en interne.
325        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        // y et z ne sont pas affectés par rotate_xw
335        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        // w et x ne sont pas affectés par rotate_yz
351        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        // Vérifie que embedded-trig-f32 respecte bien sa tolérance documentée
358        // (~2e-3) sur l'identité sin²+cos²=1, sans exiger une précision parfaite.
359        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        // Point pur sur l'axe Y (y=1)
378        let q = Quaternion::new(0.0, 0.0, 1.0, 0.0);
379        let rotated = q.rotate_yz(PI / 2.0);
380        // Après rotation de 90°, y -> z
381        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        // Point à l'origine 4D -> doit se projeter au centre de l'écran
387        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        // Point purement sur X: doit se déplacer horizontalement
397        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        // Vérifier que la norme est approximativement 1.0
411        assert!(approx_eq(normalized.norm_squared(), 1.0));
412        
413        // Vérifier que la direction est conservée (q / ||q||)
414        let norm_sq = q.norm_squared();
415        let norm = sqrt(norm_sq).unwrap();  // Utilisation de sqrt dans le test aussi
416        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    // Test avec des valeurs très claires
441    
442    // 1. Quaternion clairement normalisable
443    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    // 2. Quaternion nul (doit échouer)
449    let q = Quaternion::ZERO;
450    assert!(q.normalize().is_none());
451    
452    // 3. Quaternion très petit (doit échouer)
453    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}