phys_inertia/
math.rs

1// Copyright (C) 2020-2025 phys-inertia authors. All Rights Reserved.
2//
3// Licensed under the Apache License, Version 2.0 (the "License");
4// you may not use this file except in compliance with the License.
5// You may obtain a copy of the License at
6//
7//     http://www.apache.org/licenses/LICENSE-2.0
8//
9// Unless required by applicable law or agreed to in writing, software
10// distributed under the License is distributed on an "AS IS" BASIS,
11// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12// See the License for the specific language governing permissions and
13// limitations under the License.
14
15pub use phys_geom::math::{vec3, Real};
16
17pub type Vec3 = na::Vector3<Real>;
18pub type UnitQuat = na::UnitQuaternion<Real>;
19pub type Quat = na::Quaternion<Real>;
20pub type Mat3 = na::Matrix3<Real>;
21pub type UnitVec3 = na::UnitVector3<Real>;
22pub type Isometry3 = na::Isometry3<Real>;
23
24#[cfg(feature = "f64")]
25pub const PI: Real = std::f64::consts::PI;
26#[cfg(not(feature = "f64"))]
27pub const PI: Real = std::f32::consts::PI;
28
29#[inline]
30pub fn unit_quat(x: Real, y: Real, z: Real, w: Real) -> UnitQuat {
31    UnitQuat::from_quaternion(Quat::new(w, x, y, z))
32}
33
34pub trait UnitQuatExt {
35    fn is_near_identity(&self) -> bool;
36}
37
38impl UnitQuatExt for UnitQuat {
39    #[inline]
40    fn is_near_identity(&self) -> bool {
41        let positive_w_angle = self.w.abs().acos() * 2.0;
42        positive_w_angle < 0.002_847_144_6
43    }
44}
45
46pub trait Mat3Ext {
47    /// Diagnolisize a matrix and return the diagonal and the rotation matrix.
48    fn diagonalize(self) -> (Vec3, UnitQuat);
49}
50
51impl Mat3Ext for Mat3 {
52    /// Diagnolisize a matrix and return the diagonal and the rotation matrix.
53    fn diagonalize(self) -> (Vec3, UnitQuat) {
54        fn indexed_rotation(axis: usize, s: Real, c: Real) -> Quat {
55            let mut v = [0.; 3];
56            v[axis] = s;
57            Quat::new(c, v[0], v[1], v[2])
58        }
59
60        #[inline]
61        fn get_next_index(i: usize) -> usize {
62            (i + 1 + (i >> 1)) & 3
63        }
64
65        // jacobi rotation using quaternions (from an idea of Stan Melax, with fix for precision
66        // issues)
67        const MAX_ITERS: u32 = 24;
68        let mut q = UnitQuat::identity();
69        let mut d = Mat3::zeros();
70        for _ in 0..MAX_ITERS {
71            let axes = Mat3::from(q);
72            d = axes.transpose() * self * axes;
73            let d0 = d[(2, 1)].abs();
74            let d1 = d[(2, 0)].abs();
75            let d2 = d[(1, 0)].abs();
76            let a: usize = if d0 > d1 && d0 > d2 {
77                0
78            } else if d1 > d2 {
79                1
80            } else {
81                2
82            }; // rotation axis index, from largest off-diagonal element
83
84            let a1 = get_next_index(a);
85            let a2 = get_next_index(a1);
86            if d[(a2, a1)] == 0. || (d[(a1, a1)] - d[(a2, a2)]).abs() > 2e6 * 2. * d[(a2, a1)].abs()
87            {
88                break;
89            }
90
91            let w = (d[(a1, a1)] - d[(a2, a2)]) / (2. * d[(a2, a1)]); // cot(2 * phi), where phi is the rotation angle
92            let absw = w.abs();
93
94            let r = if absw > 1000. {
95                indexed_rotation(a, 1. / (4. * w), 1.) // h will be very close to 1, so use small
96                                                       // angle approx instead
97            } else {
98                let t = 1. / (absw + (w * w + 1.).sqrt()); // absolute value of tan phi
99                let h = 1. / (t * t + 1.).sqrt(); // absolute value of cos phi
100                assert!(h != 1.); // |w|<1000 guarantees this with typical IEEE754 machine eps (approx 6e-8)
101                indexed_rotation(
102                    a,
103                    ((1. - h) / 2.).sqrt() * w.signum(),
104                    ((1. + h) / 2.).sqrt(),
105                )
106            };
107            q = UnitQuat::new_normalize(q.into_inner() * r);
108        }
109
110        (d.diagonal(), q)
111    }
112}
113
114#[inline]
115pub(crate) fn is_near_zero(v: Vec3, epsilon: Real) -> bool {
116    v.x.abs() < epsilon && v.y.abs() < epsilon && v.z.abs() < epsilon
117}
118
119#[cfg(test)]
120mod tests {
121    use approx::assert_relative_eq;
122    use phys_geom::math::vec3;
123
124    use super::*;
125
126    #[test]
127    #[allow(clippy::excessive_precision)]
128    fn test_diagonalize_input_is_diagonal() {
129        const EPS: f32 = 1e-5;
130        let m = Mat3::from_column_slice(&[
131            1706.66675,
132            0.000_000_00,
133            0.000_000_00,
134            0.000_000_00,
135            1706.66675,
136            0.000_000_00,
137            0.000_000_00,
138            0.000_000_00,
139            1706.66675,
140        ]);
141        let (diag, rot) = m.diagonalize();
142        let expected_diag = vec3(1706.66675, 1706.66675, 1706.66675);
143        let expected_rot = UnitQuat::identity();
144        assert_relative_eq!(diag, expected_diag, epsilon = EPS);
145        assert_relative_eq!(rot, expected_rot, epsilon = EPS);
146    }
147
148    #[test]
149    #[allow(clippy::excessive_precision)]
150    fn test_diagonalize_input_is_close_to_diagonal() {
151        const EPS: f32 = 1e-5;
152
153        let m = Mat3::from_column_slice(&[
154            70_689_968.0,
155            -0.282_659_560,
156            0.038_694_873_5,
157            -0.282_659_560,
158            70_690_080.0,
159            -0.024_899_311_4,
160            0.038_694_873_5,
161            -0.024_899_311_4,
162            47_165_804.0,
163        ]);
164
165        let (diag, rot) = m.diagonalize();
166        let expected_diag = vec3(70_689_976.0, 70_690_088.0, 47_165_804.0);
167        let expected_rot = unit_quat(0., 0., 0.001_244_877_81, 0.999_999_225);
168        assert_relative_eq!(diag, expected_diag, epsilon = EPS);
169        assert_relative_eq!(rot, expected_rot, epsilon = EPS);
170    }
171
172    #[test]
173    fn test_diagonalize_random() {
174        let m = Mat3::from_column_slice(&[1., 1., 3., 1., 5., 1., 3., 1., 1.]);
175        let (diag, rot) = m.diagonalize();
176        let restored_m =
177            Mat3::from(rot) * Mat3::from_diagonal(&diag) * Mat3::from(rot).try_inverse().unwrap();
178        assert_relative_eq!(m, restored_m, epsilon = 1e-5);
179    }
180}
phys_inertia/math.rs

phys_inertia/
math.rs
