1#[derive(Debug, Clone, Copy, PartialEq)]
10#[repr(C)]
11pub struct Motor {
12 pub r_w: f64,
13 pub r_x: f64,
14 pub r_y: f64,
15 pub r_z: f64,
16 pub d_w: f64,
17 pub d_x: f64,
18 pub d_y: f64,
19 pub d_z: f64,
20}
21
22#[derive(Debug, Clone, Copy, PartialEq)]
24pub struct MotorBivector {
25 pub rx: f64,
26 pub ry: f64,
27 pub rz: f64,
28 pub dx: f64,
29 pub dy: f64,
30 pub dz: f64,
31}
32
33impl Motor {
34 pub const fn identity() -> Self {
36 Self {
37 r_w: 1.0,
38 r_x: 0.0,
39 r_y: 0.0,
40 r_z: 0.0,
41 d_w: 0.0,
42 d_x: 0.0,
43 d_y: 0.0,
44 d_z: 0.0,
45 }
46 }
47
48 pub fn from_rotation_translation(rot: [f64; 4], trans: [f64; 3]) -> Self {
50 let (rw, rx, ry, rz) = (rot[0], rot[1], rot[2], rot[3]);
51 let (tx, ty, tz) = (trans[0], trans[1], trans[2]);
52
53 let dw = -0.5 * (tx * rx + ty * ry + tz * rz);
55 let dx = 0.5 * (tx * rw + ty * rz - tz * ry);
56 let dy = 0.5 * (-tx * rz + ty * rw + tz * rx);
57 let dz = 0.5 * (tx * ry - ty * rx + tz * rw);
58
59 Self {
60 r_w: rw,
61 r_x: rx,
62 r_y: ry,
63 r_z: rz,
64 d_w: dw,
65 d_x: dx,
66 d_y: dy,
67 d_z: dz,
68 }
69 .normalize()
70 }
71
72 pub fn from_translation(tx: f64, ty: f64, tz: f64) -> Self {
74 Self::from_rotation_translation([1.0, 0.0, 0.0, 0.0], [tx, ty, tz])
75 }
76 pub fn to_rotation_translation(&self) -> ([f64; 4], [f64; 3]) {
78 let rot = [self.r_w, self.r_x, self.r_y, self.r_z];
79 let tx = 2.0
81 * (self.d_x * self.r_w - self.d_w * self.r_x + self.d_z * self.r_y
82 - self.d_y * self.r_z);
83 let ty = 2.0
84 * (self.d_y * self.r_w - self.d_z * self.r_x - self.d_w * self.r_y
85 + self.d_x * self.r_z);
86 let tz = 2.0
87 * (self.d_z * self.r_w + self.d_y * self.r_x
88 - self.d_x * self.r_y
89 - self.d_w * self.r_z);
90 let trans = [tx, ty, tz];
91 (rot, trans)
92 }
93
94 pub fn mul(&self, other: &Self) -> Self {
96 let rw = self.r_w * other.r_w
98 - self.r_x * other.r_x
99 - self.r_y * other.r_y
100 - self.r_z * other.r_z;
101 let rx = self.r_w * other.r_x + self.r_x * other.r_w + self.r_y * other.r_z
102 - self.r_z * other.r_y;
103 let ry = self.r_w * other.r_y - self.r_x * other.r_z
104 + self.r_y * other.r_w
105 + self.r_z * other.r_x;
106 let rz = self.r_w * other.r_z + self.r_x * other.r_y - self.r_y * other.r_x
107 + self.r_z * other.r_w;
108
109 let dw = self.r_w * other.d_w
111 - self.r_x * other.d_x
112 - self.r_y * other.d_y
113 - self.r_z * other.d_z
114 + self.d_w * other.r_w
115 - self.d_x * other.r_x
116 - self.d_y * other.r_y
117 - self.d_z * other.r_z;
118
119 let dx = self.r_w * other.d_x + self.r_x * other.d_w + self.r_y * other.d_z
120 - self.r_z * other.d_y
121 + self.d_w * other.r_x
122 + self.d_x * other.r_w
123 + self.d_y * other.r_z
124 - self.d_z * other.r_y;
125
126 let dy = self.r_w * other.d_y - self.r_x * other.d_z
127 + self.r_y * other.d_w
128 + self.r_z * other.d_x
129 + self.d_w * other.r_y
130 - self.d_x * other.r_z
131 + self.d_y * other.r_w
132 + self.d_z * other.r_x;
133
134 let dz = self.r_w * other.d_z + self.r_x * other.d_y - self.r_y * other.d_x
135 + self.r_z * other.d_w
136 + self.d_w * other.r_z
137 + self.d_x * other.r_y
138 - self.d_y * other.r_x
139 + self.d_z * other.r_w;
140
141 Self {
142 r_w: rw,
143 r_x: rx,
144 r_y: ry,
145 r_z: rz,
146 d_w: dw,
147 d_x: dx,
148 d_y: dy,
149 d_z: dz,
150 }
151 }
152
153 pub fn conjugate(&self) -> Self {
155 Self {
156 r_w: self.r_w,
157 r_x: -self.r_x,
158 r_y: -self.r_y,
159 r_z: -self.r_z,
160 d_w: self.d_w,
161 d_x: -self.d_x,
162 d_y: -self.d_y,
163 d_z: -self.d_z,
164 }
165 }
166
167 pub fn normalize(&self) -> Self {
169 let mag_r_sq =
170 self.r_w * self.r_w + self.r_x * self.r_x + self.r_y * self.r_y + self.r_z * self.r_z;
171 if mag_r_sq < 1e-12 {
172 return Self::identity();
173 }
174 let inv_mag_r = 1.0 / mag_r_sq.sqrt();
175 let dot_rd =
176 self.r_w * self.d_w + self.r_x * self.d_x + self.r_y * self.d_y + self.r_z * self.d_z;
177
178 let rw = self.r_w * inv_mag_r;
179 let rx = self.r_x * inv_mag_r;
180 let ry = self.r_y * inv_mag_r;
181 let rz = self.r_z * inv_mag_r;
182
183 let dw = (self.d_w - rw * dot_rd) * inv_mag_r;
184 let dx = (self.d_x - rx * dot_rd) * inv_mag_r;
185 let dy = (self.d_y - ry * dot_rd) * inv_mag_r;
186 let dz = (self.d_z - rz * dot_rd) * inv_mag_r;
187
188 Self {
189 r_w: rw,
190 r_x: rx,
191 r_y: ry,
192 r_z: rz,
193 d_w: dw,
194 d_x: dx,
195 d_y: dy,
196 d_z: dz,
197 }
198 }
199
200 pub fn log(&self) -> MotorBivector {
202 let m = if self.r_w < 0.0 {
203 Self {
204 r_w: -self.r_w,
205 r_x: -self.r_x,
206 r_y: -self.r_y,
207 r_z: -self.r_z,
208 d_w: -self.d_w,
209 d_x: -self.d_x,
210 d_y: -self.d_y,
211 d_z: -self.d_z,
212 }
213 } else {
214 *self
215 };
216
217 let sin_sq = m.r_x * m.r_x + m.r_y * m.r_y + m.r_z * m.r_z;
218 if sin_sq < 1e-12 {
219 return MotorBivector {
221 rx: 0.0,
222 ry: 0.0,
223 rz: 0.0,
224 dx: m.d_x,
225 dy: m.d_y,
226 dz: m.d_z,
227 };
228 }
229
230 let sin_theta = sin_sq.sqrt();
231 let theta = sin_theta.atan2(m.r_w);
232 let inv_sin = 1.0 / sin_theta;
233
234 let axis_x = m.r_x * inv_sin;
235 let axis_y = m.r_y * inv_sin;
236 let axis_z = m.r_z * inv_sin;
237
238 let pitch = -m.d_w * inv_sin;
239
240 let rx = theta * axis_x;
241 let ry = theta * axis_y;
242 let rz = theta * axis_z;
243
244 let dx = m.d_x - pitch * m.r_x;
245 let dy = m.d_y - pitch * m.r_y;
246 let dz = m.d_z - pitch * m.r_z;
247
248 MotorBivector {
249 rx,
250 ry,
251 rz,
252 dx,
253 dy,
254 dz,
255 }
256 }
257
258 pub fn sclerp(m0: &Self, m1: &Self, t: f64) -> Self {
261 if t <= 0.0 {
262 return *m0;
263 }
264 if t >= 1.0 {
265 return *m1;
266 }
267
268 let m0_inv = m0.conjugate();
270 let mut delta = m0_inv.mul(m1);
271
272 if delta.r_w < 0.0 {
274 delta = Self {
275 r_w: -delta.r_w,
276 r_x: -delta.r_x,
277 r_y: -delta.r_y,
278 r_z: -delta.r_z,
279 d_w: -delta.d_w,
280 d_x: -delta.d_x,
281 d_y: -delta.d_y,
282 d_z: -delta.d_z,
283 };
284 }
285
286 let bivector = delta.log();
287 let scaled_bivector = MotorBivector {
288 rx: bivector.rx * t,
289 ry: bivector.ry * t,
290 rz: bivector.rz * t,
291 dx: bivector.dx * t,
292 dy: bivector.dy * t,
293 dz: bivector.dz * t,
294 };
295
296 let delta_t = scaled_bivector.exp();
297 m0.mul(&delta_t).normalize()
298 }
299
300 pub fn transform_point(&self, p: [f64; 3]) -> [f64; 3] {
302 let (rot, trans) = self.to_rotation_translation();
303 let (rw, rx, ry, rz) = (rot[0], rot[1], rot[2], rot[3]);
304 let (px, py, pz) = (p[0], p[1], p[2]);
305
306 let ix = rw * px + ry * pz - rz * py;
308 let iy = rw * py - rx * pz + rz * px;
309 let iz = rw * pz + rx * py - ry * px;
310 let iw = -rx * px - ry * py - rz * pz;
311
312 let rx_out = ix * rw + iw * -rx + iy * -rz - iz * -ry;
313 let ry_out = iy * rw + iw * -ry + iz * -rx - ix * -rz;
314 let rz_out = iz * rw + iw * -rz + ix * -ry - iy * -rx;
315
316 [rx_out + trans[0], ry_out + trans[1], rz_out + trans[2]]
317 }
318}
319
320impl MotorBivector {
321 pub fn exp(&self) -> Motor {
323 let theta_sq = self.rx * self.rx + self.ry * self.ry + self.rz * self.rz;
324 if theta_sq < 1e-12 {
325 return Motor {
327 r_w: 1.0,
328 r_x: 0.0,
329 r_y: 0.0,
330 r_z: 0.0,
331 d_w: 0.0,
332 d_x: self.dx,
333 d_y: self.dy,
334 d_z: self.dz,
335 };
336 }
337
338 let theta = theta_sq.sqrt();
339 let sin_theta = theta.sin();
340 let cos_theta = theta.cos();
341 let inv_theta = 1.0 / theta;
342
343 let rw = cos_theta;
344 let rx = self.rx * inv_theta * sin_theta;
345 let ry = self.ry * inv_theta * sin_theta;
346 let rz = self.rz * inv_theta * sin_theta;
347
348 let dw = 0.0;
349 let dx = self.dx;
350 let dy = self.dy;
351 let dz = self.dz;
352
353 Motor {
354 r_w: rw,
355 r_x: rx,
356 r_y: ry,
357 r_z: rz,
358 d_w: dw,
359 d_x: dx,
360 d_y: dy,
361 d_z: dz,
362 }
363 .normalize()
364 }
365}
366
367#[cfg(test)]
368mod tests {
369 use super::*;
370
371 #[test]
372 fn motor_identity_transforms_point_invariant() {
373 let m = Motor::identity();
374 let p = [10.0, -5.0, 3.5];
375 let p_out = m.transform_point(p);
376 for i in 0..3 {
377 assert!((p[i] - p_out[i]).abs() < 1e-6);
378 }
379 }
380
381 #[test]
382 fn motor_pure_translation() {
383 let m = Motor::from_rotation_translation([1.0, 0.0, 0.0, 0.0], [5.0, 10.0, -2.0]);
384 let p = [1.0, 2.0, 3.0];
385 let p_out = m.transform_point(p);
386 assert!((p_out[0] - 6.0).abs() < 1e-5);
387 assert!((p_out[1] - 12.0).abs() < 1e-5);
388 assert!((p_out[2] - 1.0).abs() < 1e-5);
389 }
390
391 #[test]
392 fn sclerp_halfway_interpolation() {
393 let m0 = Motor::identity();
394 let m1 = Motor::from_rotation_translation([1.0, 0.0, 0.0, 0.0], [10.0, 20.0, 30.0]);
395 let mid = Motor::sclerp(&m0, &m1, 0.5);
396
397 let (_, trans) = mid.to_rotation_translation();
398 assert!((trans[0] - 5.0).abs() < 1e-4);
399 assert!((trans[1] - 10.0).abs() < 1e-4);
400 assert!((trans[2] - 15.0).abs() < 1e-4);
401 }
402}