embedded-so3-f32 0.1.0

// Copyright (C) 2026 Jorge Andre Castro
// Ce programme est un logiciel libre : vous pouvez le redistribuer et/ou le modifier
// selon les termes de la Licence Publique Générale GNU telle que publiée par la
// Free Software Foundation, soit la version 2 de la licence, soit (à votre convention)
// n'importe quelle version ultérieure.

//! # embedded-so3-f32
//!
//! [`Rotation`] représentée par des quaternions unitaires pour le groupe SO(3).
//!
//! Algèbre géométrique `f32` pour systèmes embarqués `no_std`.
//! Sans dépendance standard, sans `unsafe`, sans allocation.
//!
//! ## Exemple
//!
//! ```rust
//! use embedded_so3_f32::Rotation;
//!
//! // Rotation 90° autour de l'axe X  (cos 45° = sin 45° ≈ 0.7071)
//! let s = core::f32::consts::FRAC_1_SQRT_2;
//! let rot = Rotation::new(s, s, 0.0, 0.0).unwrap();
//!
//! let v = [0.0_f32, 1.0, 0.0];
//! let rotated = rot.rotate_vector(v);
//! // ≈ [0, 0, 1]
//!
//! // Composition avec l'opérateur *
//! let double = (rot * rot).unwrap();
//! // double ≈ rotation 180° autour de X
//! ```

#![no_std]
#![forbid(unsafe_code)]
#![warn(missing_docs)]

use core::ops::Mul;
use embedded_f32_sqrt::sqrt;

/// Erreurs de validation pour les opérations SO(3).
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum So3Error {
    /// La norme des composants est nulle ou trop proche de zéro.
    InvalidNorm,
    /// Une valeur `NaN` ou infinie a été détectée.
    NonFiniteValue,
}

/// Représente une rotation dans l'espace 3D (Groupe SO(3)).
///
/// Stocke un quaternion unitaire q = \[w, x, y, z\] où :
/// - `w = cos(θ/2)`
/// - `(x, y, z) = sin(θ/2) · û`  avec `û` l'axe de rotation unitaire
///
/// L'invariant `w² + x² + y² + z² = 1` est garanti à la construction
/// et maintenu après chaque composition via re-normalisation automatique.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Rotation {
    w: f32,
    x: f32,
    y: f32,
    z: f32,
}

impl Rotation {
    /// Identité : aucune rotation (quaternion `[1, 0, 0, 0]`).
    pub const IDENTITY: Self = Self { w: 1.0, x: 0.0, y: 0.0, z: 0.0 };

    /// Crée une rotation à partir d'un quaternion brut `[w, x, y, z]`.
    ///
    /// Les composants sont normalisés automatiquement.
    ///
    /// # Erreurs
    /// - [`So3Error::NonFiniteValue`] si l'un des composants est `NaN` ou infini.
    /// - [`So3Error::InvalidNorm`] si la norme est nulle (vecteur nul).
    ///
    /// # Exemple
    /// ```rust
    /// use embedded_so3_f32::Rotation;
    /// // 90° autour de Z  (w = cos 45°, z = sin 45°)
    /// let s = core::f32::consts::FRAC_1_SQRT_2;
    /// let rot = Rotation::new(s, 0.0, 0.0, s).unwrap();
    /// ```
    pub fn new(w: f32, x: f32, y: f32, z: f32) -> Result<Self, So3Error> {
        Self::normalize_raw(w, x, y, z)
    }

    /// Utilitaire interne : normalise les composants sur la sphère unité.
    fn normalize_raw(w: f32, x: f32, y: f32, z: f32) -> Result<Self, So3Error> {
        let norm_sq = w * w + x * x + y * y + z * z;

        if !norm_sq.is_finite() {
            return Err(So3Error::NonFiniteValue);
        }

        let n = sqrt(norm_sq).map_err(|_| So3Error::InvalidNorm)?;
        if n < 1e-10 {
            return Err(So3Error::InvalidNorm);
        }

        Ok(Self {
            w: w / n,
            x: x / n,
            y: y / n,
            z: z / n,
        })
    }

