1#![no_std]
2#![forbid(unsafe_code)]
3#![allow(
7 clippy::needless_range_loop,
8 clippy::manual_swap,
9 clippy::suspicious_arithmetic_impl,
10 clippy::suspicious_op_assign_impl,
11 clippy::items_after_test_module
12)]
13use core::ops::{Add, Mul, Neg, Sub};
43
44use tpt_math_linalg_fixed::{Matrix, Matrix2, Matrix3, Matrix4, Vector, Vector3, Vector4};
45use tpt_math_numeric::Scalar;
46
47#[derive(Clone, Copy, Debug, PartialEq, Eq)]
57pub struct Point<T, const D: usize> {
58 pub coords: Vector<T, D>,
60}
61
62impl<T: Scalar, const D: usize> Point<T, D> {
63 pub fn new(coords: Vector<T, D>) -> Self {
65 Point { coords }
66 }
67
68 pub fn origin() -> Self {
70 Point {
71 coords: Vector::from_fn(|_| T::zero()),
72 }
73 }
74
75 pub fn distance(&self, other: &Point<T, D>) -> T {
77 (*self - *other).norm()
78 }
79
80 pub fn translate(&self, by: &Vector<T, D>) -> Point<T, D> {
82 Point {
83 coords: self.coords + *by,
84 }
85 }
86}
87
88impl<T: Copy, const D: usize> Point<T, D> {
89 pub fn from_array(data: [T; D]) -> Self {
91 Point {
92 coords: Vector::new(data),
93 }
94 }
95}
96
97impl<T: Copy> Point<T, 2> {
98 pub fn x(&self) -> T {
100 self.coords.data[0]
101 }
102 pub fn y(&self) -> T {
104 self.coords.data[1]
105 }
106}
107
108impl<T: Copy> Point<T, 3> {
109 pub fn x(&self) -> T {
111 self.coords.data[0]
112 }
113 pub fn y(&self) -> T {
115 self.coords.data[1]
116 }
117 pub fn z(&self) -> T {
119 self.coords.data[2]
120 }
121}
122
123impl<T: Scalar, const D: usize> Add<Vector<T, D>> for Point<T, D> {
124 type Output = Point<T, D>;
125 fn add(self, rhs: Vector<T, D>) -> Point<T, D> {
126 Point {
127 coords: self.coords + rhs,
128 }
129 }
130}
131
132impl<T: Scalar, const D: usize> Sub<Vector<T, D>> for Point<T, D> {
133 type Output = Point<T, D>;
134 fn sub(self, rhs: Vector<T, D>) -> Point<T, D> {
135 Point {
136 coords: self.coords - rhs,
137 }
138 }
139}
140
141impl<T: Scalar, const D: usize> Sub<Point<T, D>> for Point<T, D> {
142 type Output = Vector<T, D>;
143 fn sub(self, rhs: Point<T, D>) -> Vector<T, D> {
144 self.coords - rhs.coords
145 }
146}
147
148pub type Point2<T> = Point<T, 2>;
150pub type Point3<T> = Point<T, 3>;
152
153#[derive(Clone, Copy, Debug, PartialEq, Eq)]
162pub struct Translation<T, const D: usize> {
163 pub vector: Vector<T, D>,
165}
166
167impl<T: Scalar, const D: usize> Translation<T, D> {
168 pub fn new(vector: Vector<T, D>) -> Self {
170 Translation { vector }
171 }
172
173 pub fn identity() -> Self {
175 Translation {
176 vector: Vector::from_fn(|_| T::zero()),
177 }
178 }
179
180 pub fn inverse(&self) -> Self {
182 Translation {
183 vector: -self.vector,
184 }
185 }
186
187 pub fn transform_point(&self, pt: &Point<T, D>) -> Point<T, D> {
189 *pt + self.vector
190 }
191
192 pub fn transform_vector(&self, vec: &Vector<T, D>) -> Vector<T, D> {
195 *vec
196 }
197}
198
199impl<T: Scalar, const D: usize> Mul for Translation<T, D> {
200 type Output = Translation<T, D>;
201 fn mul(self, rhs: Translation<T, D>) -> Translation<T, D> {
202 Translation {
203 vector: self.vector + rhs.vector,
204 }
205 }
206}
207
208#[derive(Clone, Copy, Debug, PartialEq, Eq)]
219pub struct Rotation<T, const D: usize> {
220 matrix: Matrix<T, D, D>,
221}
222
223impl<T: Scalar, const D: usize> Rotation<T, D> {
224 pub fn identity() -> Self {
226 Rotation {
227 matrix: Matrix::identity(),
228 }
229 }
230
231 pub fn from_matrix_unchecked(matrix: Matrix<T, D, D>) -> Self {
234 Rotation { matrix }
235 }
236
237 pub fn from_matrix(matrix: Matrix<T, D, D>) -> Option<Self> {
241 let mut cols = [[T::zero(); D]; D];
244 for j in 0..D {
245 for i in 0..D {
246 cols[j][i] = matrix.data[i][j]; }
248 for i in 0..j {
249 let mut dot = T::zero();
251 for k in 0..D {
252 dot = dot + cols[i][k] * cols[j][k];
253 }
254 for k in 0..D {
255 cols[j][k] = cols[j][k] - cols[i][k] * dot;
256 }
257 }
258 let mut n = T::zero();
260 for k in 0..D {
261 n = n + cols[j][k] * cols[j][k];
262 }
263 n = n.sqrt();
264 if n == T::zero() {
265 return None;
266 }
267 for k in 0..D {
268 cols[j][k] = cols[j][k] / n;
269 }
270 }
271 let last = D - 1;
274 if D > 1 {
275 let mut dot = T::zero();
276 for i in 0..last {
277 let mut d = T::zero();
278 for k in 0..D {
279 d = d + cols[i][k] * cols[last][k];
280 }
281 dot = dot + d;
282 for k in 0..D {
283 cols[last][k] = cols[last][k] - cols[i][k] * d;
284 }
285 }
286 let mut n = T::zero();
287 for k in 0..D {
288 n = n + cols[last][k] * cols[last][k];
289 }
290 n = n.sqrt();
291 if n == T::zero() {
292 return None;
293 }
294 for k in 0..D {
295 cols[last][k] = cols[last][k] / n;
296 }
297 let _ = dot;
299 }
300 let mut data = [[T::zero(); D]; D];
302 for j in 0..D {
303 for i in 0..D {
304 data[i][j] = cols[j][i];
305 }
306 }
307 let mut a = data;
311 let mut det = T::one();
312 for c in 0..D {
313 let mut p = c;
314 for r in (c + 1)..D {
315 if a[r][c].abs() > a[p][c].abs() {
316 p = r;
317 }
318 }
319 if a[p][c] == T::zero() {
320 return None;
321 }
322 if p != c {
323 a.swap(p, c);
324 det = -det;
325 }
326 det = det * a[c][c];
327 for r in (c + 1)..D {
328 let f = a[r][c] / a[c][c];
329 for k in c..D {
330 a[r][k] = a[r][k] - f * a[c][k];
331 }
332 }
333 }
334 if det < T::zero() {
335 return None;
336 }
337 Some(Rotation {
338 matrix: Matrix::new(data),
339 })
340 }
341
342 pub fn matrix(&self) -> &Matrix<T, D, D> {
344 &self.matrix
345 }
346
347 pub fn inverse(&self) -> Self
349 where
350 T: Copy,
351 {
352 Rotation {
353 matrix: self.matrix.transpose(),
354 }
355 }
356
357 pub fn transform_vector(&self, vec: &Vector<T, D>) -> Vector<T, D> {
359 self.matrix * *vec
360 }
361
362 pub fn transform_point(&self, pt: &Point<T, D>) -> Point<T, D> {
364 Point {
365 coords: self.matrix * pt.coords,
366 }
367 }
368}
369
370impl<T: Scalar, const D: usize> Mul for Rotation<T, D> {
371 type Output = Rotation<T, D>;
372 fn mul(self, rhs: Rotation<T, D>) -> Rotation<T, D> {
373 Rotation {
374 matrix: self.matrix * rhs.matrix,
375 }
376 }
377}
378
379impl<T: Scalar, const D: usize> Mul<Vector<T, D>> for Rotation<T, D> {
380 type Output = Vector<T, D>;
381 fn mul(self, rhs: Vector<T, D>) -> Vector<T, D> {
382 self.matrix * rhs
383 }
384}
385
386pub type Rotation2<T> = Rotation<T, 2>;
388pub type Rotation3<T> = Rotation<T, 3>;
390
391impl<T: Scalar> Rotation<T, 2> {
392 pub fn from_angle(angle: T) -> Self {
395 let (s, c) = angle.sin_cos();
396 Rotation {
397 matrix: Matrix2::new([[c, -s], [s, c]]),
398 }
399 }
400}
401
402impl<T: Scalar> Rotation<T, 3> {
403 pub fn from_axis_angle(axis: &Vector<T, 3>, angle: T) -> Self {
408 let n = axis.norm();
409 let (kx, ky, kz) = if n == T::zero() {
410 (T::zero(), T::zero(), T::zero())
411 } else {
412 (axis.data[0] / n, axis.data[1] / n, axis.data[2] / n)
413 };
414 let (s, c) = angle.sin_cos();
415 let t = T::one() - c;
416 let m = [
417 [c + t * kx * kx, t * kx * ky - s * kz, t * kx * kz + s * ky],
418 [t * ky * kx + s * kz, c + t * ky * ky, t * ky * kz - s * kx],
419 [t * kz * kx - s * ky, t * kz * ky + s * kx, c + t * kz * kz],
420 ];
421 Rotation {
422 matrix: Matrix3::new(m),
423 }
424 }
425
426 pub fn from_euler(roll: T, pitch: T, yaw: T) -> Self {
429 let rx = Rotation3::from_axis_angle(&Vector3::new([T::one(), T::zero(), T::zero()]), roll);
430 let ry = Rotation3::from_axis_angle(&Vector3::new([T::zero(), T::one(), T::zero()]), pitch);
431 let rz = Rotation3::from_axis_angle(&Vector3::new([T::zero(), T::zero(), T::one()]), yaw);
432 rz * ry * rx
433 }
434
435 pub fn powf(&self, t: T) -> Rotation<T, 3>
438 where
439 T: Copy,
440 {
441 let tr = self.matrix.data[0][0] + self.matrix.data[1][1] + self.matrix.data[2][2];
444 let cos_t = (tr - T::one()) / (T::one() + T::one());
445 let x = self.matrix.data[2][1] - self.matrix.data[1][2];
447 let y = self.matrix.data[0][2] - self.matrix.data[2][0];
448 let z = self.matrix.data[1][0] - self.matrix.data[0][1];
449 let axis = Vector3::new([x, y, z]);
450 let sin_t = (x * x + y * y + z * z).sqrt() / (T::one() + T::one());
453 let angle = sin_t.atan2(cos_t);
454 if angle < T::from(1e-12).unwrap_or(T::zero()) {
455 return *self;
456 }
457 Rotation3::from_axis_angle(&axis, angle * t)
458 }
459}
460
461#[derive(Clone, Copy, Debug, PartialEq, Eq)]
468pub struct Quaternion<T> {
469 pub coords: Vector4<T>,
471}
472
473impl<T: Scalar> Quaternion<T> {
474 pub fn new(x: T, y: T, z: T, w: T) -> Self {
476 Quaternion {
477 coords: Vector4::new([x, y, z, w]),
478 }
479 }
480
481 pub fn conjugate(&self) -> Self {
483 Quaternion::new(
484 -self.coords.data[0],
485 -self.coords.data[1],
486 -self.coords.data[2],
487 self.coords.data[3],
488 )
489 }
490
491 pub fn norm_squared(&self) -> T {
493 self.coords.dot(&self.coords)
494 }
495
496 pub fn norm(&self) -> T {
498 self.norm_squared().sqrt()
499 }
500
501 pub fn normalize(&self) -> Option<Self> {
503 let n = self.norm();
504 if n == T::zero() {
505 return None;
506 }
507 let inv = T::one() / n;
508 Some(Quaternion::new(
509 self.coords.data[0] * inv,
510 self.coords.data[1] * inv,
511 self.coords.data[2] * inv,
512 self.coords.data[3] * inv,
513 ))
514 }
515
516 pub fn multiply(&self, rhs: &Quaternion<T>) -> Quaternion<T> {
519 let (x1, y1, z1, w1) = (
520 self.coords.data[0],
521 self.coords.data[1],
522 self.coords.data[2],
523 self.coords.data[3],
524 );
525 let (x2, y2, z2, w2) = (
526 rhs.coords.data[0],
527 rhs.coords.data[1],
528 rhs.coords.data[2],
529 rhs.coords.data[3],
530 );
531 Quaternion::new(
532 w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2,
533 w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2,
534 w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2,
535 w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2,
536 )
537 }
538
539 pub fn rotate_vector(&self, v: &Vector<T, 3>) -> Vector<T, 3> {
541 let (vx, vy, vz) = (v.data[0], v.data[1], v.data[2]);
542 let q = self;
543 let pw = -(q.coords.data[0] * vx + q.coords.data[1] * vy + q.coords.data[2] * vz);
545 let px = q.coords.data[3] * vx + (q.coords.data[1] * vz - q.coords.data[2] * vy);
546 let py = q.coords.data[3] * vy + (q.coords.data[2] * vx - q.coords.data[0] * vz);
547 let pz = q.coords.data[3] * vz + (q.coords.data[0] * vy - q.coords.data[1] * vx);
548 let c = q.conjugate();
550 let rx = -pw * c.coords.data[0] + c.coords.data[3] * px - c.coords.data[1] * pz
551 + c.coords.data[2] * py;
552 let ry = -pw * c.coords.data[1] + c.coords.data[3] * py - c.coords.data[2] * px
553 + c.coords.data[0] * pz;
554 let rz = -pw * c.coords.data[2] + c.coords.data[3] * pz - c.coords.data[0] * py
555 + c.coords.data[1] * px;
556 Vector3::new([rx, ry, rz])
557 }
558
559 pub fn from_rotation_matrix(rot: &Rotation3<T>) -> UnitQuaternion<T> {
562 let m = rot.matrix().data;
563 let m00 = m[0][0];
564 let m01 = m[0][1];
565 let m02 = m[0][2];
566 let m10 = m[1][0];
567 let m11 = m[1][1];
568 let m12 = m[1][2];
569 let m20 = m[2][0];
570 let m21 = m[2][1];
571 let m22 = m[2][2];
572 let one = T::one();
573 let two = one + one;
574 let four = two + two;
575 let (x, y, z, w);
576 let trace = m00 + m11 + m22;
577 if trace > T::zero() {
578 let s = (trace + one).sqrt() / two;
579 w = s;
580 let inv = one / (two * two * s);
581 x = (m21 - m12) * inv;
582 y = (m02 - m20) * inv;
583 z = (m10 - m01) * inv;
584 } else if m00 > m11 && m00 > m22 {
585 let s = (one + m00 - m11 - m22).sqrt() / two;
586 x = s;
587 let inv = one / (two * two * s);
588 y = (m01 + m10) * inv;
589 z = (m02 + m20) * inv;
590 w = (m21 - m12) * inv;
591 } else if m11 > m22 {
592 let s = (one + m11 - m00 - m22).sqrt() / two;
593 y = s;
594 let inv = one / (two * two * s);
595 x = (m01 + m10) * inv;
596 z = (m12 + m21) * inv;
597 w = (m02 - m20) * inv;
598 } else {
599 let s = (one + m22 - m00 - m11).sqrt() / two;
600 z = s;
601 let inv = one / (two * two * s);
602 x = (m02 + m20) * inv;
603 y = (m12 + m21) * inv;
604 w = (m10 - m01) * inv;
605 }
606 let _ = four;
607 UnitQuaternion::from_quaternion(Quaternion::new(x, y, z, w))
608 }
609
610 pub fn slerp(a: &UnitQuaternion<T>, b: &UnitQuaternion<T>, t: T) -> UnitQuaternion<T> {
612 let qa = a.quaternion();
613 let qb = b.quaternion();
614 let dot = qa.coords.dot(&qb.coords);
615 let (bq, dot) = if dot < T::zero() {
617 (
618 Quaternion::new(
619 -qb.coords.data[0],
620 -qb.coords.data[1],
621 -qb.coords.data[2],
622 -qb.coords.data[3],
623 ),
624 -dot,
625 )
626 } else {
627 (*qb, dot)
628 };
629 let one = T::one();
630 let eps = T::from(1e-12).unwrap_or(T::zero());
631 if dot > one - eps {
632 let r = Quaternion::new(
634 qa.coords.data[0] + (bq.coords.data[0] - qa.coords.data[0]) * t,
635 qa.coords.data[1] + (bq.coords.data[1] - qa.coords.data[1]) * t,
636 qa.coords.data[2] + (bq.coords.data[2] - qa.coords.data[2]) * t,
637 qa.coords.data[3] + (bq.coords.data[3] - qa.coords.data[3]) * t,
638 );
639 return UnitQuaternion::from_quaternion(r);
640 }
641 let theta0 = dot.min(one).acos();
643 let theta = theta0 * t;
644 let sin0 = theta0.sin();
645 let (w1, w2) = if sin0 == T::zero() {
646 (one - t, t)
647 } else {
648 ((theta0 - theta).sin() / sin0, theta.sin() / sin0)
649 };
650 let r = Quaternion::new(
651 qa.coords.data[0] * w1 + bq.coords.data[0] * w2,
652 qa.coords.data[1] * w1 + bq.coords.data[1] * w2,
653 qa.coords.data[2] * w1 + bq.coords.data[2] * w2,
654 qa.coords.data[3] * w1 + bq.coords.data[3] * w2,
655 );
656 UnitQuaternion::from_quaternion(r)
657 }
658}
659
660impl<T: Scalar> Mul<Quaternion<T>> for Quaternion<T> {
661 type Output = Quaternion<T>;
662 fn mul(self, rhs: Quaternion<T>) -> Quaternion<T> {
663 self.multiply(&rhs)
664 }
665}
666
667impl<T: Scalar> Mul<T> for Quaternion<T> {
668 type Output = Quaternion<T>;
669 fn mul(self, rhs: T) -> Quaternion<T> {
671 Quaternion::new(
672 self.coords.data[0] * rhs,
673 self.coords.data[1] * rhs,
674 self.coords.data[2] * rhs,
675 self.coords.data[3] * rhs,
676 )
677 }
678}
679
680impl<T: Scalar> Add for Quaternion<T> {
681 type Output = Quaternion<T>;
682 fn add(self, rhs: Quaternion<T>) -> Quaternion<T> {
683 Quaternion::new(
684 self.coords.data[0] + rhs.coords.data[0],
685 self.coords.data[1] + rhs.coords.data[1],
686 self.coords.data[2] + rhs.coords.data[2],
687 self.coords.data[3] + rhs.coords.data[3],
688 )
689 }
690}
691
692impl<T: Scalar> Sub for Quaternion<T> {
693 type Output = Quaternion<T>;
694 fn sub(self, rhs: Quaternion<T>) -> Quaternion<T> {
695 Quaternion::new(
696 self.coords.data[0] - rhs.coords.data[0],
697 self.coords.data[1] - rhs.coords.data[1],
698 self.coords.data[2] - rhs.coords.data[2],
699 self.coords.data[3] - rhs.coords.data[3],
700 )
701 }
702}
703
704impl<T: Scalar> Neg for Quaternion<T> {
705 type Output = Quaternion<T>;
706 fn neg(self) -> Quaternion<T> {
707 Quaternion::new(
708 -self.coords.data[0],
709 -self.coords.data[1],
710 -self.coords.data[2],
711 -self.coords.data[3],
712 )
713 }
714}
715
716#[derive(Clone, Copy, Debug, PartialEq, Eq)]
721pub struct UnitQuaternion<T> {
722 quaternion: Quaternion<T>,
723}
724
725impl<T: Scalar> UnitQuaternion<T> {
726 pub fn from_quaternion(q: Quaternion<T>) -> Self {
729 UnitQuaternion {
730 quaternion: q.normalize().expect("cannot normalise a zero quaternion"),
731 }
732 }
733
734 pub fn from_axis_angle(axis: &Vector<T, 3>, angle: T) -> Self {
736 let n = axis.norm();
737 let (kx, ky, kz) = if n == T::zero() {
738 (T::zero(), T::zero(), T::zero())
739 } else {
740 (axis.data[0] / n, axis.data[1] / n, axis.data[2] / n)
741 };
742 let half = angle / (T::one() + T::one());
743 let (s, c) = half.sin_cos();
744 UnitQuaternion::from_quaternion(Quaternion::new(kx * s, ky * s, kz * s, c))
745 }
746
747 pub fn from_euler(roll: T, pitch: T, yaw: T) -> Self {
750 let qx =
751 UnitQuaternion::from_axis_angle(&Vector3::new([T::one(), T::zero(), T::zero()]), roll);
752 let qy =
753 UnitQuaternion::from_axis_angle(&Vector3::new([T::zero(), T::one(), T::zero()]), pitch);
754 let qz =
755 UnitQuaternion::from_axis_angle(&Vector3::new([T::zero(), T::zero(), T::one()]), yaw);
756 qz * qy * qx
757 }
758
759 pub fn from_rotation_matrix(rot: &Rotation3<T>) -> Self {
762 Quaternion::from_rotation_matrix(rot)
763 }
764
765 pub fn quaternion(&self) -> &Quaternion<T> {
767 &self.quaternion
768 }
769
770 pub fn inverse(&self) -> Self {
773 UnitQuaternion {
774 quaternion: self.quaternion.conjugate(),
775 }
776 }
777
778 pub fn rotate_vector(&self, v: &Vector<T, 3>) -> Vector<T, 3> {
780 self.quaternion.rotate_vector(v)
781 }
782
783 pub fn to_rotation_matrix(&self) -> Rotation3<T> {
785 Rotation3::from_quaternion(self)
786 }
787}
788
789impl<T: Scalar> Mul<UnitQuaternion<T>> for UnitQuaternion<T> {
790 type Output = UnitQuaternion<T>;
791 fn mul(self, rhs: UnitQuaternion<T>) -> UnitQuaternion<T> {
792 UnitQuaternion::from_quaternion(self.quaternion.multiply(&rhs.quaternion))
795 }
796}
797
798impl<T: Scalar> Rotation<T, 3> {
799 pub fn from_quaternion(q: &UnitQuaternion<T>) -> Self {
801 let (x, y, z, w) = (
802 q.quaternion().coords.data[0],
803 q.quaternion().coords.data[1],
804 q.quaternion().coords.data[2],
805 q.quaternion().coords.data[3],
806 );
807 let xx = x * x;
808 let yy = y * y;
809 let zz = z * z;
810 let xy = x * y;
811 let xz = x * z;
812 let yz = y * z;
813 let wx = w * x;
814 let wy = w * y;
815 let wz = w * z;
816 let two = T::one() + T::one();
817 let m = [
818 [T::one() - two * (yy + zz), two * (xy - wz), two * (xz + wy)],
819 [two * (xy + wz), T::one() - two * (xx + zz), two * (yz - wx)],
820 [two * (xz - wy), two * (yz + wx), T::one() - two * (xx + yy)],
821 ];
822 Rotation {
823 matrix: Matrix3::new(m),
824 }
825 }
826}
827
828#[derive(Clone, Copy, Debug, PartialEq, Eq)]
838pub struct Isometry<T, const D: usize> {
839 pub rotation: Rotation<T, D>,
841 pub translation: Translation<T, D>,
843}
844
845impl<T: Scalar, const D: usize> Isometry<T, D> {
846 pub fn new(translation: Translation<T, D>, rotation: Rotation<T, D>) -> Self {
848 Isometry {
849 rotation,
850 translation,
851 }
852 }
853
854 pub fn identity() -> Self {
856 Isometry {
857 rotation: Rotation::identity(),
858 translation: Translation::identity(),
859 }
860 }
861
862 pub fn from_rotation(rotation: Rotation<T, D>) -> Self {
864 Isometry {
865 rotation,
866 translation: Translation::identity(),
867 }
868 }
869
870 pub fn from_translation(translation: Translation<T, D>) -> Self {
872 Isometry {
873 rotation: Rotation::identity(),
874 translation,
875 }
876 }
877
878 pub fn inverse(&self) -> Self
882 where
883 T: Copy,
884 {
885 let r_inv = self.rotation.inverse();
886 let t_inv = Translation::new(-(r_inv.transform_vector(&self.translation.vector)));
887 Isometry {
888 rotation: r_inv,
889 translation: t_inv,
890 }
891 }
892
893 pub fn transform_point(&self, pt: &Point<T, D>) -> Point<T, D>
895 where
896 T: Copy,
897 {
898 Point {
899 coords: self.rotation.transform_vector(&pt.coords) + self.translation.vector,
900 }
901 }
902
903 pub fn transform_vector(&self, vec: &Vector<T, D>) -> Vector<T, D>
905 where
906 T: Copy,
907 {
908 self.rotation.transform_vector(vec)
909 }
910}
911
912impl<T: Scalar, const D: usize> Mul for Isometry<T, D>
913where
914 T: Copy,
915{
916 type Output = Isometry<T, D>;
917 fn mul(self, rhs: Isometry<T, D>) -> Isometry<T, D> {
918 let rotation = self.rotation * rhs.rotation;
921 let translated = self.rotation.transform_vector(&rhs.translation.vector);
922 let translation = Translation::new(translated + self.translation.vector);
923 Isometry {
924 rotation,
925 translation,
926 }
927 }
928}
929
930pub type Isometry2<T> = Isometry<T, 2>;
932pub type Isometry3<T> = Isometry<T, 3>;
934
935#[derive(Clone, Copy, Debug, PartialEq, Eq)]
944pub struct Similarity<T, const D: usize> {
945 pub rotation: Rotation<T, D>,
947 pub translation: Translation<T, D>,
949 pub scaling: T,
951}
952
953impl<T: Scalar, const D: usize> Similarity<T, D> {
954 pub fn new(scaling: T, translation: Translation<T, D>, rotation: Rotation<T, D>) -> Self {
956 Similarity {
957 rotation,
958 translation,
959 scaling,
960 }
961 }
962
963 pub fn identity() -> Self {
965 Similarity {
966 rotation: Rotation::identity(),
967 translation: Translation::identity(),
968 scaling: T::one(),
969 }
970 }
971
972 pub fn inverse(&self) -> Self
977 where
978 T: Copy,
979 {
980 let r_inv = self.rotation.inverse();
981 let inv_s = T::one() / self.scaling;
982 let t = r_inv.transform_vector(&self.translation.vector) * inv_s;
984 Similarity {
985 rotation: r_inv,
986 translation: Translation::new(-t),
987 scaling: inv_s,
988 }
989 }
990
991 pub fn transform_point(&self, pt: &Point<T, D>) -> Point<T, D>
993 where
994 T: Copy,
995 {
996 let rotated = self.rotation.transform_vector(&pt.coords);
997 Point {
998 coords: rotated * self.scaling + self.translation.vector,
999 }
1000 }
1001
1002 pub fn transform_vector(&self, vec: &Vector<T, D>) -> Vector<T, D>
1004 where
1005 T: Copy,
1006 {
1007 self.rotation.transform_vector(vec) * self.scaling
1008 }
1009}
1010
1011pub type Similarity3<T> = Similarity<T, 3>;
1013
1014#[derive(Clone, Copy, Debug, PartialEq, Eq)]
1022pub struct Scale<T, const D: usize> {
1023 pub vector: Vector<T, D>,
1025}
1026
1027impl<T: Scalar, const D: usize> Scale<T, D> {
1028 pub fn new(vector: Vector<T, D>) -> Self {
1030 Scale { vector }
1031 }
1032
1033 pub fn uniform(s: T) -> Self {
1035 Scale {
1036 vector: Vector::from_fn(|_| s),
1037 }
1038 }
1039
1040 pub fn identity() -> Self {
1042 Scale {
1043 vector: Vector::from_fn(|_| T::one()),
1044 }
1045 }
1046
1047 pub fn inverse(&self) -> Self {
1049 Scale {
1050 vector: Vector::from_fn(|i| T::one() / self.vector.data[i]),
1051 }
1052 }
1053
1054 pub fn transform_point(&self, pt: &Point<T, D>) -> Point<T, D> {
1056 Point {
1057 coords: Vector::from_fn(|i| self.vector.data[i] * pt.coords.data[i]),
1058 }
1059 }
1060
1061 pub fn transform_vector(&self, vec: &Vector<T, D>) -> Vector<T, D> {
1063 Vector::from_fn(|i| self.vector.data[i] * vec.data[i])
1064 }
1065}
1066
1067#[derive(Clone, Copy, Debug, PartialEq, Eq)]
1077pub struct Perspective3<T> {
1078 matrix: Matrix4<T>,
1079}
1080
1081impl<T: Scalar> Perspective3<T> {
1082 pub fn from_fov(aspect: T, fovy: T, znear: T, zfar: T) -> Self {
1085 let f = T::one() / (fovy / (T::one() + T::one())).tan();
1086 let m00 = f / aspect;
1087 let m11 = f;
1088 let m22 = (zfar + znear) / (znear - zfar);
1089 let m23 = (T::one() + T::one()) * zfar * znear / (znear - zfar);
1090 let m32 = -T::one();
1091 let mut data = [[T::zero(); 4]; 4];
1092 data[0][0] = m00;
1093 data[1][1] = m11;
1094 data[2][2] = m22;
1095 data[2][3] = m23;
1096 data[3][2] = m32;
1097 Perspective3 {
1098 matrix: Matrix4::new(data),
1099 }
1100 }
1101
1102 pub fn from_frustum(left: T, right: T, bottom: T, top: T, znear: T, zfar: T) -> Self {
1104 let two = T::one() + T::one();
1105 let m00 = two * znear / (right - left);
1106 let m11 = two * znear / (top - bottom);
1107 let m02 = (right + left) / (right - left);
1108 let m12 = (top + bottom) / (top - bottom);
1109 let m22 = (zfar + znear) / (znear - zfar);
1110 let m23 = two * zfar * znear / (znear - zfar);
1111 let m32 = -T::one();
1112 let mut data = [[T::zero(); 4]; 4];
1113 data[0][0] = m00;
1114 data[0][2] = m02;
1115 data[1][1] = m11;
1116 data[1][2] = m12;
1117 data[2][2] = m22;
1118 data[2][3] = m23;
1119 data[3][2] = m32;
1120 Perspective3 {
1121 matrix: Matrix4::new(data),
1122 }
1123 }
1124
1125 pub fn matrix(&self) -> &Matrix4<T> {
1127 &self.matrix
1128 }
1129}
1130
1131#[derive(Clone, Copy, Debug, PartialEq, Eq)]
1134pub struct Orthographic3<T> {
1135 matrix: Matrix4<T>,
1136}
1137
1138impl<T: Scalar> Orthographic3<T> {
1139 pub fn from_frustum(left: T, right: T, bottom: T, top: T, znear: T, zfar: T) -> Self {
1142 let two = T::one() + T::one();
1143 let m00 = two / (right - left);
1144 let m11 = two / (top - bottom);
1145 let m22 = -two / (zfar - znear);
1146 let m03 = -(right + left) / (right - left);
1147 let m13 = -(top + bottom) / (top - bottom);
1148 let m23 = -(zfar + znear) / (zfar - znear);
1149 let mut data = [[T::zero(); 4]; 4];
1150 data[0][0] = m00;
1151 data[0][3] = m03;
1152 data[1][1] = m11;
1153 data[1][3] = m13;
1154 data[2][2] = m22;
1155 data[2][3] = m23;
1156 data[3][3] = T::one();
1157 Orthographic3 {
1158 matrix: Matrix4::new(data),
1159 }
1160 }
1161
1162 pub fn matrix(&self) -> &Matrix4<T> {
1164 &self.matrix
1165 }
1166}
1167
1168#[cfg(test)]
1173mod tests {
1174 use super::*;
1175 use tpt_math_linalg_fixed::{Vector2, Vector3, Vector4};
1176
1177 type Similarity2<T> = Similarity<T, 2>;
1179 type Scale3<T> = Scale<T, 3>;
1180
1181 fn close(a: f64, b: f64, tol: f64) -> bool {
1182 (a - b).abs() < tol
1183 }
1184
1185 #[test]
1186 fn point_vector_arithmetic() {
1187 let p = Point3::new(Vector3::new([1.0, 2.0, 3.0]));
1188 let v = Vector3::new([1.0, 0.0, -1.0]);
1189 let q = p + v;
1190 assert_eq!(q.coords.data, [2.0, 2.0, 2.0]);
1191 let w = q - p;
1192 assert_eq!(w.data, [1.0, 0.0, -1.0]);
1193 assert!(close(
1194 p.distance(&Point3::new(Vector3::new([1.0, 2.0, 6.0]))),
1195 3.0,
1196 1e-12
1197 ));
1198 }
1199
1200 #[test]
1201 fn translation_composition_and_inverse() {
1202 let t1 = Translation::new(Vector2::new([1.0, 0.0]));
1203 let t2 = Translation::new(Vector2::new([0.0, 2.0]));
1204 let p = Point2::new(Vector2::new([0.0, 0.0]));
1205 let q = (t2 * t1).transform_point(&p);
1206 assert_eq!(q.coords.data, [1.0, 2.0]);
1207 let back = t1.inverse().transform_point(&q);
1208 assert_eq!(back.coords.data, [0.0, 2.0]);
1209 }
1210
1211 #[test]
1212 fn rotation2_known_values() {
1213 let r90 = Rotation2::<f64>::from_angle(core::f64::consts::FRAC_PI_2);
1214 let v = Vector2::new([1.0, 0.0]);
1215 let rv = r90.transform_vector(&v);
1216 assert!(close(rv.data[0], 0.0, 1e-12));
1217 assert!(close(rv.data[1], 1.0, 1e-12));
1218 let id = (*r90.matrix()) * (*r90.inverse().matrix());
1220 for i in 0..2 {
1221 for j in 0..2 {
1222 assert!(close(id.data[i][j], if i == j { 1.0 } else { 0.0 }, 1e-12));
1223 }
1224 }
1225 }
1226
1227 #[test]
1228 fn rotation3_axis_known_values() {
1229 let rz90 = Rotation3::<f64>::from_axis_angle(
1230 &Vector3::new([0.0, 0.0, 1.0]),
1231 core::f64::consts::FRAC_PI_2,
1232 );
1233 let v = Vector3::new([1.0, 0.0, 0.0]);
1234 let rv = rz90.transform_vector(&v);
1235 assert!(close(rv.data[0], 0.0, 1e-12));
1236 assert!(close(rv.data[1], 1.0, 1e-12));
1237 assert!(close(rv.data[2], 0.0, 1e-12));
1238
1239 let rz180 = Rotation3::<f64>::from_axis_angle(
1240 &Vector3::new([0.0, 0.0, 1.0]),
1241 core::f64::consts::PI,
1242 );
1243 let rv2 = rz180.transform_vector(&v);
1244 assert!(close(rv2.data[0], -1.0, 1e-12));
1245 assert!(close(rv2.data[1], 0.0, 1e-12));
1246 }
1247
1248 #[test]
1249 fn rotation3_from_matrix_roundtrip() {
1250 let r = Rotation3::<f64>::from_axis_angle(
1251 &Vector3::new([1.0, 1.0, 1.0]),
1252 core::f64::consts::FRAC_PI_3,
1253 );
1254 let rebuilt = Rotation::from_matrix(*r.matrix()).unwrap();
1255 for i in 0..3 {
1256 for j in 0..3 {
1257 assert!(
1258 close(rebuilt.matrix().data[i][j], r.matrix().data[i][j], 1e-10),
1259 "entry {i},{j}"
1260 );
1261 }
1262 }
1263 }
1264
1265 #[test]
1266 fn quaternion_vector_rotation() {
1267 let q = UnitQuaternion::<f64>::from_axis_angle(
1268 &Vector3::new([0.0, 0.0, 1.0]),
1269 core::f64::consts::FRAC_PI_2,
1270 );
1271 let v = Vector3::new([1.0, 0.0, 0.0]);
1272 let rv = q.rotate_vector(&v);
1273 assert!(close(rv.data[0], 0.0, 1e-12));
1274 assert!(close(rv.data[1], 1.0, 1e-12));
1275 assert!(close(rv.data[2], 0.0, 1e-12));
1276 }
1277
1278 #[test]
1279 fn rotation3_quaternion_roundtrip() {
1280 let r = Rotation3::<f64>::from_axis_angle(
1282 &Vector3::new([1.0, 2.0, 3.0]),
1283 core::f64::consts::FRAC_PI_4,
1284 );
1285 let q = UnitQuaternion::from_rotation_matrix(&r);
1287 let r2 = q.to_rotation_matrix();
1288 for i in 0..3 {
1289 for j in 0..3 {
1290 assert!(
1291 close(r2.matrix().data[i][j], r.matrix().data[i][j], 1e-10),
1292 "entry {i},{j}"
1293 );
1294 }
1295 }
1296 let q2 = UnitQuaternion::from_rotation_matrix(&r2);
1298 let dot = q.quaternion().coords.dot(&q2.quaternion().coords).abs();
1299 assert!(close(dot, 1.0, 1e-10), "quaternion double-roundtrip");
1300 }
1301
1302 #[test]
1303 fn isometry_composition_inverse_identity() {
1304 let rot = Rotation3::<f64>::from_axis_angle(
1305 &Vector3::new([0.0, 0.0, 1.0]),
1306 core::f64::consts::FRAC_PI_2,
1307 );
1308 let iso = Isometry3::new(Translation::new(Vector3::new([1.0, 2.0, 3.0])), rot);
1309 let inv = iso.inverse();
1310 let composed = inv * iso;
1312 let id = Isometry3::<f64>::identity();
1313 for i in 0..3 {
1314 for j in 0..3 {
1315 assert!(close(
1316 composed.rotation.matrix().data[i][j],
1317 id.rotation.matrix().data[i][j],
1318 1e-10
1319 ));
1320 }
1321 }
1322 for i in 0..3 {
1323 assert!(close(composed.translation.vector.data[i], 0.0, 1e-10));
1324 }
1325
1326 let p = Point3::new(Vector3::new([3.0, 1.0, 0.5]));
1328 let q = iso.transform_point(&p);
1329 let back = inv.transform_point(&q);
1330 for i in 0..3 {
1331 assert!(
1332 close(back.coords.data[i], p.coords.data[i], 1e-9),
1333 "coord {i}"
1334 );
1335 }
1336 }
1337
1338 #[test]
1339 fn similarity_inverse_recovers_point() {
1340 let rot = Rotation2::<f64>::from_angle(core::f64::consts::FRAC_PI_2);
1341 let sim = Similarity2::new(2.0, Translation::new(Vector2::new([1.0, 1.0])), rot);
1342 let p = Point2::new(Vector2::new([1.0, 0.0]));
1343 let q = sim.transform_point(&p);
1344 let back = sim.inverse().transform_point(&q);
1345 assert!(close(back.coords.data[0], 1.0, 1e-10));
1346 assert!(close(back.coords.data[1], 0.0, 1e-10));
1347 }
1348
1349 #[test]
1350 fn scale_inverse() {
1351 let s = Scale3::new(Vector3::new([2.0, 3.0, 4.0]));
1352 let p = Point3::new(Vector3::new([1.0, 1.0, 1.0]));
1353 let q = s.transform_point(&p);
1354 assert_eq!(q.coords.data, [2.0, 3.0, 4.0]);
1355 let back = s.inverse().transform_point(&q);
1356 assert!(close(back.coords.data[0], 1.0, 1e-12));
1357 }
1358
1359 #[test]
1360 fn perspective_maps_known_depth() {
1361 let proj = Perspective3::<f64>::from_fov(1.0, core::f64::consts::FRAC_PI_2, 1.0, 10.0);
1362 let view = Vector4::new([0.0, 0.0, -1.0, 1.0]);
1364 let clip = *proj.matrix() * view;
1365 assert!(close(clip.data[3], 1.0, 1e-12));
1366 assert!(close(clip.data[2] / clip.data[3], -1.0, 1e-12));
1367 let view2 = Vector4::new([0.0, 0.0, -10.0, 1.0]);
1369 let clip2 = *proj.matrix() * view2;
1370 assert!(close(clip2.data[2] / clip2.data[3], 1.0, 1e-12));
1371 }
1372
1373 #[test]
1374 fn orthographic_maps_known_depth() {
1375 let proj = Orthographic3::<f64>::from_frustum(-1.0, 1.0, -1.0, 1.0, 1.0, 10.0);
1376 let view = Vector4::new([1.0, 1.0, -1.0, 1.0]);
1377 let clip = *proj.matrix() * view;
1378 assert!(close(clip.data[0], 1.0, 1e-12));
1379 assert!(close(clip.data[1], 1.0, 1e-12));
1380 assert!(close(clip.data[2], -1.0, 1e-12));
1381 let view2 = Vector4::new([0.0, 0.0, -10.0, 1.0]);
1382 let clip2 = *proj.matrix() * view2;
1383 assert!(close(clip2.data[2], 1.0, 1e-12));
1384 }
1385
1386 #[test]
1387 fn slerp_half_way() {
1388 let id = UnitQuaternion::<f64>::from_axis_angle(&Vector3::new([0.0, 0.0, 1.0]), 0.0);
1389 let q90 = UnitQuaternion::<f64>::from_axis_angle(
1390 &Vector3::new([0.0, 0.0, 1.0]),
1391 core::f64::consts::FRAC_PI_2,
1392 );
1393 let mid = Quaternion::slerp(&id, &q90, 0.5);
1394 let expected = UnitQuaternion::<f64>::from_axis_angle(
1396 &Vector3::new([0.0, 0.0, 1.0]),
1397 core::f64::consts::FRAC_PI_4,
1398 );
1399 let dot = mid
1400 .quaternion()
1401 .coords
1402 .dot(&expected.quaternion().coords)
1403 .abs();
1404 assert!(close(dot, 1.0, 1e-10), "slerp midpoint");
1405 }
1406
1407 #[test]
1408 fn from_matrix_rejects_reflection() {
1409 let reflection = Matrix3::from_array(&[1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, -1.0]);
1411 assert!(Rotation3::from_matrix(reflection).is_none());
1412 let identity = Matrix3::from_array(&[1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]);
1413 assert!(Rotation3::from_matrix(identity).is_some());
1414 }
1415
1416 #[test]
1417 fn powf_is_finite_when_trace_rounds_past_acos_domain() {
1418 let eps = 3e-15;
1421 let m = Matrix3::from_array(&[
1422 1.0 + eps,
1423 0.0,
1424 0.0,
1425 0.0,
1426 1.0 + eps,
1427 0.0,
1428 0.0,
1429 0.0,
1430 1.0 + eps,
1431 ]);
1432 let half = Rotation3::from_matrix_unchecked(m).powf(0.5);
1433 assert!(half
1434 .matrix()
1435 .data
1436 .iter()
1437 .flatten()
1438 .all(|v: &f64| v.is_finite()));
1439 }
1440
1441 #[test]
1442 fn unit_quaternion_product_stays_unit() {
1443 let q = UnitQuaternion::<f64>::from_axis_angle(&Vector3::new([1.0, 2.0, 3.0]), 0.7);
1444 let mut r = q;
1445 for _ in 0..10_000 {
1446 r = r * q;
1447 }
1448 assert!(close(r.quaternion().norm(), 1.0, 1e-12));
1449 }
1450}