rigid_body/
lib.rs

1// # Rigid Body
2//
3// A minimalistic rigid body library
4
5/// The type of attitude for orientation, torque and wrench.
6pub type Attitude<T> = (T, [T; 3]);
7/// Reexport 3D vector from vecmath.
8pub use vecmath::Vector3;
9
10use vecmath::traits::Float;
11
12/// A minimalistic description of a rigid body.
13#[derive(Clone, Copy, Debug)]
14pub struct RigidBody<T> {
15    /// Linear position.
16    pub pos: Vector3<T>,
17    /// Linear velocity.
18    pub vel: Vector3<T>,
19    /// Linear acceleration.
20    pub acc: Vector3<T>,
21    /// Orientation (angular position).
22    pub ori: Attitude<T>,
23    /// Torque (angular velocity).
24    pub tor: Attitude<T>,
25    /// Wrench (angular acceleration).
26    pub wre: Attitude<T>,
27}
28
29impl<T: Float> RigidBody<T> {
30    /// Updates linear coordinates by moving through time.
31    pub fn update_linear(&mut self, dt: T) {
32        use vecmath::vec3_add as add;
33        use vecmath::vec3_scale as scale;
34
35        let half_dt = T::from_f64(0.5) * dt;
36        self.vel = add(self.vel, scale(self.acc, half_dt));
37        self.pos = add(self.pos, scale(self.vel, dt));
38        self.vel = add(self.vel, scale(self.acc, half_dt));
39    }
40
41    /// Updates angular coordinates by moving through time.
42    pub fn update_angular(&mut self, dt: T) {
43        let half_dt = T::from_f64(0.5) * dt;
44        self.tor = angular(self.tor, self.wre, half_dt);
45        self.ori = angular(self.ori, self.tor, dt);
46        self.tor = angular(self.tor, self.wre, half_dt);
47    }
48
49    /// Update coordinates by moving it through time.
50    pub fn update(&mut self, dt: T) {
51        self.update_linear(dt);
52        self.update_angular(dt);
53    }
54}
55
56/// Solves the analogue of `s' = s + v * t` for attitude.
57pub fn angular<T: Float>(a: Attitude<T>, b: Attitude<T>, t: T) -> Attitude<T> {
58    use vecmath::vec3_scale as scale;
59    use vecmath::vec3_dot as dot;
60    use vecmath::vec3_cross as cross;
61    use vecmath::vec3_add as add;
62
63    let angle = a.0 + dot(a.1, b.1) * b.0 * t;
64    let cos = b.0.cos();
65    let sin = b.0.sin();
66    // Uses Rodigrues' rotation formula.
67    let axis = add(scale(a.1, cos), add(scale(cross(b.1, a.1), sin),
68               scale(b.1, dot(b.1, a.1) * (T::one() - cos))));
69    (angle, axis)
70}
rigid_body/lib.rs

rigid_body/
lib.rs
