radiant_utils/math/
matrix.rs

1use prelude::*;
2use super::{Vec3, Vector};
3
4/// A 4x4 matrix.
5pub type Mat4<T = f32> = [ [ T; 4 ]; 4 ];
6
7/// Matrix-methods for 4x4 arrays.
8pub trait Matrix<T> where T: Copy { 
9    /// Sets the matrix value from another matrix.
10    fn set<M>(self: &mut Self, other: M) -> &mut Self where Mat4<T>: From<M>;
11    /// Translate matrix by given vector.
12    fn translate<V>(self: &mut Self, translation_vector: V) -> &mut Self where V: Vector<T>;
13    /// Scale matrix by given vector.
14    fn scale<V>(self: &mut Self, scaling_vector: V) -> &mut Self where V: Vector<T>;
15    /// Scales at given position.
16    fn scale_at<P, V>(self: &mut Self, position: P, scaling_vector: V) -> &mut Self where P: Vector<T>, V: Vector<T>;
17    /// Rotates the origin around z.
18    fn rotate(self: &mut Self, radians: T) -> &mut Self;
19    /// Rotates around z at given position.
20    fn rotate_at<P>(self: &mut Self, position: P, radians: T) -> &mut Self where P: Vector<T>;
21    /// Rotates around axis.
22    fn rotate_axis<V>(self: &mut Self, radians: T, axis: V) -> &mut Self where Vec3<T>: From<V>;
23    /// Rotates around axis at given position.
24    fn rotate_axis_at<P, V>(self: &mut Self, position: P, radians: T, axis: V) -> &mut Self where P: Vector<T>, Vec3<T>: From<V>;
25    /// Returns a pure rotation matrix for given matrix
26    fn get_rotation(self: &Self) -> Self;
27    /// Returns the matrix's translation vector.
28    fn get_translation(self: &Self) -> Vec3<T>;
29    /// Returns the matrix's scaling vector.
30    fn get_scale(self: &Self) -> Vec3<T>;
31    /// Get rotation matrix euler angles.
32    fn get_euler(self: &Self) -> Vec3<T>;
33}
34
35impl<T> Matrix<T> for Mat4<T> where T: Float {   
36    fn set<M>(self: &mut Self, other: M) -> &mut Self where Mat4<T>: From<M> {
37        *self = other.into();
38        self
39    }
40    fn translate<V>(self: &mut Self, translation_vector: V) -> &mut Self where V: Vector<T> {
41        let Vec3::<T>(x, y, z) = translation_vector.as_vec3(T::zero());
42        self[3][0] = self[0][0]* x + self[1][0] * y + self[2][0] * z + self[3][0];
43        self[3][1] = self[0][1]* x + self[1][1] * y + self[2][1] * z + self[3][1];
44        self[3][2] = self[0][2]* x + self[1][2] * y + self[2][2] * z + self[3][2];
45        self[3][3] = self[0][3]* x + self[1][3] * y + self[2][3] * z + self[3][3];
46        self
47    }
48    fn scale<V>(self: &mut Self, scaling_vector: V) -> &mut Self where V: Vector<T> {
49        let Vec3::<T>(x, y, z) = scaling_vector.as_vec3(T::one());
50        self[0][0] = self[0][0] * x;
51        self[0][1] = self[0][1] * x;
52        self[0][2] = self[0][2] * x;
53        self[0][3] = self[0][3] * x;
54        self[1][0] = self[1][0] * y;
55        self[1][1] = self[1][1] * y;
56        self[1][2] = self[1][2] * y;
57        self[1][3] = self[1][3] * y;
58        self[2][0] = self[2][0] * z;
59        self[2][1] = self[2][1] * z;
60        self[2][2] = self[2][2] * z;
61        self[2][3] = self[2][3] * z;
62        self
63    }
64    fn scale_at<P, V>(self: &mut Self, position: P, scaling_vector: V) -> &mut Self where P: Vector<T>, V: Vector<T> {
65        let position = position.as_vec3(T::zero());
66        self.translate(position)
67            .scale(scaling_vector)
68            .translate(-position)
69    }
70    fn rotate(self: &mut Self, radians: T) -> &mut Self {
71        let s = radians.sin();
72        let c = radians.cos();
73        let a00 = self[0][0];
74        let a01 = self[0][1];
75        let a02 = self[0][2];
76        let a03 = self[0][3];
77        let a10 = self[1][0];
78        let a11 = self[1][1];
79        let a12 = self[1][2];
80        let a13 = self[1][3];
81        self[0][0] = a00 * c + a10 * s;
82        self[0][1] = a01 * c + a11 * s;
83        self[0][2] = a02 * c + a12 * s;
84        self[0][3] = a03 * c + a13 * s;
85        self[1][0] = a10 * c - a00 * s;
86        self[1][1] = a11 * c - a01 * s;
87        self[1][2] = a12 * c - a02 * s;
88        self[1][3] = a13 * c - a03 * s;
89        self
90    }
91    fn rotate_at<P>(self: &mut Self, position: P, radians: T) -> &mut Self where P: Vector<T> {
92        let position = position.as_vec3(T::zero());
93        self.translate(position)
94            .rotate(radians)
95            .translate(-position)
96    }
97    fn rotate_axis<V>(self: &mut Self, radians: T, axis: V) -> &mut Self where Vec3<T>: From<V> {
98
99        let Vec3::<T>(mut x, mut y, mut z) = axis.into();
100        let mut len: T = (x * x + y * y + z * z).sqrt();
101
102        if len == T::zero() || len.is_normal() == false {
103            return self;
104        }
105
106        len = T::one() / len;
107        x = x * len;
108        y = y * len;
109        z = z * len;
110
111        let s: T = radians.sin();
112        let c: T = radians.cos();
113        let t: T = T::one() - c;
114
115        let a00 = self[0][0];
116        let a01 = self[0][1];
117        let a02 = self[0][2];
118        let a03 = self[0][3];
119        let a10 = self[1][0];
120        let a11 = self[1][1];
121        let a12 = self[1][2];
122        let a13 = self[1][3];
123        let a20 = self[2][0];
124        let a21 = self[2][1];
125        let a22 = self[2][2];
126        let a23 = self[2][3];
127
128        // Construct the elements of the rotation matrix
129        let b00 = x * x * t + c;
130        let b01 = y * x * t + z * s;
131        let b02 = z * x * t - y * s;
132        let b10 = x * y * t - z * s;
133        let b11 = y * y * t + c;
134        let b12 = z * y * t + x * s;
135        let b20 = x * z * t + y * s;
136        let b21 = y * z * t - x * s;
137        let b22 = z * z * t + c;
138
139        // Perform rotation-specific matrix multiplication
140        self[0][0] = a00 * b00 + a10 * b01 + a20 * b02;
141        self[0][1] = a01 * b00 + a11 * b01 + a21 * b02;
142        self[0][2] = a02 * b00 + a12 * b01 + a22 * b02;
143        self[0][3] = a03 * b00 + a13 * b01 + a23 * b02;
144        self[1][0] = a00 * b10 + a10 * b11 + a20 * b12;
145        self[1][1] = a01 * b10 + a11 * b11 + a21 * b12;
146        self[1][2] = a02 * b10 + a12 * b11 + a22 * b12;
147        self[1][3] = a03 * b10 + a13 * b11 + a23 * b12;
148        self[2][0] = a00 * b20 + a10 * b21 + a20 * b22;
149        self[2][1] = a01 * b20 + a11 * b21 + a21 * b22;
150        self[2][2] = a02 * b20 + a12 * b21 + a22 * b22;
151        self[2][3] = a03 * b20 + a13 * b21 + a23 * b22;
152
153        self
154    }
155    fn rotate_axis_at<P, V>(self: &mut Self, position: P, radians: T, axis: V) -> &mut Self where P: Vector<T>, Vec3<T>: From<V> {
156        let position = position.as_vec3(T::zero());
157        self.translate(position)
158            .rotate_axis(radians, axis)
159            .translate(-position)
160    }
161    fn get_rotation(self: &Self) -> Self {
162        let Vec3(x, y, z) = self.get_scale();
163        let a = self;
164        [
165            [ a[0][0] / x, a[0][1] / x, a[0][2] / x, T::zero()  ],
166            [ a[1][0] / y, a[1][1] / y, a[1][2] / y, T::zero(), ],
167            [ a[2][0] / z, a[2][1] / z, a[2][2] / z, T::zero(), ],
168            [ T::zero(),   T::zero(),   T::zero(),   T::one(),  ],
169        ]
170    }
171    fn get_translation(self: &Self) -> Vec3<T> {
172        let a = self;
173        Vec3(a[3][0], a[3][1], a[3][2])
174    }
175    fn get_scale(self: &Self) -> Vec3<T> {
176        let a = self;
177        let x = Vec3(a[0][0], a[0][1], a[0][2]);
178        let y = Vec3(a[1][0], a[1][1], a[1][2]);
179        let z = Vec3(a[2][0], a[2][1], a[2][2]);
180        Vec3(x.len(), y.len(), z.len())
181    }
182    #[allow(non_snake_case)]
183    fn get_euler(self: &Self) -> Vec3<T> {
184        let a = self;
185        let y: T;
186        let z: T;
187        let x: T;
188
189        let half_PI = NumCast::from(f64::consts::PI / 2.0).unwrap();
190
191    	if a[0][1] > NumCast::from(0.998).unwrap() { // singularity at north pole
192    		y = a[2][0].atan2(a[2][2]);
193    		z = half_PI;
194    		x = T::zero();
195    	} else if a[0][1] < NumCast::from(-0.998).unwrap() { // singularity at south pole
196    		y = a[2][0].atan2(a[2][2]);
197    		z = -half_PI;
198    		x = T::zero();
199    	} else {
200        	y = (-a[0][2]).atan2(a[0][0]);
201            x = (-a[2][1]).atan2(a[1][1]);
202        	z = a[0][1].asin();
203        }
204
205        Vec3(x, y, z)
206    }
207}
radiant_utils/math/matrix.rs

radiant_utils/math/
matrix.rs
