bb-geometry 0.3.0

//! This is the module covering anything related to rotations in 3d.
//! So far this is quite limited in scope.


use rand::Rng;
use std::f64::consts::PI as PI;
use crate::vector3d::Vector3D;
use crate::ALMOST_ZERO;

#[derive(Debug)]
pub struct Rotation3D {
    r_xx: f64,
    r_xy: f64,
    r_xz: f64,
    r_yx: f64,
    r_yy: f64,
    r_yz: f64,
    r_zx: f64,
    r_zy: f64,
    r_zz: f64,
}

impl Rotation3D {
    /// This creates the unit rotation, which does not rotate anything.
    ///
    /// # Example:
    /// ```
    /// use bb_geometry::vector3d::Vector3D;
    /// use bb_geometry::rotation3d::Rotation3D;
    /// let r = Rotation3D::unit();
    /// let v = Vector3D::random_vector();
    ///
    /// assert!(r.act_on(&v).almost_equals(&v));
    /// ```
    pub fn unit() -> Rotation3D {
        Rotation3D {
            r_xx: 1.0,
            r_xy: 0.0,
            r_xz: 0.0,
            r_yx: 0.0,
            r_yy: 1.0,
            r_yz: 0.0,
            r_zx: 0.0,
            r_zy: 0.0,
            r_zz: 1.0,
        }
    }

    /// This method creates a 3d rotation using a specific parameterisation in terms of Euler angles.
    ///
    /// # Examples:
    /// ```
    /// use std::f64::consts::PI;
    /// use bb_geometry::rotation3d::Rotation3D;
    /// use bb_geometry::vector3d::*;
    /// let r = Rotation3D::from_euler_angles(0.0, PI/2.0, 0.0);
    ///
    /// assert!(&r.act_on(&X).almost_equals(&Z.revert()));
    /// assert!(&r.act_on(&Y).almost_equals(&Y));
    /// assert!(&r.act_on(&Z).almost_equals(&X));
    /// ```
    pub fn from_euler_angles(psi: f64, theta: f64, phi: f64) -> Rotation3D {
        let cos_psi = f64::cos(psi);
        let sin_psi = f64::sin(psi);
        let cos_theta = f64::cos(theta);
        let sin_theta = f64::sin(theta);
        let cos_phi = f64::cos(phi);
        let sin_phi = f64::sin(phi);
        Rotation3D {
            r_xx: cos_psi * cos_theta * cos_phi + - sin_psi * sin_phi,
            r_xy: - sin_psi * cos_theta * cos_phi -  cos_psi * sin_phi,
            r_xz: cos_phi * sin_theta,
            r_yx: cos_psi * sin_phi * cos_theta + sin_psi * cos_phi,
            r_yy: -sin_psi * sin_phi * cos_theta + cos_psi * cos_phi,
            r_yz: sin_phi * sin_theta,
            r_zx: -cos_psi * sin_theta,
            r_zy: sin_psi * sin_theta,
            r_zz: cos_theta
        }
    }

    fn almost_equal_to(&self, other: &Rotation3D) -> bool {
        vec![
            self.r_xx - other.r_xx,
            self.r_xy - other.r_xy,
            self.r_xz - other.r_xz,
            self.r_yx - other.r_yx,
            self.r_yy - other.r_yy,
            self.r_yz - other.r_yz,
            self.r_zx - other.r_zx,
            self.r_zy - other.r_zy,
            self.r_zz - other.r_zz,
        ]
        .iter()
        .map(|c| c.abs())
        .reduce(f64::max)
        .unwrap()
            < ALMOST_ZERO
    }

    pub fn inverse(&self) -> Rotation3D {
        self.transpose()
    }

    pub fn transpose(&self) -> Rotation3D {
        Rotation3D {
            r_xx: self.r_xx,
            r_xy: self.r_yx,
            r_xz: self.r_zx,
            r_yx: self.r_xy,
            r_yy: self.r_yy,
            r_yz: self.r_zy,
            r_zx: self.r_xz,
            r_zy: self.r_yz,
            r_zz: self.r_zz,
        }
    }

