commonlibsse_ng\re\n/
NiMatrix3.rs

1use crate::re::NiPoint3::NiPoint3;
2use core::f32::consts::PI;
3
4/// 3x3 matrix structure compatible with C++ layout.
5#[repr(C)]
6#[derive(Debug, Clone, Copy, PartialEq)]
7pub struct NiMatrix3 {
8    /// 3x3 matrix represented as an array of arrays
9    pub entry: [[f32; 3]; 3],
10}
11
12impl NiMatrix3 {
13    /// Constructs an identity matrix.
14    #[inline]
15    pub const fn new() -> Self {
16        Self { entry: [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]] }
17    }
18
19    /// Constructs a matrix from three vectors.
20    #[inline]
21    pub const fn from_vectors(x: NiPoint3, y: NiPoint3, z: NiPoint3) -> Self {
22        Self { entry: [[x.x, x.y, x.z], [y.x, y.y, y.z], [z.x, z.y, z.z]] }
23    }
24
25    /// Returns the X axis vector.
26    #[inline]
27    pub const fn get_vector_x(&self) -> NiPoint3 {
28        NiPoint3::new(self.entry[0][0], self.entry[1][0], self.entry[2][0])
29    }
30
31    /// Returns the Y axis vector.
32    #[inline]
33    pub const fn get_vector_y(&self) -> NiPoint3 {
34        NiPoint3::new(self.entry[0][1], self.entry[1][1], self.entry[2][1])
35    }
36
37    /// Returns the Z axis vector.
38    #[inline]
39    pub const fn get_vector_z(&self) -> NiPoint3 {
40        NiPoint3::new(self.entry[0][2], self.entry[1][2], self.entry[2][2])
41    }
42
43    /// Transposes the matrix.
44    pub fn transpose(&self) -> Self {
45        let mut result = *self;
46        for i in 0..3 {
47            for j in 0..3 {
48                result.entry[i][j] = self.entry[j][i];
49            }
50        }
51        result
52    }
53
54    /// Multiplies the matrix by another matrix.
55    pub fn mul_matrix(&self, rhs: &Self) -> Self {
56        let mut result = Self::new();
57        for i in 0..3 {
58            for j in 0..3 {
59                result.entry[i][j] = (0..3).map(|k| self.entry[i][k] * rhs.entry[k][j]).sum();
60            }
61        }
62        result
63    }
64
65    /// Multiplies the matrix by a scalar.
66    pub fn mul_scalar(&self, scalar: f32) -> Self {
67        let mut result = *self;
68        for row in &mut result.entry {
69            for val in row.iter_mut() {
70                *val *= scalar;
71            }
72        }
73        result
74    }
75
76    /// Multiplies the matrix by a vector.
77    pub fn mul_vector(&self, v: &NiPoint3) -> NiPoint3 {
78        NiPoint3 {
79            x: self.entry[0][2].mul_add(v.z, self.entry[0][0].mul_add(v.x, self.entry[0][1] * v.y)),
80            y: self.entry[1][2].mul_add(v.z, self.entry[1][0].mul_add(v.x, self.entry[1][1] * v.y)),
81            z: self.entry[2][2].mul_add(v.z, self.entry[2][0].mul_add(v.x, self.entry[2][1] * v.y)),
82        }
83    }
84
85    /// Converts the matrix to Euler angles (XYZ) and returns the angles.
86    pub fn to_euler_xyz(&self) -> Option<NiPoint3> {
87        let y_angle = -self.entry[0][2].asin();
88        if y_angle.abs() < PI / 2.0 {
89            let x_angle = (-self.entry[1][2]).atan2(self.entry[2][2]);
90            let z_angle = (-self.entry[0][1]).atan2(self.entry[0][0]);
91            Some(NiPoint3::new(x_angle, y_angle, z_angle))
92        } else {
93            None
94        }
95    }
96
97    /// Sets the matrix from Euler angles (XYZ).
98    #[inline]
99    pub fn set_euler_xyz(&mut self, x: f32, y: f32, z: f32) {
100        *self = Self::from_euler_xyz(x, y, z);
101    }
102
103    /// Constructs a matrix from Euler angles.
104    pub fn from_euler_xyz(x: f32, y: f32, z: f32) -> Self {
105        let sin_x = x.sin();
106        let cos_x = x.cos();
107        let sin_y = y.sin();
108        let cos_y = y.cos();
109        let sin_z = z.sin();
110        let cos_z = z.cos();
111
112        Self {
113            entry: [
114                [cos_y * cos_z, cos_y * sin_z, -sin_y],
115                [
116                    (sin_x * sin_y).mul_add(sin_z, cos_x * cos_z),
117                    (sin_x * sin_y).mul_add(sin_z, cos_x * cos_z),
118                    sin_x * cos_y,
119                ],
120                [
121                    (cos_x * sin_y).mul_add(cos_z, sin_x * sin_z),
122                    (cos_x * sin_y).mul_add(sin_z, -(sin_x * cos_z)),
123                    cos_x * cos_y,
124                ],
125            ],
126        }
127    }
128}
129
130impl Default for NiMatrix3 {
131    #[inline]
132    fn default() -> Self {
133        Self::new()
134    }
135}
136
137impl core::ops::Mul for NiMatrix3 {
138    type Output = Self;
139
140    fn mul(self, rhs: Self) -> Self::Output {
141        self.mul_matrix(&rhs)
142    }
143}
144
145impl core::ops::Mul<NiPoint3> for NiMatrix3 {
146    type Output = NiPoint3;
147
148    fn mul(self, rhs: NiPoint3) -> Self::Output {
149        self.mul_vector(&rhs)
150    }
151}
152
153impl core::ops::Mul<f32> for NiMatrix3 {
154    type Output = Self;
155
156    fn mul(self, rhs: f32) -> Self::Output {
157        self.mul_scalar(rhs)
158    }
159}
160
161impl core::ops::Add for NiMatrix3 {
162    type Output = Self;
163
164    fn add(self, rhs: Self) -> Self::Output {
165        let mut result = self;
166        for i in 0..3 {
167            for j in 0..3 {
168                result.entry[i][j] += rhs.entry[i][j];
169            }
170        }
171        result
172    }
173}
174
175impl core::ops::Sub for NiMatrix3 {
176    type Output = Self;
177
178    fn sub(self, rhs: Self) -> Self::Output {
179        let mut result = self;
180        for i in 0..3 {
181            for j in 0..3 {
182                result.entry[i][j] -= rhs.entry[i][j];
183            }
184        }
185        result
186    }
187}
188
189#[cfg(test)]
190mod tests {
191    use super::*;
192
193    #[test]
194    fn test_matrix_operations() {
195        let m1 = NiMatrix3::from_euler_xyz(0.1, 0.2, 0.3);
196        let m2 = NiMatrix3::from_euler_xyz(0.3, 0.2, 0.1);
197        let result = m1 * m2;
198
199        let expected = NiMatrix3 {
200            entry: [
201                [1.146675, 0.42327216, -0.28814036],
202                [1.8687906, 0.9814126, 0.17860672],
203                [0.39051157, -0.28224778, 0.8589622],
204            ],
205        };
206        assert_eq!(result, expected);
207    }
208}
commonlibsse_ng\re\n/NiMatrix3.rs

commonlibsse_ng\re\n/
NiMatrix3.rs
