#![no_std]
#![forbid(unsafe_code)]
#![allow(
clippy::needless_range_loop,
clippy::manual_swap,
clippy::suspicious_arithmetic_impl,
clippy::suspicious_op_assign_impl
)]
extern crate alloc;
use core::fmt;
use core::marker::PhantomData;
use core::ops::{Add, Mul, Neg, Sub};
use tpt_math_geometry::{Isometry3, Quaternion, Rotation3, Translation, UnitQuaternion};
use tpt_math_linalg_fixed::{Matrix, Matrix3, Vector3, Vector6};
use tpt_math_numeric::Scalar;
pub type Matrix6<T> = Matrix<T, 6, 6>;
#[inline]
fn c<T: Scalar>(x: f64) -> T {
T::from(x).unwrap()
}
#[inline]
fn two<T: Scalar>() -> T {
T::one() + T::one()
}
#[inline]
fn skew<T: Scalar + Copy>(v: &Vector3<T>) -> Matrix3<T> {
Matrix3::new([
[T::zero(), -v.z(), v.y()],
[v.z(), T::zero(), -v.x()],
[-v.y(), v.x(), T::zero()],
])
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub struct Motion;
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub struct Force;
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub struct SpatialVector<T, K> {
pub data: Vector6<T>,
_marker: PhantomData<K>,
}
pub type MotionVector<T> = SpatialVector<T, Motion>;
pub type ForceVector<T> = SpatialVector<T, Force>;
impl<T: Scalar + Copy> SpatialVector<T, Motion> {
pub fn new(angular: Vector3<T>, linear: Vector3<T>) -> Self {
SpatialVector {
data: Vector6::new([
angular.x(),
angular.y(),
angular.z(),
linear.x(),
linear.y(),
linear.z(),
]),
_marker: PhantomData,
}
}
pub fn cross(&self, rhs: &SpatialVector<T, Motion>) -> SpatialVector<T, Motion> {
let mw = self.angular();
let mv = self.linear();
let nw = rhs.angular();
let nv = rhs.linear();
let top = mw.cross(&nw);
let bot = mw.cross(&nv) + mv.cross(&nw);
Self::new(top, bot)
}
pub fn cross_force(&self, rhs: &SpatialVector<T, Force>) -> SpatialVector<T, Force> {
let mw = self.angular();
let mv = self.linear();
let fw = rhs.angular();
let fv = rhs.linear();
let top = mw.cross(&fw) + mv.cross(&fv);
let bot = mw.cross(&fv);
ForceVector::new(top, bot)
}
pub fn transform_by(&self, iso: &Isometry3<T>) -> Self {
SpatialVector {
data: adjoint_motion(iso) * self.data,
_marker: PhantomData,
}
}
}
impl<T: Scalar + Copy> SpatialVector<T, Force> {
pub fn new(angular: Vector3<T>, linear: Vector3<T>) -> Self {
SpatialVector {
data: Vector6::new([
angular.x(),
angular.y(),
angular.z(),
linear.x(),
linear.y(),
linear.z(),
]),
_marker: PhantomData,
}
}
pub fn cross_motion(&self, rhs: &SpatialVector<T, Motion>) -> SpatialVector<T, Force> {
let fw = self.angular();
let fv = self.linear();
let mw = rhs.angular();
let mv = rhs.linear();
let top = fw.cross(&mw);
let bot = fw.cross(&mv) + fv.cross(&mw);
Self::new(top, bot)
}
pub fn cross(&self, rhs: &SpatialVector<T, Force>) -> SpatialVector<T, Motion> {
let fw = self.angular();
let fv = self.linear();
let gw = rhs.angular();
let gv = rhs.linear();
let top = fw.cross(&gv) + fv.cross(&gw);
let bot = fv.cross(&gw);
MotionVector::new(top, bot)
}
pub fn transform_by(&self, iso: &Isometry3<T>) -> Self {
SpatialVector {
data: adjoint_force(iso) * self.data,
_marker: PhantomData,
}
}
}
impl<T: Scalar + Copy, K> SpatialVector<T, K> {
pub fn angular(&self) -> Vector3<T> {
Vector3::new([self.data.data[0], self.data.data[1], self.data.data[2]])
}
pub fn linear(&self) -> Vector3<T> {
Vector3::new([self.data.data[3], self.data.data[4], self.data.data[5]])
}
pub fn as_vector(&self) -> &Vector6<T> {
&self.data
}
}
impl<T: Scalar + Copy, K> Add for SpatialVector<T, K> {
type Output = Self;
fn add(self, rhs: Self) -> Self {
SpatialVector {
data: self.data + rhs.data,
_marker: PhantomData,
}
}
}
impl<T: Scalar + Copy, K> Sub for SpatialVector<T, K> {
type Output = Self;
fn sub(self, rhs: Self) -> Self {
SpatialVector {
data: self.data - rhs.data,
_marker: PhantomData,
}
}
}
impl<T: Scalar + Copy, K> Neg for SpatialVector<T, K> {
type Output = Self;
fn neg(self) -> Self {
SpatialVector {
data: -self.data,
_marker: PhantomData,
}
}
}
impl<T: Scalar + Copy, K> Mul<T> for SpatialVector<T, K> {
type Output = Self;
fn mul(self, rhs: T) -> Self {
SpatialVector {
data: self.data * rhs,
_marker: PhantomData,
}
}
}
impl<T: Scalar + Copy + fmt::Debug, K> fmt::Display for SpatialVector<T, K> {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
write!(
f,
"[ω: {:?}; v: {:?}]",
self.angular().data,
self.linear().data
)
}
}
pub fn adjoint_motion<T: Scalar + Copy>(iso: &Isometry3<T>) -> Matrix6<T> {
let r = *iso.rotation.matrix();
let t = iso.translation.vector;
let tl = skew(&t) * r;
Matrix6::from_fn(|i, j| {
if i < 3 && j < 3 {
r.data[i][j]
} else if i >= 3 && j < 3 {
tl.data[i - 3][j]
} else if i >= 3 && j >= 3 {
r.data[i - 3][j - 3]
} else {
T::zero()
}
})
}
pub fn adjoint_force<T: Scalar + Copy>(iso: &Isometry3<T>) -> Matrix6<T> {
let r = *iso.rotation.matrix();
let t = iso.translation.vector;
let tr = skew(&t) * r;
Matrix6::from_fn(|i, j| {
if i < 3 && j < 3 {
r.data[i][j]
} else if i < 3 && j >= 3 {
tr.data[i][j - 3]
} else if i >= 3 && j >= 3 {
r.data[i - 3][j - 3]
} else {
T::zero()
}
})
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub struct DualQuaternion<T> {
pub real: Quaternion<T>,
pub dual: Quaternion<T>,
}
impl<T: Scalar> DualQuaternion<T> {
pub fn new(real: Quaternion<T>, dual: Quaternion<T>) -> Self {
DualQuaternion { real, dual }
}
pub fn from_real(real: Quaternion<T>) -> Self {
DualQuaternion {
real,
dual: Quaternion::new(T::zero(), T::zero(), T::zero(), T::zero()),
}
}
pub fn conjugate(&self) -> Self {
DualQuaternion {
real: self.real.conjugate(),
dual: self.dual.conjugate(),
}
}
pub fn normalize(&self) -> Option<Self> {
let n = self.real.norm();
if n == T::zero() {
return None;
}
let inv = T::one() / n;
Some(DualQuaternion {
real: self.real * inv,
dual: self.dual * inv,
})
}
pub fn multiply(&self, rhs: &DualQuaternion<T>) -> DualQuaternion<T> {
DualQuaternion {
real: self.real.multiply(&rhs.real),
dual: self.real.multiply(&rhs.dual) + self.dual.multiply(&rhs.real),
}
}
pub fn from_isometry(iso: &Isometry3<T>) -> Self {
let q = *UnitQuaternion::<T>::from_rotation_matrix(&iso.rotation).quaternion();
let t = iso.translation.vector;
let tq = Quaternion::new(t.x(), t.y(), t.z(), T::zero());
let dual = tq.multiply(&q) * (T::one() / (T::one() + T::one()));
DualQuaternion { real: q, dual }
}
pub fn to_isometry(&self) -> Isometry3<T> {
let real = self.real.normalize().unwrap_or(self.real);
let rotation = Rotation3::from_quaternion(&UnitQuaternion::from_quaternion(real));
let tq = self.dual.multiply(&real.conjugate()) * (T::one() + T::one());
let translation = Translation::new(Vector3::new([
tq.coords.data[0],
tq.coords.data[1],
tq.coords.data[2],
]));
Isometry3::new(translation, rotation)
}
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub struct Screw<T> {
pub angular: Vector3<T>,
pub linear: Vector3<T>,
}
impl<T: Scalar + Copy> Screw<T> {
pub fn new(angular: Vector3<T>, linear: Vector3<T>) -> Self {
Screw { angular, linear }
}
pub fn as_motion(&self) -> MotionVector<T> {
MotionVector::new(self.angular, self.linear)
}
pub fn exp(&self) -> Isometry3<T> {
let w = self.angular;
let v = self.linear;
let theta = w.norm();
let eps = c::<T>(1e-12);
let rotation = if theta < eps {
Rotation3::identity()
} else {
let axis = w / theta;
Rotation3::from_axis_angle(&axis, theta)
};
let p = if theta < eps {
v
} else {
let axis = w / theta;
let vl = v / theta;
let av = axis.cross(&vl); let aav = axis.cross(&av); let (s, cth) = theta.sin_cos();
let term1 = vl * theta;
let term2 = av * (T::one() - cth);
let term3 = aav * (theta - s);
term1 + term2 + term3
};
Isometry3::new(Translation::new(p), rotation)
}
pub fn log(iso: &Isometry3<T>) -> Screw<T> {
let m = iso.rotation.matrix().data;
let tr = m[0][0] + m[1][1] + m[2][2];
let cos_t = (tr - T::one()) / two();
let skew = Vector3::new([m[2][1] - m[1][2], m[0][2] - m[2][0], m[1][0] - m[0][1]]);
let sin_t = skew.dot(&skew).sqrt() / two();
let theta = sin_t.atan2(cos_t);
let eps = c::<T>(1e-12);
if theta < eps {
return Screw::new(
Vector3::new([T::zero(), T::zero(), T::zero()]),
iso.translation.vector,
);
}
let two_ = two::<T>();
let pi = c::<T>(core::f64::consts::PI);
let axis = if theta > pi - c::<T>(1e-3) {
let rpi = [
[m[0][0] + T::one(), m[0][1], m[0][2]],
[m[1][0], m[1][1] + T::one(), m[1][2]],
[m[2][0], m[2][1], m[2][2] + T::one()],
];
let n0 = rpi[0][0] * rpi[0][0] + rpi[1][0] * rpi[1][0] + rpi[2][0] * rpi[2][0];
let n1 = rpi[0][1] * rpi[0][1] + rpi[1][1] * rpi[1][1] + rpi[2][1] * rpi[2][1];
let n2 = rpi[0][2] * rpi[0][2] + rpi[1][2] * rpi[1][2] + rpi[2][2] * rpi[2][2];
let (col, norm) = if n0 >= n1 && n0 >= n2 {
([rpi[0][0], rpi[1][0], rpi[2][0]], n0)
} else if n1 >= n2 {
([rpi[0][1], rpi[1][1], rpi[2][1]], n1)
} else {
([rpi[0][2], rpi[1][2], rpi[2][2]], n2)
};
let norm = norm.sqrt();
let norm = if norm == T::zero() { T::one() } else { norm };
let ax = Vector3::new([col[0] / norm, col[1] / norm, col[2] / norm]);
if ax.dot(&skew) < T::zero() {
-ax
} else {
ax
}
} else {
let s = sin_t;
Vector3::new([
(m[2][1] - m[1][2]) / (two_ * s),
(m[0][2] - m[2][0]) / (two_ * s),
(m[1][0] - m[0][1]) / (two_ * s),
])
};
let p = iso.translation.vector;
let ap = axis.cross(&p); let aap = axis.cross(&ap); let half = theta / two_;
let cot_half = half.cos() / half.sin();
let coef = (T::one() / theta) - (cot_half / two_);
let vl = (p / theta) - (ap * (T::one() / two_)) + (aap * coef);
let angular = axis * theta;
let linear = vl * theta;
Screw::new(angular, linear)
}
}
#[cfg(test)]
mod tests {
use super::*;
use tpt_math_geometry::Point3;
type MVec = MotionVector<f64>;
type FVec = ForceVector<f64>;
#[inline]
fn close(a: f64, b: f64) -> bool {
(a - b).abs() < 1e-10
}
fn iso_z90() -> Isometry3<f64> {
let rot = Rotation3::from_axis_angle(
&Vector3::new([0.0, 0.0, 1.0]),
core::f64::consts::FRAC_PI_2,
);
Isometry3::new(Translation::new(Vector3::new([0.0, 0.0, 0.0])), rot)
}
#[test]
fn spatial_vector_accessors_and_arithmetic() {
let a = MVec::new(Vector3::new([1.0, 2.0, 3.0]), Vector3::new([4.0, 5.0, 6.0]));
assert!(close(a.angular().x(), 1.0));
assert!(close(a.linear().z(), 6.0));
let b = MVec::new(Vector3::new([0.0, 0.0, 0.0]), Vector3::new([1.0, 1.0, 1.0]));
let s = a + b;
assert!(close(s.linear().x(), 5.0));
let n = -a;
assert!(close(n.angular().x(), -1.0));
}
#[test]
fn cross_motion_motion_known() {
let m = MVec::new(Vector3::new([0.0, 0.0, 1.0]), Vector3::new([0.0, 0.0, 0.0]));
let n = MVec::new(Vector3::new([0.0, 0.0, 0.0]), Vector3::new([1.0, 0.0, 0.0]));
let r = m.cross(&n);
assert!(close(r.angular().norm(), 0.0));
assert!(close(r.linear().x(), 0.0));
assert!(close(r.linear().y(), 1.0));
assert!(close(r.linear().z(), 0.0));
}
#[test]
fn cross_motion_force_known() {
let m = MVec::new(Vector3::new([0.0, 0.0, 1.0]), Vector3::new([0.0, 0.0, 0.0]));
let f = FVec::new(Vector3::new([0.0, 0.0, 0.0]), Vector3::new([1.0, 0.0, 0.0]));
let r = m.cross_force(&f);
assert!(close(r.angular().norm(), 0.0));
assert!(close(r.linear().x(), 0.0));
assert!(close(r.linear().y(), 1.0));
assert!(close(r.linear().z(), 0.0));
}
#[test]
fn cross_force_force_known() {
let f = FVec::new(Vector3::new([0.0, 0.0, 1.0]), Vector3::new([0.0, 0.0, 0.0]));
let g = FVec::new(Vector3::new([0.0, 0.0, 0.0]), Vector3::new([1.0, 0.0, 0.0]));
let r = f.cross(&g);
assert!(close(r.angular().x(), 0.0));
assert!(close(r.angular().y(), 1.0));
assert!(close(r.angular().z(), 0.0));
assert!(close(r.linear().norm(), 0.0));
}
#[test]
fn transform_by_rotation_matches_isometry_vector() {
let iso = iso_z90();
let m = MVec::new(Vector3::new([0.0, 0.0, 0.0]), Vector3::new([1.0, 0.0, 0.0]));
let mt = m.transform_by(&iso);
assert!(close(mt.angular().norm(), 0.0));
assert!(close(mt.linear().x(), 0.0));
assert!(close(mt.linear().y(), 1.0));
let m2 = MVec::new(Vector3::new([1.0, 0.0, 0.0]), Vector3::new([0.0, 0.0, 0.0]));
let m2t = m2.transform_by(&iso);
let expected = iso
.rotation
.transform_vector(&Vector3::new([1.0, 0.0, 0.0]));
assert!(close(m2t.angular().x(), expected.data[0]));
assert!(close(m2t.angular().y(), expected.data[1]));
assert!(close(m2t.angular().z(), expected.data[2]));
}
#[test]
fn adjoint_matches_transform() {
let iso = iso_z90();
let ad = adjoint_motion(&iso);
let m = MVec::new(Vector3::new([1.0, 2.0, 3.0]), Vector3::new([4.0, 5.0, 6.0]));
let via_matrix = MVec {
data: ad * m.data,
_marker: PhantomData,
};
let via_fn = m.transform_by(&iso);
for i in 0..6 {
assert!(
close(via_matrix.data.data[i], via_fn.data.data[i]),
"idx {i}"
);
}
}
#[test]
fn transform_by_with_translation() {
let rot = Rotation3::from_axis_angle(
&Vector3::new([0.0, 0.0, 1.0]),
core::f64::consts::FRAC_PI_2,
);
let iso = Isometry3::new(Translation::new(Vector3::new([10.0, 0.0, 0.0])), rot);
let f = FVec::new(Vector3::new([0.0, 0.0, 0.0]), Vector3::new([1.0, 0.0, 0.0]));
let ft = f.transform_by(&iso);
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]));
assert!(close(ft.angular().y(), expected_top.data[1]));
assert!(close(ft.angular().z(), expected_top.data[2]));
assert!(close(ft.linear().x(), 0.0));
assert!(close(ft.linear().y(), 1.0));
}
#[test]
fn dual_quaternion_roundtrip() {
let rot = Rotation3::from_axis_angle(
&Vector3::new([0.0, 0.0, 1.0]),
core::f64::consts::FRAC_PI_2,
);
let iso = Isometry3::new(Translation::new(Vector3::new([1.0, 2.0, 3.0])), rot);
let dq = DualQuaternion::from_isometry(&iso);
let back = dq.to_isometry();
for i in 0..3 {
for j in 0..3 {
assert!(
close(
back.rotation.matrix().data[i][j],
iso.rotation.matrix().data[i][j]
),
"rot {i},{j}"
);
}
assert!(
close(
back.translation.vector.data[i],
iso.translation.vector.data[i]
),
"trans {i}"
);
}
}
#[test]
fn dual_quaternion_composition() {
let r1 = Rotation3::from_axis_angle(
&Vector3::new([0.0, 0.0, 1.0]),
core::f64::consts::FRAC_PI_2,
);
let iso1 = Isometry3::new(Translation::new(Vector3::new([1.0, 0.0, 0.0])), r1);
let r2 = Rotation3::from_axis_angle(
&Vector3::new([0.0, 1.0, 0.0]),
core::f64::consts::FRAC_PI_3,
);
let iso2 = Isometry3::new(Translation::new(Vector3::new([0.0, 2.0, 0.0])), r2);
let dq1 = DualQuaternion::from_isometry(&iso1);
let dq2 = DualQuaternion::from_isometry(&iso2);
let composed_dq = dq1.multiply(&dq2);
let composed_iso = iso1 * iso2;
let from_dq = composed_dq.to_isometry();
for i in 0..3 {
for j in 0..3 {
assert!(
close(
from_dq.rotation.matrix().data[i][j],
composed_iso.rotation.matrix().data[i][j]
),
"rot {i},{j}"
);
}
assert!(
close(
from_dq.translation.vector.data[i],
composed_iso.translation.vector.data[i]
),
"trans {i}"
);
}
}
#[test]
fn screw_exp_log_roundtrip() {
let rot = Rotation3::from_axis_angle(
&Vector3::new([1.0, 2.0, 3.0]).normalize(),
core::f64::consts::FRAC_PI_4,
);
let iso = Isometry3::new(Translation::new(Vector3::new([0.5, -1.0, 2.0])), rot);
let screw = Screw::log(&iso);
let iso2 = screw.exp();
for i in 0..3 {
for j in 0..3 {
assert!(
close(
iso2.rotation.matrix().data[i][j],
iso.rotation.matrix().data[i][j]
),
"rot {i},{j}"
);
}
assert!(
close(
iso2.translation.vector.data[i],
iso.translation.vector.data[i]
),
"trans {i}"
);
}
}
#[test]
fn screw_pure_translation() {
let iso = Isometry3::new(
Translation::new(Vector3::new([3.0, -2.0, 1.0])),
Rotation3::identity(),
);
let screw = Screw::log(&iso);
assert!(close(screw.angular.norm(), 0.0));
for i in 0..3 {
assert!(
close(screw.linear.data[i], iso.translation.vector.data[i]),
"lin {i}"
);
}
let iso2 = screw.exp();
for i in 0..3 {
assert!(
close(
iso2.translation.vector.data[i],
iso.translation.vector.data[i]
),
"trans {i}"
);
}
}
#[test]
fn screw_exp_log_point_recovery() {
let rot = Rotation3::from_axis_angle(
&Vector3::new([0.3, 1.0, -0.7]).normalize(),
core::f64::consts::FRAC_PI_3,
);
let iso = Isometry3::new(Translation::new(Vector3::new([2.0, 1.0, -3.0])), rot);
let screw = Screw::log(&iso);
let iso2 = screw.exp();
let p = Point3::new(Vector3::new([1.0, -2.0, 0.5]));
let q1 = iso.transform_point(&p);
let q2 = iso2.transform_point(&p);
for i in 0..3 {
assert!(close(q1.coords.data[i], q2.coords.data[i]), "pt {i}");
}
}
#[test]
fn screw_composition_identity() {
let a = Screw::new(Vector3::new([0.0, 0.0, 0.1]), Vector3::new([0.2, 0.0, 0.0]));
let b = Screw::new(Vector3::new([0.1, 0.0, 0.0]), Vector3::new([0.0, 0.3, 0.0]));
let iso_a = a.exp();
let iso_b = b.exp();
let iso_ab = iso_a * iso_b;
let screw_ab = Screw::log(&iso_ab);
let iso_ab2 = screw_ab.exp();
for i in 0..3 {
for j in 0..3 {
assert!(
close(
iso_ab2.rotation.matrix().data[i][j],
iso_ab.rotation.matrix().data[i][j]
),
"rot {i},{j}"
);
}
assert!(
close(
iso_ab2.translation.vector.data[i],
iso_ab.translation.vector.data[i]
),
"trans {i}"
);
}
}
#[test]
fn screw_log_180_rotation() {
let rot = Rotation3::from_axis_angle(&Vector3::new([0.0, 0.0, 1.0]), core::f64::consts::PI);
let iso = Isometry3::new(Translation::new(Vector3::new([0.0, 0.0, 0.0])), rot);
let screw = Screw::log(&iso);
assert!(close(screw.angular.x(), 0.0));
assert!(close(screw.angular.y(), 0.0));
assert!(close(screw.angular.z(), core::f64::consts::PI));
assert!(close(screw.linear.norm(), 0.0));
let iso2 = screw.exp();
for i in 0..3 {
for j in 0..3 {
assert!(
close(
iso2.rotation.matrix().data[i][j],
iso.rotation.matrix().data[i][j]
),
"rot {i},{j}"
);
}
}
}
#[test]
fn screw_log_180_rotation_with_translation() {
let rot = Rotation3::from_axis_angle(
&Vector3::new([1.0, 1.0, 1.0]).normalize(),
core::f64::consts::PI,
);
let iso = Isometry3::new(Translation::new(Vector3::new([1.0, -2.0, 0.5])), rot);
let screw = Screw::log(&iso);
assert!((screw.angular.norm() - core::f64::consts::PI).abs() < 1e-7);
let iso2 = screw.exp();
let p = Point3::new(Vector3::new([2.0, -1.0, 3.0]));
let q1 = iso.transform_point(&p);
let q2 = iso2.transform_point(&p);
for i in 0..3 {
let d = (q1.coords.data[i] - q2.coords.data[i]).abs();
assert!(d < 1e-7, "pt {i}: diff {d}");
}
}
#[test]
fn screw_log_small_rotation() {
let angle = 1e-6;
let axis = Vector3::new([1.0, 2.0, 3.0]).normalize();
let rot = Rotation3::from_axis_angle(&axis, angle);
let iso = Isometry3::new(Translation::new(Vector3::new([0.0, 0.0, 0.0])), rot);
let screw = Screw::log(&iso);
assert!(close(screw.angular.norm(), angle));
let recovered = screw.angular / angle;
assert!(close(recovered.x(), axis.x()));
assert!(close(recovered.y(), axis.y()));
assert!(close(recovered.z(), axis.z()));
}
#[test]
fn screw_small_twist_bch_approx() {
let a = Screw::new(
Vector3::new([0.01, 0.0, 0.0]),
Vector3::new([0.0, 0.02, 0.0]),
);
let b = Screw::new(
Vector3::new([0.0, 0.01, 0.0]),
Vector3::new([0.0, 0.0, 0.01]),
);
let sum = Screw::new(a.angular + b.angular, a.linear + b.linear);
let iso_ab = a.exp() * b.exp();
let iso_sum = sum.exp();
for i in 0..3 {
let dt: f64 = iso_ab.translation.vector.data[i] - iso_sum.translation.vector.data[i];
assert!(dt.abs() < 1e-3, "trans {i}");
for j in 0..3 {
let dr: f64 =
iso_ab.rotation.matrix().data[i][j] - iso_sum.rotation.matrix().data[i][j];
assert!(dr.abs() < 1e-3, "rot {i},{j}");
}
}
}
}