1use crate::sim::{atan2f, cosf, sinf, sqrtf};
2
3pub type Mat4 = [[f32; 4]; 4];
4pub type Quat = [f32; 4];
5
6pub const IDENTITY: Mat4 = [
7 [1.0, 0.0, 0.0, 0.0],
8 [0.0, 1.0, 0.0, 0.0],
9 [0.0, 0.0, 1.0, 0.0],
10 [0.0, 0.0, 0.0, 1.0],
11];
12
13pub const QUAT_IDENTITY: Quat = [0.0, 0.0, 0.0, 1.0];
14
15#[derive(Clone, Copy, Debug, PartialEq)]
16pub struct Trs {
17 pub translation: [f32; 3],
18 pub rotation: Quat,
19 pub scale: [f32; 3],
20}
21
22impl Trs {
23 pub const IDENTITY: Self = Self {
24 translation: [0.0; 3],
25 rotation: QUAT_IDENTITY,
26 scale: [1.0; 3],
27 };
28
29 pub fn matrix(&self) -> Mat4 {
30 let [x, y, z, w] = self.rotation;
31 let [sx, sy, sz] = self.scale;
32 let [tx, ty, tz] = self.translation;
33 [
34 [
35 (1.0 - 2.0 * (y * y + z * z)) * sx,
36 2.0 * (x * y + z * w) * sx,
37 2.0 * (x * z - y * w) * sx,
38 0.0,
39 ],
40 [
41 2.0 * (x * y - z * w) * sy,
42 (1.0 - 2.0 * (x * x + z * z)) * sy,
43 2.0 * (y * z + x * w) * sy,
44 0.0,
45 ],
46 [
47 2.0 * (x * z + y * w) * sz,
48 2.0 * (y * z - x * w) * sz,
49 (1.0 - 2.0 * (x * x + y * y)) * sz,
50 0.0,
51 ],
52 [tx, ty, tz, 1.0],
53 ]
54 }
55
56 pub fn finite(&self) -> bool {
57 self.translation
58 .iter()
59 .chain(&self.rotation)
60 .chain(&self.scale)
61 .all(|value| value.is_finite())
62 }
63}
64
65impl Default for Trs {
66 fn default() -> Self {
67 Self::IDENTITY
68 }
69}
70
71pub fn multiply(a: &Mat4, b: &Mat4) -> Mat4 {
72 let mut out = [[0.0; 4]; 4];
73 for (column, target) in out.iter_mut().enumerate() {
74 for (row, value) in target.iter_mut().enumerate() {
75 *value = a[0][row] * b[column][0]
76 + a[1][row] * b[column][1]
77 + a[2][row] * b[column][2]
78 + a[3][row] * b[column][3];
79 }
80 }
81 out
82}
83
84pub fn transform_point(m: &Mat4, p: [f32; 3]) -> [f32; 3] {
85 [
86 m[0][0] * p[0] + m[1][0] * p[1] + m[2][0] * p[2] + m[3][0],
87 m[0][1] * p[0] + m[1][1] * p[1] + m[2][1] * p[2] + m[3][1],
88 m[0][2] * p[0] + m[1][2] * p[1] + m[2][2] * p[2] + m[3][2],
89 ]
90}
91
92pub fn transform_vector(m: &Mat4, v: [f32; 3]) -> [f32; 3] {
93 [
94 m[0][0] * v[0] + m[1][0] * v[1] + m[2][0] * v[2],
95 m[0][1] * v[0] + m[1][1] * v[1] + m[2][1] * v[2],
96 m[0][2] * v[0] + m[1][2] * v[1] + m[2][2] * v[2],
97 ]
98}
99
100pub fn normalize3(v: [f32; 3]) -> [f32; 3] {
101 let length = sqrtf(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
102 if length > 0.0 {
103 [v[0] / length, v[1] / length, v[2] / length]
104 } else {
105 v
106 }
107}
108
109pub fn invert(m: &Mat4) -> Option<Mat4> {
110 let a = |c: usize, r: usize| m[c][r] as f64;
111 let s0 = a(0, 0) * a(1, 1) - a(0, 1) * a(1, 0);
112 let s1 = a(0, 0) * a(1, 2) - a(0, 2) * a(1, 0);
113 let s2 = a(0, 0) * a(1, 3) - a(0, 3) * a(1, 0);
114 let s3 = a(0, 1) * a(1, 2) - a(0, 2) * a(1, 1);
115 let s4 = a(0, 1) * a(1, 3) - a(0, 3) * a(1, 1);
116 let s5 = a(0, 2) * a(1, 3) - a(0, 3) * a(1, 2);
117 let c5 = a(2, 2) * a(3, 3) - a(2, 3) * a(3, 2);
118 let c4 = a(2, 1) * a(3, 3) - a(2, 3) * a(3, 1);
119 let c3 = a(2, 1) * a(3, 2) - a(2, 2) * a(3, 1);
120 let c2 = a(2, 0) * a(3, 3) - a(2, 3) * a(3, 0);
121 let c1 = a(2, 0) * a(3, 2) - a(2, 2) * a(3, 0);
122 let c0 = a(2, 0) * a(3, 1) - a(2, 1) * a(3, 0);
123 let det = s0 * c5 - s1 * c4 + s2 * c3 + s3 * c2 - s4 * c1 + s5 * c0;
124 if det == 0.0 || !det.is_finite() {
125 return None;
126 }
127 let inv = 1.0 / det;
128 let out = [
129 [
130 (a(1, 1) * c5 - a(1, 2) * c4 + a(1, 3) * c3) * inv,
131 (-a(0, 1) * c5 + a(0, 2) * c4 - a(0, 3) * c3) * inv,
132 (a(3, 1) * s5 - a(3, 2) * s4 + a(3, 3) * s3) * inv,
133 (-a(2, 1) * s5 + a(2, 2) * s4 - a(2, 3) * s3) * inv,
134 ],
135 [
136 (-a(1, 0) * c5 + a(1, 2) * c2 - a(1, 3) * c1) * inv,
137 (a(0, 0) * c5 - a(0, 2) * c2 + a(0, 3) * c1) * inv,
138 (-a(3, 0) * s5 + a(3, 2) * s2 - a(3, 3) * s1) * inv,
139 (a(2, 0) * s5 - a(2, 2) * s2 + a(2, 3) * s1) * inv,
140 ],
141 [
142 (a(1, 0) * c4 - a(1, 1) * c2 + a(1, 3) * c0) * inv,
143 (-a(0, 0) * c4 + a(0, 1) * c2 - a(0, 3) * c0) * inv,
144 (a(3, 0) * s4 - a(3, 1) * s2 + a(3, 3) * s0) * inv,
145 (-a(2, 0) * s4 + a(2, 1) * s2 - a(2, 3) * s0) * inv,
146 ],
147 [
148 (-a(1, 0) * c3 + a(1, 1) * c1 - a(1, 2) * c0) * inv,
149 (a(0, 0) * c3 - a(0, 1) * c1 + a(0, 2) * c0) * inv,
150 (-a(3, 0) * s3 + a(3, 1) * s1 - a(3, 2) * s0) * inv,
151 (a(2, 0) * s3 - a(2, 1) * s1 + a(2, 2) * s0) * inv,
152 ],
153 ];
154 let out = out.map(|column| column.map(|value| value as f32));
155 out.iter()
156 .flatten()
157 .all(|value| value.is_finite())
158 .then_some(out)
159}
160
161pub fn quat_normalize(q: Quat) -> Quat {
162 let length = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
163 if length > 0.0 {
164 [q[0] / length, q[1] / length, q[2] / length, q[3] / length]
165 } else {
166 QUAT_IDENTITY
167 }
168}
169
170pub fn quat_dot(a: Quat, b: Quat) -> f32 {
171 a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3]
172}
173
174pub fn quat_multiply(a: Quat, b: Quat) -> Quat {
175 [
176 a[3] * b[0] + a[0] * b[3] + a[1] * b[2] - a[2] * b[1],
177 a[3] * b[1] - a[0] * b[2] + a[1] * b[3] + a[2] * b[0],
178 a[3] * b[2] + a[0] * b[1] - a[1] * b[0] + a[2] * b[3],
179 a[3] * b[3] - a[0] * b[0] - a[1] * b[1] - a[2] * b[2],
180 ]
181}
182
183pub fn quat_axis_angle(axis: [f32; 3], angle: f32) -> Quat {
184 let axis = normalize3(axis);
185 let half = angle * 0.5;
186 let s = sinf(half);
187 let c = cosf(half);
188 quat_normalize([axis[0] * s, axis[1] * s, axis[2] * s, c])
189}
190
191pub fn nlerp(a: Quat, b: Quat, t: f32) -> Quat {
192 let b = if quat_dot(a, b) < 0.0 {
193 [-b[0], -b[1], -b[2], -b[3]]
194 } else {
195 b
196 };
197 quat_normalize(std::array::from_fn(|k| a[k] + (b[k] - a[k]) * t))
198}
199
200pub fn slerp(a: Quat, b: Quat, t: f32) -> Quat {
201 let mut d = quat_dot(a, b);
202 let b = if d < 0.0 {
203 d = -d;
204 [-b[0], -b[1], -b[2], -b[3]]
205 } else {
206 b
207 };
208 if d > 0.9995 {
209 return quat_normalize(std::array::from_fn(|k| a[k] + (b[k] - a[k]) * t));
210 }
211 let theta = atan2f(sqrtf((1.0 - d * d).max(0.0)), d);
212 let s = sinf(theta);
213 let wa = sinf((1.0 - t) * theta) / s;
214 let wb = sinf(t * theta) / s;
215 quat_normalize(std::array::from_fn(|k| a[k] * wa + b[k] * wb))
216}
217
218pub fn lerp3(a: [f32; 3], b: [f32; 3], t: f32) -> [f32; 3] {
219 std::array::from_fn(|k| a[k] + (b[k] - a[k]) * t)
220}