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
108
use std::mem;
use std::ops::{Add, AddAssign, Mul};
use na::{self, Isometry2, Matrix1, Matrix3, RealField, Vector3};
use crate::algebra::{Force2, Velocity2};
#[derive(Clone, Copy, Debug)]
pub struct Inertia2<N: RealField> {
pub linear: N,
pub angular: N,
}
impl<N: RealField> Inertia2<N> {
pub fn new(linear: N, angular: N) -> Self {
Inertia2 { linear, angular }
}
pub fn new_with_angular_matrix(linear: N, angular: Matrix1<N>) -> Self {
Self::new(linear, angular.x)
}
pub fn mass(&self) -> N {
self.linear
}
pub fn zero() -> Self {
Inertia2::new(na::zero(), na::zero())
}
#[inline]
pub fn angular_matrix(&self) -> &Matrix1<N> {
unsafe { mem::transmute(&self.angular) }
}
pub fn to_matrix(&self) -> Matrix3<N> {
let diag = Vector3::new(self.linear, self.linear, self.angular);
Matrix3::from_diagonal(&diag)
}
pub fn transformed(&self, _: &Isometry2<N>) -> Self {
*self
}
pub fn inverse(&self) -> Self {
let inv_mass = if self.linear.is_zero() {
N::zero()
} else {
N::one() / self.linear
};
let inv_angular = if self.angular.is_zero() {
N::zero()
} else {
N::one() / self.angular
};
Inertia2::new(inv_mass, inv_angular)
}
}
impl<N: RealField> Add<Inertia2<N>> for Inertia2<N> {
type Output = Inertia2<N>;
#[inline]
fn add(self, rhs: Inertia2<N>) -> Inertia2<N> {
Inertia2::new(self.linear + rhs.linear, self.angular + rhs.angular)
}
}
impl<N: RealField> AddAssign<Inertia2<N>> for Inertia2<N> {
#[inline]
fn add_assign(&mut self, rhs: Inertia2<N>) {
self.linear += rhs.linear;
self.angular += rhs.angular;
}
}
impl<N: RealField> Mul<Velocity2<N>> for Inertia2<N> {
type Output = Force2<N>;
#[inline]
fn mul(self, rhs: Velocity2<N>) -> Force2<N> {
Force2::new(rhs.linear * self.linear, self.angular * rhs.angular)
}
}
impl<N: RealField> Mul<Force2<N>> for Inertia2<N> {
type Output = Velocity2<N>;
#[inline]
fn mul(self, rhs: Force2<N>) -> Velocity2<N> {
Velocity2::new(rhs.linear * self.linear, self.angular * rhs.angular)
}
}