spatial-math 0.4.0-beta.1

Spatial math library for articulated body simulation
Documentation 
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

// Copyright (C) 2020-2025 spatial-math authors. All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

use crate::Real;

pub type SMatrix<const M: usize, const N: usize> = na::SMatrix<Real, M, N>;
/// Static alloc vector type alias.
pub type SVector<const N: usize> = na::SVector<Real, N>;
/// Dynamic matrix that has 6 rows and N columns.
pub type Matrix6xN = na::Matrix6xX<Real>;

pub type Mat3 = SMatrix<3, 3>;
pub type Vec3 = SVector<3>;
pub type Vec4 = SVector<4>;
pub type Vec6 = SVector<6>;
pub type UnitQuat = na::UnitQuaternion<Real>;
pub type Quat = na::Quaternion<Real>;
pub type UnitVec3 = na::Unit<Vec3>;
pub type DVector = na::DVector<Real>;

pub type MatrixViewMut<'a, const R: usize, const C: usize> =
    na::MatrixViewMut<'a, Real, na::Const<R>, na::Const<C>>;

pub type MatrixView<'a, const R: usize, const C: usize> =
    na::MatrixView<'a, Real, na::Const<R>, na::Const<C>>;

pub type DMatrix = na::DMatrix<Real>;
pub type DMatrixView<'a> = na::DMatrixView<'a, Real>;
pub type DMatrixViewMut<'a> = na::DMatrixViewMut<'a, Real>;
pub type DVectorView<'a> = na::DVectorView<'a, Real>;
pub type DVectorViewMut<'a> = na::DVectorViewMut<'a, Real>;

pub const VEC3_ZERO: Vec3 = na::vector![0.0, 0.0, 0.0];

#[inline]
pub const fn vec3(x: Real, y: Real, z: Real) -> Vec3 {
    na::vector![x, y, z]
}

#[inline]
pub const fn vec4(x: Real, y: Real, z: Real, w: Real) -> Vec4 {
    na::vector![x, y, z, w]
}

#[inline]
pub const fn vec6(array: [Real; 6]) -> Vec6 {
    na::vector![array[0], array[1], array[2], array[3], array[4], array[5]]
}

#[inline]
pub fn unit_vec3(x: Real, y: Real, z: Real) -> UnitVec3 {
    na::Unit::new_normalize(vec3(x, y, z))
}

#[inline]
pub fn unit_vec3_unchecked(x: Real, y: Real, z: Real) -> UnitVec3 {
    na::Unit::new_unchecked(vec3(x, y, z))
}

#[inline]
pub fn unit_quat(x: Real, y: Real, z: Real, w: Real) -> UnitQuat {
    #[cfg(debug_assertions)]
    {
        let norm = x * x + y * y + z * z + w * w;
        debug_assert!(norm.abs() > 1e-6, "norm is zero");
    }
    na::UnitQuaternion::new_normalize(Quat::new(w, x, y, z))
}

#[inline]
pub fn unit_quat_unchecked(x: Real, y: Real, z: Real, w: Real) -> UnitQuat {
    #[cfg(debug_assertions)]
    {
        let norm = (x * x + y * y + z * z + w * w).sqrt();
        debug_assert!(
            (norm - 1.0).abs() < 1e-6,
            "Quaternion is not normalized: {norm}",
        );
    }
    na::UnitQuaternion::new_unchecked(Quat::new(w, x, y, z))
}

#[cfg(test)]
mod tests {
    use approx::assert_relative_eq;
    use na::Quaternion;

    use crate::{UnitQuat, unit_quat, vec4};

    #[test]
    fn test_unit_quat() {
        let q = unit_quat(1.0, 2.0, 3.0, 4.0);
        let q_expected = UnitQuat::new_normalize(Quaternion::from_vector(vec4(1.0, 2.0, 3.0, 4.0)));
        assert_relative_eq!(q, q_expected);
    }
}