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
use alga::general::Real;
use object::RigidBody;
use integration::Integrator;
use integration::euler;
pub struct BodySmpEulerIntegrator;
impl BodySmpEulerIntegrator {
#[inline]
pub fn new() -> BodySmpEulerIntegrator {
BodySmpEulerIntegrator
}
}
impl<N: Real> Integrator<N, RigidBody<N>> for BodySmpEulerIntegrator {
#[inline]
fn update(&mut self, dt: N, rb: &mut RigidBody<N>) {
if rb.can_move() {
let (t, lv, av) = euler::semi_implicit_integrate(
dt.clone(),
rb.position(),
rb.center_of_mass(),
&rb.lin_vel(),
&rb.ang_vel(),
&rb.lin_acc(),
&rb.ang_acc());
rb.append_transformation(&t);
rb.set_lin_vel_internal(lv);
rb.set_ang_vel_internal(av);
}
}
}