loam_physics/
manifold.rs

1//! Contact anchors and impulses persist across steps; manifold keys use generational handles.
2
3use crate::body::{BodyId, RigidBody};
4use crate::collision::VectorOps;
5use crate::integrator::PhysicsSpace;
6use crate::response::Contact;
7use crate::response::FRICTION_COEFF;
8
9pub const MAX_POINTS: usize = 4;
10
11// Coumans, Bullet Physics SDK 3.x, gContactBreakingThreshold.
12pub const CONTACT_BREAK_DISTANCE: f32 = 0.02;
13
14const CONTACT_BREAK_DISTANCE_SQ: f32 = CONTACT_BREAK_DISTANCE * CONTACT_BREAK_DISTANCE;
15
16const MERGE_RADIUS_SQ: f32 = CONTACT_BREAK_DISTANCE_SQ;
17
18#[cfg_attr(feature = "persist", derive(serde::Serialize, serde::Deserialize))]
19#[cfg_attr(
20    feature = "persist",
21    serde(bound(
22        serialize = "S::Point: serde::Serialize, S::Vector: serde::Serialize, S::Iso: serde::Serialize, S::AngVel: serde::Serialize, S::Inertia: serde::Serialize",
23        deserialize = "S::Point: serde::Deserialize<'de>, S::Vector: serde::Deserialize<'de>, S::Iso: serde::Deserialize<'de>, S::AngVel: serde::Deserialize<'de>, S::Inertia: serde::Deserialize<'de>"
24    ))
25)]
26pub struct ContactPoint<S: PhysicsSpace> {
27    pub world_point: S::Point,
28    /// Witness in A's local frame.
29    pub anchor_a: S::Vector,
30    /// Witness in B's local frame.
31    pub anchor_b: S::Vector,
32    /// Unit, from A toward B.
33    pub normal: S::Vector,
34    pub penetration: f32,
35    pub normal_impulse: f32,
36    /// Valid within one step only: the slide direction can flip.
37    pub tangent_dir: S::Vector,
38    /// Cleared before each step; stored as a magnitude.
39    pub tangent_impulse: f32,
40    /// Snapshot before the warm-start, constant across the PGS iterations.
41    pub velocity_bias: f32,
42}
43
44impl<S: PhysicsSpace> Clone for ContactPoint<S> {
45    fn clone(&self) -> Self {
46        *self
47    }
48}
49
50impl<S: PhysicsSpace> Copy for ContactPoint<S> {}
51
52#[cfg_attr(feature = "persist", derive(serde::Serialize, serde::Deserialize))]
53#[cfg_attr(
54    feature = "persist",
55    serde(bound(
56        serialize = "S::Point: serde::Serialize, S::Vector: serde::Serialize, S::Iso: serde::Serialize, S::AngVel: serde::Serialize, S::Inertia: serde::Serialize",
57        deserialize = "S::Point: serde::Deserialize<'de>, S::Vector: serde::Deserialize<'de>, S::Iso: serde::Deserialize<'de>, S::AngVel: serde::Deserialize<'de>, S::Inertia: serde::Deserialize<'de>"
58    ))
59)]
60pub struct Manifold<S: PhysicsSpace> {
61    /// Always `< body_b`.
62    pub body_a: BodyId,
63    pub body_b: BodyId,
64    /// Set on first contact and kept.
65    pub restitution: f32,
66    pub points: Vec<ContactPoint<S>>,
67}
68
69impl<S: PhysicsSpace> Clone for Manifold<S> {
70    fn clone(&self) -> Self {
71        Self {
72            body_a: self.body_a,
73            body_b: self.body_b,
74            restitution: self.restitution,
75            points: self.points.clone(),
76        }
77    }
78
79    fn clone_from(&mut self, source: &Self) {
80        self.body_a = source.body_a;
81        self.body_b = source.body_b;
82        self.restitution = source.restitution;
83        self.points.clone_from(&source.points);
84    }
85}
86
87impl<S: PhysicsSpace> Manifold<S>
88where
89    S::Vector: VectorOps,
90{
91    pub fn new(body_a: BodyId, body_b: BodyId, restitution: f32) -> Self {
92        debug_assert!(body_a < body_b);
93        Self {
94            body_a,
95            body_b,
96            restitution,
97            points: Vec::with_capacity(MAX_POINTS),
98        }
99    }
100
101    pub(crate) fn reset(&mut self, body_a: BodyId, body_b: BodyId, restitution: f32) {
102        debug_assert!(body_a < body_b);
103        self.body_a = body_a;
104        self.body_b = body_b;
105        self.restitution = restitution;
106        self.points.clear();
107    }
108
109    pub(crate) fn is_valid(&self, space: &S, body_a: &RigidBody<S>, body_b: &RigidBody<S>) -> bool
110    where
111        S::Point: Copy + std::ops::Sub<Output = S::Vector>,
112    {
113        if !self.restitution.is_finite()
114            || self.restitution < 0.0
115            || self.points.is_empty()
116            || self.points.len() > MAX_POINTS
117        {
118            return false;
119        }
120        self.points.iter().all(|point| {
121            let normal_length = VectorOps::length_squared(point.normal);
122            let tangent_length = VectorOps::length_squared(point.tangent_dir);
123            if !space.valid_point(point.world_point)
124                || !space.valid_vector(point.anchor_a)
125                || !space.valid_vector(point.anchor_b)
126                || !space.valid_vector(point.normal)
127                || !space.valid_vector(point.tangent_dir)
128                || (normal_length - 1.0).abs() > 1.0e-4
129                || !point.penetration.is_finite()
130                || !point.normal_impulse.is_finite()
131                || point.normal_impulse < 0.0
132                || !point.tangent_impulse.is_finite()
133                || point.tangent_impulse < 0.0
134                || !point.velocity_bias.is_finite()
135            {
136                return false;
137            }
138            if point.tangent_impulse == 0.0 {
139                if tangent_length != 0.0 {
140                    return false;
141                }
142            } else {
143                let limit = point.normal_impulse * FRICTION_COEFF;
144                let slack = f32::EPSILON * limit.abs().max(1.0);
145                if (tangent_length - 1.0).abs() > 1.0e-4
146                    || VectorOps::dot(point.normal, point.tangent_dir).abs() > 1.0e-4
147                    || point.tangent_impulse > limit + slack
148                {
149                    return false;
150                }
151            }
152            let anchor_a = anchor_world_point(space, body_a, point.anchor_a);
153            let anchor_b = anchor_world_point(space, body_b, point.anchor_b);
154            if !space.valid_point(anchor_a) || !space.valid_point(anchor_b) {
155                return false;
156            }
157            let gap = space.log(anchor_a, anchor_b);
158            if !space.valid_vector(gap) {
159                return false;
160            }
161            let midpoint = space.exp(anchor_a, gap * 0.5);
162            let midpoint_error = space.log(midpoint, point.world_point);
163            let penetration_error = point.penetration + VectorOps::dot(gap, point.normal);
164            space.valid_point(midpoint)
165                && space.valid_vector(midpoint_error)
166                && VectorOps::length(midpoint_error) <= CONTACT_BREAK_DISTANCE
167                && penetration_error.is_finite()
168                && penetration_error.abs() <= CONTACT_BREAK_DISTANCE
169        })
170    }
171
172    /// Retains anchors within [`CONTACT_BREAK_DISTANCE`]; bodies must match the manifold key.
173    pub fn refresh(&mut self, space: &S, a: &RigidBody<S>, b: &RigidBody<S>) {
174        self.points.retain_mut(|cp| {
175            let pa = anchor_world_point(space, a, cp.anchor_a);
176            let pb = anchor_world_point(space, b, cp.anchor_b);
177            let gap = space.log(pa, pb);
178            let separation = VectorOps::dot(gap, cp.normal);
179            if separation > CONTACT_BREAK_DISTANCE {
180                return false;
181            }
182            let tangential = gap - cp.normal * separation;
183            if VectorOps::length_squared(tangential) > CONTACT_BREAK_DISTANCE_SQ {
184                return false;
185            }
186            cp.world_point = space.exp(pa, gap * 0.5);
187            cp.penetration = -separation;
188            true
189        });
190    }
191
192    /// Nearby contacts retain impulses; a full manifold replaces its weakest contact.
193    pub fn add_or_update(
194        &mut self,
195        space: &S,
196        a: &RigidBody<S>,
197        b: &RigidBody<S>,
198        contact: Contact<S>,
199    ) where
200        S::Point: Copy + std::ops::Sub<Output = S::Vector>,
201    {
202        let new_point = contact.point;
203        let half_gap = contact.normal * (0.5 * contact.penetration);
204        let anchor_a = local_anchor(space, a, space.exp(new_point, half_gap));
205        let anchor_b = local_anchor(space, b, space.exp(new_point, -half_gap));
206
207        for cp in &mut self.points {
208            let delta = new_point - cp.world_point;
209            if VectorOps::length_squared(delta) < MERGE_RADIUS_SQ {
210                cp.world_point = new_point;
211                cp.anchor_a = anchor_a;
212                cp.anchor_b = anchor_b;
213                cp.normal_impulse *= VectorOps::dot(cp.normal, contact.normal).max(0.0);
214                cp.tangent_impulse = 0.0;
215                cp.tangent_dir = VectorOps::zero();
216                cp.normal = contact.normal;
217                cp.penetration = contact.penetration;
218                return;
219            }
220        }
221
222        let fresh = ContactPoint {
223            world_point: new_point,
224            anchor_a,
225            anchor_b,
226            normal: contact.normal,
227            penetration: contact.penetration,
228            normal_impulse: 0.0,
229            tangent_dir: VectorOps::zero(),
230            tangent_impulse: 0.0,
231            velocity_bias: 0.0,
232        };
233
234        if self.points.len() < MAX_POINTS {
235            self.points.push(fresh);
236        } else {
237            if let Some((worst, _)) = self.points.iter().enumerate().min_by(|(_, a), (_, b)| {
238                let sa = a.normal_impulse + a.tangent_impulse.abs();
239                let sb = b.normal_impulse + b.tangent_impulse.abs();
240                sa.partial_cmp(&sb).unwrap_or(std::cmp::Ordering::Equal)
241            }) {
242                self.points[worst] = fresh;
243            }
244        }
245    }
246}
247
248// Exact inverses only because every `PhysicsSpace` is flat.
249fn local_anchor<S: PhysicsSpace>(space: &S, body: &RigidBody<S>, p: S::Point) -> S::Vector {
250    let lever = space.log(body.position, p);
251    space.iso_transport(space.iso_inverse(body.orientation), body.position, lever)
252}
253
254fn anchor_world_point<S: PhysicsSpace>(
255    space: &S,
256    body: &RigidBody<S>,
257    anchor: S::Vector,
258) -> S::Point {
259    let lever = space.iso_transport(body.orientation, body.position, anchor);
260    space.exp(body.position, lever)
261}
262
263pub const DEFAULT_PGS_ITERS: usize = 8;
264
265pub const BAUMGARTE_BETA: f32 = 0.2;
266
267/// Penetration tolerated without bias, in world units.
268pub const PENETRATION_SLOP: f32 = 0.005;
269
270pub const MAX_LINEAR_CORRECTION: f32 = 0.5;
271
272/// Restitution is suppressed below this approach speed, in world units per second.
273pub const RESTITUTION_THRESHOLD: f32 = 1.0;
274
275#[cfg(test)]
276mod tests {
277    use super::*;
278    use crate::body::BodyDef;
279    use crate::collider::Collider;
280    use crate::geometry::GeometryStore;
281    use glam::Vec2;
282    use loam_math::{Bivector, Bivector2, EuclideanR2};
283
284    const SPACE: EuclideanR2 = EuclideanR2;
285
286    fn contact(point: Vec2, normal: Vec2, penetration: f32) -> Contact<EuclideanR2> {
287        Contact {
288            normal,
289            point,
290            penetration,
291            restitution: 0.0,
292        }
293    }
294
295    fn body(position: Vec2) -> RigidBody<EuclideanR2> {
296        BodyDef::new(
297            position,
298            Vec2::ZERO,
299            Collider::sphere_at_origin(1.0),
300            1.0,
301            1.0,
302            &SPACE,
303        )
304        .unwrap()
305        .into_row(&mut GeometryStore::default(), &SPACE)
306    }
307
308    fn resting_pair() -> (RigidBody<EuclideanR2>, RigidBody<EuclideanR2>) {
309        (body(Vec2::new(0.0, 1.0)), body(Vec2::new(0.0, -1.0)))
310    }
311
312    #[test]
313    fn merged_normal_projects_the_warm_start_without_reversing_it() {
314        let (a, b) = resting_pair();
315        for (normal, expected) in [
316            (Vec2::Y, 4.2),
317            (-Vec2::Y, 0.0),
318            (Vec2::X, 0.0),
319            (Vec2::new(0.8, 0.6), 2.52),
320        ] {
321            let mut manifold = Manifold::new(BodyId::forge(0, 0), BodyId::forge(1, 0), 0.0);
322            manifold.add_or_update(&SPACE, &a, &b, contact(Vec2::ZERO, Vec2::Y, 0.01));
323            manifold.points[0].normal_impulse = 4.2;
324            let point = Vec2::new(0.01, 0.0);
325            manifold.add_or_update(&SPACE, &a, &b, contact(point, normal, 0.05));
326            assert_eq!(manifold.points.len(), 1);
327            let merged = &manifold.points[0];
328            assert_eq!(merged.world_point, point);
329            assert_eq!(merged.normal, normal);
330            assert_eq!(merged.penetration, 0.05);
331            assert!((merged.normal_impulse - expected).abs() < 1e-6);
332        }
333    }
334
335    #[test]
336    fn add_at_max_points_evicts_weakest_slot() {
337        let (a, b) = resting_pair();
338        let mut m: Manifold<EuclideanR2> =
339            Manifold::new(BodyId::forge(0, 0), BodyId::forge(1, 0), 0.0);
340        let bases = [
341            Vec2::new(0.0, 0.0),
342            Vec2::new(1.0, 0.0),
343            Vec2::new(2.0, 0.0),
344            Vec2::new(3.0, 0.0),
345        ];
346        for &p in &bases {
347            m.add_or_update(&SPACE, &a, &b, contact(p, Vec2::Y, 0.0));
348        }
349        assert_eq!(m.points.len(), MAX_POINTS);
350
351        m.points[0].normal_impulse = 5.0;
352        m.points[1].normal_impulse = 3.0;
353        m.points[2].normal_impulse = 0.5;
354        m.points[3].normal_impulse = 4.0;
355        m.points[1].tangent_impulse = -2.0;
356        m.points[2].tangent_impulse = 0.1;
357        m.points[3].tangent_impulse = 1.0;
358
359        let intruder = Vec2::new(10.0, 10.0);
360        m.add_or_update(&SPACE, &a, &b, contact(intruder, Vec2::Y, 0.0));
361
362        assert_eq!(m.points.len(), MAX_POINTS, "size must stay capped");
363        assert!(
364            m.points.iter().any(|cp| cp.world_point == intruder),
365            "new contact must be present",
366        );
367        assert!(
368            m.points.iter().all(|cp| cp.world_point != bases[2]),
369            "the lowest-impulse slot must be evicted",
370        );
371        assert!(
372            m.points.iter().any(|cp| cp.world_point == bases[0]),
373            "high-impulse slots must be retained",
374        );
375    }
376
377    #[test]
378    fn new_slot_below_capacity_leaves_others_intact() {
379        let (a, b) = resting_pair();
380        let mut m: Manifold<EuclideanR2> =
381            Manifold::new(BodyId::forge(0, 0), BodyId::forge(1, 0), 0.0);
382        m.add_or_update(&SPACE, &a, &b, contact(Vec2::ZERO, Vec2::Y, 0.0));
383        m.points[0].normal_impulse = 9.0;
384        m.points[0].tangent_impulse = 0.5;
385        m.points[0].tangent_dir = Vec2::X;
386
387        m.add_or_update(&SPACE, &a, &b, contact(Vec2::new(1.0, 0.0), Vec2::Y, 0.0));
388
389        assert_eq!(m.points.len(), 2);
390        let original = &m.points[0];
391        assert_eq!(original.world_point, Vec2::ZERO, "original geometry intact");
392        assert!((original.normal_impulse - 9.0).abs() < 1e-6);
393        assert!((original.tangent_impulse - 0.5).abs() < 1e-6);
394        assert_eq!(original.tangent_dir, Vec2::X);
395        let added = &m.points[1];
396        assert_eq!(added.normal_impulse, 0.0, "fresh slot starts at zero");
397        assert_eq!(added.tangent_impulse, 0.0);
398    }
399
400    fn touching_manifold(
401        penetration: f32,
402    ) -> (
403        Manifold<EuclideanR2>,
404        RigidBody<EuclideanR2>,
405        RigidBody<EuclideanR2>,
406    ) {
407        let (a, b) = resting_pair();
408        let mut m: Manifold<EuclideanR2> =
409            Manifold::new(BodyId::forge(0, 0), BodyId::forge(1, 0), 0.0);
410        m.add_or_update(&SPACE, &a, &b, contact(Vec2::ZERO, -Vec2::Y, penetration));
411        (m, a, b)
412    }
413
414    #[test]
415    fn refresh_reads_back_the_penetration_the_narrowphase_reported() {
416        let (mut m, a, b) = touching_manifold(0.006);
417        m.refresh(&SPACE, &a, &b);
418        assert_eq!(m.points.len(), 1, "an unmoved contact must survive");
419        assert!(
420            (m.points[0].penetration - 0.006).abs() < 1e-6,
421            "refresh reported {} for a 0.006 penetration, so it is measuring \
422             drift and not separation",
423            m.points[0].penetration,
424        );
425        assert!(
426            (m.points[0].world_point - Vec2::ZERO).length() < 1e-6,
427            "an unmoved contact moved to {}",
428            m.points[0].world_point,
429        );
430    }
431
432    #[test]
433    fn refresh_drops_a_point_the_body_has_lifted_off() {
434        let (mut m, mut a, b) = touching_manifold(0.006);
435        m.points[0].normal_impulse = 3.0;
436        a.position.y += 0.006 + CONTACT_BREAK_DISTANCE + 1e-4;
437        m.refresh(&SPACE, &a, &b);
438        assert!(
439            m.points.is_empty(),
440            "a point past the break distance was solved again with stale \
441             geometry: separation {}",
442            -m.points[0].penetration,
443        );
444    }
445
446    #[test]
447    fn refresh_keeps_a_lifted_point_inside_the_break_distance_with_its_impulse() {
448        let (mut m, mut a, b) = touching_manifold(0.006);
449        m.points[0].normal_impulse = 3.0;
450        m.points[0].tangent_impulse = -0.5;
451        let lift = 0.006 + 0.5 * CONTACT_BREAK_DISTANCE;
452        a.position.y += lift;
453        m.refresh(&SPACE, &a, &b);
454        assert_eq!(m.points.len(), 1, "the point is still inside the band");
455        assert!(
456            (m.points[0].penetration + (lift - 0.006)).abs() < 1e-6,
457            "penetration {} does not track the lift",
458            m.points[0].penetration,
459        );
460        assert!(
461            (m.points[0].normal_impulse - 3.0).abs() < 1e-6
462                && (m.points[0].tangent_impulse + 0.5).abs() < 1e-6,
463            "a surviving point lost its warm-start impulses",
464        );
465    }
466
467    #[test]
468    fn refresh_drops_a_point_the_bodies_slid_apart_across_the_normal() {
469        let (mut m, mut a, b) = touching_manifold(0.006);
470        a.position.x += CONTACT_BREAK_DISTANCE + 1e-4;
471        m.refresh(&SPACE, &a, &b);
472        assert!(
473            m.points.is_empty(),
474            "a point the body slid off kept describing the feature it left",
475        );
476    }
477
478    #[test]
479    fn refresh_follows_a_point_through_a_body_rotation() {
480        let (mut m, mut a, b) = touching_manifold(0.006);
481        a.orientation.rotation = Bivector2(std::f32::consts::FRAC_PI_4).exp();
482        m.refresh(&SPACE, &a, &b);
483        assert!(
484            m.points.is_empty(),
485            "a rotation that swept the contact a full radius away left it in \
486             the manifold",
487        );
488
489        let (mut m, mut a, b) = touching_manifold(0.006);
490        let theta = 0.5 * CONTACT_BREAK_DISTANCE;
491        a.orientation.rotation = Bivector2(theta).exp();
492        m.refresh(&SPACE, &a, &b);
493        assert_eq!(m.points.len(), 1, "a tiny rotation must not break contact");
494        let expected = 0.006 - (1.0 - theta.cos());
495        assert!(
496            (m.points[0].penetration - expected).abs() < 2e-6,
497            "penetration {} does not track the turn's sagitta {expected}",
498            m.points[0].penetration,
499        );
500    }
501
502    #[test]
503    fn refresh_retains_in_slot_order() {
504        let (a, b) = resting_pair();
505        let mut m: Manifold<EuclideanR2> =
506            Manifold::new(BodyId::forge(0, 0), BodyId::forge(1, 0), 0.0);
507        let kept = [Vec2::new(-0.5, 0.0), Vec2::new(0.5, 0.0)];
508        m.add_or_update(&SPACE, &a, &b, contact(kept[0], -Vec2::Y, 0.006));
509        m.add_or_update(&SPACE, &a, &b, contact(Vec2::ZERO, -Vec2::Y, -0.1));
510        m.add_or_update(&SPACE, &a, &b, contact(kept[1], -Vec2::Y, 0.006));
511
512        m.refresh(&SPACE, &a, &b);
513
514        let surviving: Vec<Vec2> = m.points.iter().map(|cp| cp.world_point).collect();
515        assert_eq!(
516            surviving.len(),
517            2,
518            "the separated slot must be the only one dropped"
519        );
520        for (slot, expected) in surviving.iter().zip(kept) {
521            assert!(
522                (*slot - expected).length() < 1e-6,
523                "retain reordered the slots: {surviving:?}",
524            );
525        }
526    }
527}
loam_physics/manifold.rs

loam_physics/
manifold.rs
