1#![no_std]
2#![forbid(unsafe_code)]
3#![allow(
7 clippy::needless_range_loop,
8 clippy::manual_swap,
9 clippy::suspicious_arithmetic_impl,
10 clippy::suspicious_op_assign_impl
11)]
12extern 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
76pub 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#[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#[derive(Clone, Copy, Debug, PartialEq, Eq)]
105pub struct Motion;
106#[derive(Clone, Copy, Debug, PartialEq, Eq)]
108pub struct Force;
109
110#[derive(Clone, Copy, Debug, PartialEq, Eq)]
119pub struct SpatialVector<T, K> {
120 pub data: Vector6<T>,
123 _marker: PhantomData<K>,
124}
125
126pub type MotionVector<T> = SpatialVector<T, Motion>;
128pub type ForceVector<T> = SpatialVector<T, Force>;
130
131impl<T: Scalar + Copy> SpatialVector<T, Motion> {
132 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 pub fn cross(&self, rhs: &SpatialVector<T, Motion>) -> SpatialVector<T, Motion> {
149 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 pub fn cross_force(&self, rhs: &SpatialVector<T, Force>) -> SpatialVector<T, Force> {
161 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 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 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 pub fn cross_motion(&self, rhs: &SpatialVector<T, Motion>) -> SpatialVector<T, Force> {
203 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 pub fn cross(&self, rhs: &SpatialVector<T, Force>) -> SpatialVector<T, Motion> {
215 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 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
238impl<T: Scalar + Copy, K> SpatialVector<T, K> {
240 pub fn angular(&self) -> Vector3<T> {
242 Vector3::new([self.data.data[0], self.data.data[1], self.data.data[2]])
243 }
244
245 pub fn linear(&self) -> Vector3<T> {
247 Vector3::new([self.data.data[3], self.data.data[4], self.data.data[5]])
248 }
249
250 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 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
310pub 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
340pub 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#[derive(Clone, Copy, Debug, PartialEq, Eq)]
374pub struct DualQuaternion<T> {
375 pub real: Quaternion<T>,
377 pub dual: Quaternion<T>,
379}
380
381impl<T: Scalar> DualQuaternion<T> {
382 pub fn new(real: Quaternion<T>, dual: Quaternion<T>) -> Self {
384 DualQuaternion { real, dual }
385 }
386
387 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 pub fn conjugate(&self) -> Self {
397 DualQuaternion {
398 real: self.real.conjugate(),
399 dual: self.dual.conjugate(),
400 }
401 }
402
403 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 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 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 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#[derive(Clone, Copy, Debug, PartialEq, Eq)]
467pub struct Screw<T> {
468 pub angular: Vector3<T>,
470 pub linear: Vector3<T>,
472}
473
474impl<T: Scalar + Copy> Screw<T> {
475 pub fn new(angular: Vector3<T>, linear: Vector3<T>) -> Self {
477 Screw { angular, linear }
478 }
479
480 pub fn as_motion(&self) -> MotionVector<T> {
482 MotionVector::new(self.angular, self.linear)
483 }
484
485 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); let aav = axis.cross(&av); 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 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 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 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); let aap = axis.cross(&ap); 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#[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 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 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 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 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 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 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 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 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])); let expected_top = Vector3::new([10.0, 0.0, 0.0]).cross(&rfv); 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 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 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 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 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 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 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 assert!((screw.angular.norm() - core::f64::consts::PI).abs() < 1e-7);
965 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 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 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 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 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}