    pub fn act_on(&self, v: &Vector3D) -> Vector3D {
        Vector3D::new(
            self.r_xx * v.x + self.r_xy * v.y + self.r_xz * v.z ,
            self.r_yx * v.x + self.r_yy * v.y + self.r_yz * v.z,
            self.r_zx * v.x + self.r_zy * v.y + self.r_zz * v.z,
        )
    }

    pub fn generate_random_rotations(n: usize) -> Vec<Rotation3D> {
        let mut rng = rand::rng();
        (0..n)
            .map(|_| {
                Rotation3D::from_euler_angles(
                    rng.random_range(-3.14..=3.14),
                    rng.random_range(0.0..=3.14),
                    rng.random_range(-3.14..=3.14),
                )
            })
            .collect()
    }

    pub fn components(&self) -> [[f64; 3]; 3] {
        [
            [self.r_xx, self.r_xy, self.r_xz],
            [self.r_yx, self.r_yy, self.r_yz],
            [self.r_zx, self.r_zy, self.r_zz],
        ]
    }

}

#[cfg(test)]
mod test {
    use crate::vector3d::{X, Y, Z};
    use super::*;

    #[test]
    fn identity_does_not_map() {
        // given
        let unit = Rotation3D::unit();
        let vectors = Vector3D::generate_random_vectors(10);

        // when
        let mapped_vectors = vectors
            .iter()
            .map(|v| unit.act_on(v))
            .collect::<Vec<Vector3D>>();

        // then
        for (x, y) in vectors.iter().zip(mapped_vectors.iter()) {
            assert!(x.almost_equals(y));
        }
    }

    #[test]
    fn transpose_is_inverse() {
        // given
        let vectors = Vector3D::generate_random_vectors(10);
        let rotations = Rotation3D::generate_random_rotations(10);

        for i in 0..10 {
            // when
            let inverse_rotation = rotations[i].inverse();
            let mapped_vector = inverse_rotation.act_on(&rotations[i].act_on(&vectors[i]));

            // then
            assert!(
                mapped_vector.almost_equals(&vectors[i]),
                "Vector {:?} got mapped to {:?}",
                mapped_vector,
                vectors[i]
            );
        }
    }

    #[test]
    fn directions_tests() {

        // given
        let r = Rotation3D::from_euler_angles(0.0, PI / 2.0, PI / 2.0);

        // when & then
        test_vector_equality(&r.act_on(&X), &Z.revert());
        test_vector_equality(&r.act_on(&Y), &X.revert());
        test_vector_equality(&r.act_on(&Z), &Y);

        // given
        let r = Rotation3D::from_euler_angles(PI / 2.0, PI /2.0, PI / 2.0);

        // when & then
        test_vector_equality(&r.act_on(&X), &X.revert());
        test_vector_equality(&r.act_on(&Y), &Z);
        test_vector_equality(&r.act_on(&Z), &Y);
    }

    #[test]
    fn compare_rotations() {
        // given
        let r1 = Rotation3D::from_euler_angles(0.0, PI / 2.0, 0.0);
        let r2 = Rotation3D::from_euler_angles(0.0, -PI / 2.0, 0.0).inverse();

        // when & then
        test_rotation_equality(&r1, &r2);
    }

    fn test_vector_equality(v1: &Vector3D, v2: &Vector3D) {
        assert!(v1.almost_equals(&v2), "Comparing two vectors failed: \n {:?} \n {:?}", v1, v2);
    }

    fn test_rotation_equality(r1: &Rotation3D, r2: &Rotation3D) {
        assert!(r1.almost_equal_to(&r2), "Comparing rotations failed: \n {:?} \n {:?}", r1, r2);
    }

}