    /// Compose deux rotations : `R_new = self ∘ other`.
    ///
    /// Équivalent à `self * other` via l'implémentation de [`Mul`].
    ///
    /// Applique une re-normalisation automatique pour contenir la dérive
    /// numérique inhérente à l'arithmétique `f32` sur SO(3).
    ///
    /// # Erreurs
    /// Retourne [`So3Error::InvalidNorm`] ou [`So3Error::NonFiniteValue`]
    /// si le résultat est dégénéré (cas extrêmement rare avec des entrées valides).
    pub fn compose(&self, other: &Rotation) -> Result<Self, So3Error> {
        let w = self.w * other.w - self.x * other.x - self.y * other.y - self.z * other.z;
        let x = self.w * other.x + self.x * other.w + self.y * other.z - self.z * other.y;
        let y = self.w * other.y - self.x * other.z + self.y * other.w + self.z * other.x;
        let z = self.w * other.z + self.x * other.y - self.y * other.x + self.z * other.w;

        Self::normalize_raw(w, x, y, z)
    }

    /// Applique la rotation à un vecteur `[x, y, z]`.
    ///
    /// Utilise la formule optimisée de Rodrigues pour quaternions :
    /// `v' = v + 2w·(q⃗ × v) + 2·(q⃗ × (q⃗ × v))`
    ///
    /// Cette forme évite le produit `p·q·p⁻¹` naïf (28 multiplications)
    /// et ne nécessite que 15 multiplications.
    pub fn rotate_vector(&self, v: [f32; 3]) -> [f32; 3] {
        let [vx, vy, vz] = v;

        // t = 2 * q_vec × v
        let tx = 2.0 * (self.y * vz - self.z * vy);
        let ty = 2.0 * (self.z * vx - self.x * vz);
        let tz = 2.0 * (self.x * vy - self.y * vx);

        // v' = v + w·t + q_vec × t
        [
            vx + self.w * tx + (self.y * tz - self.z * ty),
            vy + self.w * ty + (self.z * tx - self.x * tz),
            vz + self.w * tz + (self.x * ty - self.y * tx),
        ]
    }

    /// Retourne la rotation inverse.
    ///
    /// Pour un quaternion unitaire, l'inverse est le conjugué :
    /// `q⁻¹ = [w, -x, -y, -z]`.
    ///
    /// Aucune normalisation n'est nécessaire car le conjugué d'un
    /// quaternion unitaire est déjà unitaire.
    pub fn inverse(&self) -> Self {
        Self {
            w: self.w,
            x: -self.x,
            y: -self.y,
            z: -self.z,
        }
    }

    /// Retourne les composants bruts sous la forme `[w, x, y, z]`.
    pub fn as_array(&self) -> [f32; 4] {
        [self.w, self.x, self.y, self.z]
    }
}

/// Opérateur `*` pour composer deux rotations.
///
/// Équivalent à [`Rotation::compose`].
///
/// # Exemple
/// ```rust
/// use embedded_so3_f32::Rotation;
/// let s = core::f32::consts::FRAC_1_SQRT_2;
/// let r = Rotation::new(s, s, 0.0, 0.0).unwrap(); // 90° autour de X
/// let r180 = (r * r).unwrap();                      // 180° autour de X
/// ```
impl Mul for Rotation {
    type Output = Result<Rotation, So3Error>;

    fn mul(self, rhs: Self) -> Self::Output {
        self.compose(&rhs)
    }
}

/// Opérateur `*` par référence pour éviter la copie inutile.
impl Mul for &Rotation {
    type Output = Result<Rotation, So3Error>;

    fn mul(self, rhs: Self) -> Self::Output {
        self.compose(rhs)
    }
}

