1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
// SPDX-FileCopyrightText: 2026 Daisuke Nagao
//
// SPDX-License-Identifier: MIT OR Apache-2.0
#[cfg(feature = "builtin")]
use crate::Builtin;
use crate::vector::{ForceVector, MotionVector, SpatialMatrix};
use crate::{SpatialRepresentation, SpatialScalar};
macro_rules! define_spatial_transform {
($($generics:tt)*) => {
/// A compact spatial coordinate transform from a source frame to a destination frame.
///
/// Applying the transform (see [`Self::transform_motion`]/[`Self::transform_force`])
/// re-expresses a vector given in source-frame coordinates as the same physical
/// quantity in destination-frame coordinates.
///
/// The translation is the vector from the source origin to the destination origin,
/// expressed in source coordinates. If `E` is the stored rotation and `r` is the
/// stored translation, the motion and force matrices are respectively
/// `[[E, 0], [-E[r]×, E]]` and `[[E, -E[r]×], [0, E]]`.
///
/// `rotation` must be a proper orthogonal rotation, per the invariant on
/// [`SpatialRepresentation::Rotation3`]; this is not validated.
#[derive(Clone, Copy, Debug, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[cfg_attr(
feature = "serde",
serde(bound(
serialize = "R::Rotation3: serde::Serialize, R::Vector3: serde::Serialize",
deserialize = "R::Rotation3: serde::Deserialize<'de>, R::Vector3: serde::Deserialize<'de>"
))
)]
pub struct SpatialTransform<$($generics)*> {
rotation: R::Rotation3,
translation: R::Vector3,
}
};
}
#[cfg(feature = "builtin")]
define_spatial_transform!(T: SpatialScalar = f64, R: SpatialRepresentation<T> = Builtin);
#[cfg(not(feature = "builtin"))]
define_spatial_transform!(T: SpatialScalar, R: SpatialRepresentation<T>);
impl<T, R> SpatialTransform<T, R>
where
T: SpatialScalar,
R: SpatialRepresentation<T>,
{
/// Creates a transform from a source frame to a destination frame.
///
/// `rotation` is the source-to-destination rotation and `translation` is the
/// source-coordinate vector from the source origin to the destination origin.
/// Neither is validated; see the struct-level invariant on `rotation`.
pub fn new(rotation: R::Rotation3, translation: R::Vector3) -> Self {
Self {
rotation,
translation,
}
}
/// Returns the identity transform.
pub fn identity() -> Self {
Self::new(R::rotation3_identity(), R::vector3_zero())
}
/// Returns the source-to-destination rotation.
pub fn rotation(&self) -> &R::Rotation3 {
&self.rotation
}
/// Returns the source-coordinate vector from the source origin to the destination origin.
pub fn translation(&self) -> &R::Vector3 {
&self.translation
}
/// Re-expresses a source-frame motion vector in the destination frame.
///
/// Equivalent to `self.motion_matrix() * motion` but computed directly,
/// without materializing the matrix.
pub fn transform_motion(&self, motion: &MotionVector<T, R>) -> MotionVector<T, R> {
MotionVector::new(
R::rotation3_transform_vector(&self.rotation, &motion.angular),
R::rotation3_transform_vector(
&self.rotation,
&R::vector3_sub(
&motion.linear,
&R::vector3_cross(&self.translation, &motion.angular),
),
),
)
}
/// Re-expresses a source-frame force vector in the destination frame.
///
/// Equivalent to `self.force_matrix() * force` but computed directly,
/// without materializing the matrix. This is the dual of
/// [`Self::transform_motion`]: it preserves the motion/force dot product,
/// i.e. `self.transform_motion(m).dot(&self.transform_force(f)) == m.dot(&f)`.
pub fn transform_force(&self, force: &ForceVector<T, R>) -> ForceVector<T, R> {
ForceVector::new(
R::rotation3_transform_vector(
&self.rotation,
&R::vector3_sub(
&force.moment,
&R::vector3_cross(&self.translation, &force.force),
),
),
R::rotation3_transform_vector(&self.rotation, &force.force),
)
}
/// Returns the 6x6 matrix form of [`Self::transform_motion`].
pub fn motion_matrix(&self) -> SpatialMatrix<T, R> {
let rotation = R::rotation3_to_matrix3(&self.rotation);
let zero = R::matrix3_zero();
let rotation_cross = R::matrix3_mul(&rotation, &R::vector3_cross_matrix(&self.translation));
let lower_left = R::matrix3_sub(&zero, &rotation_cross);
R::matrix6_from_blocks(&rotation, &zero, &lower_left, &rotation)
}
/// Returns the 6x6 matrix form of [`Self::transform_force`].
pub fn force_matrix(&self) -> SpatialMatrix<T, R> {
let rotation = R::rotation3_to_matrix3(&self.rotation);
let zero = R::matrix3_zero();
let rotation_cross = R::matrix3_mul(&rotation, &R::vector3_cross_matrix(&self.translation));
let upper_right = R::matrix3_sub(&zero, &rotation_cross);
R::matrix6_from_blocks(&rotation, &upper_right, &zero, &rotation)
}
/// Returns the transform from the destination frame back to the source frame.
///
/// `self.inverse().transform_motion(&self.transform_motion(&m)) == m` for any
/// motion vector `m` (and likewise for [`Self::transform_force`]), up to
/// floating-point rounding.
pub fn inverse(&self) -> Self {
let rotation = R::rotation3_inverse(&self.rotation);
let translation = R::vector3_neg(&R::rotation3_transform_vector(
&self.rotation,
&self.translation,
));
Self::new(rotation, translation)
}
/// Composes this transform with `next`, applying `self` and then `next`.
///
/// `next`'s source frame must be `self`'s destination frame; the result maps
/// `self`'s source frame directly to `next`'s destination frame:
/// `self.then(next).transform_motion(&m) == next.transform_motion(&self.transform_motion(&m))`.
pub fn then(&self, next: &Self) -> Self {
let rotation = R::rotation3_inverse(&self.rotation);
Self::new(
R::rotation3_mul(&next.rotation, &self.rotation),
R::vector3_add(
&self.translation,
&R::rotation3_transform_vector(&rotation, &next.translation),
),
)
}
}