phyz_math/
spatial.rs

1//! 6D spatial algebra following Featherstone's "Rigid Body Dynamics Algorithms".
2//!
3//! Convention: spatial vectors are [angular; linear] (Featherstone order).
4//! A spatial motion vector (twist): [w; v]
5//! A spatial force vector (wrench): [tau; f]
6//!
7//! This module re-exports tang's block-structured spatial types with f64 specialization.
8
9/// 6D spatial vector -- either a motion vector (twist) or force vector (wrench).
10///
11/// Stored as two Vec3s (angular, linear) instead of a flat Vec6.
12pub type SpatialVec = tang::SpatialVec<f64>;
13
14/// 6x6 spatial matrix stored as four 3x3 blocks.
15///
16/// ```text
17/// | upper_left  upper_right |
18/// | lower_left  lower_right |
19/// ```
20pub type SpatialMat = tang::SpatialMat<f64>;
21
22/// Plucker transform: rigid body transformation acting on spatial vectors.
23///
24/// Represents a coordinate transform from frame A to frame B.
25/// Stored as rotation R and translation p (position of B's origin in A's frame).
26///
27/// **Do not write `xf.rot * v` or `xf.rot.transpose() * v` by hand.** For a
28/// body transform from forward kinematics, `rot` is the *world→body* rotation
29/// and `pos` is the body origin in world coordinates — a convention every raw
30/// call site must independently remember, and getting it backwards is
31/// invisible at identity. Use the named, direction-carrying methods on
32/// [`SpatialTransformExt`] instead: [`SpatialTransformExt::body_to_world_point`],
33/// [`SpatialTransformExt::world_to_body_point`],
34/// [`SpatialTransformExt::body_to_world_dir`], and
35/// [`SpatialTransformExt::world_to_body_dir`].
36pub type SpatialTransform = tang::SpatialTransform<f64>;
37
38/// Spatial inertia of a rigid body about its center of mass.
39///
40/// Stored as mass, center of mass offset, and rotational inertia.
41/// Can be converted to a 6x6 spatial inertia matrix.
42pub type SpatialInertia = tang::SpatialInertia<f64>;
43
44/// Direction-carrying frame conversions for [`SpatialTransform`].
45///
46/// **The convention, stated once and authoritatively:** in a body transform
47/// produced by forward kinematics, `rot` is the **world→body** rotation
48/// (Featherstone's `E`) and `pos` is the **body origin in world coordinates**.
49/// So mapping a body-frame vector out to world requires `rot.transpose()`,
50/// and mapping a world-frame vector into the body frame requires `rot`.
51///
52/// Every call site that writes `xf.rot * v` or `xf.rot.transpose() * v` by
53/// hand must independently remember which direction `rot` points — and a
54/// mistake is invisible at identity, only inverting once the body tilts.
55/// These methods exist so no call site ever writes that arithmetic by hand
56/// again: the method name carries the direction, and the point/dir split
57/// carries whether the translation applies.
58///
59/// * `*_point` methods transform **positions** (rotation *and* translation).
60/// * `*_dir` methods transform **directions** — axes, forces, velocities,
61///   offsets — with rotation only.
62pub trait SpatialTransformExt {
63    /// Map a point in body coordinates to world coordinates:
64    /// `pos + rot.transpose() * p`.
65    fn body_to_world_point(&self, p: crate::Vec3) -> crate::Vec3;
66
67    /// Map a point in world coordinates to body coordinates:
68    /// `rot * (p - pos)`.
69    fn world_to_body_point(&self, p: crate::Vec3) -> crate::Vec3;
70
71    /// Rotate a direction (axis, force, velocity, offset) from body to world:
72    /// `rot.transpose() * d`. No translation.
73    fn body_to_world_dir(&self, d: crate::Vec3) -> crate::Vec3;
74
75    /// Rotate a direction (axis, force, velocity, offset) from world to body:
76    /// `rot * d`. No translation.
77    fn world_to_body_dir(&self, d: crate::Vec3) -> crate::Vec3;
78}
79
80impl SpatialTransformExt for SpatialTransform {
81    #[inline]
82    fn body_to_world_point(&self, p: crate::Vec3) -> crate::Vec3 {
83        self.pos + self.rot.transpose() * p
84    }
85
86    #[inline]
87    fn world_to_body_point(&self, p: crate::Vec3) -> crate::Vec3 {
88        self.rot * (p - self.pos)
89    }
90
91    #[inline]
92    fn body_to_world_dir(&self, d: crate::Vec3) -> crate::Vec3 {
93        self.rot.transpose() * d
94    }
95
96    #[inline]
97    fn world_to_body_dir(&self, d: crate::Vec3) -> crate::Vec3 {
98        self.rot * d
99    }
100}
101
102#[cfg(test)]
103mod tests {
104    use super::*;
105    use crate::{Mat3, Vec3};
106
107    const EPS: f64 = 1e-10;
108
109    fn assert_vec_eq(a: Vec3, b: Vec3, msg: &str) {
110        assert!((a.x - b.x).abs() < EPS, "{msg}: x: {} vs {}", a.x, b.x);
111        assert!((a.y - b.y).abs() < EPS, "{msg}: y: {} vs {}", a.y, b.y);
112        assert!((a.z - b.z).abs() < EPS, "{msg}: z: {} vs {}", a.z, b.z);
113    }
114
115    fn assert_sv_eq(a: &SpatialVec, b: &SpatialVec, msg: &str) {
116        assert_vec_eq(a.angular, b.angular, &format!("{msg} angular"));
117        assert_vec_eq(a.linear, b.linear, &format!("{msg} linear"));
118    }
119
120    #[test]
121    fn test_frame_conversion_round_trips() {
122        let xf = SpatialTransform::new(
123            Mat3::rotation_z(0.5).mul_mat(&Mat3::rotation_x(-0.3)),
124            Vec3::new(1.0, -2.0, 3.0),
125        );
126        let p = Vec3::new(0.7, -1.1, 2.9);
127        assert_vec_eq(
128            xf.world_to_body_point(xf.body_to_world_point(p)),
129            p,
130            "point round trip b->w->b",
131        );
132        assert_vec_eq(
133            xf.body_to_world_point(xf.world_to_body_point(p)),
134            p,
135            "point round trip w->b->w",
136        );
137        assert_vec_eq(
138            xf.world_to_body_dir(xf.body_to_world_dir(p)),
139            p,
140            "dir round trip b->w->b",
141        );
142        assert_vec_eq(
143            xf.body_to_world_dir(xf.world_to_body_dir(p)),
144            p,
145            "dir round trip w->b->w",
146        );
147    }
148
149    #[test]
150    fn test_frame_conversion_known_rotation() {
151        use std::f64::consts::FRAC_PI_2;
152        // Body frame rotated +90 deg about world Y: body->world is R_y(pi/2),
153        // so the stored world->body rotation is R_y(-pi/2).
154        let pos = Vec3::new(1.0, 2.0, 3.0);
155        let xf = SpatialTransform::new(Mat3::rotation_y(-FRAC_PI_2), pos);
156
157        // R_y(pi/2) * (1,0,0) = (0,0,-1): the body x-axis points down world -z.
158        assert_vec_eq(
159            xf.body_to_world_dir(Vec3::x()),
160            Vec3::new(0.0, 0.0, -1.0),
161            "body x-axis in world",
162        );
163        // Points get the translation on top of the rotation.
164        assert_vec_eq(
165            xf.body_to_world_point(Vec3::x()),
166            Vec3::new(1.0, 2.0, 2.0),
167            "body point (1,0,0) in world",
168        );
169        // The inverse maps agree with hand-computed values.
170        assert_vec_eq(
171            xf.world_to_body_dir(Vec3::new(0.0, 0.0, -1.0)),
172            Vec3::x(),
173            "world -z in body",
174        );
175        assert_vec_eq(
176            xf.world_to_body_point(Vec3::new(1.0, 2.0, 2.0)),
177            Vec3::x(),
178            "world point back to body",
179        );
180    }
181
182    #[test]
183    fn test_point_vs_dir_distinction() {
184        let xf = SpatialTransform::new(Mat3::rotation_z(0.8), Vec3::new(5.0, -4.0, 3.0));
185        let v = Vec3::new(1.0, 2.0, 3.0);
186        // Directions never see the translation.
187        assert_vec_eq(
188            xf.body_to_world_point(v) - xf.body_to_world_dir(v),
189            xf.pos,
190            "point = dir + translation",
191        );
192        assert_vec_eq(
193            xf.body_to_world_dir(Vec3::zero()),
194            Vec3::zero(),
195            "zero dir stays zero",
196        );
197        assert_vec_eq(
198            xf.body_to_world_point(Vec3::zero()),
199            xf.pos,
200            "body origin maps to pos",
201        );
202        assert_vec_eq(
203            xf.world_to_body_point(xf.pos),
204            Vec3::zero(),
205            "pos maps to body origin",
206        );
207    }
208
209    #[test]
210    fn test_spatial_vec_cross_motion() {
211        let v1 = SpatialVec::new(Vec3::new(0.0, 0.0, 1.0), Vec3::zero());
212        let v2 = SpatialVec::new(Vec3::new(1.0, 0.0, 0.0), Vec3::zero());
213        let result = v1.cross_motion(&v2);
214        // [0,0,1] x [1,0,0] = [0,1,0]
215        assert!((result.angular.y - 1.0).abs() < EPS);
216    }
217
218    #[test]
219    fn test_transform_identity() {
220        let xf = SpatialTransform::identity();
221        let v = SpatialVec::new(Vec3::new(1.0, 2.0, 3.0), Vec3::new(4.0, 5.0, 6.0));
222        let result = xf.apply_motion(&v);
223        assert_sv_eq(&result, &v, "identity");
224    }
225
226    #[test]
227    fn test_transform_inverse_roundtrip() {
228        let xf = SpatialTransform::new(Mat3::rotation_z(0.5), Vec3::new(1.0, 2.0, 3.0));
229        let v = SpatialVec::new(Vec3::new(1.0, 0.0, 0.0), Vec3::new(0.0, 1.0, 0.0));
230
231        let forward = xf.apply_motion(&v);
232        let back = xf.inv_apply_motion(&forward);
233        assert_sv_eq(&back, &v, "roundtrip");
234    }
235
236    #[test]
237    fn test_compose_transforms() {
238        let xf1 = SpatialTransform::from_translation(Vec3::new(1.0, 0.0, 0.0));
239        let xf2 = SpatialTransform::from_translation(Vec3::new(0.0, 2.0, 0.0));
240        let composed = xf1.compose(&xf2);
241        assert_vec_eq(composed.pos, Vec3::new(1.0, 2.0, 0.0), "compose pos");
242    }
243
244    #[test]
245    fn test_spatial_inertia_point_mass() {
246        let si = SpatialInertia::point_mass(2.0, Vec3::new(0.0, 1.0, 0.0));
247        let mat = si.to_matrix();
248        // Bottom-right should be 2*I3
249        assert!((mat.lower_right[(0, 0)] - 2.0).abs() < EPS);
250        assert!((mat.lower_right[(1, 1)] - 2.0).abs() < EPS);
251        assert!((mat.lower_right[(2, 2)] - 2.0).abs() < EPS);
252    }
253
254    #[test]
255    fn test_spatial_inertia_sphere() {
256        let si = SpatialInertia::sphere(5.0, 0.1);
257        let expected_i = 2.0 / 5.0 * 5.0 * 0.01;
258        assert!((si.inertia[(0, 0)] - expected_i).abs() < EPS);
259        assert!((si.inertia[(1, 1)] - expected_i).abs() < EPS);
260        assert!((si.inertia[(2, 2)] - expected_i).abs() < EPS);
261    }
262}
263
264#[cfg(test)]
265mod prop_tests {
266    use super::*;
267    use crate::Quat;
268    use crate::{Mat3, Vec3};
269    use proptest::prelude::*;
270
271    const EPS: f64 = 1e-9;
272
273    fn arb_pos() -> impl Strategy<Value = Vec3> {
274        (-10.0..10.0_f64, -10.0..10.0_f64, -10.0..10.0_f64).prop_map(|(x, y, z)| Vec3::new(x, y, z))
275    }
276
277    fn arb_angle() -> impl Strategy<Value = f64> {
278        -std::f64::consts::PI..std::f64::consts::PI
279    }
280
281    fn arb_unit_vec() -> impl Strategy<Value = Vec3> {
282        (-1.0..1.0_f64, -1.0..1.0_f64, -1.0..1.0_f64)
283            .prop_filter("non-zero axis", |(x, y, z)| x * x + y * y + z * z > 0.01)
284            .prop_map(|(x, y, z)| Vec3::new(x, y, z).normalize())
285    }
286
287    fn arb_transform() -> impl Strategy<Value = SpatialTransform> {
288        (arb_unit_vec(), arb_angle(), arb_pos()).prop_map(|(axis, angle, pos)| {
289            let rot = Mat3::rotation_axis(axis, angle);
290            SpatialTransform::new(rot, pos)
291        })
292    }
293
294    fn arb_spatial_vec() -> impl Strategy<Value = SpatialVec> {
295        (arb_pos(), arb_pos()).prop_map(|(a, l)| SpatialVec::new(a, l))
296    }
297
298    proptest! {
299        #[test]
300        fn compose_with_inverse_is_identity(xf in arb_transform()) {
301            let result = xf.compose(&xf.inverse());
302            let id = SpatialTransform::identity();
303            for i in 0..3 {
304                for j in 0..3 {
305                    prop_assert!((result.rot[(i, j)] - id.rot[(i, j)]).abs() < EPS,
306                        "rot[{},{}]: {} vs {}", i, j, result.rot[(i, j)], id.rot[(i, j)]);
307                }
308            }
309            for coord in [result.pos.x, result.pos.y, result.pos.z].iter().zip([id.pos.x, id.pos.y, id.pos.z].iter()) {
310                let (a, b) = coord;
311                prop_assert!((a - b).abs() < EPS, "pos: {} vs {}", a, b);
312            }
313        }
314
315        #[test]
316        fn compose_is_associative(
317            a in arb_transform(),
318            b in arb_transform(),
319            c in arb_transform(),
320        ) {
321            let ab_c = a.compose(&b).compose(&c);
322            let a_bc = a.compose(&b.compose(&c));
323            for i in 0..3 {
324                for j in 0..3 {
325                    prop_assert!((ab_c.rot[(i, j)] - a_bc.rot[(i, j)]).abs() < EPS,
326                        "rot[{},{}]: {} vs {}", i, j, ab_c.rot[(i, j)], a_bc.rot[(i, j)]);
327                }
328            }
329            for coord in [(ab_c.pos.x, a_bc.pos.x), (ab_c.pos.y, a_bc.pos.y), (ab_c.pos.z, a_bc.pos.z)] {
330                let (a, b) = coord;
331                prop_assert!((a - b).abs() < EPS, "pos: {} vs {}", a, b);
332            }
333        }
334
335        #[test]
336        fn apply_force_matches_matrix(xf in arb_transform(), f in arb_spatial_vec()) {
337            let applied = xf.apply_force(&f);
338            let mat_result = xf.to_force_matrix().mul_vec(&f);
339            let a = applied.as_array();
340            let b = mat_result.as_array();
341            for i in 0..6 {
342                prop_assert!((a[i] - b[i]).abs() < EPS,
343                    "component {}: {} vs {}", i, a[i], b[i]);
344            }
345        }
346
347        #[test]
348        fn apply_motion_matches_matrix(xf in arb_transform(), v in arb_spatial_vec()) {
349            let applied = xf.apply_motion(&v);
350            let mat_result = xf.to_motion_matrix().mul_vec(&v);
351            let a = applied.as_array();
352            let b = mat_result.as_array();
353            for i in 0..6 {
354                prop_assert!((a[i] - b[i]).abs() < EPS,
355                    "component {}: {} vs {}", i, a[i], b[i]);
356            }
357        }
358
359        #[test]
360        fn sphere_inertia_matrix_is_symmetric(
361            mass in 0.1..100.0_f64,
362            radius in 0.01..10.0_f64,
363        ) {
364            let si = SpatialInertia::sphere(mass, radius);
365            let mat = si.to_matrix();
366            // Check that upper_right == lower_left^T
367            for i in 0..3 {
368                for j in 0..3 {
369                    prop_assert!((mat.upper_right[(i, j)] - mat.lower_left[(j, i)]).abs() < EPS,
370                        "ur/ll not symmetric at ({},{})", i, j);
371                }
372            }
373            // Check ul and lr symmetric individually
374            for i in 0..3 {
375                for j in 0..3 {
376                    prop_assert!((mat.upper_left[(i, j)] - mat.upper_left[(j, i)]).abs() < EPS,
377                        "ul not symmetric at ({},{})", i, j);
378                    prop_assert!((mat.lower_right[(i, j)] - mat.lower_right[(j, i)]).abs() < EPS,
379                        "lr not symmetric at ({},{})", i, j);
380                }
381            }
382        }
383
384        #[test]
385        fn quat_to_matrix_is_rotation(
386            axis in arb_unit_vec(),
387            angle in arb_angle(),
388        ) {
389            let q = Quat::from_axis_angle(axis, angle).normalize();
390            let m = q.to_matrix();
391            // determinant should be 1
392            let det = m.determinant();
393            prop_assert!((det - 1.0).abs() < EPS, "det = {}", det);
394            // R * R^T should be identity
395            let rrt = m * m.transpose();
396            let id = Mat3::identity();
397            for i in 0..3 {
398                for j in 0..3 {
399                    prop_assert!((rrt[(i, j)] - id[(i, j)]).abs() < EPS,
400                        "R*R^T[{},{}]: {} vs {}", i, j, rrt[(i, j)], id[(i, j)]);
401                }
402            }
403        }
404
405        #[test]
406        fn quat_matrix_roundtrip(
407            axis in arb_unit_vec(),
408            angle in arb_angle(),
409        ) {
410            let q = Quat::from_axis_angle(axis, angle).normalize();
411            let m = q.to_matrix();
412            let q2 = Quat::from_matrix(&m).normalize();
413            // q and -q represent the same rotation
414            let same = (q.w - q2.w).abs() < EPS
415                && (q.v.x - q2.v.x).abs() < EPS
416                && (q.v.y - q2.v.y).abs() < EPS
417                && (q.v.z - q2.v.z).abs() < EPS;
418            let neg = (q.w + q2.w).abs() < EPS
419                && (q.v.x + q2.v.x).abs() < EPS
420                && (q.v.y + q2.v.y).abs() < EPS
421                && (q.v.z + q2.v.z).abs() < EPS;
422            prop_assert!(same || neg,
423                "roundtrip failed: q={:?}, q2={:?}", q, q2);
424        }
425    }
426}
phyz_math/spatial.rs

phyz_math/
spatial.rs
