bb-geometry 0.3.0

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


use rand::Rng;
use std::f64::consts::PI as PI;
use crate::vector4d::Vector4D;
use crate::ALMOST_ZERO;

#[derive(Debug)]
pub struct Rotation4D {
    r_xx: f64,
    r_xy: f64,
    r_xz: f64,
    r_xt: f64,
    r_yx: f64,
    r_yy: f64,
    r_yz: f64,
    r_yt: f64,
    r_zx: f64,
    r_zy: f64,
    r_zz: f64,
    r_zt: f64,
    r_tx: f64,
    r_ty: f64,
    r_tz: f64,
    r_tt: f64,
}

impl Rotation4D {
    /// This creates the unit rotation, which does not rotate anything.
    ///
    /// # Example:
    /// ```
    /// use bb_geometry::vector4d::Vector4D;
    /// use bb_geometry::rotation4d::Rotation4D;
    /// let r = Rotation4D::unit();
    /// let v = Vector4D::random_vector();
    ///
    /// assert!(r.act_on(&v).almost_equals(&v));
    /// ```
    pub fn unit() -> Rotation4D {
        Rotation4D {
            r_xx: 1.0,
            r_xy: 0.0,
            r_xz: 0.0,
            r_xt: 0.0,
            r_yx: 0.0,
            r_yy: 1.0,
            r_yz: 0.0,
            r_yt: 0.0,
            r_zx: 0.0,
            r_zy: 0.0,
            r_zz: 1.0,
            r_zt: 0.0,
            r_tx: 0.0,
            r_ty: 0.0,
            r_tz: 0.0,
            r_tt: 1.0,
        }
    }

    /// This method creates a 4d rotation using a specific parameterisation in terms of Euler angles.
    /// Since only 4 angles are used, this is not sufficient to generate every rotation in the 6d space SO(4).
    /// Still, it is enough to generate those needed for a 4d camera, which assert that a 4d head is never tilted.
    ///
    /// # Examples:
    /// ```
    /// use std::f64::consts::PI;
    /// use bb_geometry::rotation4d::Rotation4D;
    /// use bb_geometry::vector4d::*;
    /// let r = Rotation4D::from_euler_angles(0.0, PI/2.0, 0.0, 0.0);
    ///
    /// assert!(&r.act_on(&E_X).almost_equals(&E_X));
    /// assert!(&r.act_on(&E_Y).almost_equals(&E_Y));
    /// assert!(&r.act_on(&E_Z).almost_equals(&E_T.revert()));
    /// assert!(&r.act_on(&E_T).almost_equals(&E_Z));
    /// ```
    pub fn from_euler_angles(psi: f64, theta: f64, phi1: f64, phi2: f64) -> Rotation4D {
        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_phi1 = f64::cos(phi1);
        let sin_phi1 = f64::sin(phi1);
        let cos_phi2 = f64::cos(phi2);
        let sin_phi2 = f64::sin(phi2);
        Rotation4D {
            r_xx: cos_phi1 * cos_psi + sin_phi1 * sin_phi2 * sin_psi,
            r_xy: cos_phi1 * sin_psi - sin_phi1 * sin_phi2 * cos_psi,
            r_xz: sin_phi1 * cos_phi2 * cos_theta,
            r_xt: sin_phi1 * cos_phi2 * sin_theta,
            r_yx: -cos_phi2 * sin_psi,
            r_yy: cos_phi2 * cos_psi,
            r_yz: sin_phi2 * cos_theta,
            r_yt: sin_phi2 * sin_theta,
            r_zx: -sin_phi1 * cos_psi + cos_phi1 * sin_phi2 * sin_psi,
            r_zy: -sin_phi1 * sin_psi - cos_phi1 * sin_phi2 * cos_psi,
            r_zz: cos_phi1 * cos_phi2 * cos_theta,
            r_zt: cos_phi1 * cos_phi2 * sin_theta,
            r_tx: 0.0,
            r_ty: 0.0,
            r_tz: -sin_theta,
            r_tt: cos_theta,
        }
    }

    fn almost_equal_to(&self, other: &Rotation4D) -> bool {
        vec![
            self.r_tt - other.r_tt,
            self.r_tz - other.r_tz,
            self.r_tx - other.r_tx,
            self.r_ty - other.r_ty,
            self.r_xx - other.r_xx,
            self.r_xy - other.r_xy,
            self.r_xz - other.r_xz,
            self.r_xt - other.r_xt,
            self.r_yx - other.r_yx,
            self.r_yy - other.r_yy,
            self.r_yz - other.r_yz,
            self.r_yt - other.r_yt,
            self.r_zt - other.r_zt,
            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) -> Rotation4D {
        self.transpose()
    }

    pub fn transpose(&self) -> Rotation4D {
        Rotation4D {
            r_xx: self.r_xx,
            r_xy: self.r_yx,
            r_xz: self.r_zx,
            r_xt: self.r_tx,
            r_yx: self.r_xy,
            r_yy: self.r_yy,
            r_yz: self.r_zy,
            r_yt: self.r_ty,
            r_zx: self.r_xz,
            r_zy: self.r_yz,
            r_zz: self.r_zz,
            r_zt: self.r_tz,
            r_tx: self.r_xt,
            r_ty: self.r_yt,
            r_tz: self.r_zt,
            r_tt: self.r_tt,
        }
    }

    pub fn act_on(&self, v: &Vector4D) -> Vector4D {
        Vector4D::new(
            self.r_xx * v.x + self.r_xy * v.y + self.r_xz * v.z + self.r_xt * v.t,
            self.r_yx * v.x + self.r_yy * v.y + self.r_yz * v.z + self.r_yt * v.t,
            self.r_zx * v.x + self.r_zy * v.y + self.r_zz * v.z + self.r_zt * v.t,
            self.r_tx * v.x + self.r_ty * v.y + self.r_tz * v.z + self.r_tt * v.t,
        )
    }

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

}

#[cfg(test)]
mod test {
    use crate::vector4d::{E_X, E_Y, E_Z, E_T};
    use super::*;

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

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

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

    #[test]
    fn transpose_is_inverse() {
        // given
        let vectors = Vector4D::generate_random_vectors(10);
        let rotations = Rotation4D::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 = Rotation4D::from_euler_angles(0.0, PI / 2.0, PI / 2.0, 0.0);

        // when & then
        test_vector_equality(&r.act_on(&E_X), &E_Z.revert());
        test_vector_equality(&r.act_on(&E_Y), &E_Y);
        test_vector_equality(&r.act_on(&E_Z), &E_T.revert());
        test_vector_equality(&r.act_on(&E_T), &E_X);

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

        // when & then
        test_vector_equality(&r.act_on(&E_X), &E_X);
        test_vector_equality(&r.act_on(&E_Y), &E_Z.revert());
        test_vector_equality(&r.act_on(&E_Z), &E_T.revert());
        test_vector_equality(&r.act_on(&E_T), &E_Y);

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

        // when & then
        test_vector_equality(&r.act_on(&E_X), &E_Z.revert());
        test_vector_equality(&r.act_on(&E_Y), &E_X.revert());
        test_vector_equality(&r.act_on(&E_Z), &E_T.revert());
        test_vector_equality(&r.act_on(&E_T), &E_Y);
    }

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

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

        // given
        let r1 = Rotation4D::from_euler_angles(0.0, PI / 2.0, 0.0, 0.0);
        let r2 = Rotation4D::from_euler_angles(0.0, PI / 2.0, 2.0 * PI, 0.0);
        let r3 = Rotation4D::from_euler_angles(0.0, PI / 2.0, 0.0, PI * 2.0);

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

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

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

}