spatial6 1.3.0

Backend-agnostic 6D spatial vectors, transforms, and inertias for rigid-body dynamics
Documentation
// SPDX-FileCopyrightText: 2026 Daisuke Nagao
//
// SPDX-License-Identifier: MIT OR Apache-2.0

use spatial6::{
    ArticulatedBodyInertia, ForceVector, MotionSubspace, MotionVector, RigidBodyInertia,
    SpatialRepresentation, SpatialScalar, SpatialTransform,
};

fn assert_num_traits_float<T: num_traits::Float>() {}

fn assert_spatial_scalar_is_float<T: SpatialScalar>() {
    assert_num_traits_float::<T>();
}

fn assert_close(actual: f64, expected: f64) {
    let tolerance = 1.0e-10 + 1.0e-10 * actual.abs().max(expected.abs());
    assert!((actual - expected).abs() <= tolerance);
}

#[test]
fn spatial_scalar_uses_num_traits_float() {
    assert_spatial_scalar_is_float::<f32>();
    assert_spatial_scalar_is_float::<f64>();
}

#[derive(Clone, Copy, Debug, PartialEq)]
struct Custom;

impl SpatialRepresentation<f64> for Custom {
    type Vector3 = [f64; 3];
    type Vector6 = [f64; 6];
    type Matrix3 = [[f64; 3]; 3];
    type Matrix6 = [[f64; 6]; 6];
    type Rotation3 = [[f64; 3]; 3];

    fn vector3_from_array(value: [f64; 3]) -> Self::Vector3 {
        value
    }

    fn vector3_to_array(value: &Self::Vector3) -> [f64; 3] {
        *value
    }

    fn vector6_from_array(value: [f64; 6]) -> Self::Vector6 {
        value
    }

    fn vector6_to_array(value: &Self::Vector6) -> [f64; 6] {
        *value
    }

    fn matrix3_from_array(value: [[f64; 3]; 3]) -> Self::Matrix3 {
        value
    }

    fn matrix3_to_array(value: &Self::Matrix3) -> [[f64; 3]; 3] {
        *value
    }

    fn matrix6_from_array(value: [[f64; 6]; 6]) -> Self::Matrix6 {
        value
    }

    fn matrix6_to_array(value: &Self::Matrix6) -> [[f64; 6]; 6] {
        *value
    }

    fn rotation3_from_array(value: [[f64; 3]; 3]) -> Self::Rotation3 {
        value
    }

    fn rotation3_to_array(value: &Self::Rotation3) -> [[f64; 3]; 3] {
        *value
    }
}

#[test]
fn default_matrix3_add_preserves_row_major_order() {
    let left = [[1.0, 2.0, 4.0], [8.0, 16.0, 32.0], [64.0, 128.0, 256.0]];
    let right = [[0.5, 1.5, 2.5], [3.5, 4.5, 5.5], [6.5, 7.5, 8.5]];

    assert_eq!(
        <Custom as SpatialRepresentation<f64>>::matrix3_add(&left, &right),
        [[1.5, 3.5, 6.5], [11.5, 20.5, 37.5], [70.5, 135.5, 264.5]]
    );
}

