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

use super::Vec3D;
use std::ops::Mul;

/// Helper enum for when you need to choose an axis
pub enum SpatialAxis {
    X,
    Y,
    Z,
}

/// The `Transform3D` struct is used to manipulate the position of objects in 3D space
#[derive(Debug, Clone, Copy)]
pub struct Transform3D {
    /// The position of the object in 3D space
    pub translation: Vec3D,
    /// The rotation of the object, applied in radians
    pub rotation: Vec3D,
    /// The object's scale
    pub scale: Vec3D,
}

impl Transform3D {
    pub const DEFAULT: Self = Self::new_trs(Vec3D::ZERO, Vec3D::ZERO, Vec3D::ONE);

    /// Create a Transform3D with chosen translation, rotation and scale
    pub const fn new_trs(translation: Vec3D, rotation: Vec3D, scale: Vec3D) -> Self {
        Self {
            translation,
            rotation,
            scale,
        }
    }

    /// Create a Transform3D with chosen translation and rotation
    pub const fn new_tr(translation: Vec3D, rotation: Vec3D) -> Self {
        Self {
            translation,
            rotation,
            scale: Vec3D::ONE,
        }
    }

    /// Create a Transform3D with chosen translation
    pub const fn new_t(translation: Vec3D) -> Self {
        Self {
            translation,
            rotation: Vec3D::ZERO,
            scale: Vec3D::ONE,
        }
    }

    pub fn rotate_one_axis(translation: Vec3D, axis: SpatialAxis, single_rotation: f64) -> Vec3D {
        let mut translation = translation;
        let (x, y) = match axis {
            SpatialAxis::X => (&mut translation.y, &mut translation.z),
            SpatialAxis::Y => (&mut translation.x, &mut translation.z),
            SpatialAxis::Z => (&mut translation.x, &mut translation.y),
        };

        let s = single_rotation.sin();
        let c = single_rotation.cos();
        (*x, *y) = (*x * c - *y * s, *x * s + *y * c);

        translation
    }

    #[allow(clippy::let_and_return)]
    pub fn rotate(&self, value: Vec3D) -> Vec3D {
        let ry = Self::rotate_one_axis(value, SpatialAxis::Y, self.rotation.y);
        let rx = Self::rotate_one_axis(ry, SpatialAxis::X, self.rotation.x);
        let rz = Self::rotate_one_axis(rx, SpatialAxis::Z, self.rotation.z);

        rz
    }
}

impl Default for Transform3D {
    fn default() -> Self {
        Self::DEFAULT
    }
}

impl Mul<Transform3D> for Transform3D {
    type Output = Transform3D;

    fn mul(self, rhs: Transform3D) -> Self::Output {
        Self::new_trs(
            self.translation + rhs.translation,
            self.rotation + rhs.rotation,
            self.scale * rhs.scale,
        )
    }
}

impl Mul<Vec3D> for Transform3D {
    type Output = Vec3D;

    #[allow(clippy::let_and_return)]
    fn mul(self, rhs: Vec3D) -> Self::Output {
        let scaled = rhs * self.scale;
        let rotated = self.rotate(scaled);
        let translated = rotated + self.translation;

        translated
    }
}