nphysics2d/algebra/
inertia3.rs

1use std::ops::{Add, AddAssign, Mul, Neg};
2
3use crate::algebra::{Force3, Velocity3};
4use na::{self, Isometry3, Matrix3, Matrix6, RealField, U3};
5
6/// The inertia of a rigid body grouping both its mass and its angular inertia.
7#[derive(Clone, Copy, Debug)]
8pub struct Inertia3<N: RealField + Copy> {
9    /// The linear part (mass) of the inertia.
10    pub linear: N,
11    /// The angular inertia.
12    pub angular: Matrix3<N>,
13}
14
15impl<N: RealField + Copy> Inertia3<N> {
16    /// Creates an inertia from its linear and angular components.
17    pub fn new(linear: N, angular: Matrix3<N>) -> Self {
18        Inertia3 { linear, angular }
19    }
20
21    /// Creates an inertia from its linear and angular components.
22    pub fn new_with_angular_matrix(linear: N, angular: Matrix3<N>) -> Self {
23        Self::new(linear, angular)
24    }
25
26    /// Get the mass.
27    pub fn mass(&self) -> N {
28        self.linear
29    }
30
31    /// Get the inverse mass.
32    ///
33    /// Returns 0.0 if the mass is 0.0.
34    pub fn inv_mass(&self) -> N {
35        if self.linear.is_zero() {
36            N::zero()
37        } else {
38            self.linear
39        }
40    }
41
42    /// Create a zero inertia.
43    pub fn zero() -> Self {
44        Inertia3::new(na::zero(), na::zero())
45    }
46
47    /// Get the angular inertia tensor.
48    #[inline]
49    pub fn angular_matrix(&self) -> &Matrix3<N> {
50        &self.angular
51    }
52
53    /// Convert the inertia into a matrix where the mass is represented as a 3x3
54    /// diagonal matrix on the upper-left corner, and the angular part as a 3x3
55    /// matrix on the lower-rigth corner.
56    pub fn to_matrix(&self) -> Matrix6<N> {
57        let mut res = Matrix6::zeros();
58        res.fixed_slice_mut::<3, 3>(3, 3).copy_from(&self.angular);
59
60        res.m11 = self.linear;
61        res.m22 = self.linear;
62        res.m33 = self.linear;
63
64        res
65    }
66
67    /// Compute the inertia on the given coordinate frame.
68    pub fn transformed(&self, i: &Isometry3<N>) -> Self {
69        let rot = i.rotation.to_rotation_matrix();
70        Inertia3::new(self.linear, rot * self.angular * rot.inverse())
71    }
72
73    /// Inverts this inertia matrix.
74    ///
75    /// Sets the angular part to zero if it is not invertible.
76    #[cfg(not(feature = "improved_fixed_point_support"))]
77    pub fn inverse(&self) -> Self {
78        let inv_mass = if self.linear.is_zero() {
79            N::zero()
80        } else {
81            N::one() / self.linear
82        };
83
84        let inv_angular = self.angular.try_inverse().unwrap_or_else(na::zero);
85        Inertia3::new(inv_mass, inv_angular)
86    }
87
88    #[cfg(feature = "improved_fixed_point_support")]
89    /// Inverts this inertia matrix using preconditioning.
90    ///
91    /// Sets the angular part to zero if it is not invertible.
92    pub fn inverse(&self) -> Self {
93        let inv_mass = if self.linear.is_zero() {
94            N::zero()
95        } else {
96            N::one() / self.linear
97        };
98
99        // NOTE: the fixed-point number may not have enough bits to
100        // compute the determinant so a simple preconditioning helps.
101        let diag = self.angular.diagonal();
102        let mut preconditioned = self.angular;
103
104        // NOTE: we can't precompute inv_diag = diag.map(|e| 1 / e)
105        // because we may not even have enough bits to compute this inverse.
106        for i in 0..3 {
107            if !diag[i].is_zero() {
108                let mut row_i = preconditioned.row_mut(i);
109                row_i /= diag[i];
110            }
111        }
112
113        // println!("Angular: {}", self.angular);
114        // println!("Diagonal: {}", self.angular.diagonal());
115        // println!("Preconditioned matrix: {}", preconditioned);
116
117        let mut inv_angular = preconditioned.try_inverse().unwrap_or_else(na::zero);
118
119        for i in 0..3 {
120            if !diag[i].is_zero() {
121                let mut col_i = inv_angular.column_mut(i);
122                col_i /= diag[i];
123            }
124        }
125
126        Inertia3::new(inv_mass, inv_angular)
127    }
128}
129
130impl<N: RealField + Copy> Neg for Inertia3<N> {
131    type Output = Self;
132
133    #[inline]
134    fn neg(self) -> Self {
135        Self::new(-self.linear, -self.angular)
136    }
137}
138
139impl<N: RealField + Copy> Add<Inertia3<N>> for Inertia3<N> {
140    type Output = Inertia3<N>;
141
142    #[inline]
143    fn add(self, rhs: Inertia3<N>) -> Inertia3<N> {
144        Inertia3::new(self.linear + rhs.linear, self.angular + rhs.angular)
145    }
146}
147
148impl<N: RealField + Copy> AddAssign<Inertia3<N>> for Inertia3<N> {
149    #[inline]
150    fn add_assign(&mut self, rhs: Inertia3<N>) {
151        self.linear += rhs.linear;
152        self.angular += rhs.angular;
153    }
154}
155
156impl<N: RealField + Copy> Mul<Velocity3<N>> for Inertia3<N> {
157    type Output = Force3<N>;
158
159    #[inline]
160    fn mul(self, rhs: Velocity3<N>) -> Force3<N> {
161        Force3::new(rhs.linear * self.linear, self.angular * rhs.angular)
162    }
163}
164
165// NOTE: This is meaningful when `self` is the inverse inertia.
166impl<N: RealField + Copy> Mul<Force3<N>> for Inertia3<N> {
167    type Output = Velocity3<N>;
168
169    #[inline]
170    fn mul(self, rhs: Force3<N>) -> Velocity3<N> {
171        Velocity3::new(rhs.linear * self.linear, self.angular * rhs.angular)
172    }
173}
nphysics2d/algebra/inertia3.rs

nphysics2d/algebra/
inertia3.rs
