Skip to main content

tpt_math_geometry/
lib.rs

1#![no_std]
2#![forbid(unsafe_code)]
3// Fixed-size matrix/vector algorithms are clearest with explicit indexing over
4// fixed-size arrays; the indexed-loop and manual-swap lints do not fit this
5// code, and the operator-impl lints misfire on componentwise arithmetic.
6#![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)]
13//! Geometry module built on [`tpt_math_linalg_fixed`].
14//!
15//! A from-scratch, const-generic geometry layer (matching the breadth of
16//! `nalgebra`'s geometry module) with **no `nalgebra` dependency** and **no
17//! allocator** — everything here is stack-allocated fixed-size storage. It
18//! provides points, translations, rotations (2-D and 3-D), unit quaternions,
19//! isometries, similarities, uniform scaling and the perspective / orthographic
20//! projection matrices.
21//!
22//! # Conventions (stated explicitly)
23//!
24//! * **Active (alibi) rotations.** A rotation acts on a vector `v` as `R * v`
25//!   (matrix–vector multiplication with the vector on the right), which rotates
26//!   the *point/vector* rather than the *coordinate frame*.
27//! * **Column vectors.** Points and vectors are column vectors; transformations
28//!   are applied as `M * v`.
29//! * **Right-handed coordinates.** The 2-D rotation by a positive angle is
30//!   counter-clockwise. The 3-D right-handed basis is `x × y = z`.
31//! * **Euler angles** (3-D) use the intrinsic Tait–Bryan order
32//!   `Rz(yaw) · Ry(pitch) · Rx(roll)` — that is, the vector is first rolled
33//!   about `x`, then pitched about `y`, then yawed about `z`.
34//! * **Quaternions are Hamilton quaternions** `q = w + x i + y j + z k` (scalar
35//!   part `w` last), matching `nalgebra`'s memory layout.
36//! * **Projection matrices** are right-handed, look down `-z`, with Normalised
37//!   Device Coordinates `z` in `[-1, 1]` (OpenGL-style depth range).
38//! * **Isometry composition** is written as `B * A` and means "apply `A`,
39//!   then `B`": `(B * A)(p) = B.rotation * (A.rotation * p + A.translation)
40//!   + B.translation`.
41
42use 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// ===========================================================================
48// Point
49// ===========================================================================
50
51/// A point in `D`-dimensional Euclidean space.
52///
53/// A point is an affine location; unlike a [`Vector`], subtracting two points
54/// yields a displacement [`Vector`], and a point can be offset by a [`Vector`]
55/// but not directly added to another point.
56#[derive(Clone, Copy, Debug, PartialEq, Eq)]
57pub struct Point<T, const D: usize> {
58    /// The coordinates of the point.
59    pub coords: Vector<T, D>,
60}
61
62impl<T: Scalar, const D: usize> Point<T, D> {
63    /// Build a point from its coordinates.
64    pub fn new(coords: Vector<T, D>) -> Self {
65        Point { coords }
66    }
67
68    /// The origin (all coordinates zero).
69    pub fn origin() -> Self {
70        Point {
71            coords: Vector::from_fn(|_| T::zero()),
72        }
73    }
74
75    /// Euclidean distance to another point.
76    pub fn distance(&self, other: &Point<T, D>) -> T {
77        (*self - *other).norm()
78    }
79
80    /// Translate the point by a vector, returning a new point.
81    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    /// Build a point from a fixed-size coordinate array.
90    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    /// First coordinate.
99    pub fn x(&self) -> T {
100        self.coords.data[0]
101    }
102    /// Second coordinate.
103    pub fn y(&self) -> T {
104        self.coords.data[1]
105    }
106}
107
108impl<T: Copy> Point<T, 3> {
109    /// First coordinate.
110    pub fn x(&self) -> T {
111        self.coords.data[0]
112    }
113    /// Second coordinate.
114    pub fn y(&self) -> T {
115        self.coords.data[1]
116    }
117    /// Third coordinate.
118    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
148/// 2-D point.
149pub type Point2<T> = Point<T, 2>;
150/// 3-D point.
151pub type Point3<T> = Point<T, 3>;
152
153// ===========================================================================
154// Translation
155// ===========================================================================
156
157/// A translation in `D`-dimensional space.
158///
159/// Translations form an abelian group under composition; `T2 * T1` shifts by
160/// `T1.vector` and then by `T2.vector`.
161#[derive(Clone, Copy, Debug, PartialEq, Eq)]
162pub struct Translation<T, const D: usize> {
163    /// The translation vector.
164    pub vector: Vector<T, D>,
165}
166
167impl<T: Scalar, const D: usize> Translation<T, D> {
168    /// Build a translation from a vector.
169    pub fn new(vector: Vector<T, D>) -> Self {
170        Translation { vector }
171    }
172
173    /// The identity translation (zero vector).
174    pub fn identity() -> Self {
175        Translation {
176            vector: Vector::from_fn(|_| T::zero()),
177        }
178    }
179
180    /// The inverse translation: negates the vector.
181    pub fn inverse(&self) -> Self {
182        Translation {
183            vector: -self.vector,
184        }
185    }
186
187    /// Apply the translation to a point.
188    pub fn transform_point(&self, pt: &Point<T, D>) -> Point<T, D> {
189        *pt + self.vector
190    }
191
192    /// Apply the translation to a vector (a no-op: vectors are translation
193    /// invariant).
194    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// ===========================================================================
209// Rotation
210// ===========================================================================
211
212/// An orthogonal, orientation-preserving (determinant `+1`) linear map in
213/// `D` dimensions, stored as its rotation matrix.
214///
215/// Orthogonality is guaranteed by construction: every constructor produces a
216/// proper rotation matrix, and [`Rotation::inverse`] is the exact transpose
217/// (no renormalisation needed).
218#[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    /// The identity rotation (the `D×D` identity matrix).
225    pub fn identity() -> Self {
226        Rotation {
227            matrix: Matrix::identity(),
228        }
229    }
230
231    /// Build a rotation from a matrix *without* checking that it is a proper
232    /// rotation. The caller must ensure orthogonality and unit determinant.
233    pub fn from_matrix_unchecked(matrix: Matrix<T, D, D>) -> Self {
234        Rotation { matrix }
235    }
236
237    /// Build a rotation from an (approximately) orthogonal matrix, by
238    /// orthonormalising it via Gram–Schmidt. Returns `None` if the matrix is
239    /// numerically singular.
240    pub fn from_matrix(matrix: Matrix<T, D, D>) -> Option<Self> {
241        // Orthonormalise columns via Gram–Schmidt, then re-orthogonalise the
242        // last column against all the previous ones.
243        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]; // copy the j-th column of the input
247            }
248            for i in 0..j {
249                // subtract projection onto column i
250                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            // normalise column j
259            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        // Re-orthogonalise the last column against the first D-1 (Gram–Schmidt
272        // drift correction) and renormalise.
273        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            // silence unused warning when D == 1
298            let _ = dot;
299        }
300        // Build a row-major matrix from the column storage.
301        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        // Reject reflections: a proper rotation has determinant +1, an improper
308        // orthogonal matrix has -1. Gaussian elimination gives the determinant;
309        // every pivot of an orthonormal matrix is non-zero, so no breakdown.
310        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    /// Borrow the underlying rotation matrix.
343    pub fn matrix(&self) -> &Matrix<T, D, D> {
344        &self.matrix
345    }
346
347    /// The inverse rotation (the matrix transpose).
348    pub fn inverse(&self) -> Self
349    where
350        T: Copy,
351    {
352        Rotation {
353            matrix: self.matrix.transpose(),
354        }
355    }
356
357    /// Rotate a vector: `R * v`.
358    pub fn transform_vector(&self, vec: &Vector<T, D>) -> Vector<T, D> {
359        self.matrix * *vec
360    }
361
362    /// Rotate a point about the origin: `R * p`.
363    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
386/// 2-D rotation.
387pub type Rotation2<T> = Rotation<T, 2>;
388/// 3-D rotation.
389pub type Rotation3<T> = Rotation<T, 3>;
390
391impl<T: Scalar> Rotation<T, 2> {
392    /// Build a 2-D rotation by `angle` radians (counter-clockwise for a positive
393    /// angle in a right-handed plane).
394    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    /// Build a 3-D rotation of `angle` radians about a (normalised) `axis`.
404    ///
405    /// Uses Rodrigues' rotation formula; the axis is normalised internally, so a
406    /// non-unit axis is accepted.
407    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    /// Build a 3-D rotation from intrinsic Tait–Bryan Euler angles
427    /// `(roll, pitch, yaw)` using the order `Rz(yaw) · Ry(pitch) · Rx(roll)`.
428    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    /// Override the generic [`Rotation::powf`] with a proper 3-D
436    /// axis-angle interpolation.
437    pub fn powf(&self, t: T) -> Rotation<T, 3>
438    where
439        T: Copy,
440    {
441        // Convert to an axis-angle via the trace identity, then scale the
442        // angle. `cos(theta) = (trace - 1) / 2`.
443        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        // Axis from the skew-symmetric part, whose norm is 2 sin(theta).
446        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        // atan2 is well-conditioned for every angle, unlike acos near 0 and pi,
451        // and never leaves its domain when the trace rounds past ±1.
452        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// ===========================================================================
462// Quaternion
463// ===========================================================================
464
465/// A quaternion `q = w + x i + y j + z k` (Hamilton convention, scalar `w`
466/// last).
467#[derive(Clone, Copy, Debug, PartialEq, Eq)]
468pub struct Quaternion<T> {
469    /// Components `[x, y, z, w]` (scalar `w` last, matching `nalgebra`).
470    pub coords: Vector4<T>,
471}
472
473impl<T: Scalar> Quaternion<T> {
474    /// Build a quaternion from its `[x, y, z, w]` components.
475    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    /// The quaternion conjugate `w - x i - y j - z k`.
482    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    /// The squared norm `w² + x² + y² + z²`.
492    pub fn norm_squared(&self) -> T {
493        self.coords.dot(&self.coords)
494    }
495
496    /// The norm `sqrt(w² + x² + y² + z²)`.
497    pub fn norm(&self) -> T {
498        self.norm_squared().sqrt()
499    }
500
501    /// Normalise to a unit quaternion. Returns `None` if the norm is zero.
502    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    /// Hamilton product `self * rhs` (composition of rotations:
517    /// `rhs` is applied first, then `self`).
518    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    /// Rotate a 3-D vector by this (unit) quaternion: `q * (0, v) * q⁻¹`.
540    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        // p = q * (0, v)
544        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        // result = p * conj(q)
549        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    /// Build a unit quaternion from a 3-D rotation matrix (the inverse of
560    /// [`Rotation3::from_quaternion`]).
561    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    /// Spherical linear interpolation between two unit quaternions.
611    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        // Take the shorter arc.
616        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            // Nearly parallel: linear interp + renormalise.
633            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        // `dot` is in [0, 1 - eps] here; the clamp guards against rounding.
642        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    /// Component-wise scaling by a scalar.
670    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/// A unit (normalised) quaternion, representing a 3-D rotation.
717///
718/// The unit-norm invariant is enforced on construction, so composition and
719/// inversion are numerically safe without re-normalisation.
720#[derive(Clone, Copy, Debug, PartialEq, Eq)]
721pub struct UnitQuaternion<T> {
722    quaternion: Quaternion<T>,
723}
724
725impl<T: Scalar> UnitQuaternion<T> {
726    /// Build from an arbitrary quaternion, normalising it. Panics if the
727    /// quaternion is zero.
728    pub fn from_quaternion(q: Quaternion<T>) -> Self {
729        UnitQuaternion {
730            quaternion: q.normalize().expect("cannot normalise a zero quaternion"),
731        }
732    }
733
734    /// Build from a rotation about an axis by an angle.
735    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    /// Build from intrinsic Tait–Bryan Euler angles `(roll, pitch, yaw)` using
748    /// the order `Rz(yaw) · Ry(pitch) · Rx(roll)`.
749    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    /// Build from a rotation matrix (see
760    /// [`Quaternion::from_rotation_matrix`]).
761    pub fn from_rotation_matrix(rot: &Rotation3<T>) -> Self {
762        Quaternion::from_rotation_matrix(rot)
763    }
764
765    /// Borrow the underlying unit quaternion.
766    pub fn quaternion(&self) -> &Quaternion<T> {
767        &self.quaternion
768    }
769
770    /// The inverse rotation (the conjugate, which for a unit quaternion equals
771    /// the reciprocal).
772    pub fn inverse(&self) -> Self {
773        UnitQuaternion {
774            quaternion: self.quaternion.conjugate(),
775        }
776    }
777
778    /// Rotate a 3-D vector.
779    pub fn rotate_vector(&self, v: &Vector<T, 3>) -> Vector<T, 3> {
780        self.quaternion.rotate_vector(v)
781    }
782
783    /// Convert to the equivalent 3-D rotation matrix.
784    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        // Renormalise: rounding in the Hamilton product otherwise accumulates
793        // as the quaternion is repeatedly composed.
794        UnitQuaternion::from_quaternion(self.quaternion.multiply(&rhs.quaternion))
795    }
796}
797
798impl<T: Scalar> Rotation<T, 3> {
799    /// Build a 3-D rotation from a unit quaternion.
800    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// ===========================================================================
829// Isometry
830// ===========================================================================
831
832/// A rigid motion: a rotation followed by a translation.
833///
834/// Composition is written `B * A` and means "apply `A`, then `B`":
835/// `(B * A)(p) = B.rotation * (A.rotation * p + A.translation)
836/// + B.translation`.
837#[derive(Clone, Copy, Debug, PartialEq, Eq)]
838pub struct Isometry<T, const D: usize> {
839    /// The (linear) rotation part.
840    pub rotation: Rotation<T, D>,
841    /// The (affine) translation part.
842    pub translation: Translation<T, D>,
843}
844
845impl<T: Scalar, const D: usize> Isometry<T, D> {
846    /// Build an isometry from a rotation and a translation.
847    pub fn new(translation: Translation<T, D>, rotation: Rotation<T, D>) -> Self {
848        Isometry {
849            rotation,
850            translation,
851        }
852    }
853
854    /// The identity isometry (no rotation, no translation).
855    pub fn identity() -> Self {
856        Isometry {
857            rotation: Rotation::identity(),
858            translation: Translation::identity(),
859        }
860    }
861
862    /// An isometry with only a rotation (zero translation).
863    pub fn from_rotation(rotation: Rotation<T, D>) -> Self {
864        Isometry {
865            rotation,
866            translation: Translation::identity(),
867        }
868    }
869
870    /// An isometry with only a translation (identity rotation).
871    pub fn from_translation(translation: Translation<T, D>) -> Self {
872        Isometry {
873            rotation: Rotation::identity(),
874            translation,
875        }
876    }
877
878    /// The inverse isometry.
879    ///
880    /// If `self(p) = R p + t`, then `self.inverse()(p) = Rᵀ p - Rᵀ t`.
881    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    /// Apply to a point: `R * p + t`.
894    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    /// Apply to a vector: `R * v` (translation invariant).
904    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        // (B * A): rotation = B.rotation * A.rotation,
919        // translation = B.rotation * A.translation + B.translation.
920        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
930/// 2-D isometry.
931pub type Isometry2<T> = Isometry<T, 2>;
932/// 3-D isometry.
933pub type Isometry3<T> = Isometry<T, 3>;
934
935// ===========================================================================
936// Similarity
937// ===========================================================================
938
939/// A similarity transform: a uniform scaling, then a rotation, then a
940/// translation.
941///
942/// `self(p) = s * (R * p) + t`.
943#[derive(Clone, Copy, Debug, PartialEq, Eq)]
944pub struct Similarity<T, const D: usize> {
945    /// The (linear) rotation part.
946    pub rotation: Rotation<T, D>,
947    /// The (affine) translation part.
948    pub translation: Translation<T, D>,
949    /// The uniform scaling factor `s`.
950    pub scaling: T,
951}
952
953impl<T: Scalar, const D: usize> Similarity<T, D> {
954    /// Build a similarity from a scaling factor, a rotation and a translation.
955    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    /// The identity similarity (`s = 1`, no rotation, no translation).
964    pub fn identity() -> Self {
965        Similarity {
966            rotation: Rotation::identity(),
967            translation: Translation::identity(),
968            scaling: T::one(),
969        }
970    }
971
972    /// The inverse similarity.
973    ///
974    /// If `self(p) = s (R p) + t`, then
975    /// `self.inverse()(p) = (1/s) Rᵀ (p - t)`.
976    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        // translation of the inverse: -(1/s) Rᵀ t
983        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    /// Apply to a point: `s * (R * p) + t`.
992    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    /// Apply to a vector: `s * (R * v)` (translation invariant).
1003    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
1011/// 3-D similarity.
1012pub type Similarity3<T> = Similarity<T, 3>;
1013
1014// ===========================================================================
1015// Scale
1016// ===========================================================================
1017
1018/// A non-uniform (axis-aligned) scaling.
1019///
1020/// `self(p) = diag(components) * p`.
1021#[derive(Clone, Copy, Debug, PartialEq, Eq)]
1022pub struct Scale<T, const D: usize> {
1023    /// Per-axis scaling factors.
1024    pub vector: Vector<T, D>,
1025}
1026
1027impl<T: Scalar, const D: usize> Scale<T, D> {
1028    /// Build a scaling from per-axis factors.
1029    pub fn new(vector: Vector<T, D>) -> Self {
1030        Scale { vector }
1031    }
1032
1033    /// Uniform scaling by `s`.
1034    pub fn uniform(s: T) -> Self {
1035        Scale {
1036            vector: Vector::from_fn(|_| s),
1037        }
1038    }
1039
1040    /// The identity scaling (all factors `1`).
1041    pub fn identity() -> Self {
1042        Scale {
1043            vector: Vector::from_fn(|_| T::one()),
1044        }
1045    }
1046
1047    /// The inverse scaling (componentwise reciprocals).
1048    pub fn inverse(&self) -> Self {
1049        Scale {
1050            vector: Vector::from_fn(|i| T::one() / self.vector.data[i]),
1051        }
1052    }
1053
1054    /// Apply to a point: componentwise multiply.
1055    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    /// Apply to a vector: componentwise multiply.
1062    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// ===========================================================================
1068// Projections
1069// ===========================================================================
1070
1071/// A right-handed perspective projection (view space → clip space), looking
1072/// down `-z`, with NDC depth in `[-1, 1]` (OpenGL-style).
1073///
1074/// Use [`Perspective3::from_fov`] to build from a vertical field-of-view and
1075/// aspect ratio, or [`Perspective3::from_frustum`] for explicit frustum bounds.
1076#[derive(Clone, Copy, Debug, PartialEq, Eq)]
1077pub struct Perspective3<T> {
1078    matrix: Matrix4<T>,
1079}
1080
1081impl<T: Scalar> Perspective3<T> {
1082    /// Build from vertical field-of-view `fovy`, `aspect` ratio and near/far
1083    /// clip planes (`znear`, `zfar`).
1084    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    /// Build from explicit frustum bounds (right-handed, `-z` forward).
1103    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    /// The 4×4 projection matrix.
1126    pub fn matrix(&self) -> &Matrix4<T> {
1127        &self.matrix
1128    }
1129}
1130
1131/// A right-handed orthographic projection (view space → clip space), looking
1132/// down `-z`, with NDC depth in `[-1, 1]` (OpenGL-style).
1133#[derive(Clone, Copy, Debug, PartialEq, Eq)]
1134pub struct Orthographic3<T> {
1135    matrix: Matrix4<T>,
1136}
1137
1138impl<T: Scalar> Orthographic3<T> {
1139    /// Build from the axis-aligned frustum bounds (right-handed, `-z` forward),
1140    /// mapping `z ∈ [-znear, -zfar]` to NDC `z ∈ [-1, 1]`.
1141    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    /// The 4×4 projection matrix.
1163    pub fn matrix(&self) -> &Matrix4<T> {
1164        &self.matrix
1165    }
1166}
1167
1168// ===========================================================================
1169// Tests
1170// ===========================================================================
1171
1172#[cfg(test)]
1173mod tests {
1174    use super::*;
1175    use tpt_math_linalg_fixed::{Vector2, Vector3, Vector4};
1176
1177    // Convenience aliases used by the tests below.
1178    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        // R * Rᵀ ≈ I
1219        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        // Start from a rotation built via axis-angle ...
1281        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        // ... convert to a unit quaternion and back.
1286        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        // And back to a quaternion from the rebuilt rotation.
1297        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        // inv * iso should be the identity (within tolerance).
1311        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        // transform_point then inverse should recover the original point.
1327        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        // A point at z = -znear in view space maps to NDC z = -1 (w = znear).
1363        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        // A point at z = -zfar maps to NDC z = +1.
1368        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        // Halfway should equal the 45° rotation.
1395        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        // det = -1: an improper orthogonal matrix is a reflection, not a rotation.
1410        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        // A trace a few ulps above 3 makes cos θ slightly greater than 1. The
1419        // unclamped acos then returned NaN and poisoned the axis.
1420        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}