// TESTS UNITAIRES 

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn test_identity_stable() {
        let v = [1.0, 2.0, 3.0];
        let res = Rotation::IDENTITY.rotate_vector(v);
        assert!((res[0] - 1.0).abs() < 1e-7);
        assert!((res[1] - 2.0).abs() < 1e-7);
        assert!((res[2] - 3.0).abs() < 1e-7);
    }

    #[test]
    fn test_rotation_90_x() {
        // Convention : q = [cos(θ/2), sin(θ/2)·x̂]
        // 90° autour de X → θ/2 = 45° → w = x = cos(45°) = √2/2
        let s = core::f32::consts::FRAC_1_SQRT_2;
        let rot = Rotation::new(s, s, 0.0, 0.0).unwrap();
        let res = rot.rotate_vector([0.0, 1.0, 0.0]);

        // Y → Z sous une rotation 90° autour de X
        assert!(res[0].abs() < 1e-6, "x devrait être 0, obtenu {}", res[0]);
        assert!(res[1].abs() < 1e-6, "y devrait être 0, obtenu {}", res[1]);
        assert!((res[2] - 1.0).abs() < 1e-6, "z devrait être 1, obtenu {}", res[2]);
    }

    #[test]
    fn test_mul_operator() {
        let s = core::f32::consts::FRAC_1_SQRT_2;
        let r90 = Rotation::new(s, s, 0.0, 0.0).unwrap(); // 90° autour de X

        // 90° * 90° = 180° autour de X → Y → -Y
        let r180 = (r90 * r90).unwrap();
        let res = r180.rotate_vector([0.0, 1.0, 0.0]);

        assert!(res[0].abs() < 1e-6);
        assert!((res[1] + 1.0).abs() < 1e-6, "y devrait être -1, obtenu {}", res[1]);
        assert!(res[2].abs() < 1e-6);
    }

    #[test]
    fn test_mul_ref_operator() {
        let s = core::f32::consts::FRAC_1_SQRT_2;
        let r = Rotation::new(s, 0.0, s, 0.0).unwrap();
        // Les deux formes doivent donner le même résultat
        let via_compose = r.compose(&r).unwrap();
        let via_mul = (&r * &r).unwrap();
        assert_eq!(via_compose.as_array(), via_mul.as_array());
    }

    #[test]
    fn test_composition_stability() {
        let s = core::f32::consts::FRAC_1_SQRT_2;
        let r1 = Rotation::new(s, 0.0, s, 0.0).unwrap(); // 90° Y
        let mut current = Rotation::IDENTITY;

        // 100 compositions successives : la norme ne doit pas dériver
        for _ in 0..100 {
            current = current.compose(&r1).unwrap();
        }

        let [w, x, y, z] = current.as_array();
        let norm_sq = w * w + x * x + y * y + z * z;
        assert!(
            (norm_sq - 1.0).abs() < 1e-6,
            "La norme a dérivé : {}",
            norm_sq
        );
    }

    #[test]
    fn test_inverse_property() {
        let rot = Rotation::new(1.0, 2.0, 3.0, 4.0).unwrap();
        let res = rot.compose(&rot.inverse()).unwrap();

        assert!((res.w - 1.0).abs() < 1e-6);
        assert!(res.x.abs() < 1e-6);
        assert!(res.y.abs() < 1e-6);
        assert!(res.z.abs() < 1e-6);
    }

    #[test]
    fn test_inverse_via_mul() {
        let rot = Rotation::new(1.0, 2.0, 3.0, 4.0).unwrap();
        let res = (rot * rot.inverse()).unwrap();

        assert!((res.w - 1.0).abs() < 1e-6);
        assert!(res.x.abs() < 1e-6);
        assert!(res.y.abs() < 1e-6);
        assert!(res.z.abs() < 1e-6);
    }

    #[test]
    fn test_invalid_inputs() {
        assert_eq!(
            Rotation::new(0.0, 0.0, 0.0, 0.0),
            Err(So3Error::InvalidNorm)
        );
        assert_eq!(
            Rotation::new(f32::NAN, 0.0, 0.0, 0.0),
            Err(So3Error::NonFiniteValue)
        );
        assert_eq!(
            Rotation::new(f32::INFINITY, 0.0, 0.0, 0.0),
            Err(So3Error::NonFiniteValue)
        );
    }
}