loam_physics/
field_contact.rs

1use std::collections::HashMap;
2
3use loam_shape::field::{DistanceField, FieldKind};
4
5use crate::body::RigidBody;
6use crate::collider::ColliderKind;
7use crate::geometry::GeometryStore;
8use crate::integrator::PhysicsSpace;
9
10#[derive(Clone, Copy, Debug, PartialEq, Eq)]
11pub enum FieldRefusal {
12    /// No query is registered for the body's collider kind.
13    Collider(ColliderKind),
14    Dimension(u32),
15    /// The field's value is not an exact distance, so it is not a separation.
16    Kind(FieldKind),
17    Numerical,
18    DegenerateGradient,
19}
20
21/// `error` bounds the separation sample and the absolute field value at `witness`.
22pub struct FieldContact<S: PhysicsSpace> {
23    pub separation: f32,
24    pub normal: S::Vector,
25    pub witness: S::Point,
26    pub error: f32,
27}
28
29impl<S: PhysicsSpace> Clone for FieldContact<S> {
30    fn clone(&self) -> Self {
31        *self
32    }
33}
34
35impl<S: PhysicsSpace> Copy for FieldContact<S> {}
36
37pub type FieldContactFn<S> = fn(
38    body: &RigidBody<S>,
39    geometry: &GeometryStore,
40    field: &dyn DistanceField,
41    space: &S,
42) -> Result<FieldContact<S>, FieldRefusal>;
43
44pub struct FieldNarrowphase<S: PhysicsSpace> {
45    dispatch: HashMap<ColliderKind, FieldContactFn<S>>,
46    order: Vec<ColliderKind>,
47}
48
49impl<S: PhysicsSpace> Default for FieldNarrowphase<S> {
50    fn default() -> Self {
51        Self {
52            dispatch: HashMap::new(),
53            order: Vec::new(),
54        }
55    }
56}
57
58impl<S: PhysicsSpace> FieldNarrowphase<S> {
59    pub fn new() -> Self {
60        Self::default()
61    }
62
63    pub fn register(&mut self, body: ColliderKind, query: FieldContactFn<S>) {
64        if self.dispatch.insert(body, query).is_none() {
65            self.order.push(body);
66        }
67    }
68
69    pub fn registrations(&self) -> &[ColliderKind] {
70        &self.order
71    }
72
73    pub fn test(
74        &self,
75        body: &RigidBody<S>,
76        geometry: &GeometryStore,
77        field: &dyn DistanceField,
78        space: &S,
79    ) -> Result<FieldContact<S>, FieldRefusal> {
80        let kind = body.collider().kind();
81        match self.dispatch.get(&kind) {
82            Some(&query) => query(body, geometry, field, space),
83            None => Err(FieldRefusal::Collider(kind)),
84        }
85    }
86}
87
88#[cfg(feature = "r3")]
89mod r3 {
90    use glam::Vec3;
91    use loam_math::EuclideanR3;
92    use loam_shape::field::{DistanceField, FieldKind, MIN_GRADIENT_NORM};
93
94    use super::{FieldContact, FieldNarrowphase, FieldRefusal};
95    use crate::body::RigidBody;
96    use crate::collider::{Collider, ColliderKind};
97    use crate::geometry::GeometryStore;
98
99    fn finite_upper(value: f64) -> Option<f32> {
100        if !value.is_finite() || value > f32::MAX as f64 {
101            return None;
102        }
103        let mut rounded = value as f32;
104        if (rounded as f64) < value {
105            rounded = rounded.next_up();
106        }
107        Some(rounded)
108    }
109
110    /// Separation is the field distance at the sphere's center minus its radius; the field must be an exact distance.
111    pub fn sphere_against_field(
112        body: &RigidBody<EuclideanR3>,
113        geometry: &GeometryStore,
114        field: &dyn DistanceField,
115        _space: &EuclideanR3,
116    ) -> Result<FieldContact<EuclideanR3>, FieldRefusal> {
117        let dimension = field.dimension();
118        if dimension != 3 {
119            return Err(FieldRefusal::Dimension(dimension));
120        }
121        if field.field_kind() != FieldKind::ExactDistance {
122            return Err(FieldRefusal::Kind(field.field_kind()));
123        }
124        let Some(&Collider::Sphere { radius, .. }) = geometry.get(body.collider()) else {
125            return Err(FieldRefusal::Collider(body.collider().kind()));
126        };
127        let center = body.position;
128        let point = [center.x, center.y, center.z, 0.0];
129        let distance = field.distance(point);
130        let distance_error = field.error_at(point);
131        if !distance.is_finite() || !distance_error.is_finite() || distance_error < 0.0 {
132            return Err(FieldRefusal::Numerical);
133        }
134        let gradient = field.gradient(point);
135        let direction = Vec3::new(gradient[0], gradient[1], gradient[2]);
136        let length = direction.length();
137        if !length.is_finite() {
138            return Err(FieldRefusal::Numerical);
139        }
140        if length < MIN_GRADIENT_NORM {
141            return Err(FieldRefusal::DegenerateGradient);
142        }
143        let normal = direction / length;
144        if !normal.is_finite() {
145            return Err(FieldRefusal::Numerical);
146        }
147        let separation = distance - radius;
148        let witness = center - normal * distance;
149        if !separation.is_finite() || !witness.is_finite() {
150            return Err(FieldRefusal::Numerical);
151        }
152        let separation_roundoff = (separation as f64 - (distance as f64 - radius as f64)).abs();
153        let Some(separation_error) = finite_upper(distance_error as f64 + separation_roundoff)
154        else {
155            return Err(FieldRefusal::Numerical);
156        };
157        let witness_point = [witness.x, witness.y, witness.z, 0.0];
158        let witness_distance = field.distance(witness_point);
159        let witness_error = field.error_at(witness_point);
160        if !witness_distance.is_finite() || !witness_error.is_finite() || witness_error < 0.0 {
161            return Err(FieldRefusal::Numerical);
162        }
163        let residual_bound = witness_distance.abs() as f64 + witness_error as f64;
164        let Some(witness_residual) = finite_upper(residual_bound) else {
165            return Err(FieldRefusal::Numerical);
166        };
167        let error = separation_error.max(witness_residual);
168        if !error.is_finite() {
169            return Err(FieldRefusal::Numerical);
170        }
171        Ok(FieldContact {
172            separation,
173            normal,
174            witness,
175            error,
176        })
177    }
178
179    pub fn register_field_contacts(narrowphase: &mut FieldNarrowphase<EuclideanR3>) {
180        narrowphase.register(ColliderKind::Sphere, sphere_against_field);
181    }
182}
183
184#[cfg(feature = "r3")]
185pub use r3::{register_field_contacts, sphere_against_field};
186
187#[cfg(all(test, feature = "r3"))]
188mod tests {
189    use glam::Vec3;
190    use loam_math::EuclideanR3;
191    use loam_shape::field::{DistanceField, FieldKind};
192
193    use super::*;
194    use crate::body::BodyId;
195    use crate::collider::Collider;
196    use crate::edit::EditError;
197    use crate::euclidean_r3::{halfspace_body_r3, sphere_body_r3};
198    use crate::manifold::PENETRATION_SLOP;
199    use crate::world::{FieldId, World};
200
201    struct Ground;
202
203    impl DistanceField for Ground {
204        fn field_kind(&self) -> FieldKind {
205            FieldKind::ExactDistance
206        }
207
208        fn dimension(&self) -> u32 {
209            3
210        }
211
212        fn distance(&self, point: [f32; 4]) -> f32 {
213            point[1]
214        }
215
216        fn error_at(&self, _point: [f32; 4]) -> f32 {
217            0.0
218        }
219    }
220
221    struct Bounded;
222
223    impl DistanceField for Bounded {
224        fn field_kind(&self) -> FieldKind {
225            FieldKind::ConservativeBound
226        }
227
228        fn dimension(&self) -> u32 {
229            3
230        }
231
232        fn distance(&self, point: [f32; 4]) -> f32 {
233            point[1] * 0.5
234        }
235
236        fn error_at(&self, _point: [f32; 4]) -> f32 {
237            1.0
238        }
239    }
240
241    struct UncertainGround;
242
243    impl DistanceField for UncertainGround {
244        fn field_kind(&self) -> FieldKind {
245            FieldKind::ExactDistance
246        }
247
248        fn dimension(&self) -> u32 {
249            3
250        }
251
252        fn distance(&self, point: [f32; 4]) -> f32 {
253            point[1]
254        }
255
256        fn error_at(&self, _point: [f32; 4]) -> f32 {
257            0.1
258        }
259    }
260
261    struct Shell;
262
263    impl DistanceField for Shell {
264        fn field_kind(&self) -> FieldKind {
265            FieldKind::ExactDistance
266        }
267
268        fn dimension(&self) -> u32 {
269            3
270        }
271
272        fn distance(&self, point: [f32; 4]) -> f32 {
273            (point[0] * point[0] + point[1] * point[1] + point[2] * point[2]).sqrt() - 1.0
274        }
275
276        fn error_at(&self, _point: [f32; 4]) -> f32 {
277            0.0
278        }
279    }
280
281    fn narrowphase() -> FieldNarrowphase<EuclideanR3> {
282        let mut np = FieldNarrowphase::new();
283        register_field_contacts(&mut np);
284        np
285    }
286
287    fn one_body(collider: Collider, at: Vec3) -> World<EuclideanR3> {
288        let mut world = World::new(EuclideanR3);
289        world.push_body(
290            crate::body::BodyDef::new(at, Vec3::ZERO, collider, 1.0, 1.0, &EuclideanR3)
291                .expect("body def"),
292        );
293        world
294    }
295
296    const RADIUS: f32 = 0.5;
297
298    fn ground_world() -> (World<EuclideanR3>, BodyId, FieldId, BodyId) {
299        let mut world = World::new(EuclideanR3);
300        register_field_contacts(&mut world.field_narrowphase);
301        world.set_gravity(Some(Vec3::new(0.0, -9.8, 0.0))).unwrap();
302        let anchor = world.push_body(halfspace_body_r3(Vec3::Y, 0.0).expect("anchor"));
303        world.bodies[anchor].restitution = 0.0;
304        let field = world
305            .insert_field(anchor, Box::new(Ground))
306            .expect("field handle");
307        let ball = world.push_body(
308            sphere_body_r3(Vec3::new(0.0, 2.0, 0.0), Vec3::ZERO, RADIUS, 1.0).expect("ball"),
309        );
310        world.bodies[ball].restitution = 0.0;
311        (world, anchor, field, ball)
312    }
313
314    #[test]
315    fn a_certain_field_supports_a_sphere_while_an_uncertain_sign_creates_no_contact() {
316        let (mut world, anchor, field, ball) = ground_world();
317        assert_eq!(
318            world.bind_field(anchor, field),
319            Err(EditError::AnchorBindsOwnField)
320        );
321        world.bind_field(ball, field).expect("binding");
322
323        for _ in 0..600 {
324            world.step(1.0 / 240.0).unwrap();
325        }
326        let rest = world.bodies[ball].position.y;
327        assert!(
328            (rest - (RADIUS - PENETRATION_SLOP)).abs() < 1.0e-3,
329            "the ball rests at {rest}, not at radius minus the solver slop"
330        );
331
332        let mut uncertain = World::new(EuclideanR3);
333        register_field_contacts(&mut uncertain.field_narrowphase);
334        let anchor = uncertain.push_body(halfspace_body_r3(Vec3::Y, 0.0).expect("anchor"));
335        let field = uncertain
336            .insert_field(anchor, Box::new(UncertainGround))
337            .expect("field handle");
338        let ball = uncertain
339            .push_body(sphere_body_r3(Vec3::Y * 0.45, Vec3::ZERO, RADIUS, 1.0).expect("ball"));
340        uncertain.bind_field(ball, field).expect("binding");
341        uncertain.step(0.0).unwrap();
342        assert_eq!(uncertain.manifolds().len(), 0);
343    }
344
345    #[test]
346    fn a_binding_made_after_a_snapshot_is_gone_after_the_restore() {
347        let (mut world, _, field, ball) = ground_world();
348        let saved = world.snapshot();
349        world.bind_field(ball, field).expect("binding");
350        assert_eq!(world.field_bindings().len(), 1);
351        world.restore(&saved).expect("restore");
352        assert!(
353            world.field_bindings().is_empty(),
354            "the binding survived a restore that predates it"
355        );
356    }
357
358    #[test]
359    fn a_restore_whose_field_list_differs_is_refused_and_leaves_the_world_alone() {
360        let (mut world, _, field, ball) = ground_world();
361        world.bind_field(ball, field).expect("binding");
362        for _ in 0..60 {
363            world.step(1.0 / 240.0).unwrap();
364        }
365        let saved = world.snapshot();
366        let second = world.push_body(halfspace_body_r3(Vec3::Y, -8.0).expect("anchor"));
367        world
368            .insert_field(second, Box::new(Ground))
369            .expect("field handle");
370        let height = world.bodies[ball].position.y;
371
372        assert_eq!(world.restore(&saved), Err(EditError::FieldMismatch));
373        assert_eq!(world.bodies[ball].position.y, height);
374        assert_eq!(world.field_bindings().len(), 1);
375    }
376
377    #[test]
378    fn a_conservative_bound_field_refuses_a_sphere_and_names_the_field_kind() {
379        let world = one_body(Collider::sphere_at_origin(0.5), Vec3::new(0.0, 1.0, 0.0));
380        let id = world.bodies.id_at(0);
381        assert_eq!(
382            narrowphase()
383                .test(&world.bodies[id], world.geometry(), &Bounded, &EuclideanR3)
384                .err(),
385            Some(FieldRefusal::Kind(FieldKind::ConservativeBound))
386        );
387    }
388
389    #[test]
390    fn a_hull_body_refuses_an_exact_field_and_names_the_collider_kind() {
391        let corners: Vec<Vec3> = (0..8)
392            .map(|i| {
393                Vec3::new(
394                    if i & 1 == 0 { -0.5 } else { 0.5 },
395                    if i & 2 == 0 { -0.5 } else { 0.5 },
396                    if i & 4 == 0 { -0.5 } else { 0.5 },
397                )
398            })
399            .collect();
400        let world = one_body(
401            Collider::ConvexPolytope3D { vertices: corners },
402            Vec3::new(0.0, 1.0, 0.0),
403        );
404        let id = world.bodies.id_at(0);
405        assert_eq!(
406            narrowphase()
407                .test(&world.bodies[id], world.geometry(), &Ground, &EuclideanR3)
408                .err(),
409            Some(FieldRefusal::Collider(ColliderKind::ConvexPolytope3D))
410        );
411    }
412
413    #[test]
414    fn a_degenerate_gradient_refuses_rather_than_inventing_a_normal() {
415        let world = one_body(Collider::sphere_at_origin(0.5), Vec3::ZERO);
416        let id = world.bodies.id_at(0);
417        assert_eq!(
418            narrowphase()
419                .test(&world.bodies[id], world.geometry(), &Shell, &EuclideanR3)
420                .err(),
421            Some(FieldRefusal::DegenerateGradient)
422        );
423    }
424}
loam_physics/field_contact.rs

loam_physics/
field_contact.rs
