nphysics3d/algebra/
inertia2.rs

1use std::mem;
2use std::ops::{Add, AddAssign, Mul, Neg};
3
4use crate::algebra::{Force2, Velocity2};
5use na::{self, Isometry2, Matrix1, Matrix3, RealField, Vector3};
6
7/// The inertia of a rigid body grouping both its mass and its angular inertia.
8#[derive(Clone, Copy, Debug)]
9pub struct Inertia2<N: RealField + Copy> {
10    /// The linear part (mass) of the inertia.
11    pub linear: N,
12    /// The angular inertia.
13    pub angular: N,
14}
15
16impl<N: RealField + Copy> Inertia2<N> {
17    /// Creates an inertia from its linear and angular components.
18    pub fn new(linear: N, angular: N) -> Self {
19        Inertia2 { linear, angular }
20    }
21
22    /// Creates an inertia from its linear and angular components.
23    pub fn new_with_angular_matrix(linear: N, angular: Matrix1<N>) -> Self {
24        Self::new(linear, angular.x)
25    }
26
27    /// Get the mass.
28    pub fn mass(&self) -> N {
29        self.linear
30    }
31
32    /// Get the inverse mass.
33    ///
34    /// Returns 0.0 if the mass is 0.0.
35    pub fn inv_mass(&self) -> N {
36        if self.linear.is_zero() {
37            N::zero()
38        } else {
39            self.linear
40        }
41    }
42
43    /// Create a zero inertia.
44    pub fn zero() -> Self {
45        Inertia2::new(na::zero(), na::zero())
46    }
47
48    /// Get the angular inertia tensor.
49    #[inline]
50    pub fn angular_matrix(&self) -> &Matrix1<N> {
51        unsafe { mem::transmute(&self.angular) }
52    }
53
54    /// Convert the inertia into a matrix where the mass is represented as a 2x2
55    /// diagonal matrix on the upper-left corner, and the angular part as a 1x1
56    /// matrix on the lower-rigth corner.
57    pub fn to_matrix(&self) -> Matrix3<N> {
58        let diag = Vector3::new(self.linear, self.linear, self.angular);
59        Matrix3::from_diagonal(&diag)
60    }
61
62    /// Compute the inertia on the given coordinate frame.
63    pub fn transformed(&self, _: &Isometry2<N>) -> Self {
64        *self
65    }
66
67    /// Inverts this inetia matrix.
68    ///
69    /// Sets the angular part to zero if it is not invertible.
70    pub fn inverse(&self) -> Self {
71        let inv_mass = if self.linear.is_zero() {
72            N::zero()
73        } else {
74            N::one() / self.linear
75        };
76        let inv_angular = if self.angular.is_zero() {
77            N::zero()
78        } else {
79            N::one() / self.angular
80        };
81        Inertia2::new(inv_mass, inv_angular)
82    }
83}
84
85impl<N: RealField + Copy> Neg for Inertia2<N> {
86    type Output = Self;
87
88    #[inline]
89    fn neg(self) -> Self {
90        Self::new(-self.linear, -self.angular)
91    }
92}
93
94impl<N: RealField + Copy> Add<Inertia2<N>> for Inertia2<N> {
95    type Output = Inertia2<N>;
96
97    #[inline]
98    fn add(self, rhs: Inertia2<N>) -> Inertia2<N> {
99        Inertia2::new(self.linear + rhs.linear, self.angular + rhs.angular)
100    }
101}
102
103impl<N: RealField + Copy> AddAssign<Inertia2<N>> for Inertia2<N> {
104    #[inline]
105    fn add_assign(&mut self, rhs: Inertia2<N>) {
106        self.linear += rhs.linear;
107        self.angular += rhs.angular;
108    }
109}
110
111impl<N: RealField + Copy> Mul<Velocity2<N>> for Inertia2<N> {
112    type Output = Force2<N>;
113
114    #[inline]
115    fn mul(self, rhs: Velocity2<N>) -> Force2<N> {
116        Force2::new(rhs.linear * self.linear, self.angular * rhs.angular)
117    }
118}
119
120// NOTE: This is meaningful when `self` is the inverse inertia.
121impl<N: RealField + Copy> Mul<Force2<N>> for Inertia2<N> {
122    type Output = Velocity2<N>;
123
124    #[inline]
125    fn mul(self, rhs: Force2<N>) -> Velocity2<N> {
126        Velocity2::new(rhs.linear * self.linear, self.angular * rhs.angular)
127    }
128}
nphysics3d/algebra/inertia2.rs

nphysics3d/algebra/
inertia2.rs
