Skip to main content

tpt_math_spatial/
lib.rs

1#![no_std]
2#![forbid(unsafe_code)]
3// Fixed-size 6-D spatial algebra is clearest with explicit indexing over
4// fixed-size arrays; the indexed-loop and arithmetic-impl lints do not fit
5// this code.
6#![allow(
7    clippy::needless_range_loop,
8    clippy::manual_swap,
9    clippy::suspicious_arithmetic_impl,
10    clippy::suspicious_op_assign_impl
11)]
12//! In-house (no external-crate) spatial kinematics for the `tpt-math`
13//! substrate: Featherstone 6-D spatial vectors, Plücker / adjoint transforms,
14//! dual quaternions and screw theory (exponential / logarithmic maps).
15//!
16//! Everything is built on [`tpt_math_geometry`] ([`Isometry3`],
17//! `Quaternion`, `UnitQuaternion`, …) and [`tpt_math_linalg_fixed`], is
18//! `#![no_std]` and allocator-free (stack-allocated fixed-size storage), and
19//! carries **no `nalgebra` dependency** (matching the workspace license policy,
20//! ADR-0007). It exists to fill the spatial-algebra gap in-house.
21//!
22//! # Conventions (stated explicitly)
23//!
24//! * **Active (alibi) rotations** — a transform acts on a vector `v` as
25//!   `R * v`; the *point/vector* is rotated, not the coordinate frame.
26//! * **Column vectors** — transformations apply as `M * v`.
27//! * **Right-handed coordinates** — positive angle about `+z` rotates `x → y`;
28//!   `x × y = z`.
29//! * **Spatial (6-D) vectors** are stored `[angular (top 3); linear (bottom
30//!   3)]`: a motion vector `v = (ω, v)` and a force/wrench
31//!   `f = (n, f)` (moment, force).
32//! * **Spatial cross product** follows Featherstone's `crm` / `crf`
33//!   operators (see [`SpatialVector::cross`] and friends); the result kind is
34//!   determined by the operand kinds (motion⊗motion→motion, motion⊗force→force,
35//!   force⊗motion→force, force⊗force→motion).
36//! * **Screw exponential** `exp` maps a twist `(ω, v) ∈ se(3)` to an
37//!   [`Isometry3`]; `log` is its inverse. The convention is the standard
38//!   Lynch–Park `se(3)` exponential with an **active**, right-handed rotation.
39//! * **Dual quaternions** are Hamilton quaternions `q + ε·qₑ` representing a
40//!   rigid transform (rotation quaternion + half the translation quaternion
41//!   pre-multiplied by the rotation).
42//!
43//! # Examples
44//!
45//! ```
46//! use tpt_math_spatial::{MotionVector, Screw};
47//! use tpt_math_geometry::{Isometry3, Rotation3, Translation};
48//! use tpt_math_linalg_fixed::Vector3;
49//!
50//! // A 90° active rotation about +Z.
51//! let rot = Rotation3::from_axis_angle(&Vector3::new([0.0_f64, 0.0, 1.0]), std::f64::consts::FRAC_PI_2);
52//! let iso = Isometry3::new(Translation::new(Vector3::new([0.0, 0.0, 0.0])), rot);
53//!
54//! // A pure-linear motion (1,0,0): after the rotation its linear part is (0,1,0).
55//! let m = MotionVector::new(Vector3::new([0.0, 0.0, 0.0]), Vector3::new([1.0, 0.0, 0.0]));
56//! let m2 = m.transform_by(&iso);
57//! assert!((m2.linear().x()).abs() < 1e-12);
58//! assert!((m2.linear().y() - 1.0).abs() < 1e-12);
59//!
60//! // A screw twist exponentiates to the same isometry.
61//! let screw = Screw::new(Vector3::new([0.0, 0.0, std::f64::consts::FRAC_PI_2]), Vector3::new([0.0, 0.0, 0.0]));
62//! let iso2 = screw.exp();
63//! assert!((iso2.rotation.matrix().data[0][1] - (-1.0)).abs() < 1e-12);
64//! ```
65
66extern crate alloc;
67
68use core::fmt;
69use core::marker::PhantomData;
70use core::ops::{Add, Mul, Neg, Sub};
71
72use tpt_math_geometry::{Isometry3, Quaternion, Rotation3, Translation, UnitQuaternion};
73use tpt_math_linalg_fixed::{Matrix, Matrix3, Vector3, Vector6};
74use tpt_math_numeric::Scalar;
75
76/// A `6 × 6` matrix (fixed-size, row-major).
77pub type Matrix6<T> = Matrix<T, 6, 6>;
78
79#[inline]
80fn c<T: Scalar>(x: f64) -> T {
81    T::from(x).unwrap()
82}
83
84#[inline]
85fn two<T: Scalar>() -> T {
86    T::one() + T::one()
87}
88
89/// Skew-symmetric (`×`) matrix of a 3-vector.
90#[inline]
91fn skew<T: Scalar + Copy>(v: &Vector3<T>) -> Matrix3<T> {
92    Matrix3::new([
93        [T::zero(), -v.z(), v.y()],
94        [v.z(), T::zero(), -v.x()],
95        [-v.y(), v.x(), T::zero()],
96    ])
97}
98
99// ===========================================================================
100// Spatial (6-D) vectors
101// ===========================================================================
102
103/// Marker type for **motion**-type spatial vectors (velocities, accelerations).
104#[derive(Clone, Copy, Debug, PartialEq, Eq)]
105pub struct Motion;
106/// Marker type for **force**-type spatial vectors (wrenches: moment + force).
107#[derive(Clone, Copy, Debug, PartialEq, Eq)]
108pub struct Force;
109
110/// A 6-D spatial vector `v = [angular (top 3); linear (bottom 3)]`, tagged by
111/// `K ∈ {`[`Motion`], [`Force`]`}` to control cross-product and transform
112/// semantics (Featherstone's `crm` / `crf` operators).
113///
114/// * A [`MotionVector`] is `(ω, v)` (angular velocity, linear velocity).
115/// * A [`ForceVector`] is `(n, f)` (moment, force).
116///
117/// See the [crate-level documentation](crate) for the full convention.
118#[derive(Clone, Copy, Debug, PartialEq, Eq)]
119pub struct SpatialVector<T, K> {
120    /// The raw 6-components `[angular₀, angular₁, angular₂, linear₀, linear₁,
121    /// linear₂]`.
122    pub data: Vector6<T>,
123    _marker: PhantomData<K>,
124}
125
126/// A **motion** spatial vector `(ω, v)`.
127pub type MotionVector<T> = SpatialVector<T, Motion>;
128/// A **force** / wrench spatial vector `(n, f)`.
129pub type ForceVector<T> = SpatialVector<T, Force>;
130
131impl<T: Scalar + Copy> SpatialVector<T, Motion> {
132    /// Build a motion vector from its angular (top) and linear (bottom) halves.
133    pub fn new(angular: Vector3<T>, linear: Vector3<T>) -> Self {
134        SpatialVector {
135            data: Vector6::new([
136                angular.x(),
137                angular.y(),
138                angular.z(),
139                linear.x(),
140                linear.y(),
141                linear.z(),
142            ]),
143            _marker: PhantomData,
144        }
145    }
146
147    /// `motion ⊗ motion → motion` (Featherstone `crm`).
148    pub fn cross(&self, rhs: &SpatialVector<T, Motion>) -> SpatialVector<T, Motion> {
149        // (mω × nω, mω × nv + mv × nω)
150        let mw = self.angular();
151        let mv = self.linear();
152        let nw = rhs.angular();
153        let nv = rhs.linear();
154        let top = mw.cross(&nw);
155        let bot = mw.cross(&nv) + mv.cross(&nw);
156        Self::new(top, bot)
157    }
158
159    /// `motion ⊗ force → force` (Featherstone `crm` applied to a force).
160    pub fn cross_force(&self, rhs: &SpatialVector<T, Force>) -> SpatialVector<T, Force> {
161        // (mω × fω + mv × fv, mω × fv)
162        let mw = self.angular();
163        let mv = self.linear();
164        let fw = rhs.angular();
165        let fv = rhs.linear();
166        let top = mw.cross(&fw) + mv.cross(&fv);
167        let bot = mw.cross(&fv);
168        ForceVector::new(top, bot)
169    }
170
171    /// Transform this motion vector by an [`Isometry3`] using the motion
172    /// Plücker adjoint `[R, 0; t×R, R]`.
173    ///
174    /// For a pure rotation this reduces to `R` acting on both halves, matching
175    /// [`Isometry3::transform_vector`] componentwise.
176    pub fn transform_by(&self, iso: &Isometry3<T>) -> Self {
177        SpatialVector {
178            data: adjoint_motion(iso) * self.data,
179            _marker: PhantomData,
180        }
181    }
182}
183
184impl<T: Scalar + Copy> SpatialVector<T, Force> {
185    /// Build a force vector from its angular (moment, top) and linear (force,
186    /// bottom) halves.
187    pub fn new(angular: Vector3<T>, linear: Vector3<T>) -> Self {
188        SpatialVector {
189            data: Vector6::new([
190                angular.x(),
191                angular.y(),
192                angular.z(),
193                linear.x(),
194                linear.y(),
195                linear.z(),
196            ]),
197            _marker: PhantomData,
198        }
199    }
200
201    /// `force ⊗ motion → force` (Featherstone `crf`).
202    pub fn cross_motion(&self, rhs: &SpatialVector<T, Motion>) -> SpatialVector<T, Force> {
203        // (fω × mω, fω × mv + fv × mω)
204        let fw = self.angular();
205        let fv = self.linear();
206        let mw = rhs.angular();
207        let mv = rhs.linear();
208        let top = fw.cross(&mw);
209        let bot = fw.cross(&mv) + fv.cross(&mw);
210        Self::new(top, bot)
211    }
212
213    /// `force ⊗ force → motion` (Featherstone `crf`).
214    pub fn cross(&self, rhs: &SpatialVector<T, Force>) -> SpatialVector<T, Motion> {
215        // (fω × gv + fv × gω, fv × gω)
216        let fw = self.angular();
217        let fv = self.linear();
218        let gw = rhs.angular();
219        let gv = rhs.linear();
220        let top = fw.cross(&gv) + fv.cross(&gw);
221        let bot = fv.cross(&gw);
222        MotionVector::new(top, bot)
223    }
224
225    /// Transform this force vector by an [`Isometry3`] using the force
226    /// Plücker adjoint `[R, t×R; 0, R]`.
227    ///
228    /// For a pure rotation this reduces to `R` acting on both halves, matching
229    /// [`Isometry3::transform_vector`] componentwise.
230    pub fn transform_by(&self, iso: &Isometry3<T>) -> Self {
231        SpatialVector {
232            data: adjoint_force(iso) * self.data,
233            _marker: PhantomData,
234        }
235    }
236}
237
238// Shared accessors / transforms, generic over the kind marker.
239impl<T: Scalar + Copy, K> SpatialVector<T, K> {
240    /// The angular half (top 3 components).
241    pub fn angular(&self) -> Vector3<T> {
242        Vector3::new([self.data.data[0], self.data.data[1], self.data.data[2]])
243    }
244
245    /// The linear half (bottom 3 components).
246    pub fn linear(&self) -> Vector3<T> {
247        Vector3::new([self.data.data[3], self.data.data[4], self.data.data[5]])
248    }
249
250    /// Borrow the raw 6-vector.
251    pub fn as_vector(&self) -> &Vector6<T> {
252        &self.data
253    }
254}
255
256impl<T: Scalar + Copy, K> Add for SpatialVector<T, K> {
257    type Output = Self;
258    /// # Panics
259    ///
260    /// Inherits `T`'s `+` semantics (never panics for floats).
261    fn add(self, rhs: Self) -> Self {
262        SpatialVector {
263            data: self.data + rhs.data,
264            _marker: PhantomData,
265        }
266    }
267}
268
269impl<T: Scalar + Copy, K> Sub for SpatialVector<T, K> {
270    type Output = Self;
271    fn sub(self, rhs: Self) -> Self {
272        SpatialVector {
273            data: self.data - rhs.data,
274            _marker: PhantomData,
275        }
276    }
277}
278
279impl<T: Scalar + Copy, K> Neg for SpatialVector<T, K> {
280    type Output = Self;
281    fn neg(self) -> Self {
282        SpatialVector {
283            data: -self.data,
284            _marker: PhantomData,
285        }
286    }
287}
288
289impl<T: Scalar + Copy, K> Mul<T> for SpatialVector<T, K> {
290    type Output = Self;
291    fn mul(self, rhs: T) -> Self {
292        SpatialVector {
293            data: self.data * rhs,
294            _marker: PhantomData,
295        }
296    }
297}
298
299impl<T: Scalar + Copy + fmt::Debug, K> fmt::Display for SpatialVector<T, K> {
300    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
301        write!(
302            f,
303            "[ω: {:?}; v: {:?}]",
304            self.angular().data,
305            self.linear().data
306        )
307    }
308}
309
310// ===========================================================================
311// Adjoint (Plücker) transforms
312// ===========================================================================
313
314/// The **motion** adjoint `Ad_X` of an isometry `(R, t)`:
315///
316/// ```text
317/// Ad_X = [ R      0   ]
318///        [ t×R    R   ]
319/// ```
320///
321/// Acting on a [`MotionVector`] `(ω, v)` it yields
322/// `(R ω, R v + t × (R ω))`.
323pub fn adjoint_motion<T: Scalar + Copy>(iso: &Isometry3<T>) -> Matrix6<T> {
324    let r = *iso.rotation.matrix();
325    let t = iso.translation.vector;
326    let tl = skew(&t) * r;
327    Matrix6::from_fn(|i, j| {
328        if i < 3 && j < 3 {
329            r.data[i][j]
330        } else if i >= 3 && j < 3 {
331            tl.data[i - 3][j]
332        } else if i >= 3 && j >= 3 {
333            r.data[i - 3][j - 3]
334        } else {
335            T::zero()
336        }
337    })
338}
339
340/// The **force** adjoint `Ad_X*` of an isometry `(R, t)`:
341///
342/// ```text
343/// Ad_X* = [ R      t×R ]
344///         [ 0      R   ]
345/// ```
346///
347/// Acting on a [`ForceVector`] `(n, f)` it yields
348/// `(R n + t × (R f), R f)`.
349pub fn adjoint_force<T: Scalar + Copy>(iso: &Isometry3<T>) -> Matrix6<T> {
350    let r = *iso.rotation.matrix();
351    let t = iso.translation.vector;
352    let tr = skew(&t) * r;
353    Matrix6::from_fn(|i, j| {
354        if i < 3 && j < 3 {
355            r.data[i][j]
356        } else if i < 3 && j >= 3 {
357            tr.data[i][j - 3]
358        } else if i >= 3 && j >= 3 {
359            r.data[i - 3][j - 3]
360        } else {
361            T::zero()
362        }
363    })
364}
365
366// ===========================================================================
367// Dual quaternions
368// ===========================================================================
369
370/// A dual quaternion `q = real + ε·dual`, each part a Hamilton
371/// [`Quaternion`] (`w` last). A *unit* dual quaternion represents a rigid
372/// transform (rotation + translation) with no scaling.
373#[derive(Clone, Copy, Debug, PartialEq, Eq)]
374pub struct DualQuaternion<T> {
375    /// The real (rotation) part.
376    pub real: Quaternion<T>,
377    /// The dual (translation) part.
378    pub dual: Quaternion<T>,
379}
380
381impl<T: Scalar> DualQuaternion<T> {
382    /// Build a dual quaternion from its real and dual quaternion parts.
383    pub fn new(real: Quaternion<T>, dual: Quaternion<T>) -> Self {
384        DualQuaternion { real, dual }
385    }
386
387    /// A dual quaternion with a zero dual part (a pure rotation).
388    pub fn from_real(real: Quaternion<T>) -> Self {
389        DualQuaternion {
390            real,
391            dual: Quaternion::new(T::zero(), T::zero(), T::zero(), T::zero()),
392        }
393    }
394
395    /// The (Hamilton) conjugate `(real*, dual*)`.
396    pub fn conjugate(&self) -> Self {
397        DualQuaternion {
398            real: self.real.conjugate(),
399            dual: self.dual.conjugate(),
400        }
401    }
402
403    /// Normalise to a unit dual quaternion by dividing both parts by the norm
404    /// of the real part. Returns `None` if the real part has zero norm.
405    pub fn normalize(&self) -> Option<Self> {
406        let n = self.real.norm();
407        if n == T::zero() {
408            return None;
409        }
410        let inv = T::one() / n;
411        Some(DualQuaternion {
412            real: self.real * inv,
413            dual: self.dual * inv,
414        })
415    }
416
417    /// Hamilton product of two dual quaternions
418    /// `(r₁, d₁) * (r₂, d₂) = (r₁ r₂, r₁ d₂ + d₁ r₂)`.
419    pub fn multiply(&self, rhs: &DualQuaternion<T>) -> DualQuaternion<T> {
420        DualQuaternion {
421            real: self.real.multiply(&rhs.real),
422            dual: self.real.multiply(&rhs.dual) + self.dual.multiply(&rhs.real),
423        }
424    }
425
426    /// Build a unit dual quaternion from a rigid transform.
427    ///
428    /// Given an isometry with rotation quaternion `q` and translation `t`, the
429    /// dual quaternion is `q + ε·(½·t_q·q)` where `t_q = (t_x, t_y, t_z, 0)`.
430    pub fn from_isometry(iso: &Isometry3<T>) -> Self {
431        let q = *UnitQuaternion::<T>::from_rotation_matrix(&iso.rotation).quaternion();
432        let t = iso.translation.vector;
433        let tq = Quaternion::new(t.x(), t.y(), t.z(), T::zero());
434        let dual = tq.multiply(&q) * (T::one() / (T::one() + T::one()));
435        DualQuaternion { real: q, dual }
436    }
437
438    /// Recover the rigid transform represented by this (unit) dual quaternion.
439    ///
440    /// The rotation is the real part; the translation is recovered from
441    /// `2·dual·real*` (taking the vector part).
442    pub fn to_isometry(&self) -> Isometry3<T> {
443        let real = self.real.normalize().unwrap_or(self.real);
444        let rotation = Rotation3::from_quaternion(&UnitQuaternion::from_quaternion(real));
445        let tq = self.dual.multiply(&real.conjugate()) * (T::one() + T::one());
446        let translation = Translation::new(Vector3::new([
447            tq.coords.data[0],
448            tq.coords.data[1],
449            tq.coords.data[2],
450        ]));
451        Isometry3::new(translation, rotation)
452    }
453}
454
455// ===========================================================================
456// Screw theory
457// ===========================================================================
458
459/// A screw (twist) in `se(3)`, stored as the 6-D spatial motion vector
460/// `(ω, v)` where `ω` is the angular part (rotation axis × angle) and `v` is
461/// the linear part.
462///
463/// The [`Screw::exp`] map sends this twist to an [`Isometry3`] via the standard
464/// `se(3)` matrix exponential (Lynch–Park convention, active right-handed
465/// rotation), and [`Screw::log`] is its inverse.
466#[derive(Clone, Copy, Debug, PartialEq, Eq)]
467pub struct Screw<T> {
468    /// The angular part (axis × angle).
469    pub angular: Vector3<T>,
470    /// The linear part.
471    pub linear: Vector3<T>,
472}
473
474impl<T: Scalar + Copy> Screw<T> {
475    /// Build a screw from its angular and linear 3-vectors.
476    pub fn new(angular: Vector3<T>, linear: Vector3<T>) -> Self {
477        Screw { angular, linear }
478    }
479
480    /// View the screw as a [`MotionVector`].
481    pub fn as_motion(&self) -> MotionVector<T> {
482        MotionVector::new(self.angular, self.linear)
483    }
484
485    /// The exponential map `exp: se(3) → SE(3)`.
486    ///
487    /// For `ω = 0` this is a pure translation by `v`; otherwise it is a rotation
488    /// by `|ω|` about `ω/|ω|` with the Lynch–Park transported translation.
489    pub fn exp(&self) -> Isometry3<T> {
490        let w = self.angular;
491        let v = self.linear;
492        let theta = w.norm();
493        let eps = c::<T>(1e-12);
494        let rotation = if theta < eps {
495            Rotation3::identity()
496        } else {
497            let axis = w / theta;
498            Rotation3::from_axis_angle(&axis, theta)
499        };
500        let p = if theta < eps {
501            v
502        } else {
503            let axis = w / theta;
504            let vl = v / theta;
505            let av = axis.cross(&vl); // A·vl
506            let aav = axis.cross(&av); // A²·vl
507            let (s, cth) = theta.sin_cos();
508            let term1 = vl * theta;
509            let term2 = av * (T::one() - cth);
510            let term3 = aav * (theta - s);
511            term1 + term2 + term3
512        };
513        Isometry3::new(Translation::new(p), rotation)
514    }
515
516    /// The logarithmic map `log: SE(3) → se(3)` — the inverse of [`Screw::exp`].
517    ///
518    /// Recovers the screw `(ω, v)` whose exponential equals `iso`. For a pure
519    /// translation this is `(0, translation)`.
520    ///
521    /// # Numerical singularities
522    ///
523    /// `log` has two well-known singularities, both guarded here:
524    ///
525    /// * **`theta ≈ 0` (identity rotation).** Handled by the early-return
526    ///   branch — the off-diagonal axis formula divides by `sin(theta)`, which
527    ///   vanishes at `theta = 0`.
528    /// * **`theta ≈ π` (180° rotation).** The same off-diagonal formula divides
529    ///   by `sin(theta)`, which also vanishes at `theta = π`. The angle is taken
530    ///   as `atan2(sin θ, cos θ)` with `cos θ = (tr − 1) / 2` and `sin θ = |skew|
531    ///   / 2`, which stays in domain under rounding (unlike `acos`, which can
532    ///   yield `NaN` near these limits), and the axis is extracted from `(R + I)
533    ///   = 2·(w wᵀ)` instead of the off-diagonal differences in the `theta ≈ π`
534    ///   regime.
535    pub fn log(iso: &Isometry3<T>) -> Screw<T> {
536        let m = iso.rotation.matrix().data;
537        let tr = m[0][0] + m[1][1] + m[2][2];
538        // cos(theta) = (trace - 1) / 2 and sin(theta) = |skew| / 2, where the
539        // skew part (R - Rᵀ) has norm 2 sin(theta). theta = atan2(sin, cos) is
540        // well-conditioned everywhere and cannot leave its domain, unlike
541        // acos near 0 and pi where rounding pushes the argument out of range.
542        let cos_t = (tr - T::one()) / two();
543        let skew = Vector3::new([m[2][1] - m[1][2], m[0][2] - m[2][0], m[1][0] - m[0][1]]);
544        let sin_t = skew.dot(&skew).sqrt() / two();
545        let theta = sin_t.atan2(cos_t);
546        let eps = c::<T>(1e-12);
547        if theta < eps {
548            return Screw::new(
549                Vector3::new([T::zero(), T::zero(), T::zero()]),
550                iso.translation.vector,
551            );
552        }
553        let two_ = two::<T>();
554        let pi = c::<T>(core::f64::consts::PI);
555        // Angular axis.
556        //
557        // * For general theta the standard off-diagonal formula
558        //   `w = (R - R^T) / (2 sin theta)` is exact and well-conditioned.
559        // * Near theta ~= pi that formula divides by sin(theta) -> 0 (the
560        //   *second* log singularity) and is catastrophically ill-conditioned.
561        //   There we instead take the *direction* from (R + I) = 2 w w^T (exact
562        //   at theta = pi, well-conditioned provided no axis component is tiny)
563        //   and recover the *sign* from the skew part (R - R^T) = 2 sin(theta)
564        //   [w]x, whose sign is unambiguous (sin theta > 0 for theta in (0, pi)).
565        let axis = if theta > pi - c::<T>(1e-3) {
566            let rpi = [
567                [m[0][0] + T::one(), m[0][1], m[0][2]],
568                [m[1][0], m[1][1] + T::one(), m[1][2]],
569                [m[2][0], m[2][1], m[2][2] + T::one()],
570            ];
571            let n0 = rpi[0][0] * rpi[0][0] + rpi[1][0] * rpi[1][0] + rpi[2][0] * rpi[2][0];
572            let n1 = rpi[0][1] * rpi[0][1] + rpi[1][1] * rpi[1][1] + rpi[2][1] * rpi[2][1];
573            let n2 = rpi[0][2] * rpi[0][2] + rpi[1][2] * rpi[1][2] + rpi[2][2] * rpi[2][2];
574            let (col, norm) = if n0 >= n1 && n0 >= n2 {
575                ([rpi[0][0], rpi[1][0], rpi[2][0]], n0)
576            } else if n1 >= n2 {
577                ([rpi[0][1], rpi[1][1], rpi[2][1]], n1)
578            } else {
579                ([rpi[0][2], rpi[1][2], rpi[2][2]], n2)
580            };
581            let norm = norm.sqrt();
582            let norm = if norm == T::zero() { T::one() } else { norm };
583            let ax = Vector3::new([col[0] / norm, col[1] / norm, col[2] / norm]);
584            if ax.dot(&skew) < T::zero() {
585                -ax
586            } else {
587                ax
588            }
589        } else {
590            let s = sin_t;
591            Vector3::new([
592                (m[2][1] - m[1][2]) / (two_ * s),
593                (m[0][2] - m[2][0]) / (two_ * s),
594                (m[1][0] - m[0][1]) / (two_ * s),
595            ])
596        };
597        let p = iso.translation.vector;
598        let ap = axis.cross(&p); // A·p
599        let aap = axis.cross(&ap); // A²·p
600                                   // coef = 1/theta - (1 + cos theta) / (2 sin theta)
601                                   //      = 1/theta - (1/2) cot(theta/2).  The half-angle form avoids the
602                                   // sin(theta) division (stable across the whole (0, pi) range, including
603                                   // the theta ~= pi limit where (1 + cos theta)/(2 sin theta) -> 0).
604        let half = theta / two_;
605        let cot_half = half.cos() / half.sin();
606        let coef = (T::one() / theta) - (cot_half / two_);
607        let vl = (p / theta) - (ap * (T::one() / two_)) + (aap * coef);
608        let angular = axis * theta;
609        let linear = vl * theta;
610        Screw::new(angular, linear)
611    }
612}
613
614// ===========================================================================
615// Tests
616// ===========================================================================
617
618#[cfg(test)]
619mod tests {
620    use super::*;
621    use tpt_math_geometry::Point3;
622
623    type MVec = MotionVector<f64>;
624    type FVec = ForceVector<f64>;
625
626    #[inline]
627    fn close(a: f64, b: f64) -> bool {
628        (a - b).abs() < 1e-10
629    }
630
631    fn iso_z90() -> Isometry3<f64> {
632        let rot = Rotation3::from_axis_angle(
633            &Vector3::new([0.0, 0.0, 1.0]),
634            core::f64::consts::FRAC_PI_2,
635        );
636        Isometry3::new(Translation::new(Vector3::new([0.0, 0.0, 0.0])), rot)
637    }
638
639    #[test]
640    fn spatial_vector_accessors_and_arithmetic() {
641        let a = MVec::new(Vector3::new([1.0, 2.0, 3.0]), Vector3::new([4.0, 5.0, 6.0]));
642        assert!(close(a.angular().x(), 1.0));
643        assert!(close(a.linear().z(), 6.0));
644        let b = MVec::new(Vector3::new([0.0, 0.0, 0.0]), Vector3::new([1.0, 1.0, 1.0]));
645        let s = a + b;
646        assert!(close(s.linear().x(), 5.0));
647        let n = -a;
648        assert!(close(n.angular().x(), -1.0));
649    }
650
651    #[test]
652    fn cross_motion_motion_known() {
653        // m = (ω=(0,0,1), v=0), n = (ω=0, v=(1,0,0)).
654        // m × n = (0, 0×0 + (0,0,1)×(1,0,0)) = (0, (0,1,0)).
655        let m = MVec::new(Vector3::new([0.0, 0.0, 1.0]), Vector3::new([0.0, 0.0, 0.0]));
656        let n = MVec::new(Vector3::new([0.0, 0.0, 0.0]), Vector3::new([1.0, 0.0, 0.0]));
657        let r = m.cross(&n);
658        assert!(close(r.angular().norm(), 0.0));
659        assert!(close(r.linear().x(), 0.0));
660        assert!(close(r.linear().y(), 1.0));
661        assert!(close(r.linear().z(), 0.0));
662    }
663
664    #[test]
665    fn cross_motion_force_known() {
666        // m = (ω=(0,0,1), v=0), f = (n=0, f=(1,0,0)).
667        // m × f = (mω×fω + mv×fv, mω×fv) = (0, (0,0,1)×(1,0,0)) = (0, (0,1,0)).
668        let m = MVec::new(Vector3::new([0.0, 0.0, 1.0]), Vector3::new([0.0, 0.0, 0.0]));
669        let f = FVec::new(Vector3::new([0.0, 0.0, 0.0]), Vector3::new([1.0, 0.0, 0.0]));
670        let r = m.cross_force(&f);
671        assert!(close(r.angular().norm(), 0.0));
672        assert!(close(r.linear().x(), 0.0));
673        assert!(close(r.linear().y(), 1.0));
674        assert!(close(r.linear().z(), 0.0));
675    }
676
677    #[test]
678    fn cross_force_force_known() {
679        // f = (n=(0,0,1), f=0), g = (n=0, f=(1,0,0)).
680        // f × g = ((0,0,1)×(1,0,0) + 0, 0) = ((0,1,0), 0) → motion.
681        let f = FVec::new(Vector3::new([0.0, 0.0, 1.0]), Vector3::new([0.0, 0.0, 0.0]));
682        let g = FVec::new(Vector3::new([0.0, 0.0, 0.0]), Vector3::new([1.0, 0.0, 0.0]));
683        let r = f.cross(&g);
684        assert!(close(r.angular().x(), 0.0));
685        assert!(close(r.angular().y(), 1.0));
686        assert!(close(r.angular().z(), 0.0));
687        assert!(close(r.linear().norm(), 0.0));
688    }
689
690    #[test]
691    fn transform_by_rotation_matches_isometry_vector() {
692        let iso = iso_z90();
693        // Pure-linear motion (1,0,0) → linear part rotates to (0,1,0), angular to 0.
694        let m = MVec::new(Vector3::new([0.0, 0.0, 0.0]), Vector3::new([1.0, 0.0, 0.0]));
695        let mt = m.transform_by(&iso);
696        assert!(close(mt.angular().norm(), 0.0));
697        assert!(close(mt.linear().x(), 0.0));
698        assert!(close(mt.linear().y(), 1.0));
699
700        // Angular part should transform like a vector under R.
701        let m2 = MVec::new(Vector3::new([1.0, 0.0, 0.0]), Vector3::new([0.0, 0.0, 0.0]));
702        let m2t = m2.transform_by(&iso);
703        // R applied to (1,0,0) about z by 90° → (0,1,0).
704        let expected = iso
705            .rotation
706            .transform_vector(&Vector3::new([1.0, 0.0, 0.0]));
707        assert!(close(m2t.angular().x(), expected.data[0]));
708        assert!(close(m2t.angular().y(), expected.data[1]));
709        assert!(close(m2t.angular().z(), expected.data[2]));
710    }
711
712    #[test]
713    fn adjoint_matches_transform() {
714        let iso = iso_z90();
715        let ad = adjoint_motion(&iso);
716        let m = MVec::new(Vector3::new([1.0, 2.0, 3.0]), Vector3::new([4.0, 5.0, 6.0]));
717        let via_matrix = MVec {
718            data: ad * m.data,
719            _marker: PhantomData,
720        };
721        let via_fn = m.transform_by(&iso);
722        for i in 0..6 {
723            assert!(
724                close(via_matrix.data.data[i], via_fn.data.data[i]),
725                "idx {i}"
726            );
727        }
728    }
729
730    #[test]
731    fn transform_by_with_translation() {
732        // Rotation 90° about Z plus translation (10, 0, 0).
733        let rot = Rotation3::from_axis_angle(
734            &Vector3::new([0.0, 0.0, 1.0]),
735            core::f64::consts::FRAC_PI_2,
736        );
737        let iso = Isometry3::new(Translation::new(Vector3::new([10.0, 0.0, 0.0])), rot);
738        // A force at the origin with linear force (1,0,0) and zero moment.
739        // Force adjoint: fω' = R fω + t × (R fv), fv' = R fv.
740        let f = FVec::new(Vector3::new([0.0, 0.0, 0.0]), Vector3::new([1.0, 0.0, 0.0]));
741        let ft = f.transform_by(&iso);
742        let rfv = rot.transform_vector(&Vector3::new([1.0, 0.0, 0.0])); // (0,1,0)
743        let expected_top = Vector3::new([10.0, 0.0, 0.0]).cross(&rfv); // t × (R fv)
744        assert!(close(ft.angular().x(), expected_top.data[0]));
745        assert!(close(ft.angular().y(), expected_top.data[1]));
746        assert!(close(ft.angular().z(), expected_top.data[2]));
747        assert!(close(ft.linear().x(), 0.0));
748        assert!(close(ft.linear().y(), 1.0));
749    }
750
751    #[test]
752    fn dual_quaternion_roundtrip() {
753        let rot = Rotation3::from_axis_angle(
754            &Vector3::new([0.0, 0.0, 1.0]),
755            core::f64::consts::FRAC_PI_2,
756        );
757        let iso = Isometry3::new(Translation::new(Vector3::new([1.0, 2.0, 3.0])), rot);
758        let dq = DualQuaternion::from_isometry(&iso);
759        let back = dq.to_isometry();
760        for i in 0..3 {
761            for j in 0..3 {
762                assert!(
763                    close(
764                        back.rotation.matrix().data[i][j],
765                        iso.rotation.matrix().data[i][j]
766                    ),
767                    "rot {i},{j}"
768                );
769            }
770            assert!(
771                close(
772                    back.translation.vector.data[i],
773                    iso.translation.vector.data[i]
774                ),
775                "trans {i}"
776            );
777        }
778    }
779
780    #[test]
781    fn dual_quaternion_composition() {
782        let r1 = Rotation3::from_axis_angle(
783            &Vector3::new([0.0, 0.0, 1.0]),
784            core::f64::consts::FRAC_PI_2,
785        );
786        let iso1 = Isometry3::new(Translation::new(Vector3::new([1.0, 0.0, 0.0])), r1);
787        let r2 = Rotation3::from_axis_angle(
788            &Vector3::new([0.0, 1.0, 0.0]),
789            core::f64::consts::FRAC_PI_3,
790        );
791        let iso2 = Isometry3::new(Translation::new(Vector3::new([0.0, 2.0, 0.0])), r2);
792
793        let dq1 = DualQuaternion::from_isometry(&iso1);
794        let dq2 = DualQuaternion::from_isometry(&iso2);
795        let composed_dq = dq1.multiply(&dq2);
796        let composed_iso = iso1 * iso2;
797        let from_dq = composed_dq.to_isometry();
798        for i in 0..3 {
799            for j in 0..3 {
800                assert!(
801                    close(
802                        from_dq.rotation.matrix().data[i][j],
803                        composed_iso.rotation.matrix().data[i][j]
804                    ),
805                    "rot {i},{j}"
806                );
807            }
808            assert!(
809                close(
810                    from_dq.translation.vector.data[i],
811                    composed_iso.translation.vector.data[i]
812                ),
813                "trans {i}"
814            );
815        }
816    }
817
818    #[test]
819    fn screw_exp_log_roundtrip() {
820        // Random-ish isometry.
821        let rot = Rotation3::from_axis_angle(
822            &Vector3::new([1.0, 2.0, 3.0]).normalize(),
823            core::f64::consts::FRAC_PI_4,
824        );
825        let iso = Isometry3::new(Translation::new(Vector3::new([0.5, -1.0, 2.0])), rot);
826        let screw = Screw::log(&iso);
827        let iso2 = screw.exp();
828        for i in 0..3 {
829            for j in 0..3 {
830                assert!(
831                    close(
832                        iso2.rotation.matrix().data[i][j],
833                        iso.rotation.matrix().data[i][j]
834                    ),
835                    "rot {i},{j}"
836                );
837            }
838            assert!(
839                close(
840                    iso2.translation.vector.data[i],
841                    iso.translation.vector.data[i]
842                ),
843                "trans {i}"
844            );
845        }
846    }
847
848    #[test]
849    fn screw_pure_translation() {
850        let iso = Isometry3::new(
851            Translation::new(Vector3::new([3.0, -2.0, 1.0])),
852            Rotation3::identity(),
853        );
854        let screw = Screw::log(&iso);
855        assert!(close(screw.angular.norm(), 0.0));
856        for i in 0..3 {
857            assert!(
858                close(screw.linear.data[i], iso.translation.vector.data[i]),
859                "lin {i}"
860            );
861        }
862        let iso2 = screw.exp();
863        for i in 0..3 {
864            assert!(
865                close(
866                    iso2.translation.vector.data[i],
867                    iso.translation.vector.data[i]
868                ),
869                "trans {i}"
870            );
871        }
872    }
873
874    #[test]
875    fn screw_exp_log_point_recovery() {
876        // exp(log(iso)) must act identically to iso on an arbitrary point.
877        let rot = Rotation3::from_axis_angle(
878            &Vector3::new([0.3, 1.0, -0.7]).normalize(),
879            core::f64::consts::FRAC_PI_3,
880        );
881        let iso = Isometry3::new(Translation::new(Vector3::new([2.0, 1.0, -3.0])), rot);
882        let screw = Screw::log(&iso);
883        let iso2 = screw.exp();
884        let p = Point3::new(Vector3::new([1.0, -2.0, 0.5]));
885        let q1 = iso.transform_point(&p);
886        let q2 = iso2.transform_point(&p);
887        for i in 0..3 {
888            assert!(close(q1.coords.data[i], q2.coords.data[i]), "pt {i}");
889        }
890    }
891
892    #[test]
893    fn screw_composition_identity() {
894        // exp(a) * exp(b) exponentiates (as an isometry product) to a transform
895        // whose log exponentiates back to itself.
896        let a = Screw::new(Vector3::new([0.0, 0.0, 0.1]), Vector3::new([0.2, 0.0, 0.0]));
897        let b = Screw::new(Vector3::new([0.1, 0.0, 0.0]), Vector3::new([0.0, 0.3, 0.0]));
898        let iso_a = a.exp();
899        let iso_b = b.exp();
900        let iso_ab = iso_a * iso_b;
901        let screw_ab = Screw::log(&iso_ab);
902        let iso_ab2 = screw_ab.exp();
903        for i in 0..3 {
904            for j in 0..3 {
905                assert!(
906                    close(
907                        iso_ab2.rotation.matrix().data[i][j],
908                        iso_ab.rotation.matrix().data[i][j]
909                    ),
910                    "rot {i},{j}"
911                );
912            }
913            assert!(
914                close(
915                    iso_ab2.translation.vector.data[i],
916                    iso_ab.translation.vector.data[i]
917                ),
918                "trans {i}"
919            );
920        }
921    }
922
923    #[test]
924    fn screw_log_180_rotation() {
925        // theta = pi is the *second* log singularity: the standard off-diagonal
926        // axis formula divides by sin(pi) = 0. log must fall back to the
927        // (R + I) = 2 w w^T extraction and recover a clean 180° twist.
928        let rot = Rotation3::from_axis_angle(&Vector3::new([0.0, 0.0, 1.0]), core::f64::consts::PI);
929        let iso = Isometry3::new(Translation::new(Vector3::new([0.0, 0.0, 0.0])), rot);
930        let screw = Screw::log(&iso);
931        assert!(close(screw.angular.x(), 0.0));
932        assert!(close(screw.angular.y(), 0.0));
933        assert!(close(screw.angular.z(), core::f64::consts::PI));
934        assert!(close(screw.linear.norm(), 0.0));
935        // Round-trips back to the same isometry.
936        let iso2 = screw.exp();
937        for i in 0..3 {
938            for j in 0..3 {
939                assert!(
940                    close(
941                        iso2.rotation.matrix().data[i][j],
942                        iso.rotation.matrix().data[i][j]
943                    ),
944                    "rot {i},{j}"
945                );
946            }
947        }
948    }
949
950    #[test]
951    fn screw_log_180_rotation_with_translation() {
952        // Same singularity, now with a non-trivial translation so the linear
953        // part of the screw is also exercised.
954        let rot = Rotation3::from_axis_angle(
955            &Vector3::new([1.0, 1.0, 1.0]).normalize(),
956            core::f64::consts::PI,
957        );
958        let iso = Isometry3::new(Translation::new(Vector3::new([1.0, -2.0, 0.5])), rot);
959        let screw = Screw::log(&iso);
960        // Angular magnitude must be pi (to within the rounding inherited from
961        // the irrational-axis rotation matrix — its trace is only ~-1, so the
962        // recovered angle is ~pi - O(1e-8)). The stricter correctness check is
963        // the point-round-trip below.
964        assert!((screw.angular.norm() - core::f64::consts::PI).abs() < 1e-7);
965        // exp(log(iso)) must reproduce iso's action on a point. At theta ~= pi
966        // the log is ill-conditioned (the recovered axis carries an
967        // O(pi - theta) error inherited from the rotation matrix), so allow the
968        // relaxed ~1e-7 tolerance that the singularity implies rather than 1e-10.
969        let iso2 = screw.exp();
970        let p = Point3::new(Vector3::new([2.0, -1.0, 3.0]));
971        let q1 = iso.transform_point(&p);
972        let q2 = iso2.transform_point(&p);
973        for i in 0..3 {
974            let d = (q1.coords.data[i] - q2.coords.data[i]).abs();
975            assert!(d < 1e-7, "pt {i}: diff {d}");
976        }
977    }
978
979    #[test]
980    fn screw_log_small_rotation() {
981        // theta ~= 0 is the *first* log singularity. The early-return branch
982        // must give a clean small-rotation twist (angular = angle * axis).
983        let angle = 1e-6;
984        let axis = Vector3::new([1.0, 2.0, 3.0]).normalize();
985        let rot = Rotation3::from_axis_angle(&axis, angle);
986        let iso = Isometry3::new(Translation::new(Vector3::new([0.0, 0.0, 0.0])), rot);
987        let screw = Screw::log(&iso);
988        assert!(close(screw.angular.norm(), angle));
989        // Axis direction recovered up to the tiny angle's precision.
990        let recovered = screw.angular / angle;
991        assert!(close(recovered.x(), axis.x()));
992        assert!(close(recovered.y(), axis.y()));
993        assert!(close(recovered.z(), axis.z()));
994    }
995
996    #[test]
997    fn screw_small_twist_bch_approx() {
998        // For small twists, exp(a) * exp(b) ≈ exp(a + b) up to the (second-order)
999        // Baker–Campbell–Hausdorff commutator term, which is O(||a||·||b||).
1000        let a = Screw::new(
1001            Vector3::new([0.01, 0.0, 0.0]),
1002            Vector3::new([0.0, 0.02, 0.0]),
1003        );
1004        let b = Screw::new(
1005            Vector3::new([0.0, 0.01, 0.0]),
1006            Vector3::new([0.0, 0.0, 0.01]),
1007        );
1008        let sum = Screw::new(a.angular + b.angular, a.linear + b.linear);
1009        let iso_ab = a.exp() * b.exp();
1010        let iso_sum = sum.exp();
1011        // Loose tolerance: the first-order approximation error is ~1e-3 here.
1012        for i in 0..3 {
1013            let dt: f64 = iso_ab.translation.vector.data[i] - iso_sum.translation.vector.data[i];
1014            assert!(dt.abs() < 1e-3, "trans {i}");
1015            for j in 0..3 {
1016                let dr: f64 =
1017                    iso_ab.rotation.matrix().data[i][j] - iso_sum.rotation.matrix().data[i][j];
1018                assert!(dr.abs() < 1e-3, "rot {i},{j}");
1019            }
1020        }
1021    }
1022}