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
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
// 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,
}
}
/// Creates a transform from pose parts.
///
/// The returned transform maps the reference frame (source) to the local
/// frame (destination). `rotation_local_to_reference` is the local-to-reference
/// rotation; the stored rotation is its inverse because this type stores a
/// source-to-destination rotation. `position_in_reference` becomes the stored
/// translation unchanged. Rotation validity is not checked.
pub fn from_pose_parts(
rotation_local_to_reference: R::Rotation3,
position_in_reference: R::Vector3,
) -> Self {
let rotation = R::rotation3_inverse(&rotation_local_to_reference);
Self::new(rotation, position_in_reference)
}
/// 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
}
/// Returns the destination frame's pose relative to the source frame.
///
/// The source frame is the reference frame and the destination frame is the
/// local frame. The returned pair is `(local-to-reference rotation, local origin in reference coordinates)`.
/// Its rotation is the inverse of the
/// stored source-to-destination rotation, while its position is copied from
/// the stored translation unchanged. Unlike [`Self::inverse`], this conversion
/// does not change the translation.
///
/// # Examples
///
/// ```
/// # #[cfg(feature = "builtin")]
/// # {
/// use spatial6::SpatialTransform;
///
/// let transform = SpatialTransform::<f64>::new(
/// [[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]],
/// [1.0, 2.0, 3.0],
/// );
/// let (rotation_local_to_reference, position_in_reference) =
/// transform.to_pose_parts();
/// assert_eq!(
/// rotation_local_to_reference,
/// [[0.0, 1.0, 0.0], [-1.0, 0.0, 0.0], [0.0, 0.0, 1.0]],
/// );
/// assert_eq!(position_in_reference, [1.0, 2.0, 3.0]);
/// let restored = SpatialTransform::<f64>::from_pose_parts(
/// rotation_local_to_reference,
/// position_in_reference,
/// );
/// assert_eq!(restored, transform);
/// # }
/// ```
pub fn to_pose_parts(&self) -> (R::Rotation3, R::Vector3) {
(R::rotation3_inverse(&self.rotation), 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),
),
)
}
}