loam_physics/
euclidean_r4.rs

1//! World angular velocity composes as `rotation_current * delta_rotor` under Rotor4's left-first convention.
2
3use glam::Vec4;
4
5use loam_math::{Bivector, Bivector4, EuclideanR4, Iso4Flat, Rotor};
6use loam_shape::polytope::Polytope4;
7
8use crate::body::{BodyDef, RigidBody};
9use crate::collider::{Collider, ColliderKind};
10use crate::collision::{epa_r4, gjk_intersect_r4, GjkResult4, PosedHull4, Sphere4 as GjkSphere4};
11use crate::geometry::GeometryStore;
12use crate::integrator::{BroadphaseBound, PhysicsSpace};
13use crate::narrowphase::Narrowphase;
14use crate::response::Contact;
15
16pub fn omega_cross_r(omega: Bivector4, r: glam::Vec4) -> glam::Vec4 {
17    -omega.contract_vec(r)
18}
19
20fn inv_inertia(body: &RigidBody<EuclideanR4>) -> f32 {
21    if body.inv_mass() > 0.0 && body.inertia > 0.0 {
22        1.0 / body.inertia
23    } else {
24        0.0
25    }
26}
27
28impl PhysicsSpace for EuclideanR4 {
29    type AngVel = Bivector4;
30    type Inertia = f32;
31
32    fn broadphase_bound(&self) -> BroadphaseBound {
33        BroadphaseBound::Certified
34    }
35
36    fn supports_collider(&self, kind: ColliderKind) -> bool {
37        matches!(
38            kind,
39            ColliderKind::Sphere | ColliderKind::ConvexPolytope4D | ColliderKind::HalfSpace4D
40        )
41    }
42
43    fn valid_vector(&self, vector: Vec4) -> bool {
44        vector.is_finite()
45    }
46
47    fn valid_orientation(&self, orientation: Iso4Flat) -> bool {
48        let r = orientation.rotation;
49        let product = r * r.inverse();
50        r.s.is_finite()
51            && r.xy.is_finite()
52            && r.xz.is_finite()
53            && r.xw.is_finite()
54            && r.yz.is_finite()
55            && r.yw.is_finite()
56            && r.zw.is_finite()
57            && r.xyzw.is_finite()
58            && (product.s - 1.0).abs() <= 1e-4
59            && product.xy.abs() <= 1e-4
60            && product.xz.abs() <= 1e-4
61            && product.xw.abs() <= 1e-4
62            && product.yz.abs() <= 1e-4
63            && product.yw.abs() <= 1e-4
64            && product.zw.abs() <= 1e-4
65            && product.xyzw.abs() <= 1e-4
66            && orientation.translation.is_finite()
67    }
68
69    fn valid_angular_velocity(&self, angular_velocity: Bivector4) -> bool {
70        angular_velocity.xy.is_finite()
71            && angular_velocity.xz.is_finite()
72            && angular_velocity.xw.is_finite()
73            && angular_velocity.yz.is_finite()
74            && angular_velocity.yw.is_finite()
75            && angular_velocity.zw.is_finite()
76    }
77
78    fn valid_inertia(&self, inertia: f32) -> bool {
79        inertia.is_finite() && inertia >= 0.0 && (inertia == 0.0 || inertia.recip().is_finite())
80    }
81
82    fn integrate_orientation(&self, iso: Iso4Flat, omega: Bivector4, dt: f32) -> Iso4Flat {
83        debug_assert!(
84            omega.xy.is_finite()
85                && omega.xz.is_finite()
86                && omega.xw.is_finite()
87                && omega.yz.is_finite()
88                && omega.yw.is_finite()
89                && omega.zw.is_finite(),
90            "non-finite Bivector4 angular velocity in integrate_orientation",
91        );
92        let delta = (omega * dt).exp();
93        let composed = iso.rotation * delta;
94        Iso4Flat {
95            rotation: composed.normalize(),
96            translation: iso.translation,
97        }
98    }
99
100    fn apply_inv_inertia(&self, inertia: f32, torque: Bivector4) -> Bivector4 {
101        if inertia > 0.0 {
102            torque * (1.0 / inertia)
103        } else {
104            Bivector4::ZERO
105        }
106    }
107
108    fn wedge(&self, a: Vec4, b: Vec4) -> Bivector4 {
109        Bivector4::wedge(a, b)
110    }
111
112    fn velocity_at_point(&self, body: &RigidBody<EuclideanR4>, p: Vec4) -> Vec4 {
113        let r = p - body.position;
114        body.velocity + omega_cross_r(body.angular_velocity, r)
115    }
116
117    fn effective_mass_inv(
118        &self,
119        a: &RigidBody<EuclideanR4>,
120        b: &RigidBody<EuclideanR4>,
121        contact_point: Vec4,
122        direction: Vec4,
123    ) -> f32 {
124        let ra = contact_point - a.position;
125        let rb = contact_point - b.position;
126        let ra_wedge = Bivector4::wedge(ra, direction);
127        let rb_wedge = Bivector4::wedge(rb, direction);
128        a.inv_mass()
129            + b.inv_mass()
130            + ra_wedge.magnitude_squared() * inv_inertia(a)
131            + rb_wedge.magnitude_squared() * inv_inertia(b)
132    }
133
134    fn apply_contact_impulse(
135        &self,
136        a: &mut RigidBody<EuclideanR4>,
137        b: &mut RigidBody<EuclideanR4>,
138        contact_point: Vec4,
139        direction: Vec4,
140        magnitude: f32,
141    ) {
142        let ra = contact_point - a.position;
143        let rb = contact_point - b.position;
144        let lin = direction * magnitude;
145        a.velocity -= lin * a.inv_mass();
146        b.velocity += lin * b.inv_mass();
147
148        let inv_i_a = inv_inertia(a);
149        let inv_i_b = inv_inertia(b);
150        a.angular_velocity = a.angular_velocity + Bivector4::wedge(ra, lin) * (-inv_i_a);
151        b.angular_velocity = b.angular_velocity + Bivector4::wedge(rb, lin) * inv_i_b;
152    }
153}
154
155fn sphere_sphere_r4(
156    a: &RigidBody<EuclideanR4>,
157    b: &RigidBody<EuclideanR4>,
158    geometry: &GeometryStore,
159    space: &EuclideanR4,
160) -> Option<Contact<EuclideanR4>> {
161    let Some(&Collider::Sphere { radius: ra, .. }) = geometry.get(a.collider()) else {
162        return None;
163    };
164    let Some(&Collider::Sphere { radius: rb, .. }) = geometry.get(b.collider()) else {
165        return None;
166    };
167
168    use loam_math::Space;
169    let d = space.distance(a.position, b.position);
170    let combined = ra + rb;
171    if d >= combined {
172        return None;
173    }
174    let log = space.log(a.position, b.position);
175    let len = log.length();
176    let normal = if len > 1e-8 { log / len } else { Vec4::Y };
177
178    let surface_a = a.position + normal * ra;
179    let surface_b = b.position - normal * rb;
180    let point = (surface_a + surface_b) * 0.5;
181
182    Some(Contact {
183        normal,
184        point,
185        penetration: combined - d,
186        restitution: (a.restitution + b.restitution) * 0.5,
187    })
188}
189
190fn sphere_halfspace_r4(
191    a: &RigidBody<EuclideanR4>,
192    b: &RigidBody<EuclideanR4>,
193    geometry: &GeometryStore,
194    _space: &EuclideanR4,
195) -> Option<Contact<EuclideanR4>> {
196    let Some(&Collider::Sphere { radius, .. }) = geometry.get(a.collider()) else {
197        return None;
198    };
199    let Some(&Collider::HalfSpace4D { normal, offset }) = geometry.get(b.collider()) else {
200        return None;
201    };
202    let normal = b.orientation.rotation.apply(normal);
203    let offset = offset + normal.dot(b.position);
204    let signed = a.position.dot(normal) - offset;
205    let penetration = radius - signed;
206    if penetration <= 0.0 {
207        return None;
208    }
209    let contact_normal = -normal;
210    let point = a.position - normal * radius;
211    Some(Contact {
212        normal: contact_normal,
213        point,
214        penetration,
215        restitution: (a.restitution + b.restitution) * 0.5,
216    })
217}
218
219fn polytope_halfspace_r4(
220    a: &RigidBody<EuclideanR4>,
221    b: &RigidBody<EuclideanR4>,
222    geometry: &GeometryStore,
223    _space: &EuclideanR4,
224) -> Option<Contact<EuclideanR4>> {
225    let Some(Collider::ConvexPolytope4D { vertices: va_local }) = geometry.get(a.collider()) else {
226        return None;
227    };
228    let Some(&Collider::HalfSpace4D {
229        normal: plane_n,
230        offset,
231    }) = geometry.get(b.collider())
232    else {
233        return None;
234    };
235    let plane_n = b.orientation.rotation.apply(plane_n);
236    let offset = offset + plane_n.dot(b.position);
237
238    let mut deepest = Vec4::ZERO;
239    let mut deepest_depth = 0.0_f32;
240    for &v_local in va_local {
241        let v_world = a.orientation.rotation.apply(v_local) + a.position;
242        let signed = v_world.dot(plane_n) - offset;
243        let depth = -signed;
244        if depth > deepest_depth {
245            deepest_depth = depth;
246            deepest = v_world;
247        }
248    }
249    if deepest_depth <= 0.0 {
250        return None;
251    }
252    Some(Contact {
253        normal: -plane_n,
254        point: deepest,
255        penetration: deepest_depth,
256        restitution: (a.restitution + b.restitution) * 0.5,
257    })
258}
259
260fn polytope4_bounding_radius(local_vertices: &[Vec4]) -> f32 {
261    local_vertices
262        .iter()
263        .map(|v| v.length_squared())
264        .fold(0.0_f32, f32::max)
265        .sqrt()
266}
267
268fn validate_contact4(
269    info: &crate::collision::ContactInfo4,
270    a: &RigidBody<EuclideanR4>,
271    b: &RigidBody<EuclideanR4>,
272) -> Option<Contact<EuclideanR4>> {
273    if !info.penetration.is_finite()
274        || info.penetration <= 0.0
275        || !info.normal.is_finite()
276        || !info.point.is_finite()
277    {
278        return None;
279    }
280    let n2 = info.normal.length_squared();
281    if !(0.5..=1.5).contains(&n2) {
282        return None;
283    }
284    Some(Contact {
285        normal: info.normal,
286        point: info.point,
287        penetration: info.penetration,
288        restitution: (a.restitution + b.restitution) * 0.5,
289    })
290}
291
292fn polytope_polytope_r4(
293    a: &RigidBody<EuclideanR4>,
294    b: &RigidBody<EuclideanR4>,
295    geometry: &GeometryStore,
296    _space: &EuclideanR4,
297) -> Option<Contact<EuclideanR4>> {
298    let Some(Collider::ConvexPolytope4D { vertices: va_local }) = geometry.get(a.collider()) else {
299        return None;
300    };
301    let Some(Collider::ConvexPolytope4D { vertices: vb_local }) = geometry.get(b.collider()) else {
302        return None;
303    };
304
305    let ra = polytope4_bounding_radius(va_local);
306    let rb = polytope4_bounding_radius(vb_local);
307    let center_dist_sq = (b.position - a.position).length_squared();
308    let combined = ra + rb;
309    if center_dist_sq > combined * combined {
310        return None;
311    }
312
313    let hull_a = PosedHull4 {
314        local: va_local,
315        position: a.position,
316        rotation: a.orientation.rotation,
317    };
318    let hull_b = PosedHull4 {
319        local: vb_local,
320        position: b.position,
321        rotation: b.orientation.rotation,
322    };
323
324    let initial_dir = b.position - a.position;
325    let simplex = match gjk_intersect_r4(&hull_a, &hull_b, initial_dir) {
326        GjkResult4::Intersecting { simplex } => simplex,
327        GjkResult4::Separated => return None,
328    };
329    let info = epa_r4(&hull_a, &hull_b, simplex, combined)?;
330    validate_contact4(&info, a, b)
331}
332
333fn sphere_polytope_r4(
334    a: &RigidBody<EuclideanR4>,
335    b: &RigidBody<EuclideanR4>,
336    geometry: &GeometryStore,
337    _space: &EuclideanR4,
338) -> Option<Contact<EuclideanR4>> {
339    let Some(&Collider::Sphere { radius, .. }) = geometry.get(a.collider()) else {
340        return None;
341    };
342    let Some(Collider::ConvexPolytope4D { vertices: vb_local }) = geometry.get(b.collider()) else {
343        return None;
344    };
345
346    let rb = polytope4_bounding_radius(vb_local);
347    let center_dist_sq = (b.position - a.position).length_squared();
348    let combined = radius + rb;
349    if center_dist_sq > combined * combined {
350        return None;
351    }
352
353    let support_a = GjkSphere4 {
354        center: a.position,
355        radius,
356    };
357    let support_b = PosedHull4 {
358        local: vb_local,
359        position: b.position,
360        rotation: b.orientation.rotation,
361    };
362    let initial_dir = b.position - a.position;
363    let simplex = match gjk_intersect_r4(&support_a, &support_b, initial_dir) {
364        GjkResult4::Intersecting { simplex } => simplex,
365        GjkResult4::Separated => return None,
366    };
367    let info = epa_r4(&support_a, &support_b, simplex, combined)?;
368    validate_contact4(&info, a, b)
369}
370
371pub fn register_default_narrowphase(np: &mut Narrowphase<EuclideanR4>) {
372    np.register(ColliderKind::Sphere, ColliderKind::Sphere, sphere_sphere_r4);
373    np.register(
374        ColliderKind::Sphere,
375        ColliderKind::HalfSpace4D,
376        sphere_halfspace_r4,
377    );
378    np.register(
379        ColliderKind::ConvexPolytope4D,
380        ColliderKind::ConvexPolytope4D,
381        polytope_polytope_r4,
382    );
383    np.register(
384        ColliderKind::Sphere,
385        ColliderKind::ConvexPolytope4D,
386        sphere_polytope_r4,
387    );
388    np.register(
389        ColliderKind::ConvexPolytope4D,
390        ColliderKind::HalfSpace4D,
391        polytope_halfspace_r4,
392    );
393}
394
395pub fn ball4_inertia(mass: f32, radius: f32) -> f32 {
396    mass * radius * radius / 3.0
397}
398
399/// Uniform-density moment about any centroidal rotation plane.
400pub fn regular_polytope4_inertia(shape: Polytope4, mass: f32, circumradius: f32) -> f32 {
401    const SQRT_5: f32 = 2.236_068;
402    // Lasserre and Avrachenkov, American Mathematical Monthly 108, 2001.
403    let mean_radius_sq = match shape {
404        Polytope4::Pentatope => 1.0 / 6.0,
405        Polytope4::Tesseract => 1.0 / 3.0,
406        Polytope4::Cell16 => 4.0 / 15.0,
407        Polytope4::Cell24 => 13.0 / 30.0,
408        Polytope4::Cell600 => (11.0 + 3.0 * SQRT_5) / 30.0,
409        Polytope4::Cell120 => (215.0 + 69.0 * SQRT_5) / 600.0,
410    };
411    0.5 * mass * circumradius * circumradius * mean_radius_sq
412}
413
414pub fn sphere_body_r4(
415    position: Vec4,
416    velocity: Vec4,
417    radius: f32,
418    mass: f32,
419) -> Option<BodyDef<EuclideanR4>> {
420    BodyDef::new(
421        position,
422        velocity,
423        Collider::sphere_at_origin(radius),
424        mass,
425        ball4_inertia(mass, radius),
426        &EuclideanR4,
427    )
428}
429
430/// The solid occupies `dot(p, normalize(normal)) <= offset`.
431pub fn halfspace4_body_r4(normal: Vec4, offset: f32) -> Option<BodyDef<EuclideanR4>> {
432    let n = normal.try_normalize()?;
433    BodyDef::fixed(
434        Vec4::ZERO,
435        Collider::HalfSpace4D { normal: n, offset },
436        1.0,
437        &EuclideanR4,
438    )
439}
440
441/// Uses a bounding-ball inertia approximation.
442pub fn polytope_body_r4(
443    position: Vec4,
444    velocity: Vec4,
445    vertices: Vec<Vec4>,
446    mass: f32,
447) -> Option<BodyDef<EuclideanR4>> {
448    let bounding_r_sq = vertices
449        .iter()
450        .map(|v| v.length_squared())
451        .fold(0.0, f32::max);
452    let inertia = mass * bounding_r_sq / 3.0;
453    BodyDef::new(
454        position,
455        velocity,
456        Collider::ConvexPolytope4D { vertices },
457        mass,
458        inertia,
459        &EuclideanR4,
460    )
461}
462
463pub use loam_shape::polytope_geom::*;
464
465#[cfg(test)]
466mod tests {
467    use super::*;
468
469    use crate::world::World;
470
471    fn assert_close(a: f32, b: f32, tol: f32) {
472        assert!(
473            (a - b).abs() <= tol,
474            "expected {a} close to {b} (tol {tol})"
475        );
476    }
477
478    #[test]
479    fn wall_contact_leaves_through_the_near_face_in_either_pair_order() {
480        let mut np = Narrowphase::<EuclideanR4>::new();
481        register_default_narrowphase(&mut np);
482        let mut geometry = crate::geometry::GeometryStore::default();
483
484        let wall = BodyDef::fixed(
485            Vec4::ZERO,
486            Collider::ConvexPolytope4D {
487                vertices: tesseract_vertices(0.2)
488                    .into_iter()
489                    .map(|v| v * Vec4::new(1.0, 20.0, 20.0, 20.0))
490                    .collect(),
491            },
492            1.0,
493            &EuclideanR4,
494        )
495        .unwrap()
496        .into_row(&mut geometry, &EuclideanR4);
497        let ball = sphere_body_r4(Vec4::new(-0.05, 0.0, 0.0, 0.0), Vec4::ZERO, 0.2, 1.0)
498            .unwrap()
499            .into_row(&mut geometry, &EuclideanR4);
500
501        let forward = np
502            .test(&ball, &wall, &geometry, &EuclideanR4)
503            .expect("overlapping");
504        assert!(
505            (-forward.normal).dot(Vec4::X) < -0.99,
506            "ball leaves along {:?}, not back out of the near face",
507            -forward.normal
508        );
509        let reversed = np
510            .test(&wall, &ball, &geometry, &EuclideanR4)
511            .expect("overlapping");
512        assert!(
513            reversed.normal.dot(Vec4::X) < -0.99,
514            "flipped pair leaves the ball along {:?}",
515            reversed.normal
516        );
517        assert_close(forward.penetration, reversed.penetration, 1e-6);
518    }
519
520    #[test]
521    fn sphere_settles_on_4d_floor() {
522        let mut world = World::new(EuclideanR4);
523        register_default_narrowphase(&mut world.narrowphase);
524        world
525            .set_gravity(Some(Vec4::new(0.0, -9.8, 0.0, 0.0)))
526            .unwrap();
527        let _floor = world.push_body(halfspace4_body_r4(Vec4::Y, 0.0).unwrap());
528        let ball = world.push_body(
529            sphere_body_r4(Vec4::new(0.0, 2.0, 0.0, 0.0), Vec4::ZERO, 0.5, 1.0).unwrap(),
530        );
531        for _ in 0..300 {
532            world.step(1.0 / 60.0).unwrap();
533        }
534        let body = &world.bodies[ball];
535        let lowest = body.position.y - 0.5;
536        assert!(
537            (-0.05..=0.05).contains(&lowest),
538            "ball tunneled through 4D floor: y_bottom = {lowest}"
539        );
540        assert!(
541            body.velocity.length() < 0.5,
542            "ball still moving: |v| = {}",
543            body.velocity.length()
544        );
545    }
546
547    #[test]
548    fn pentatope_settles_on_4d_floor() {
549        let mut world = World::new(EuclideanR4);
550        register_default_narrowphase(&mut world.narrowphase);
551        world
552            .set_gravity(Some(Vec4::new(0.0, -9.8, 0.0, 0.0)))
553            .unwrap();
554        let floor = world.push_body(halfspace4_body_r4(Vec4::Y, 0.0).unwrap());
555        let body_id = world.push_body(
556            polytope_body_r4(
557                Vec4::new(0.0, 3.0, 0.0, 0.0),
558                Vec4::ZERO,
559                pentatope_vertices(0.5),
560                1.0,
561            )
562            .unwrap(),
563        );
564        world.bodies[floor].restitution = 0.0;
565        world.bodies[body_id].restitution = 0.0;
566
567        for _ in 0..600 {
568            world.step(1.0 / 60.0).unwrap();
569        }
570        let body = &world.bodies[body_id];
571
572        assert!(
573            body.position.y.is_finite() && (-0.5..=1.0).contains(&body.position.y),
574            "pentatope position out of expected resting band: y = {}",
575            body.position.y
576        );
577        assert!(
578            body.position.x.abs() < 5.0
579                && body.position.z.abs() < 5.0
580                && body.position.w.abs() < 5.0,
581            "pentatope drifted too far horizontally: pos = {:?}",
582            body.position
583        );
584
585        assert!(
586            body.velocity.length() < 1.0,
587            "pentatope still moving after 10 s: |v| = {}, v = {:?}",
588            body.velocity.length(),
589            body.velocity
590        );
591
592        let omega = body.angular_velocity;
593        let omega_mag2 = omega.xy * omega.xy
594            + omega.xz * omega.xz
595            + omega.xw * omega.xw
596            + omega.yz * omega.yz
597            + omega.yw * omega.yw
598            + omega.zw * omega.zw;
599        assert!(
600            omega_mag2.is_finite() && omega_mag2 < 4.0,
601            "pentatope angular velocity blew up: |ω|² = {omega_mag2}, ω = {omega:?}"
602        );
603    }
604
605    #[test]
606    fn a_flat_landing_bounces_to_its_restitution_height_at_any_step_size() {
607        const HALF: f32 = 0.225;
608        const RESTITUTION: f32 = 0.05;
609        for dt in [1.0 / 240.0, 1.0 / 960.0, 1.0 / 1920.0] {
610            let mut world = World::new(EuclideanR4);
611            register_default_narrowphase(&mut world.narrowphase);
612            world
613                .set_gravity(Some(Vec4::new(0.0, -9.8, 0.0, 0.0)))
614                .unwrap();
615            let floor = world.push_body(halfspace4_body_r4(Vec4::Y, 0.0).unwrap());
616            let body = world.push_body(
617                polytope_body_r4(
618                    Vec4::new(0.0, 4.0, 0.0, 0.0),
619                    Vec4::ZERO,
620                    tesseract_vertices(HALF),
621                    1.0,
622                )
623                .unwrap(),
624            );
625            world.bodies[floor].restitution = RESTITUTION;
626            world.bodies[body].restitution = RESTITUTION;
627            let mut landed = false;
628            let mut apex = f32::MIN;
629            for _ in 0..(2.0 / dt) as usize {
630                world.step(dt).unwrap();
631                let body = &world.bodies[body];
632                landed |= body.velocity.y > 0.0;
633                if landed {
634                    apex = apex.max(body.position.y);
635                }
636            }
637            // Restitution alone lifts an 8.3 m/s landing by (0.05 * 8.3)^2 / 19.6, under 1 cm.
638            assert!(
639                apex - HALF < 0.03,
640                "at dt {dt} the landing bounced {} above rest",
641                apex - HALF
642            );
643        }
644    }
645
646    const CORNER_DROP_DT: f32 = 1.0 / 240.0;
647    const CORNER_DROP_CIRCUMRADIUS: f32 = 0.45;
648    const CORNER_DROP_GRAVITY: f32 = -9.8;
649    const CORNER_DROP_DRAG: f32 = 1.2;
650
651    struct CornerDrop {
652        world: World<EuclideanR4>,
653        body: crate::body::BodyId,
654        decay: f32,
655    }
656
657    impl CornerDrop {
658        fn new() -> Self {
659            let mut world = World::new(EuclideanR4);
660            register_default_narrowphase(&mut world.narrowphase);
661            world
662                .set_gravity(Some(Vec4::new(0.0, CORNER_DROP_GRAVITY, 0.0, 0.0)))
663                .unwrap();
664            let floor = world.push_body(halfspace4_body_r4(Vec4::Y, 0.0).unwrap());
665            world.bodies[floor].restitution = 0.05;
666            let body = world.push_body(
667                polytope_body_r4(
668                    Vec4::new(0.0, 1.6, 0.0, 0.0),
669                    Vec4::ZERO,
670                    cell24_vertices(CORNER_DROP_CIRCUMRADIUS),
671                    1.0,
672                )
673                .unwrap(),
674            );
675            world.bodies[body].restitution = 0.05;
676            world.bodies[body].orientation.rotation = Bivector4::new(0.0, 0.0, 0.0, 0.6, 0.4, 0.0)
677                .exp()
678                .normalize();
679            world.bodies[body].inertia =
680                regular_polytope4_inertia(Polytope4::Cell24, 1.0, CORNER_DROP_CIRCUMRADIUS);
681            Self {
682                world,
683                body,
684                decay: (-CORNER_DROP_DRAG * CORNER_DROP_DT).exp(),
685            }
686        }
687
688        fn step(&mut self) {
689            self.world.step(CORNER_DROP_DT).unwrap();
690            let decay = self.decay;
691            let body = &mut self.world.bodies[self.body];
692            body.angular_velocity = body.angular_velocity * decay;
693        }
694
695        fn height(&self) -> f32 {
696            self.world.bodies[self.body].position.y
697        }
698
699        fn angular_speed(&self) -> f32 {
700            self.world.bodies[self.body].angular_velocity.magnitude()
701        }
702
703        fn energy(&self) -> f32 {
704            let body = &self.world.bodies[self.body];
705            0.5 * body.velocity.length_squared()
706                + 0.5 * body.inertia * body.angular_velocity.magnitude().powi(2)
707                - CORNER_DROP_GRAVITY * body.position.y
708        }
709    }
710
711    #[test]
712    fn a_hull_dropped_on_a_corner_settles_without_climbing_its_own_contacts() {
713        const LANDING_STEPS: usize = 400;
714        const RESTING_STEPS: usize = 800;
715        const CLIMB_TOLERANCE: f32 = 5e-4;
716
717        let mut drop = CornerDrop::new();
718        let start_energy = drop.energy();
719        let mut landing_spin = 0.0_f32;
720        for _ in 0..LANDING_STEPS {
721            drop.step();
722            landing_spin = landing_spin.max(drop.angular_speed());
723        }
724        assert!(
725            landing_spin > 1.0,
726            "the hull never rocked over, so a settle proves nothing: peak |ω| \
727             was {landing_spin} rad/s"
728        );
729
730        let resting_height = drop.height();
731        let mut peak_height = resting_height;
732        for _ in 0..RESTING_STEPS {
733            drop.step();
734            peak_height = peak_height.max(drop.height());
735        }
736
737        assert!(
738            peak_height - resting_height < CLIMB_TOLERANCE,
739            "a resting hull climbed {} against gravity over {RESTING_STEPS} \
740             steps",
741            peak_height - resting_height,
742        );
743        assert!(
744            drop.angular_speed() < 0.02,
745            "a resting hull held {} rad/s against a {CORNER_DROP_DRAG}/s \
746             damper, so the contact solve is re-injecting it",
747            drop.angular_speed(),
748        );
749        assert!(
750            drop.energy() < start_energy,
751            "the drop ended with {} of energy against the {start_energy} it \
752             started with",
753            drop.energy(),
754        );
755    }
756
757    #[test]
758    fn falling_sphere_accelerates_in_r4() {
759        let mut world = World::new(EuclideanR4);
760        register_default_narrowphase(&mut world.narrowphase);
761        world
762            .set_gravity(Some(Vec4::new(0.0, -9.8, 0.0, 0.0)))
763            .unwrap();
764
765        let id = world.push_body(
766            sphere_body_r4(Vec4::new(0.0, 5.0, 0.0, 0.0), Vec4::ZERO, 0.5, 1.0).unwrap(),
767        );
768        world.step(1.0 / 60.0).unwrap();
769        let body = &world.bodies[id];
770        assert!(body.velocity.y < -0.1 && body.velocity.y > -0.2);
771        assert_close(body.velocity.x, 0.0, 1e-6);
772        assert_close(body.velocity.z, 0.0, 1e-6);
773        assert_close(body.velocity.w, 0.0, 1e-6);
774    }
775
776    #[test]
777    fn head_on_sphere_collision_reverses_velocity() {
778        let mut world = World::new(EuclideanR4);
779        register_default_narrowphase(&mut world.narrowphase);
780
781        world.push_body(
782            sphere_body_r4(
783                Vec4::new(-1.0, 0.0, 0.0, 0.0),
784                Vec4::new(2.0, 0.0, 0.0, 0.0),
785                0.5,
786                1.0,
787            )
788            .unwrap(),
789        );
790        world.push_body(
791            sphere_body_r4(
792                Vec4::new(1.0, 0.0, 0.0, 0.0),
793                Vec4::new(-2.0, 0.0, 0.0, 0.0),
794                0.5,
795                1.0,
796            )
797            .unwrap(),
798        );
799
800        for _ in 0..120 {
801            world.step(1.0 / 120.0).unwrap();
802        }
803        let a = &world.bodies[0];
804        let b = &world.bodies[1];
805        assert!(
806            a.velocity.x < 0.0,
807            "body 0 should bounce back: v.x = {}",
808            a.velocity.x
809        );
810        assert!(
811            b.velocity.x > 0.0,
812            "body 1 should bounce back: v.x = {}",
813            b.velocity.x
814        );
815        assert_close(a.velocity.y, 0.0, 1e-4);
816        assert_close(a.velocity.z, 0.0, 1e-4);
817        assert_close(a.velocity.w, 0.0, 1e-4);
818    }
819
820    #[test]
821    fn sphere_sphere_off_plane_contact_resolves_along_line_of_centers() {
822        let mut world = World::new(EuclideanR4);
823        register_default_narrowphase(&mut world.narrowphase);
824        let a_pos = Vec4::new(-0.8, -0.4, 0.3, 0.2);
825        let b_pos = Vec4::new(0.8, 0.4, -0.3, -0.2);
826        let a = world
827            .push_body(sphere_body_r4(a_pos, (b_pos - a_pos).normalize() * 2.0, 0.5, 1.0).unwrap());
828        let b = world
829            .push_body(sphere_body_r4(b_pos, (a_pos - b_pos).normalize() * 2.0, 0.5, 1.0).unwrap());
830        for _ in 0..120 {
831            world.step(1.0 / 120.0).unwrap();
832        }
833        let rel = world.bodies[b].velocity - world.bodies[a].velocity;
834        let axis = (b_pos - a_pos).normalize();
835        let v_along = rel.dot(axis);
836        assert!((rel - axis * v_along).length() < 1e-4);
837        assert!(
838            v_along > 0.0,
839            "relative velocity should now be separating: {v_along}"
840        );
841    }
842
843    #[test]
844    fn sphere_inside_tesseract_produces_contact() {
845        let mut world = World::new(EuclideanR4);
846        register_default_narrowphase(&mut world.narrowphase);
847        let _a = world.push_body(sphere_body_r4(Vec4::ZERO, Vec4::ZERO, 0.3, 1.0).unwrap());
848        let _b = world.push_body(
849            polytope_body_r4(Vec4::ZERO, Vec4::ZERO, tesseract_vertices(0.8), 0.0).unwrap(),
850        );
851        let pair_found = {
852            let geometry = world.geometry().clone();
853            let (a, b) = world.bodies.dense_mut().split_at_mut(1);
854            world
855                .narrowphase
856                .test(&a[0], &b[0], &geometry, &EuclideanR4)
857                .is_some()
858        };
859        assert!(
860            pair_found,
861            "sphere inside tesseract should produce a contact"
862        );
863    }
864
865    #[test]
866    fn separated_pentatopes_produce_no_contact() {
867        let mut world = World::new(EuclideanR4);
868        register_default_narrowphase(&mut world.narrowphase);
869        let _a = world.push_body(
870            polytope_body_r4(Vec4::ZERO, Vec4::ZERO, pentatope_vertices(1.0), 1.0).unwrap(),
871        );
872        let _b = world.push_body(
873            polytope_body_r4(
874                Vec4::new(10.0, 0.0, 0.0, 0.0),
875                Vec4::ZERO,
876                pentatope_vertices(1.0),
877                1.0,
878            )
879            .unwrap(),
880        );
881        let geometry = world.geometry().clone();
882        let (a, b) = world.bodies.dense_mut().split_at_mut(1);
883        assert!(world
884            .narrowphase
885            .test(&a[0], &b[0], &geometry, &EuclideanR4)
886            .is_none());
887    }
888
889    #[test]
890    fn integrated_orientation_advances_a_body_point_along_the_world_frame_omega() {
891        let space = EuclideanR4;
892        let start = Iso4Flat {
893            rotation: Bivector4::new(0.8, 0.0, 0.0, std::f32::consts::FRAC_PI_2, 0.0, 0.9).exp(),
894            translation: Vec4::ZERO,
895        };
896        let omega = Bivector4::new(0.7, 0.0, 0.0, 0.0, 0.5, 0.0);
897        let local = Vec4::new(0.5, -0.5, 0.5, 0.5);
898        let dt = 1e-3;
899
900        let before = start.rotation.apply(local);
901        let after = space
902            .integrate_orientation(start, omega, dt)
903            .rotation
904            .apply(local);
905        let residual = (after - (before + omega_cross_r(omega, before) * dt)).length();
906        assert!(
907            residual < 1e-5,
908            "integrated orientation left the world-frame field ω⌋r: residual \
909             {residual} over a step of {}",
910            (after - before).length()
911        );
912    }
913
914    #[test]
915    fn orientation_integration_preserves_unit_rotor() {
916        let space = EuclideanR4;
917        let mut iso = Iso4Flat::IDENTITY;
918        let omega = Bivector4::new(0.2, 0.0, 0.0, 0.0, 0.0, 0.15);
919        for _ in 0..1000 {
920            iso = space.integrate_orientation(iso, omega, 1.0 / 60.0);
921        }
922        let n = iso.rotation.norm_squared();
923        assert!(
924            (n - 1.0).abs() < 1e-3,
925            "rotor drifted off the unit manifold: |R|² = {n}"
926        );
927    }
928}
loam_physics/euclidean_r4.rs

loam_physics/
euclidean_r4.rs
