cvmath/mat/
transform3.rs

1/*!
23D affine transformation matrix.
3*/
4
5use super::*;
6
7/// 3D affine transformation matrix.
8///
9/// Each field _a_<sub>i</sub><sub>j</sub> represents the _i_-th row and _j_-th column of the matrix.
10///
11/// The third row is implied to be `[0, 0, 0, 1]` and is omitted.
12///
13/// Row-major storage with column-major semantics.
14///
15/// Stored in row-major order (fields appear in reading order),
16/// but interpreted as column-major: each column is a transformed basis vector,
17/// and matrices are applied to column vectors via `mat * vec`.
18#[derive(Copy, Clone, Default, PartialEq)]
19#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
20#[repr(C)]
21pub struct Transform3<T> {
22	pub a11: T, pub a12: T, pub a13: T, pub a14: T,
23	pub a21: T, pub a22: T, pub a23: T, pub a24: T,
24	pub a31: T, pub a32: T, pub a33: T, pub a34: T,
25}
26
27/// Transform3 constructor.
28#[allow(non_snake_case)]
29#[inline]
30pub const fn Transform3<T>(
31	a11: T, a12: T, a13: T, a14: T,
32	a21: T, a22: T, a23: T, a24: T,
33	a31: T, a32: T, a33: T, a34: T,
34) -> Transform3<T> {
35	Transform3 { a11, a12, a13, a14, a21, a22, a23, a24, a31, a32, a33, a34 }
36}
37
38#[cfg(feature = "dataview")]
39unsafe impl<T: dataview::Pod> dataview::Pod for Transform3<T> {}
40
41//----------------------------------------------------------------
42// Constructors
43
44impl<T> Transform3<T> {
45	/// Constructs a new matrix from components.
46	#[inline]
47	pub const fn new(
48		a11: T, a12: T, a13: T, a14: T,
49		a21: T, a22: T, a23: T, a24: T,
50		a31: T, a32: T, a33: T, a34: T,
51	) -> Transform3<T> {
52		Transform3 {
53			a11, a12, a13, a14,
54			a21, a22, a23, a24,
55			a31, a32, a33, a34,
56		}
57	}
58}
59
60impl<T: Zero> Transform3<T> {
61	/// Zero matrix.
62	pub const ZERO: Transform3<T> = Transform3 {
63		a11: T::ZERO, a12: T::ZERO, a13: T::ZERO, a14: T::ZERO,
64		a21: T::ZERO, a22: T::ZERO, a23: T::ZERO, a24: T::ZERO,
65		a31: T::ZERO, a32: T::ZERO, a33: T::ZERO, a34: T::ZERO,
66	};
67}
68
69impl<T: Zero + One> Transform3<T> {
70	/// Identity matrix.
71	pub const IDENTITY: Transform3<T> = Transform3 {
72		a11: T::ONE,  a12: T::ZERO, a13: T::ZERO, a14: T::ZERO,
73		a21: T::ZERO, a22: T::ONE,  a23: T::ZERO, a24: T::ZERO,
74		a31: T::ZERO, a32: T::ZERO, a33: T::ONE,  a34: T::ZERO,
75	};
76}
77
78impl<T: Float> Transform3<T> {
79	/// Translation matrix.
80	#[inline]
81	pub fn translate(trans: Vec3<T>) -> Transform3<T> {
82		let Vec3 { x: a14, y: a24, z: a34 } = trans;
83		Transform3 { a14, a24, a34, ..Transform3::IDENTITY }
84	}
85
86	/// Scaling matrix.
87	///
88	/// Scales around the origin.
89	#[inline]
90	pub fn scale(scale: Vec3<T>) -> Transform3<T> {
91		let Vec3 { x: a11, y: a22, z: a33 } = scale;
92		Transform3 { a11, a22, a33, ..Transform3::IDENTITY }
93	}
94
95	/// Rotation matrix around an axis.
96	#[inline]
97	pub fn rotate(axis: Vec3<T>, angle: Angle<T>) -> Transform3<T> {
98		Mat3::rotate(axis, angle).transform3()
99	}
100}
101
102impl<T: Float> Transform3<T> {
103	/// Look-at matrix.
104	#[inline]
105	pub fn look_at(position: Vec3<T>, target: Vec3<T>, ref_up: Vec3<T>, hand: Hand) -> Transform3<T> {
106		let forward = match hand {
107			Hand::LH => target - position,
108			Hand::RH => position - target,
109		}.norm();
110
111		let side = ref_up.cross(forward).norm();
112		let up = forward.cross(side);
113
114		let Vec3 { x: a11, y: a12, z: a13 } = side;
115		let a14 = -side.dot(position);
116
117		let Vec3 { x: a21, y: a22, z: a23 } = up;
118		let a24 = -up.dot(position);
119
120		let Vec3 { x: a31, y: a32, z: a33 } = forward;
121		let a34 = -forward.dot(position);
122
123		Transform3 {
124			a11, a12, a13, a14,
125			a21, a22, a23, a24,
126			a31, a32, a33, a34,
127		}
128	}
129
130	/// Orthographic projection matrix.
131	///
132	/// Clip and hand parameters only affect the Z coordinate.
133	#[inline]
134	pub fn ortho(bounds: Bounds3<T>, (hand, clip): (Hand, Clip)) -> Transform3<T> {
135		let Bounds3 {
136			mins: Vec3 { x: left, y: bottom, z: near },
137			maxs: Vec3 { x: right, y: top, z: far },
138		} = bounds;
139
140		debug_assert!(T::ZERO < near && near < far);
141
142		let a11 = T::TWO / (right - left);
143		let a14 = -(right + left) / (right - left);
144		let a22 = T::TWO / (top - bottom);
145		let a24 = -(top + bottom) / (top - bottom);
146		let a33 = match clip { Clip::ZO => T::ONE, Clip::NO => T::TWO } / (far - near);
147		let a33 = match hand { Hand::LH => a33, Hand::RH => -a33 };
148		let a34 = -match clip { Clip::ZO => near, Clip::NO => far + near } / (far - near);
149
150		Transform3 { a11, a14, a22, a24, a33, a34, ..Self::IDENTITY }
151	}
152}
153
154//----------------------------------------------------------------
155// Conversions
156
157impl<T: Zero + One> Transform3<T> {
158	/// Converts to a 4x4 matrix.
159	#[inline]
160	pub fn mat4(self) -> Mat4<T> {
161		self.into()
162	}
163}
164
165impl<T> Transform3<T> {
166	#[inline]
167	fn as_array(&self) -> &[T; 12] {
168		unsafe { mem::transmute(self)}
169	}
170	/// Imports the matrix from a row-major layout.
171	#[inline]
172	pub fn from_row_major(mat: [[T; 4]; 3]) -> Transform3<T> {
173		let [[a11, a12, a13, a14], [a21, a22, a23, a24], [a31, a32, a33, a34]] = mat;
174		Transform3 {
175			a11, a12, a13, a14,
176			a21, a22, a23, a24,
177			a31, a32, a33, a34,
178		}
179	}
180	/// Imports the matrix from a column-major layout.
181	#[inline]
182	pub fn from_column_major(mat: [[T; 3]; 4]) -> Transform3<T> {
183		let [[a11, a21, a31], [a12, a22, a32], [a13, a23, a33], [a14, a24, a34]] = mat;
184		Transform3 {
185			a11, a12, a13, a14,
186			a21, a22, a23, a24,
187			a31, a32, a33, a34,
188		}
189	}
190	/// Exports the matrix as a row-major array.
191	#[inline]
192	pub fn into_row_major(self) -> [[T; 4]; 3] {
193		[
194			[self.a11, self.a12, self.a13, self.a14],
195			[self.a21, self.a22, self.a23, self.a24],
196			[self.a31, self.a32, self.a33, self.a34],
197		]
198	}
199	/// Exports the matrix as a column-major array.
200	#[inline]
201	pub fn into_column_major(self) -> [[T; 3]; 4] {
202		[
203			[self.a11, self.a21, self.a31],
204			[self.a12, self.a22, self.a32],
205			[self.a13, self.a23, self.a33],
206			[self.a14, self.a24, self.a34],
207		]
208	}
209}
210
211//----------------------------------------------------------------
212// Decomposition
213
214impl<T> Transform3<T> {
215	/// Composes the matrix from basis vectors.
216	#[inline]
217	pub fn compose(x: Vec3<T>, y: Vec3<T>, z: Vec3<T>, t: Vec3<T>) -> Transform3<T> {
218		Transform3 {
219			a11: x.x, a12: y.x, a13: z.x, a14: t.x,
220			a21: x.y, a22: y.y, a23: z.y, a24: t.y,
221			a31: x.z, a32: y.z, a33: z.z, a34: t.z,
222		}
223	}
224	/// Gets the transformed X basis vector.
225	#[inline]
226	pub fn x(self) -> Vec3<T> {
227		Vec3 { x: self.a11, y: self.a21, z: self.a31 }
228	}
229	/// Gets the transformed Y basis vector.
230	#[inline]
231	pub fn y(self) -> Vec3<T> {
232		Vec3 { x: self.a12, y: self.a22, z: self.a32 }
233	}
234	/// Gets the transformed Z basis vector.
235	#[inline]
236	pub fn z(self) -> Vec3<T> {
237		Vec3 { x: self.a13, y: self.a23, z: self.a33 }
238	}
239	/// Gets the translation vector.
240	#[inline]
241	pub fn t(self) -> Vec3<T> {
242		Vec3 { x: self.a14, y: self.a24, z: self.a34 }
243	}
244	/// Gets the rotation matrix.
245	#[inline]
246	pub fn mat3(self) -> Mat3<T> {
247		Mat3 {
248			a11: self.a11, a12: self.a12, a13: self.a13,
249			a21: self.a21, a22: self.a22, a23: self.a23,
250			a31: self.a31, a32: self.a32, a33: self.a33,
251		}
252	}
253}
254
255//----------------------------------------------------------------
256// Operations
257
258impl<T: Scalar> Transform3<T> {
259	/// Computes the determinant.
260	#[inline]
261	pub fn det(self) -> T {
262		self.a11 * (self.a22 * self.a33 - self.a23 * self.a32) +
263		self.a12 * (self.a23 * self.a31 - self.a21 * self.a33) +
264		self.a13 * (self.a21 * self.a32 - self.a22 * self.a31)
265	}
266	/// Computes the trace.
267	#[inline]
268	pub fn trace(self) -> T {
269		self.a11 + self.a22 + self.a33 + T::ONE
270	}
271	/// Computes the squared Frobenius norm (sum of squares of all matrix elements).
272	///
273	/// This measure is useful for quickly checking matrix magnitude or comparing matrices without the cost of a square root operation.
274	///
275	/// To check if a matrix is effectively zero, test if `flat_norm_sqr()` is below a small epsilon threshold.
276	#[inline]
277	pub fn flat_norm_sqr(self) -> T {
278		self.a11 * self.a11 + self.a12 * self.a12 + self.a13 * self.a13 + self.a14 * self.a14 +
279		self.a21 * self.a21 + self.a22 * self.a22 + self.a23 * self.a23 + self.a24 * self.a24 +
280		self.a31 * self.a31 + self.a32 * self.a32 + self.a33 * self.a33 + self.a34 * self.a34
281	}
282	#[inline]
283	pub fn try_invert(self) -> Option<Transform3<T>> where T: Float {
284		let det = self.det();
285		if det == T::ZERO {
286			return None;
287		}
288
289		let inv_det = T::ONE / det;
290		Some(Transform3 {
291			a11: (self.a22 * self.a33 - self.a23 * self.a32) * inv_det,
292			a12: (self.a13 * self.a32 - self.a12 * self.a33) * inv_det,
293			a13: (self.a12 * self.a23 - self.a13 * self.a22) * inv_det,
294			a14: (self.a12 * (self.a24 * self.a33 - self.a23 * self.a34) +
295				self.a13 * (self.a22 * self.a34 - self.a24 * self.a32) +
296				self.a14 * (self.a23 * self.a32 - self.a22 * self.a33)) * inv_det,
297			a21: (self.a23 * self.a31 - self.a21 * self.a33) * inv_det,
298			a22: (self.a11 * self.a33 - self.a13 * self.a31) * inv_det,
299			a23: (self.a13 * self.a21 - self.a11 * self.a23) * inv_det,
300			a24: (self.a11 * (self.a23 * self.a34 - self.a24 * self.a33) +
301				self.a13 * (self.a24 * self.a31 - self.a21 * self.a34) +
302				self.a14 * (self.a21 * self.a33 - self.a23 * self.a31)) * inv_det,
303			a31: (self.a21 * self.a32 - self.a22 * self.a31) * inv_det,
304			a32: (self.a12 * self.a31 - self.a11 * self.a32) * inv_det,
305			a33: (self.a11 * self.a22 - self.a12 * self.a21) * inv_det,
306			a34: (self.a11 * (self.a24 * self.a32 - self.a22 * self.a34) +
307				self.a12 * (self.a21 * self.a34 - self.a24 * self.a31) +
308				self.a14 * (self.a22 * self.a31 - self.a21 * self.a32)) * inv_det,
309		})
310	}
311	/// Computes the inverse matrix.
312	///
313	/// Returns the zero matrix if the determinant is near zero.
314	#[inline]
315	pub fn inverse(self) -> Transform3<T> where T: Float {
316		self.try_invert().unwrap_or(Transform3::ZERO)
317	}
318	/// Linear interpolation between the matrix elements.
319	#[inline]
320	pub fn lerp(self, rhs: Transform3<T>, t: T) -> Transform3<T> where T: Float {
321		Transform3 {
322			a11: self.a11 + (rhs.a11 - self.a11) * t,
323			a12: self.a12 + (rhs.a12 - self.a12) * t,
324			a13: self.a13 + (rhs.a13 - self.a13) * t,
325			a14: self.a14 + (rhs.a14 - self.a14) * t,
326
327			a21: self.a21 + (rhs.a21 - self.a21) * t,
328			a22: self.a22 + (rhs.a22 - self.a22) * t,
329			a23: self.a23 + (rhs.a23 - self.a23) * t,
330			a24: self.a24 + (rhs.a24 - self.a24) * t,
331
332			a31: self.a31 + (rhs.a31 - self.a31) * t,
333			a32: self.a32 + (rhs.a32 - self.a32) * t,
334			a33: self.a33 + (rhs.a33 - self.a33) * t,
335			a34: self.a34 + (rhs.a34 - self.a34) * t,
336		}
337	}
338}
339
340//----------------------------------------------------------------
341// Operators
342
343impl<T: Copy + ops::Add<Output = T> + ops::Mul<Output = T>> ops::Mul<Vec3<T>> for Transform3<T> {
344	type Output = Vec3<T>;
345	#[inline]
346	fn mul(self, rhs: Vec3<T>) -> Vec3<T> {
347		Vec3 {
348			x: rhs.x * self.a11 + rhs.y * self.a12 + rhs.z * self.a13 + self.a14,
349			y: rhs.x * self.a21 + rhs.y * self.a22 + rhs.z * self.a23 + self.a24,
350			z: rhs.x * self.a31 + rhs.y * self.a32 + rhs.z * self.a33 + self.a34,
351		}
352	}
353}
354
355impl<T: Copy + ops::Add<Output = T> + ops::Mul<Output = T>> ops::Mul<Vec4<T>> for Transform3<T> {
356	type Output = Vec3<T>;
357	#[inline]
358	fn mul(self, rhs: Vec4<T>) -> Vec3<T> {
359		Vec3 {
360			x: rhs.x * self.a11 + rhs.y * self.a12 + rhs.z * self.a13 + rhs.w * self.a14,
361			y: rhs.x * self.a21 + rhs.y * self.a22 + rhs.z * self.a23 + rhs.w * self.a24,
362			z: rhs.x * self.a31 + rhs.y * self.a32 + rhs.z * self.a33 + rhs.w * self.a34,
363		}
364	}
365}
366
367impl<T: Copy + ops::Add<Output = T> + ops::Mul<Output = T>> ops::Mul<Transform3<T>> for Transform3<T> {
368	type Output = Transform3<T>;
369	#[inline]
370	fn mul(self, rhs: Transform3<T>) -> Transform3<T> {
371		Transform3 {
372			a11: self.a11 * rhs.a11 + self.a12 * rhs.a21 + self.a13 * rhs.a31,
373			a12: self.a11 * rhs.a12 + self.a12 * rhs.a22 + self.a13 * rhs.a32,
374			a13: self.a11 * rhs.a13 + self.a12 * rhs.a23 + self.a13 * rhs.a33,
375			a14: self.a11 * rhs.a14 + self.a12 * rhs.a24 + self.a13 * rhs.a34 + self.a14,
376
377			a21: self.a21 * rhs.a11 + self.a22 * rhs.a21 + self.a23 * rhs.a31,
378			a22: self.a21 * rhs.a12 + self.a22 * rhs.a22 + self.a23 * rhs.a32,
379			a23: self.a21 * rhs.a13 + self.a22 * rhs.a23 + self.a23 * rhs.a33,
380			a24: self.a21 * rhs.a14 + self.a22 * rhs.a24 + self.a23 * rhs.a34 + self.a24,
381
382			a31: self.a31 * rhs.a11 + self.a32 * rhs.a21 + self.a33 * rhs.a31,
383			a32: self.a31 * rhs.a12 + self.a32 * rhs.a22 + self.a33 * rhs.a32,
384			a33: self.a31 * rhs.a13 + self.a32 * rhs.a23 + self.a33 * rhs.a33,
385			a34: self.a31 * rhs.a14 + self.a32 * rhs.a24 + self.a33 * rhs.a34 + self.a34,
386		}
387	}
388}
389impl<T: Copy + ops::Add<Output = T> + ops::Mul<Output = T>> ops::Mul<Mat3<T>> for Transform3<T> {
390	type Output = Transform3<T>;
391	#[inline]
392	fn mul(self, rhs: Mat3<T>) -> Transform3<T> {
393		Transform3 {
394			a11: self.a11 * rhs.a11 + self.a12 * rhs.a21 + self.a13 * rhs.a31,
395			a12: self.a11 * rhs.a12 + self.a12 * rhs.a22 + self.a13 * rhs.a32,
396			a13: self.a11 * rhs.a13 + self.a12 * rhs.a23 + self.a13 * rhs.a33,
397			a14: self.a14,
398
399			a21: self.a21 * rhs.a11 + self.a22 * rhs.a21 + self.a23 * rhs.a31,
400			a22: self.a21 * rhs.a12 + self.a22 * rhs.a22 + self.a23 * rhs.a32,
401			a23: self.a21 * rhs.a13 + self.a22 * rhs.a23 + self.a23 * rhs.a33,
402			a24: self.a24,
403
404			a31: self.a31 * rhs.a11 + self.a32 * rhs.a21 + self.a33 * rhs.a31,
405			a32: self.a31 * rhs.a12 + self.a32 * rhs.a22 + self.a33 * rhs.a32,
406			a33: self.a31 * rhs.a13 + self.a32 * rhs.a23 + self.a33 * rhs.a33,
407			a34: self.a34,
408		}
409	}
410}
411impl<T: Copy + ops::Add<Output = T> + ops::Mul<Output = T>> ops::MulAssign<Mat3<T>> for Transform3<T> {
412	#[inline]
413	fn mul_assign(&mut self, rhs: Mat3<T>) {
414		*self = *self * rhs;
415	}
416}
417impl<T: Copy + ops::Add<Output = T> + ops::Mul<Output = T>> ops::MulAssign<Transform3<T>> for Transform3<T> {
418	#[inline]
419	fn mul_assign(&mut self, rhs: Transform3<T>) {
420		*self = *self * rhs;
421	}
422}
423
424impl_mat_mul_scalar!(Transform3);
425impl_mat_mul_vec!(Transform3, Vec3);
426impl_mat_mul_vec!(Transform3, Vec4);
427impl_mat_mul_mat!(Transform3);
428
429//----------------------------------------------------------------
430// Formatting
431
432impl<T: fmt::Display> fmt::Display for Transform3<T> {
433	fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
434		f.write_str("Transform3(")?;
435		print::print(&move |i| &self.as_array()[i], 0x23, f)?;
436		f.write_str(")")
437	}
438}
439impl<T: fmt::Debug> fmt::Debug for Transform3<T> {
440	fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
441		f.write_str("Transform3(")?;
442		print::print(&move |i| print::Debug(&self.as_array()[i]), 0x23, f)?;
443		f.write_str(")")
444	}
445}
446
447//----------------------------------------------------------------
448// Tests
449
450#[test]
451fn test_inverse() {
452	let mut rng = urandom::seeded(42);
453
454	for _ in 0..1000 {
455		let x = Vec3(
456			rng.range(-10.0..10.0),
457			rng.range(-10.0..10.0),
458			rng.range(-10.0..10.0),
459		);
460		let y = Vec3(
461			rng.range(-10.0..10.0),
462			rng.range(-10.0..10.0),
463			rng.range(-10.0..10.0),
464		);
465		let z = Vec3(
466			rng.range(-10.0..10.0),
467			rng.range(-10.0..10.0),
468			rng.range(-10.0..10.0),
469		);
470		let t = Vec3(
471			rng.range(-10.0..10.0),
472			rng.range(-10.0..10.0),
473			rng.range(-10.0..10.0),
474		);
475
476		let mat = Transform3::compose(x, y, z, t);
477		let inv = mat.inverse();
478		let _identity = mat * inv;
479
480		let p = Vec3(
481			rng.range(-10.0..10.0),
482			rng.range(-10.0..10.0),
483			rng.range(-1.0..1.0),
484		);
485		let projected = mat * p;
486		let unprojected = inv * projected;
487		let error = (unprojected - p).len();
488		assert!(error < 1e-6, "Failed for mat: {mat:?}, p: {p:?}, error: {error}");
489	}
490}
cvmath/mat/transform3.rs

cvmath/mat/
transform3.rs
