rhusics_core/physics/
mass.rs

1use std;
2use std::ops::Mul;
3
4use cgmath::{BaseFloat, Basis2, Matrix, Matrix3, Quaternion, SquareMatrix, Zero};
5
6use super::{Material, Volume};
7
8/// Mass
9///
10/// Mass properties for a body. Inertia is the moment of inertia in the base pose. For retrieving
11/// the inertia tensor for the body in its current orientation, see `world_inertia` and
12/// `world_inverse_inertia`.
13///
14/// ### Type parameters:
15///
16/// - `I`: Inertia type, usually `Scalar` or `Matrix3`, see `Inertia` for more information.
17#[derive(Debug, Clone)]
18#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
19pub struct Mass<S, I> {
20    mass: S,
21    inverse_mass: S,
22
23    inertia: I,
24    inverse_inertia: I,
25}
26
27/// Moment of inertia, used for abstracting over 2D/3D inertia tensors
28pub trait Inertia: Mul<Self, Output = Self> + Copy {
29    /// Orientation type for rotating the inertia to create a world space inertia tensor
30    type Orientation;
31    /// Return the infinite inertia
32    fn infinite() -> Self;
33    /// Compute the inverse of the inertia
34    fn invert(&self) -> Self;
35    /// Compute the inertia tensor in the bodies given orientation
36    fn tensor(&self, orientation: &Self::Orientation) -> Self;
37}
38
39impl Inertia for f32 {
40    type Orientation = Basis2<f32>;
41
42    fn infinite() -> Self {
43        std::f32::INFINITY
44    }
45
46    fn invert(&self) -> Self {
47        if *self == 0. || self.is_infinite() {
48            0.
49        } else {
50            1. / *self
51        }
52    }
53
54    fn tensor(&self, _: &Basis2<f32>) -> Self {
55        *self
56    }
57}
58
59impl Inertia for f64 {
60    type Orientation = Basis2<f64>;
61
62    fn invert(&self) -> Self {
63        if *self == 0. || self.is_infinite() {
64            0.
65        } else {
66            1. / *self
67        }
68    }
69
70    fn tensor(&self, _: &Basis2<f64>) -> Self {
71        *self
72    }
73
74    fn infinite() -> Self {
75        std::f64::INFINITY
76    }
77}
78
79impl<S> Inertia for Matrix3<S>
80where
81    S: BaseFloat,
82{
83    type Orientation = Quaternion<S>;
84
85    fn invert(&self) -> Self {
86        if self.x.x.is_infinite() {
87            Matrix3::zero()
88        } else {
89            SquareMatrix::invert(self).unwrap_or_else(Matrix3::zero)
90        }
91    }
92
93    fn tensor(&self, orientation: &Quaternion<S>) -> Self {
94        let mat3 = Matrix3::from(*orientation);
95        mat3 * (*self * mat3.transpose())
96    }
97
98    fn infinite() -> Self {
99        Matrix3::from_value(S::infinity())
100    }
101}
102
103impl<S, I> Mass<S, I>
104where
105    S: BaseFloat,
106    I: Inertia,
107{
108    /// Create new mass object
109    pub fn new(mass: S) -> Self {
110        Self::new_with_inertia(mass, I::infinite())
111    }
112
113    /// Create new infinite mass object
114    pub fn infinite() -> Self {
115        Self::new_with_inertia(S::infinity(), I::infinite())
116    }
117
118    /// Create new mass object with given inertia
119    pub fn new_with_inertia(mass: S, inertia: I) -> Self {
120        let inverse_mass = if mass.is_infinite() {
121            S::zero()
122        } else {
123            S::one() / mass
124        };
125        let inverse_inertia = inertia.invert();
126        Mass {
127            mass,
128            inverse_mass,
129            inertia,
130            inverse_inertia,
131        }
132    }
133
134    /// Compute mass from the given volume shape and material
135    pub fn from_volume_and_material<V>(volume: &V, material: &Material) -> Self
136    where
137        V: Volume<S, I>,
138    {
139        volume.get_mass(material)
140    }
141
142    /// Get mass
143    pub fn mass(&self) -> S {
144        self.mass
145    }
146
147    /// Get inverse mass
148    pub fn inverse_mass(&self) -> S {
149        self.inverse_mass
150    }
151
152    /// Get inertia in local space
153    pub fn local_inertia(&self) -> I {
154        self.inertia
155    }
156
157    /// Get inertia tensor in world space
158    ///
159    /// ### Parameters:
160    ///
161    /// - `orientation`: The current orientation of the body
162    pub fn world_inertia(&self, orientation: &I::Orientation) -> I {
163        self.inertia.tensor(orientation)
164    }
165
166    /// Get inverse inertia in local space
167    pub fn local_inverse_inertia(&self) -> I {
168        self.inverse_inertia
169    }
170
171    /// Get inverse inertia tensor in world space
172    ///
173    /// ### Parameters:
174    ///
175    /// - `orientation`: The current orientation of the body
176    pub fn world_inverse_inertia(&self, orientation: &I::Orientation) -> I {
177        self.inverse_inertia.tensor(orientation)
178    }
179}
rhusics_core/physics/mass.rs

rhusics_core/physics/
mass.rs
