drew_sim/
math.rs

1//! Primitives mathématiques : vecteurs 3D et rotations Euler.
2
3/// Vecteur / point 3D (x, y, z).
4pub type Vec3 = [f32; 3];
5
6/// Addition de deux vecteurs.
7#[inline]
8pub fn add(a: Vec3, b: Vec3) -> Vec3 {
9    [a[0] + b[0], a[1] + b[1], a[2] + b[2]]
10}
11
12/// Soustraction de deux vecteurs.
13#[inline]
14pub fn sub(a: Vec3, b: Vec3) -> Vec3 {
15    [a[0] - b[0], a[1] - b[1], a[2] - b[2]]
16}
17
18/// Mise à l'échelle d'un vecteur par un scalaire.
19#[inline]
20pub fn scale(a: Vec3, s: f32) -> Vec3 {
21    [a[0] * s, a[1] * s, a[2] * s]
22}
23
24/// Représente les 3 angles de rotation en radians (Roulis, Tangage, Lacet).
25///
26/// Convention : Roll autour de Z, Pitch autour de X, Yaw autour de Y,
27/// appliqués dans cet ordre (Roll puis Pitch puis Yaw).
28#[derive(Clone, Copy, Debug, Default, PartialEq)]
29pub struct Rotation3D {
30    pub roll: f32,
31    pub pitch: f32,
32    pub yaw: f32,
33}
34
35impl Rotation3D {
36    pub const IDENTITY: Self = Self { roll: 0.0, pitch: 0.0, yaw: 0.0 };
37
38    pub fn new(roll: f32, pitch: f32, yaw: f32) -> Self {
39        Self { roll, pitch, yaw }
40    }
41
42    /// Applique la rotation Roll -> Pitch -> Yaw à un point 3D.
43    pub fn rotate_point(&self, p: Vec3) -> Vec3 {
44        // Roll (Z)
45        let (sin_r, cos_r) = self.roll.sin_cos();
46        let x1 = p[0] * cos_r - p[1] * sin_r;
47        let y1 = p[0] * sin_r + p[1] * cos_r;
48        let z1 = p[2];
49
50        // Pitch (X)
51        let (sin_p, cos_p) = self.pitch.sin_cos();
52        let x2 = x1;
53        let y2 = y1 * cos_p - z1 * sin_p;
54        let z2 = y1 * sin_p + z1 * cos_p;
55
56        // Yaw (Y)
57        let (sin_y, cos_y) = self.yaw.sin_cos();
58        let x3 = x2 * cos_y + z2 * sin_y;
59        let y3 = y2;
60        let z3 = -x2 * sin_y + z2 * cos_y;
61
62        [x3, y3, z3]
63    }
64
65    /// Compose deux rotations (approximation par angles d'Euler cumulés).
66    /// Pour une composition rigoureuse, préférer une future implémentation
67    /// via quaternions (voir `embedded-quaternion-f32`).
68    pub fn compose(&self, other: Rotation3D) -> Rotation3D {
69        Rotation3D {
70            roll: self.roll + other.roll,
71            pitch: self.pitch + other.pitch,
72            yaw: self.yaw + other.yaw,
73        }
74    }
75}
76
77#[cfg(test)]
78mod tests {
79    use super::*;
80
81    #[test]
82    fn identity_rotation_is_noop() {
83        let p = [1.0, 2.0, 3.0];
84        let r = Rotation3D::IDENTITY;
85        let out = r.rotate_point(p);
86        for i in 0..3 {
87            assert!((out[i] - p[i]).abs() < 1e-6);
88        }
89    }
90}
drew_sim/math.rs

drew_sim/
math.rs