#[test]
fn user_defined_representation_drives_the_complete_api() {
    let velocity = MotionVector::<f64, Custom>::from_vector([1.0, 2.0, 3.0, 4.0, 5.0, 6.0]);
    let force = ForceVector::<f64, Custom>::from_vector([10.0, 11.0, 12.0, 13.0, 14.0, 15.0]);

    assert_eq!(velocity.to_vector(), [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]);
    assert_eq!(velocity.dot(&force), 280.0);
    let subspace = MotionSubspace::from_columns([velocity]);
    assert_eq!(subspace.apply(&[2.0]), velocity * 2.0);
    assert_eq!(subspace.generalized_force(&force), [280.0]);
    assert_eq!(
        velocity.cross_force(&force).to_vector(),
        [-18.0, 36.0, -18.0, -12.0, 24.0, -12.0]
    );

    let transform = SpatialTransform::<f64, Custom>::new(
        [[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]],
        [3.0, 5.0, 7.0],
    );
    assert_eq!(
        transform.transform_motion(&velocity).to_vector(),
        [-2.0, 1.0, 3.0, -7.0, 3.0, 5.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, [3.0, 5.0, 7.0]);
    assert_eq!(
        SpatialTransform::<f64, Custom>::from_pose_parts(
            rotation_local_to_reference,
            position_in_reference,
        ),
        transform
    );

    let inertia = RigidBodyInertia::<f64, Custom>::try_new(
        2.0,
        [1.0, 2.0, 3.0],
        [[4.0, 0.0, 0.0], [0.0, 5.0, 0.0], [0.0, 0.0, 6.0]],
    )
    .unwrap();
    let other = RigidBodyInertia::<f64, Custom>::try_new(
        3.0,
        [-2.0, 1.0, 0.5],
        [[7.0, 0.2, -0.1], [0.2, 8.0, 0.3], [-0.1, 0.3, 9.0]],
    )
    .unwrap();
    let combined = inertia.try_combined(&other).unwrap();
    assert_close(combined.mass(), 5.0);
    for (actual, expected) in (*combined.center_of_mass())
        .into_iter()
        .zip([-0.8, 1.4, 1.5])
    {
        assert_close(actual, expected);
    }
    let acceleration = MotionVector::<f64, Custom>::new([1.0, 0.0, 0.0], [0.0, 1.0, 0.0]);
    assert_eq!(
        inertia.apply(&acceleration).to_vector(),
        [24.0, -4.0, -4.0, 0.0, -4.0, 4.0]
    );
    for (actual, expected) in combined
        .apply(&acceleration)
        .to_vector()
        .into_iter()
        .zip((inertia.apply(&acceleration) + other.apply(&acceleration)).to_vector())
    {
        assert_close(actual, expected);
    }

    let articulated =
        ArticulatedBodyInertia::<f64, Custom>::try_from_matrix(inertia.matrix()).unwrap();
    assert_eq!(articulated.matrix(), inertia.matrix());
    let zeros = ArticulatedBodyInertia::<f64, Custom>::zeros();
    assert_eq!(zeros.matrix(), [[0.0; 6]; 6]);
    assert_eq!(
        zeros.apply(&acceleration),
        ForceVector::<f64, Custom>::zeros()
    );
    assert_eq!(
        articulated.apply(&acceleration),
        inertia.apply(&acceleration)
    );

    let converted = ArticulatedBodyInertia::try_from(&inertia).unwrap();
    assert_eq!(converted, articulated);

    let combined_articulated = articulated.try_combined(&articulated).unwrap();
    assert_eq!(
        combined_articulated.apply(&acceleration),
        inertia.apply(&acceleration) + inertia.apply(&acceleration)
    );

    let update = ForceVector::<f64, Custom>::from_array([1.0, -2.0, 0.5, 3.0, -1.0, 2.0]);
    let updated = articulated.try_rank_one_updated(0.25, &update).unwrap();
    let mut expected_updated = inertia.matrix();
    let update_array = update.to_array();
    for (row, expected_row) in expected_updated.iter_mut().enumerate() {
        for (column, expected) in expected_row.iter_mut().enumerate() {
            *expected += 0.25 * update_array[row] * update_array[column];
        }
    }
    assert_eq!(updated.matrix(), expected_updated);
    assert_eq!(
        updated.apply(&MotionVector::<f64, Custom>::zeros()),
        ForceVector::<f64, Custom>::zeros()
    );
    assert_eq!(
        articulated
            .try_transformed(&SpatialTransform::<f64, Custom>::identity())
            .unwrap(),
        articulated
    );
}

#[cfg(feature = "serde")]
#[test]
fn custom_representation_abi_serde_has_no_marker_serde_bound() {
    let subspace = MotionSubspace::from_columns(std::array::from_fn::<_, 33, _>(|index| {
        MotionVector::<f64, Custom>::from_array([index as f64, 2.0, 3.0, 4.0, 5.0, 6.0])
    }));
    let json = serde_json::to_string(&subspace).unwrap();
    assert_eq!(
        serde_json::from_str::<MotionSubspace<33, f64, Custom>>(&json).unwrap(),
        subspace
    );

    let inertia = ArticulatedBodyInertia::<f64, Custom>::try_from_matrix([
        [1.0, 0.2, 0.0, 0.0, 0.0, 0.0],
        [0.2, 2.0, 0.3, 0.0, 0.0, 0.0],
        [0.0, 0.3, 3.0, 0.4, 0.0, 0.0],
        [0.0, 0.0, 0.4, 4.0, 0.5, 0.0],
        [0.0, 0.0, 0.0, 0.5, 5.0, 0.6],
        [0.0, 0.0, 0.0, 0.0, 0.6, 6.0],
    ])
    .unwrap();
    let json = serde_json::to_string(&inertia).unwrap();
    assert_eq!(
        serde_json::from_str::<ArticulatedBodyInertia<f64, Custom>>(&json).unwrap(),
        inertia
    );

    let transform = SpatialTransform::<f64, Custom>::new(
        [[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]],
        [3.0, 5.0, 7.0],
    );
    let (rotation_local_to_reference, position_in_reference) = transform.to_pose_parts();
    assert_eq!(
        SpatialTransform::<f64, Custom>::from_pose_parts(
            rotation_local_to_reference,
            position_in_reference,
        ),
        transform
    );
}

#[cfg(feature = "builtin")]
#[test]
fn builtin_representation_is_the_default() {
    use spatial6::Builtin;

    let default: MotionVector = MotionVector::new([1.0, 2.0, 3.0], [4.0, 5.0, 6.0]);
    let explicit = MotionVector::<f64, Builtin>::new([1.0, 2.0, 3.0], [4.0, 5.0, 6.0]);

    assert_eq!(default.to_vector(), explicit.to_vector());
}