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
use na;
use alga::general::Real;
use math::{Point, Vector, Orientation, Rotation, Translation, Isometry};
pub fn explicit_integrate<N: Real>(dt: N,
p: &Isometry<N>, c: &Point<N>,
lv: &Vector<N>, av: &Orientation<N>,
lf: &Vector<N>, af: &Orientation<N>)
-> (Isometry<N>, Vector<N>, Orientation<N>) {
(
displacement(dt.clone(), p, c, lv, av),
*lv + *lf * dt,
*av + *af * dt
)
}
pub fn explicit_integrate_wo_rotation<N: Real>(dt: N, p: &Point<N>, lv: &Vector<N>, lf: &Vector<N>)
-> (Point<N>, Vector<N>) {
(
*p + *lv * dt,
*lv + *lf * dt
)
}
pub fn semi_implicit_integrate<N: Real>(dt: N,
p: &Isometry<N>, c: &Point<N>,
lv: &Vector<N>, av: &Orientation<N>,
l_acc: &Vector<N>, a_acc: &Orientation<N>)
-> (Isometry<N>, Vector<N>, Orientation<N>) {
let nlv = *lv + *l_acc * dt;
let nav = *av + *a_acc * dt;
(
displacement(dt.clone(), p, c, &nlv, &nav),
nlv,
nav
)
}
pub fn semi_implicit_integrate_wo_rotation<N: Real>(dt: N, p: &Point<N>, lv: &Vector<N>, lf: &Vector<N>)
-> (Point<N>, Vector<N>) {
let nlv = *lv + *lf * dt;
(
*p + nlv * dt,
nlv
)
}
pub fn displacement<N: Real>(dt: N, _: &Isometry<N>, center_of_mass: &Point<N>,
lin_vel: &Vector<N>, ang_vel: &Orientation<N>) -> Isometry<N> {
let mut res: Isometry<N> = na::one();
let rot = Rotation::from_scaled_axis(ang_vel * dt);
res.append_rotation_wrt_point_mut(&rot, center_of_mass);
res.append_translation_mut(&Translation::from_vector(lin_vel * dt));
res
}