1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
/*
 * File: rotation.rs
 * Project: geometry
 * Created Date: 26/11/2023
 * Author: Shun Suzuki
 * -----
 * Last Modified: 27/11/2023
 * Modified By: Shun Suzuki (suzuki@hapis.k.u-tokyo.ac.jp)
 * -----
 * Copyright (c) 2023 Shun Suzuki. All rights reserved.
 *
 */

use crate::defined::float;

use super::{UnitQuaternion, Vector3};

pub struct Deg;
pub struct Rad;

#[derive(Clone, Copy)]
pub enum Angle {
    Deg(float),
    Rad(float),
}

impl Angle {
    fn to_radians(self) -> float {
        match self {
            Self::Deg(a) => a.to_radians(),
            Self::Rad(a) => a,
        }
    }
}

impl std::ops::Mul<Deg> for float {
    type Output = Angle;

    fn mul(self, _rhs: Deg) -> Self::Output {
        Self::Output::Deg(self)
    }
}

impl std::ops::Mul<Rad> for float {
    type Output = Angle;

    fn mul(self, _rhs: Rad) -> Self::Output {
        Self::Output::Rad(self)
    }
}

pub enum EulerAngle {
    ZYZ(Angle, Angle, Angle),
}

impl From<EulerAngle> for UnitQuaternion {
    fn from(angle: EulerAngle) -> Self {
        match angle {
            EulerAngle::ZYZ(z1, y, z2) => {
                UnitQuaternion::from_axis_angle(&Vector3::z_axis(), z1.to_radians())
                    * UnitQuaternion::from_axis_angle(&Vector3::y_axis(), y.to_radians())
                    * UnitQuaternion::from_axis_angle(&Vector3::z_axis(), z2.to_radians())
            }
        }
    }
}

#[cfg(test)]
mod tests {

    use super::*;
    use crate::defined::PI;

    macro_rules! assert_approx_eq_vec3 {
        ($a:expr, $b:expr $(,)?) => {
            assert_approx_eq::assert_approx_eq!($a.x, $b.x);
            assert_approx_eq::assert_approx_eq!($a.y, $b.y);
            assert_approx_eq::assert_approx_eq!($a.z, $b.z);
        };
    }

    #[test]
    fn angle_clone() {
        let a = 90.0 * Deg;
        let b = a.clone();
        assert_eq!(a.to_radians(), b.to_radians());
    }

    #[test]
    fn to_radians() {
        assert_approx_eq::assert_approx_eq!((90.0 * Deg).to_radians(), PI / 2.0);
        assert_approx_eq::assert_approx_eq!((PI / 2.0 * Rad).to_radians(), PI / 2.0);
    }

    #[test]
    fn test_rotation() {
        let rot: UnitQuaternion = EulerAngle::ZYZ(90.0 * Deg, 0.0 * Deg, 0.0 * Deg).into();
        assert_approx_eq_vec3!(rot.transform_vector(&Vector3::x()), Vector3::y());
        assert_approx_eq_vec3!(rot.transform_vector(&Vector3::y()), -Vector3::x());
        assert_approx_eq_vec3!(rot.transform_vector(&Vector3::z()), Vector3::z());

        let rot: UnitQuaternion = EulerAngle::ZYZ(0.0 * Deg, 90.0 * Deg, 0.0 * Deg).into();
        assert_approx_eq_vec3!(rot.transform_vector(&Vector3::x()), -Vector3::z());
        assert_approx_eq_vec3!(rot.transform_vector(&Vector3::y()), Vector3::y());
        assert_approx_eq_vec3!(rot.transform_vector(&Vector3::z()), Vector3::x());

        let rot: UnitQuaternion = EulerAngle::ZYZ(0.0 * Deg, 0.0 * Deg, 90.0 * Deg).into();
        assert_approx_eq_vec3!(rot.transform_vector(&Vector3::x()), Vector3::y());
        assert_approx_eq_vec3!(rot.transform_vector(&Vector3::y()), -Vector3::x());
        assert_approx_eq_vec3!(rot.transform_vector(&Vector3::z()), Vector3::z());

        let rot: UnitQuaternion = EulerAngle::ZYZ(0.0 * Deg, 90.0 * Deg, 90.0 * Deg).into();
        assert_approx_eq_vec3!(rot.transform_vector(&Vector3::x()), Vector3::y());
        assert_approx_eq_vec3!(rot.transform_vector(&Vector3::y()), Vector3::z());
        assert_approx_eq_vec3!(rot.transform_vector(&Vector3::z()), Vector3::x());

        let rot: UnitQuaternion = EulerAngle::ZYZ(90.0 * Deg, 90.0 * Deg, 0.0 * Deg).into();
        assert_approx_eq_vec3!(rot.transform_vector(&Vector3::x()), -Vector3::z());
        assert_approx_eq_vec3!(rot.transform_vector(&Vector3::y()), -Vector3::x());
        assert_approx_eq_vec3!(rot.transform_vector(&Vector3::z()), Vector3::y());
    }
}