loam_physics/
body.rs

1use std::ops::{Add, Deref, Index, IndexMut, Mul};
2
3use loam_math::Bivector;
4
5use crate::collider::{Collider, ColliderKind};
6use crate::collision::VectorOps;
7use crate::edit::EditError;
8use crate::geometry::{ColliderRef, GeometryStore};
9use crate::integrator::PhysicsSpace;
10
11#[cfg_attr(feature = "persist", derive(serde::Serialize, serde::Deserialize))]
12#[cfg_attr(
13    feature = "persist",
14    serde(bound(
15        serialize = "S::Point: serde::Serialize, S::Vector: serde::Serialize, S::Iso: serde::Serialize, S::AngVel: serde::Serialize, S::Inertia: serde::Serialize",
16        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>"
17    ))
18)]
19pub struct RigidBody<S: PhysicsSpace> {
20    pub position: S::Point,
21    pub velocity: S::Vector,
22    pub orientation: S::Iso,
23    pub angular_velocity: S::AngVel,
24
25    mass: f32,
26    inv_mass: f32,
27    sleeping: bool,
28    pub inertia: S::Inertia,
29
30    collider: ColliderRef,
31
32    pub restitution: f32,
33
34    pub collision_group: u32,
35    pub collision_mask: u32,
36}
37
38impl<S: PhysicsSpace> Clone for RigidBody<S> {
39    fn clone(&self) -> Self {
40        *self
41    }
42}
43
44impl<S: PhysicsSpace> Copy for RigidBody<S> {}
45
46pub struct BodyDef<S: PhysicsSpace> {
47    position: S::Point,
48    velocity: S::Vector,
49    collider: Collider,
50    mass: f32,
51    inertia: S::Inertia,
52}
53
54impl<S: PhysicsSpace> BodyDef<S> {
55    pub fn new(
56        position: S::Point,
57        velocity: S::Vector,
58        collider: Collider,
59        mass: f32,
60        inertia: S::Inertia,
61        space: &S,
62    ) -> Option<Self> {
63        if !valid_mass(mass)
64            || !space.valid_initial_state(position, velocity, inertia)
65            || !valid_collider(space, &collider, mass)
66        {
67            return None;
68        }
69        Some(Self {
70            position,
71            velocity,
72            collider,
73            mass,
74            inertia,
75        })
76    }
77
78    pub fn fixed(
79        position: S::Point,
80        collider: Collider,
81        inertia: S::Inertia,
82        space: &S,
83    ) -> Option<Self>
84    where
85        S::Vector: Default,
86    {
87        Self::new(
88            position,
89            S::Vector::default(),
90            collider,
91            0.0,
92            inertia,
93            space,
94        )
95    }
96
97    pub fn mass(&self) -> f32 {
98        self.mass
99    }
100
101    pub fn collider(&self) -> &Collider {
102        &self.collider
103    }
104
105    pub(crate) fn into_row(self, geometry: &mut GeometryStore, space: &S) -> RigidBody<S> {
106        let inv_mass = if self.mass > 0.0 {
107            1.0 / self.mass
108        } else {
109            0.0
110        };
111        RigidBody {
112            sleeping: false,
113            collision_group: GROUP_DEFAULT,
114            collision_mask: MASK_ALL,
115            position: self.position,
116            velocity: self.velocity,
117            orientation: space.iso_identity(),
118            angular_velocity: <S::AngVel as Bivector>::zero(),
119            mass: self.mass,
120            inv_mass,
121            inertia: self.inertia,
122            collider: geometry.prepare(self.collider),
123            restitution: 0.2,
124        }
125    }
126}
127
128pub const GROUP_DEFAULT: u32 = 1;
129
130pub const MASK_ALL: u32 = u32::MAX;
131
132impl<S: PhysicsSpace> RigidBody<S> {
133    pub fn mass(&self) -> f32 {
134        self.mass
135    }
136
137    /// Sleeping bodies have zero effective inverse mass until woken.
138    pub fn inv_mass(&self) -> f32 {
139        if self.sleeping {
140            0.0
141        } else {
142            self.inv_mass
143        }
144    }
145
146    pub fn is_static(&self) -> bool {
147        self.mass == 0.0
148    }
149
150    pub fn is_sleeping(&self) -> bool {
151        self.sleeping
152    }
153
154    pub fn sleep(&mut self)
155    where
156        S::Vector: Default,
157    {
158        if !self.is_static() {
159            self.velocity = S::Vector::default();
160            self.angular_velocity = S::AngVel::zero();
161            self.sleeping = true;
162        }
163    }
164
165    pub fn wake(&mut self) {
166        self.sleeping = false;
167    }
168
169    /// Invalid mass leaves the body unchanged.
170    pub fn set_mass(&mut self, mass: f32) -> bool {
171        self.set_mass_properties(mass, self.inertia).is_ok()
172    }
173
174    pub fn collider(&self) -> ColliderRef {
175        self.collider
176    }
177
178    pub(crate) fn set_mass_properties(
179        &mut self,
180        mass: f32,
181        inertia: S::Inertia,
182    ) -> Result<(), EditError> {
183        if !valid_mass(mass) {
184            return Err(EditError::InvalidMass);
185        }
186        if mass > 0.0 && is_halfspace_kind(self.collider.kind()) {
187            return Err(EditError::DynamicHalfSpace);
188        }
189        self.mass = mass;
190        self.inv_mass = if mass > 0.0 { 1.0 / mass } else { 0.0 };
191        self.inertia = inertia;
192        self.wake();
193        Ok(())
194    }
195
196    pub(crate) fn replace_collider(
197        &mut self,
198        geometry: &mut GeometryStore,
199        space: &S,
200        collider: Collider,
201        inertia: S::Inertia,
202    ) -> Result<(), EditError> {
203        if self.mass > 0.0 && is_halfspace_kind(collider.kind()) {
204            geometry.stash(collider);
205            return Err(EditError::DynamicHalfSpace);
206        }
207        if !valid_collider(space, &collider, self.mass) {
208            geometry.stash(collider);
209            return Err(EditError::UnsupportedCollider);
210        }
211        let replacement = geometry.prepare(collider);
212        geometry.release(self.collider);
213        self.collider = replacement;
214        self.inertia = inertia;
215        self.wake();
216        Ok(())
217    }
218
219    // Baraff 1997, "Physically Based Modeling: Rigid Body Simulation", colliding contact.
220    pub fn apply_impulse(&mut self, impulse: S::Vector)
221    where
222        S::Vector: Add<Output = S::Vector> + Mul<f32, Output = S::Vector>,
223    {
224        if self.is_static() {
225            return;
226        }
227        self.wake();
228        self.velocity = self.velocity + impulse * self.inv_mass;
229    }
230
231    // Baraff 1997, "Physically Based Modeling: Rigid Body Simulation", colliding contact.
232    pub fn apply_impulse_at_point(&mut self, space: &S, impulse: S::Vector, point: S::Point)
233    where
234        S::Vector: Add<Output = S::Vector> + Mul<f32, Output = S::Vector>,
235    {
236        if self.is_static() {
237            return;
238        }
239        self.wake();
240        self.velocity = self.velocity + impulse * self.inv_mass;
241        let lever = space.log(self.position, point);
242        let torque = space.wedge(lever, impulse);
243        self.angular_velocity =
244            self.angular_velocity + space.apply_inv_inertia(self.inertia, torque);
245    }
246}
247
248fn valid_mass(mass: f32) -> bool {
249    mass.is_finite() && mass >= 0.0 && (mass == 0.0 || mass.recip().is_finite())
250}
251
252fn is_halfspace_kind(kind: ColliderKind) -> bool {
253    matches!(kind, ColliderKind::HalfSpace | ColliderKind::HalfSpace4D)
254}
255
256pub(crate) fn valid_collider<S: PhysicsSpace>(space: &S, collider: &Collider, mass: f32) -> bool {
257    if !space.supports_collider(collider.kind())
258        || (mass > 0.0 && is_halfspace_kind(collider.kind()))
259    {
260        return false;
261    }
262    match collider {
263        Collider::Sphere { center, radius } => {
264            center.is_finite()
265                && *center == glam::Vec3::ZERO
266                && radius.is_finite()
267                && *radius >= 0.0
268        }
269        Collider::HalfSpace { normal, offset } => {
270            normal.is_finite() && offset.is_finite() && (normal.length_squared() - 1.0).abs() < 1e-5
271        }
272        Collider::HalfSpace4D { normal, offset } => {
273            normal.is_finite() && offset.is_finite() && (normal.length_squared() - 1.0).abs() < 1e-5
274        }
275        Collider::Polygon2D { vertices } => convex_ccw_polygon(vertices),
276        Collider::ConvexPolytope3D { vertices } => {
277            !vertices.is_empty() && vertices.iter().all(|v| v.is_finite())
278        }
279        Collider::ConvexPolytope4D { vertices } => {
280            !vertices.is_empty() && vertices.iter().all(|v| v.is_finite())
281        }
282        _ => false,
283    }
284}
285
286fn convex_ccw_polygon(vertices: &[glam::Vec2]) -> bool {
287    if vertices.len() < 3 || !vertices.iter().all(|v| v.is_finite()) {
288        return false;
289    }
290    let mut has_area = false;
291    for i in 0..vertices.len() {
292        let origin = vertices[i];
293        let edge = vertices[(i + 1) % vertices.len()] - origin;
294        for &vertex in vertices {
295            let offset = vertex - origin;
296            let cross = edge.perp_dot(offset);
297            if !cross.is_finite() || cross < 0.0 {
298                return false;
299            }
300            has_area |= cross > 0.0;
301        }
302    }
303    has_area
304}
305
306/// A body handle is valid only in its owning arena and is ordered by slot, then generation.
307#[cfg_attr(feature = "persist", derive(serde::Serialize, serde::Deserialize))]
308#[derive(Clone, Copy, Debug, PartialEq, Eq, PartialOrd, Ord, Hash)]
309pub struct BodyId {
310    slot: u32,
311    generation: u32,
312}
313
314impl BodyId {
315    pub fn slot(self) -> u32 {
316        self.slot
317    }
318
319    pub fn generation(self) -> u32 {
320        self.generation
321    }
322
323    #[cfg(test)]
324    pub(crate) fn forge(slot: u32, generation: u32) -> Self {
325        Self { slot, generation }
326    }
327}
328
329const STALE_HANDLE: &str = "BodyId refers to a despawned body";
330
331#[cfg_attr(feature = "persist", derive(serde::Serialize, serde::Deserialize))]
332#[derive(Clone)]
333struct Slot {
334    generation: u32,
335    dense: Option<u32>,
336}
337
338/// Compaction preserves handles; callers must not exchange bodies between mutable references.
339#[cfg_attr(feature = "persist", derive(serde::Serialize, serde::Deserialize))]
340#[cfg_attr(
341    feature = "persist",
342    serde(bound(
343        serialize = "S::Point: serde::Serialize, S::Vector: serde::Serialize, S::Iso: serde::Serialize, S::AngVel: serde::Serialize, S::Inertia: serde::Serialize",
344        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>"
345    ))
346)]
347pub struct BodyArena<S: PhysicsSpace> {
348    dense: Vec<RigidBody<S>>,
349    ids: Vec<BodyId>,
350    slots: Vec<Slot>,
351    free: Vec<u32>,
352}
353
354impl<S: PhysicsSpace> Clone for BodyArena<S> {
355    fn clone(&self) -> Self {
356        Self {
357            dense: self.dense.clone(),
358            ids: self.ids.clone(),
359            slots: self.slots.clone(),
360            free: self.free.clone(),
361        }
362    }
363}
364
365impl<S: PhysicsSpace> Default for BodyArena<S> {
366    fn default() -> Self {
367        Self::new()
368    }
369}
370
371impl<S: PhysicsSpace> BodyArena<S> {
372    pub fn new() -> Self {
373        Self {
374            dense: Vec::new(),
375            ids: Vec::new(),
376            slots: Vec::new(),
377            free: Vec::new(),
378        }
379    }
380
381    pub fn spawn(&mut self, body: RigidBody<S>) -> BodyId {
382        let dense = self.dense.len() as u32;
383        let slot = match self.free.pop() {
384            Some(slot) => {
385                self.slots[slot as usize].dense = Some(dense);
386                slot
387            }
388            None => {
389                self.slots.push(Slot {
390                    generation: 0,
391                    dense: Some(dense),
392                });
393                (self.slots.len() - 1) as u32
394            }
395        };
396        let id = BodyId {
397            slot,
398            generation: self.slots[slot as usize].generation,
399        };
400        self.dense.push(body);
401        self.ids.push(id);
402        id
403    }
404
405    pub fn despawn(&mut self, id: BodyId) -> Option<RigidBody<S>> {
406        let dense = self.dense_index(id)?;
407        let slot = &mut self.slots[id.slot as usize];
408        slot.dense = None;
409        if let Some(next) = slot.generation.checked_add(1) {
410            slot.generation = next;
411            self.free.push(id.slot);
412        }
413
414        let removed = self.dense.swap_remove(dense);
415        self.ids.swap_remove(dense);
416        if let Some(&moved) = self.ids.get(dense) {
417            self.slots[moved.slot as usize].dense = Some(dense as u32);
418        }
419        Some(removed)
420    }
421
422    /// Valid only until the next [`Self::despawn`].
423    pub fn dense_index(&self, id: BodyId) -> Option<usize> {
424        let slot = self.slots.get(id.slot as usize)?;
425        if slot.generation != id.generation {
426            return None;
427        }
428        slot.dense.map(|dense| dense as usize)
429    }
430
431    pub fn id_at(&self, dense: usize) -> BodyId {
432        self.ids[dense]
433    }
434
435    pub fn get(&self, id: BodyId) -> Option<&RigidBody<S>> {
436        self.dense_index(id).map(|dense| &self.dense[dense])
437    }
438
439    pub fn get_mut(&mut self, id: BodyId) -> Option<&mut RigidBody<S>> {
440        match self.dense_index(id) {
441            Some(dense) => Some(&mut self.dense[dense]),
442            None => None,
443        }
444    }
445
446    pub fn iter_mut(&mut self) -> std::slice::IterMut<'_, RigidBody<S>> {
447        self.dense.iter_mut()
448    }
449
450    pub(crate) fn dense_mut(&mut self) -> &mut [RigidBody<S>] {
451        &mut self.dense
452    }
453
454    pub(crate) fn validate(&self, space: &S, geometry: &GeometryStore) -> Result<(), EditError>
455    where
456        S::Vector: VectorOps,
457        S::AngVel: PartialEq,
458    {
459        if self.dense.len() != self.ids.len() {
460            return Err(EditError::InvalidBodyArena);
461        }
462        let mut occupied = vec![false; self.slots.len()];
463        for (dense, (&id, body)) in self.ids.iter().zip(&self.dense).enumerate() {
464            let Ok(dense_index) = u32::try_from(dense) else {
465                return Err(EditError::InvalidBodyArena);
466            };
467            let Some(slot) = self.slots.get(id.slot as usize) else {
468                return Err(EditError::InvalidBodyArena);
469            };
470            if occupied[id.slot as usize]
471                || slot.generation != id.generation
472                || slot.dense != Some(dense_index)
473            {
474                return Err(EditError::InvalidBodyArena);
475            }
476            occupied[id.slot as usize] = true;
477            let expected_inv_mass = if body.mass > 0.0 {
478                body.mass.recip()
479            } else {
480                0.0
481            };
482            let Some(collider) = geometry.get(body.collider) else {
483                return Err(EditError::InvalidGeometry);
484            };
485            if !valid_mass(body.mass)
486                || body.inv_mass != expected_inv_mass
487                || !space.valid_initial_state(body.position, body.velocity, body.inertia)
488                || !space.valid_orientation(body.orientation)
489                || !space.valid_angular_velocity(body.angular_velocity)
490                || (body.sleeping
491                    && (body.mass == 0.0
492                        || body.velocity != S::Vector::zero()
493                        || body.angular_velocity != S::AngVel::zero()))
494                || !body.restitution.is_finite()
495                || body.restitution < 0.0
496                || body.collider.kind() != collider.kind()
497                || !valid_collider(space, collider, body.mass)
498            {
499                return Err(EditError::InvalidBody);
500            }
501        }
502        let mut free = vec![false; self.slots.len()];
503        for &slot in &self.free {
504            let Some(entry) = self.slots.get(slot as usize) else {
505                return Err(EditError::InvalidBodyArena);
506            };
507            if free[slot as usize] || entry.dense.is_some() || occupied[slot as usize] {
508                return Err(EditError::InvalidBodyArena);
509            }
510            free[slot as usize] = true;
511        }
512        for (slot, entry) in self.slots.iter().enumerate() {
513            if occupied[slot] != entry.dense.is_some()
514                || (!occupied[slot] && entry.generation != u32::MAX && !free[slot])
515            {
516                return Err(EditError::InvalidBodyArena);
517            }
518        }
519        geometry.validate(self.dense.iter().map(|body| body.collider))
520    }
521}
522
523impl<S: PhysicsSpace> Deref for BodyArena<S> {
524    type Target = [RigidBody<S>];
525
526    fn deref(&self) -> &Self::Target {
527        &self.dense
528    }
529}
530
531impl<S: PhysicsSpace> Index<BodyId> for BodyArena<S> {
532    type Output = RigidBody<S>;
533
534    fn index(&self, id: BodyId) -> &RigidBody<S> {
535        self.get(id).unwrap_or_else(|| panic!("{STALE_HANDLE}"))
536    }
537}
538
539impl<S: PhysicsSpace> IndexMut<BodyId> for BodyArena<S> {
540    fn index_mut(&mut self, id: BodyId) -> &mut RigidBody<S> {
541        self.get_mut(id).unwrap_or_else(|| panic!("{STALE_HANDLE}"))
542    }
543}
544
545impl<S: PhysicsSpace> Index<usize> for BodyArena<S> {
546    type Output = RigidBody<S>;
547
548    fn index(&self, dense: usize) -> &RigidBody<S> {
549        &self.dense[dense]
550    }
551}
552
553impl<S: PhysicsSpace> IndexMut<usize> for BodyArena<S> {
554    fn index_mut(&mut self, dense: usize) -> &mut RigidBody<S> {
555        &mut self.dense[dense]
556    }
557}
558
559#[cfg(test)]
560mod tests {
561    use super::*;
562    use crate::world::World;
563    use glam::{Vec3, Vec4};
564    use loam_math::{Bivector3, Bivector4, EuclideanR3, EuclideanR4};
565
566    fn row<S: PhysicsSpace>(def: BodyDef<S>, space: &S) -> RigidBody<S> {
567        def.into_row(&mut GeometryStore::default(), space)
568    }
569
570    #[test]
571    fn dynamic_halfspaces_are_rejected_in_both_dimensions() {
572        assert!(BodyDef::new(
573            Vec3::ZERO,
574            Vec3::ZERO,
575            Collider::HalfSpace {
576                normal: Vec3::Y,
577                offset: 0.0
578            },
579            1.0,
580            1.0,
581            &EuclideanR3
582        )
583        .is_none());
584        assert!(BodyDef::new(
585            Vec4::ZERO,
586            Vec4::ZERO,
587            Collider::HalfSpace4D {
588                normal: Vec4::Y,
589                offset: 0.0
590            },
591            1.0,
592            1.0,
593            &EuclideanR4
594        )
595        .is_none());
596    }
597
598    #[test]
599    fn sleeping_keeps_nominal_mass_and_an_explicit_impulse_wakes_the_body() {
600        let mut body = body_r3(Vec3::ZERO, 2.0, 1.0);
601        body.velocity = Vec3::Y;
602        body.angular_velocity = Bivector3::new(1.0, 0.0, 0.0);
603        body.sleep();
604        assert!(!body.is_static());
605        assert_eq!(body.mass(), 2.0);
606        assert_eq!(body.inv_mass(), 0.0);
607        crate::integrate_body(&EuclideanR3, &mut body, 1.0);
608        assert_eq!(body.position, Vec3::ZERO);
609        body.apply_impulse(Vec3::X * 4.0);
610        assert!(!body.is_sleeping());
611        assert_eq!(body.inv_mass(), 0.5);
612        assert_eq!(body.velocity, Vec3::X * 2.0);
613        assert_eq!(body.angular_velocity, Bivector3::ZERO);
614    }
615
616    #[test]
617    fn nonfinite_initial_state_is_rejected() {
618        for invalid in [f32::NAN, f32::INFINITY, f32::NEG_INFINITY] {
619            for (position, velocity, inertia) in [
620                (Vec3::splat(invalid), Vec3::ZERO, 1.0),
621                (Vec3::ZERO, Vec3::splat(invalid), 1.0),
622                (Vec3::ZERO, Vec3::ZERO, invalid),
623            ] {
624                assert!(BodyDef::new(
625                    position,
626                    velocity,
627                    Collider::sphere_at_origin(0.5),
628                    1.0,
629                    inertia,
630                    &EuclideanR3
631                )
632                .is_none());
633            }
634        }
635    }
636
637    #[test]
638    fn invalid_replacements_preserve_the_previous_body() {
639        let mut world = World::new(EuclideanR3);
640        let id = world.push_body(def_r3(Vec3::ZERO, 2.0, 1.0));
641        for mass in [-1.0, f32::NAN, f32::INFINITY, f32::from_bits(1)] {
642            assert!(!world.bodies[id].set_mass(mass));
643            assert_eq!(world.bodies[id].mass(), 2.0);
644            assert_eq!(world.bodies[id].inv_mass(), 0.5);
645        }
646        for collider in [
647            Collider::ConvexPolytope3D { vertices: vec![] },
648            Collider::Box3 {
649                half_extents: Vec3::ONE,
650            },
651            Collider::sphere_at_origin(f32::NAN),
652            Collider::sphere_at(Vec3::X, 1.0),
653            Collider::HalfSpace {
654                normal: Vec3::Y,
655                offset: 0.0,
656            },
657        ] {
658            assert!(BodyDef::new(
659                Vec3::ZERO,
660                Vec3::ZERO,
661                collider.clone(),
662                2.0,
663                1.0,
664                &EuclideanR3
665            )
666            .is_none());
667            assert!(world.set_collider(id, collider, 1.0).is_err());
668            let kept = world.collider(&world.bodies[id]);
669            assert!(matches!(kept, Some(Collider::Sphere { radius, .. }) if *radius == 0.5));
670        }
671    }
672
673    fn def_r3(position: Vec3, mass: f32, inertia: f32) -> BodyDef<EuclideanR3> {
674        BodyDef::new(
675            position,
676            Vec3::ZERO,
677            Collider::sphere_at_origin(0.5),
678            mass,
679            inertia,
680            &EuclideanR3,
681        )
682        .unwrap()
683    }
684
685    fn body_r3(position: Vec3, mass: f32, inertia: f32) -> RigidBody<EuclideanR3> {
686        row(def_r3(position, mass, inertia), &EuclideanR3)
687    }
688
689    fn body_r4(position: Vec4, mass: f32, inertia: f32) -> RigidBody<EuclideanR4> {
690        row(
691            BodyDef::new(
692                position,
693                Vec4::ZERO,
694                Collider::sphere_at_origin(0.5),
695                mass,
696                inertia,
697                &EuclideanR4,
698            )
699            .unwrap(),
700            &EuclideanR4,
701        )
702    }
703
704    #[test]
705    fn linear_impulse_changes_velocity_by_impulse_over_mass() {
706        let mut body = body_r3(Vec3::ZERO, 4.0, 0.5);
707        body.velocity = Vec3::new(1.0, 0.0, 0.0);
708        body.apply_impulse(Vec3::new(8.0, -4.0, 2.0));
709        assert_eq!(body.velocity, Vec3::new(3.0, -1.0, 0.5));
710        assert_eq!(body.angular_velocity, Bivector3::ZERO);
711    }
712
713    #[test]
714    fn central_impulse_produces_no_spin_and_matches_linear_form() {
715        let position = Vec3::new(2.0, -1.0, 3.0);
716        let impulse = Vec3::new(8.0, -4.0, 2.0);
717
718        let mut at_point = body_r3(position, 4.0, 0.5);
719        at_point.apply_impulse_at_point(&EuclideanR3, impulse, position);
720
721        let mut linear = body_r3(position, 4.0, 0.5);
722        linear.apply_impulse(impulse);
723
724        assert_eq!(at_point.velocity, linear.velocity);
725        assert_eq!(at_point.angular_velocity, Bivector3::ZERO);
726    }
727
728    #[test]
729    fn off_center_impulse_spins_body_by_inverse_inertia_times_lever_wedge_impulse_r3() {
730        let mut body = body_r3(Vec3::ZERO, 2.0, 0.5);
731        body.apply_impulse_at_point(
732            &EuclideanR3,
733            Vec3::new(3.0, 0.0, 0.0),
734            Vec3::new(0.0, 2.0, 0.0),
735        );
736
737        assert_eq!(body.velocity, Vec3::new(1.5, 0.0, 0.0));
738        assert_eq!(body.angular_velocity, Bivector3::new(-12.0, 0.0, 0.0));
739    }
740
741    #[test]
742    fn off_center_impulse_spins_body_by_inverse_inertia_times_lever_wedge_impulse_r4() {
743        let mut body = body_r4(Vec4::ZERO, 2.0, 0.5);
744        body.apply_impulse_at_point(
745            &EuclideanR4,
746            Vec4::new(3.0, 0.0, 0.0, 0.0),
747            Vec4::new(0.0, 0.0, 0.0, 2.0),
748        );
749
750        assert_eq!(body.velocity, Vec4::new(1.5, 0.0, 0.0, 0.0));
751        let expected = Bivector4 {
752            xw: -12.0,
753            ..Bivector4::ZERO
754        };
755        assert_eq!(body.angular_velocity, expected);
756    }
757
758    #[test]
759    fn equal_and_opposite_impulses_conserve_linear_and_angular_momentum() {
760        let space = EuclideanR3;
761
762        let a_velocity_before = Vec3::new(0.5, -0.25, 1.0);
763        let a_spin_before = Bivector3::new(0.125, -0.5, 0.25);
764        let mut a = body_r3(Vec3::new(-1.0, 0.0, 0.0), 2.0, 0.5);
765        a.velocity = a_velocity_before;
766        a.angular_velocity = a_spin_before;
767
768        let b_velocity_before = Vec3::new(-0.75, 0.5, 0.125);
769        let b_spin_before = Bivector3::new(-0.25, 0.0, 0.5);
770        let mut b = body_r3(Vec3::new(1.0, 0.5, 0.0), 4.0, 0.25);
771        b.velocity = b_velocity_before;
772        b.angular_velocity = b_spin_before;
773
774        let momenta = |a: &RigidBody<EuclideanR3>, b: &RigidBody<EuclideanR3>| {
775            let linear = a.velocity * a.mass + b.velocity * b.mass;
776            let angular = space.wedge(a.position, a.velocity * a.mass)
777                + a.angular_velocity * a.inertia
778                + space.wedge(b.position, b.velocity * b.mass)
779                + b.angular_velocity * b.inertia;
780            (linear, angular)
781        };
782
783        let (linear_before, angular_before) = momenta(&a, &b);
784
785        let point = Vec3::new(0.0, 0.25, 0.0);
786        let impulse = Vec3::new(1.5, -0.5, 0.25);
787        a.apply_impulse_at_point(&space, -impulse, point);
788        b.apply_impulse_at_point(&space, impulse, point);
789
790        let (linear_after, angular_after) = momenta(&a, &b);
791
792        assert!((a.velocity - a_velocity_before).length() > 0.1);
793        assert!((b.velocity - b_velocity_before).length() > 0.1);
794        assert!((a.angular_velocity + a_spin_before * -1.0).magnitude() > 0.1);
795        assert!((b.angular_velocity + b_spin_before * -1.0).magnitude() > 0.1);
796
797        let linear_drift = (linear_after - linear_before).length();
798        assert!(
799            linear_drift < 1e-5,
800            "linear momentum drifted by {linear_drift}"
801        );
802        let angular_drift = (angular_after + angular_before * -1.0).magnitude();
803        assert!(
804            angular_drift < 1e-5,
805            "angular momentum drifted by {angular_drift}"
806        );
807    }
808
809    #[test]
810    fn static_body_ignores_both_impulse_forms() {
811        let mut body = row(
812            BodyDef::<EuclideanR3>::fixed(
813                Vec3::ZERO,
814                Collider::sphere_at_origin(0.5),
815                1.0,
816                &EuclideanR3,
817            )
818            .unwrap(),
819            &EuclideanR3,
820        );
821        body.apply_impulse(Vec3::new(5.0, 5.0, 5.0));
822        body.apply_impulse_at_point(
823            &EuclideanR3,
824            Vec3::new(5.0, 0.0, 0.0),
825            Vec3::new(0.0, 1.0, 0.0),
826        );
827        assert_eq!(body.velocity, Vec3::ZERO);
828        assert_eq!(body.angular_velocity, Bivector3::ZERO);
829    }
830
831    #[test]
832    fn stale_handle_never_resolves_after_its_slot_is_recycled() {
833        let mut arena = BodyArena::new();
834        let doomed = arena.spawn(body_r3(Vec3::X, 1.0, 1.0));
835        assert!(arena.despawn(doomed).is_some());
836
837        let recycled = arena.spawn(body_r3(Vec3::Y, 1.0, 1.0));
838        assert_eq!(
839            recycled.slot(),
840            doomed.slot(),
841            "the free slot was not reused, so this test is not exercising aliasing"
842        );
843        assert_ne!(recycled.generation(), doomed.generation());
844
845        assert!(arena.get(doomed).is_none());
846        assert!(arena.dense_index(doomed).is_none());
847        assert_eq!(arena[recycled].position, Vec3::Y);
848        assert!(
849            arena.despawn(doomed).is_none(),
850            "stale despawn must be inert"
851        );
852        assert!(
853            arena.get(recycled).is_some(),
854            "a stale despawn removed the live body sharing its slot"
855        );
856    }
857
858    #[test]
859    fn despawn_keeps_storage_dense_and_survivor_handles_valid() {
860        let mut arena = BodyArena::new();
861        let first = arena.spawn(body_r3(Vec3::X, 1.0, 1.0));
862        let middle = arena.spawn(body_r3(Vec3::Y, 1.0, 1.0));
863        let last = arena.spawn(body_r3(Vec3::Z, 1.0, 1.0));
864
865        assert_eq!(arena.despawn(middle).map(|b| b.position), Some(Vec3::Y));
866
867        assert_eq!(arena.len(), 2);
868        assert_eq!(arena[first].position, Vec3::X);
869        assert_eq!(
870            arena[last].position,
871            Vec3::Z,
872            "the moved body's handle broke"
873        );
874        assert_eq!(arena.id_at(arena.dense_index(last).unwrap()), last);
875        let positions: Vec<Vec3> = arena.iter().map(|b| b.position).collect();
876        assert_eq!(positions, vec![Vec3::X, Vec3::Z]);
877    }
878
879    #[test]
880    fn every_dense_position_resolves_back_through_its_own_handle() {
881        let mut arena = BodyArena::new();
882        let assert_consistent = |arena: &BodyArena<EuclideanR3>| {
883            for dense in 0..arena.len() {
884                assert_eq!(
885                    arena.dense_index(arena.id_at(dense)),
886                    Some(dense),
887                    "slot table disagrees with dense position {dense}"
888                );
889            }
890        };
891
892        let mut live = Vec::new();
893        for i in 0..5 {
894            live.push(arena.spawn(body_r3(Vec3::splat(i as f32), 1.0, 1.0)));
895            assert_consistent(&arena);
896        }
897        for victim in [live[0], live[2], live[3]] {
898            assert!(arena.despawn(victim).is_some());
899            assert_consistent(&arena);
900        }
901        arena.spawn(body_r3(Vec3::ZERO, 1.0, 1.0));
902        assert_consistent(&arena);
903    }
904
905    #[test]
906    fn decoded_arena_with_a_wrong_dense_id_is_rejected() {
907        let mut geometry = GeometryStore::default();
908        let mut arena = BodyArena::new();
909        let first = arena.spawn(def_r3(Vec3::X, 1.0, 1.0).into_row(&mut geometry, &EuclideanR3));
910        let second = arena.spawn(def_r3(Vec3::Y, 1.0, 1.0).into_row(&mut geometry, &EuclideanR3));
911        assert!(arena.validate(&EuclideanR3, &geometry).is_ok());
912
913        arena.ids.swap(0, 1);
914        assert_eq!(
915            arena.validate(&EuclideanR3, &geometry),
916            Err(EditError::InvalidBodyArena)
917        );
918        assert_eq!(arena.get(first).map(|body| body.position), Some(Vec3::X));
919        assert_eq!(arena.get(second).map(|body| body.position), Some(Vec3::Y));
920    }
921
922    #[test]
923    fn a_maximum_generation_slot_is_reused_once_then_retired() {
924        let mut geometry = GeometryStore::default();
925        let mut arena = BodyArena::new();
926        let first = arena.spawn(def_r3(Vec3::X, 1.0, 1.0).into_row(&mut geometry, &EuclideanR3));
927        arena.ids[0].generation = u32::MAX - 1;
928        arena.slots[0].generation = u32::MAX - 1;
929        let first = BodyId {
930            slot: first.slot,
931            generation: u32::MAX - 1,
932        };
933
934        let removed = arena.despawn(first).unwrap();
935        geometry.release(removed.collider());
936        assert!(arena.validate(&EuclideanR3, &geometry).is_ok());
937        let last = arena.spawn(def_r3(Vec3::Y, 1.0, 1.0).into_row(&mut geometry, &EuclideanR3));
938        assert_eq!(last.generation(), u32::MAX);
939        let removed = arena.despawn(last).unwrap();
940        geometry.release(removed.collider());
941        assert!(arena.validate(&EuclideanR3, &geometry).is_ok());
942
943        let fresh = arena.spawn(def_r3(Vec3::Z, 1.0, 1.0).into_row(&mut geometry, &EuclideanR3));
944        assert_ne!(fresh.slot(), last.slot());
945    }
946}
loam_physics/body.rs

loam_physics/
body.rs
