loam_physics/
integrator.rs

1use std::ops::Mul;
2
3use loam_math::{Bivector, IsometryGroup, Space};
4
5use crate::body::RigidBody;
6
7#[derive(Clone, Copy, Debug, PartialEq, Eq)]
8pub enum BroadphaseBound {
9    /// Chart distance is a lower bound on separation everywhere, seams and identifications included.
10    Certified,
11    /// No distance pruning; every pair that passes the static and mask filters is a candidate.
12    Unknown,
13}
14
15pub trait PhysicsSpace: Space + IsometryGroup + Send + Sync {
16    type AngVel: Bivector + Send + Sync;
17
18    type Inertia: Copy + Send + Sync;
19
20    fn broadphase_bound(&self) -> BroadphaseBound;
21
22    fn supports_collider(&self, kind: crate::ColliderKind) -> bool;
23
24    fn valid_vector(&self, vector: Self::Vector) -> bool;
25
26    fn valid_inertia(&self, inertia: Self::Inertia) -> bool;
27
28    fn valid_orientation(&self, orientation: Self::Iso) -> bool;
29
30    fn valid_angular_velocity(&self, angular_velocity: Self::AngVel) -> bool;
31
32    fn valid_initial_state(
33        &self,
34        position: Self::Point,
35        velocity: Self::Vector,
36        inertia: Self::Inertia,
37    ) -> bool {
38        self.valid_point(position) && self.valid_vector(velocity) && self.valid_inertia(inertia)
39    }
40
41    fn integrate_orientation(&self, iso: Self::Iso, omega: Self::AngVel, dt: f32) -> Self::Iso;
42
43    fn apply_inv_inertia(&self, inertia: Self::Inertia, torque: Self::AngVel) -> Self::AngVel;
44
45    fn wedge(&self, a: Self::Vector, b: Self::Vector) -> Self::AngVel;
46
47    fn velocity_at_point(&self, body: &RigidBody<Self>, p: Self::Point) -> Self::Vector
48    where
49        Self: Sized;
50
51    /// Sleeping and static bodies contribute zero.
52    fn effective_mass_inv(
53        &self,
54        a: &RigidBody<Self>,
55        b: &RigidBody<Self>,
56        contact_point: Self::Point,
57        direction: Self::Vector,
58    ) -> f32
59    where
60        Self: Sized;
61
62    /// Subtracts from A, adds to B.
63    fn apply_contact_impulse(
64        &self,
65        a: &mut RigidBody<Self>,
66        b: &mut RigidBody<Self>,
67        contact_point: Self::Point,
68        direction: Self::Vector,
69        magnitude: f32,
70    ) where
71        Self: Sized;
72}
73
74/// Velocity is transported to the new position before orientation advances.
75pub fn integrate_body<S>(space: &S, body: &mut RigidBody<S>, dt: f32)
76where
77    S: PhysicsSpace,
78    S::Vector: Mul<f32, Output = S::Vector>,
79{
80    if body.inv_mass() == 0.0 {
81        return;
82    }
83
84    let p_old = body.position;
85    let v_dt = body.velocity * dt;
86    let p_new = space.exp(p_old, v_dt);
87    body.velocity = space.parallel_transport(p_old, p_new, body.velocity);
88    body.position = p_new;
89    body.orientation = space.integrate_orientation(body.orientation, body.angular_velocity, dt);
90}
91
92#[cfg(test)]
93mod tests {
94    use super::*;
95    use crate::body::BodyDef;
96    use crate::collider::Collider;
97    use crate::geometry::GeometryStore;
98    use glam::Vec3;
99    use loam_math::EuclideanR3;
100
101    fn row(def: BodyDef<EuclideanR3>) -> RigidBody<EuclideanR3> {
102        def.into_row(&mut GeometryStore::default(), &EuclideanR3)
103    }
104
105    #[test]
106    fn static_body_skips_integration() {
107        let mut body = row(BodyDef::<EuclideanR3>::fixed(
108            Vec3::ZERO,
109            Collider::sphere_at_origin(0.5),
110            1.0,
111            &EuclideanR3,
112        )
113        .unwrap());
114        body.velocity = Vec3::new(10.0, 0.0, 0.0);
115        integrate_body(&EuclideanR3, &mut body, 1.0);
116        assert_eq!(body.position, Vec3::ZERO);
117    }
118
119    #[test]
120    fn dynamic_body_in_e3_moves_linearly() {
121        let mut body = row(BodyDef::<EuclideanR3>::new(
122            Vec3::ZERO,
123            Vec3::new(1.0, 2.0, -3.0),
124            Collider::sphere_at_origin(0.1),
125            1.0,
126            0.1,
127            &EuclideanR3,
128        )
129        .unwrap());
130        integrate_body(&EuclideanR3, &mut body, 0.5);
131        assert_eq!(body.position, Vec3::new(0.5, 1.0, -1.5));
132        assert_eq!(body.velocity, Vec3::new(1.0, 2.0, -3.0));
133    }
134
135    #[test]
136    fn zero_dt_does_not_advance_state() {
137        let mut body = row(BodyDef::<EuclideanR3>::new(
138            Vec3::new(2.0, 3.0, 5.0),
139            Vec3::new(7.0, 11.0, 13.0),
140            Collider::sphere_at_origin(0.1),
141            1.0,
142            0.1,
143            &EuclideanR3,
144        )
145        .unwrap());
146        let before = (body.position, body.velocity);
147        integrate_body(&EuclideanR3, &mut body, 0.0);
148        assert_eq!((body.position, body.velocity), before);
149    }
150}
loam_physics/integrator.rs

loam_physics/
integrator.rs
