Skip to main content

concinnity_core/physics/sim/
world.rs

1// The simulation itself: body storage, and the order the stages run in.
2//
3// Bodies live in a fixed-capacity pool, so a full world declines a body rather
4// than growing under a caller's feet, and a slot's generation makes a handle
5// to a removed body read as absent. That is also what lets every other stage
6// index its own arrays by slot: the pool's slot is the one identity the whole
7// step agrees on.
8//
9// Nothing on the step path allocates, and neither does a query. Every buffer
10// is reserved when the simulation is built and reused, which is what lets the
11// step run inside a frame budget rather than at the allocator's convenience.
12
13use alloc::vec::Vec;
14
15use crate::memory::{Pool, PoolHandle};
16
17use crate::physics::{
18    BodyHandle, CharacterMove, CharacterMoveInput, ColliderShape, ContactHit, DynamicParams,
19    Fanout, JointSpec, LayerMask, RayHit, SensorCrossing,
20};
21
22use super::body::Body;
23use super::broadphase::{Proxy, Role, SweepPrune};
24use super::ccd::{self, Ccd};
25use super::character::{self, CharacterCapsule, CharacterConfig};
26use super::collide::heightfield::{Heightfield, Heightfields};
27use super::config::SimConfig;
28use super::contact::{ContactCache, Manifold, carry_impulses};
29use super::impact::Impacts;
30use super::island::Islands;
31use super::joint::{Joint, JointFrame, JointSet};
32#[cfg(test)]
33use super::math::Mat3;
34use super::math::{Quat, Vec3};
35use super::narrow::{self, Narrow};
36use super::query::{self, RayQuery};
37#[cfg(test)]
38use super::query::{ShapeCast, ShapeCastHit};
39use super::scene::Scene;
40use super::sensor::Sensors;
41use super::solver::{self, Solver, SolverBody};
42
43// Friction of a character capsule against what it is driven into. A character
44// is moved by resolving a translation rather than by the solver, so this only
45// governs what the capsule pushes.
46const CHARACTER_FRICTION: f32 = 0.5;
47
48// What a step is worth handing out, in units of one body's integration -- a
49// contact costs about twenty of those and a joint about sixteen.
50//
51// The threshold is high on purpose. Gathering a pool's workers from a thread
52// that is not one of them costs about as much as three hundred of these units,
53// and the gathering is serial: a step that only just covers it comes out level
54// at best. So a step hands its work out once it is worth an order of magnitude
55// more than the gathering, and keeps it otherwise.
56const CONTACT_COST: usize = 20;
57const JOINT_COST: usize = 16;
58const MIN_FANOUT_COST: usize = 4000;
59
60/// A rigid-body simulation: bodies fall under gravity, collide, and come to
61/// rest.
62///
63/// The capacity given at construction is the whole reservation. Adding past it
64/// returns `None`; stepping never allocates.
65///
66/// # Examples
67///
68/// ```
69/// use concinnity_core::physics::{ColliderShape, DynamicParams, LayerMask, Simulation};
70///
71/// let mut sim = Simulation::with_capacity(2);
72/// sim.add_fixed(
73///     &ColliderShape::Cuboid { half_extents: [10.0, 0.5, 10.0] },
74///     [0.0, -0.5, 0.0],
75///     [0.0; 3],
76///     0.8,
77///     LayerMask::ALL,
78/// );
79/// let ball = sim
80///     .add_dynamic(
81///         &ColliderShape::Ball { radius: 0.5 },
82///         [0.0, 5.0, 0.0],
83///         [0.0; 3],
84///         DynamicParams {
85///             mass: 1.0,
86///             friction: 0.5,
87///             restitution: 0.0,
88///             gravity_scale: 1.0,
89///             linear_damping: 0.0,
90///         },
91///         LayerMask::ALL,
92///     )
93///     .expect("room in the pool");
94///
95/// for _ in 0..180 {
96///     sim.step(1.0 / 60.0);
97/// }
98///
99/// let (position, _rotation) = sim.body_pose_quat(ball).expect("a live body");
100/// assert!(
101///     (position[1] - 0.5).abs() < 0.02,
102///     "the ball rests on the floor, at y = {}",
103///     position[1]
104/// );
105/// ```
106pub struct Simulation {
107    config: SimConfig,
108    character: CharacterConfig,
109    bodies: Pool<Body>,
110    broadphase: SweepPrune,
111    contacts: ContactCache,
112    fields: Heightfields,
113    narrow: Narrow,
114    joints: JointSet,
115    islands: Islands,
116    solver: Solver,
117    sensors: Sensors,
118    impacts: Impacts,
119    ccd: Ccd,
120    /// Workers the per-worker scratch was reserved for. One until a caller
121    /// says otherwise, which is what makes the serial path the default.
122    workers: usize,
123    /// Steps asked to split further than the reservation allows.
124    worker_overflows: u32,
125}
126
127impl core::fmt::Debug for Simulation {
128    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
129        f.debug_struct("Simulation")
130            .field("bodies", &self.bodies.len())
131            .field("capacity", &self.bodies.capacity())
132            .field("joints", &self.joints.len())
133            .finish()
134    }
135}
136
137impl Simulation {
138    /// Reserve room for `capacity` bodies, with the default tuning.
139    pub fn with_capacity(capacity: usize) -> Self {
140        Self::new(SimConfig::default(), capacity)
141    }
142
143    /// Reserve room for `capacity` bodies, tuned by `config`.
144    pub fn new(config: SimConfig, capacity: usize) -> Self {
145        Simulation {
146            config,
147            character: CharacterConfig::default(),
148            bodies: Pool::with_capacity(capacity),
149            broadphase: SweepPrune::with_capacity(capacity),
150            // Two contacts per body covers a stack, which is what the
151            // reservation is sized for. A scene denser than that grows these
152            // buffers once, early, and never again.
153            contacts: ContactCache::with_capacity(capacity * 2),
154            fields: Heightfields::new(),
155            narrow: Narrow::new(),
156            // A world with more joints than bodies is unusual enough to pay
157            // for one reservation while it is being built; nothing added
158            // afterwards is on the step path.
159            joints: JointSet::with_capacity(capacity),
160            islands: Islands::with_capacity(capacity),
161            solver: Solver::with_capacity(capacity),
162            // A crossing is a boundary rather than a state, so a world
163            // records far fewer of them than it has bodies. A hit is one per
164            // contact pair, so the queue is reserved the way the contact list
165            // above is.
166            sensors: Sensors::with_capacity(capacity),
167            impacts: Impacts::with_capacity(capacity * 2),
168            ccd: Ccd::with_capacity(capacity),
169            workers: 1,
170            worker_overflows: 0,
171        }
172    }
173
174    /// Reserve the scratch a step needs to split into `workers` pieces, and
175    /// return how many it will actually use.
176    ///
177    /// Call this once while the world is built, with the worker count of the
178    /// fan-out that will be stepping it. A simulation nobody calls this on
179    /// reserves nothing and steps on the calling thread, which is what lets a
180    /// host with no threads use the same simulation unchanged.
181    ///
182    /// Splitting never changes what a step produces, so the reserved count is
183    /// a ceiling rather than a promise: a step handed a wider fan-out uses
184    /// this many pieces of it and leaves the rest of it idle.
185    pub fn reserve_workers(&mut self, workers: usize) -> usize {
186        let capacity = self.bodies.capacity();
187        self.workers = workers.clamp(1, solver::MAX_WORKERS);
188        self.broadphase.reserve_workers(self.workers, capacity);
189        self.narrow.reserve_workers(self.workers, capacity);
190        self.workers
191    }
192
193    /// Workers the step splits into at most: what
194    /// [`Simulation::reserve_workers`] settled on.
195    pub fn workers(&self) -> usize {
196        self.workers
197    }
198
199    #[cfg(test)]
200    /// Steps that were handed a wider fan-out than the reservation covers,
201    /// since the count was last cleared. Each one ran on the reserved number
202    /// of workers instead, which changes nothing about the result.
203    pub(crate) fn worker_overflows(&self) -> u32 {
204        self.worker_overflows
205    }
206
207    #[cfg(test)]
208    /// Forget the count above.
209    pub(crate) fn clear_worker_overflows(&mut self) {
210        self.worker_overflows = 0;
211    }
212
213    /// Tune the character controller. `grounded` is true for a gravity-bound
214    /// character, which climbs steps and stays attached to the ground, and
215    /// false for a free-flying camera, which does neither. A `max_slope_deg`
216    /// of `0` disables the climb limit.
217    pub fn configure_character(&mut self, max_slope_deg: f32, step_height: f32, grounded: bool) {
218        self.character = CharacterConfig::new(max_slope_deg, step_height, grounded);
219    }
220
221    /// Build the capsule a character move is resolved against: a cylinder of
222    /// `2 * half_height` capped by hemispheres of `radius`.
223    ///
224    /// A caller holds one per character across the fixed ticks rather than
225    /// building one per move.
226    pub fn character_shape(half_height: f32, radius: f32) -> CharacterCapsule {
227        CharacterCapsule::new(half_height, radius)
228    }
229
230    /// The tuning this simulation steps with.
231    pub fn config(&self) -> &SimConfig {
232        &self.config
233    }
234
235    #[cfg(test)]
236    /// Re-tune the simulation. Takes effect on the next step.
237    pub(crate) fn set_config(&mut self, config: SimConfig) {
238        self.config = config;
239    }
240
241    /// Bodies the pool can hold.
242    pub fn capacity(&self) -> usize {
243        self.bodies.capacity()
244    }
245
246    /// Bodies currently in the simulation.
247    pub fn body_count(&self) -> usize {
248        self.bodies.len()
249    }
250
251    /// Colliders currently in the simulation. A body carries exactly one
252    /// shape, so this is the body count until compound shapes exist.
253    pub fn collider_count(&self) -> usize {
254        self.bodies.len()
255    }
256
257    /// Joints currently constraining bodies.
258    pub fn joint_count(&self) -> usize {
259        self.joints.len()
260    }
261
262    #[cfg(test)]
263    /// Sensor pairs currently overlapping: what the regions are holding,
264    /// rather than what crossed a boundary to get there.
265    pub(crate) fn sensor_overlap_count(&self) -> usize {
266        self.sensors.overlap_count()
267    }
268
269    /// Contact points the last step solved.
270    #[cfg(test)]
271    pub(crate) fn contact_count(&self) -> usize {
272        self.contacts
273            .manifolds()
274            .iter()
275            .map(|m| m.count as usize)
276            .sum()
277    }
278
279    /// Bytes reserved for bodies, bounds, contacts, the solver's arrays, and
280    /// the per-worker buffers [`Simulation::reserve_workers`] set aside: what
281    /// the simulation costs whatever its occupancy.
282    pub fn reserved_bytes(&self) -> u64 {
283        self.bodies.reserved_bytes()
284            + self.broadphase.reserved_bytes()
285            + self.contacts.reserved_bytes()
286            + self.fields.reserved_bytes()
287            + self.narrow.reserved_bytes()
288            + self.joints.reserved_bytes()
289            + self.islands.reserved_bytes()
290            + self.solver.reserved_bytes()
291            + self.sensors.reserved_bytes()
292            + self.impacts.reserved_bytes()
293            + self.ccd.reserved_bytes()
294    }
295
296    /// Add an immovable body. `None` when the pool is full.
297    pub fn add_fixed(
298        &mut self,
299        shape: &ColliderShape,
300        pos: [f32; 3],
301        euler_deg: [f32; 3],
302        friction: f32,
303        mask: LayerMask,
304    ) -> Option<BodyHandle> {
305        self.add(Body::fixed(
306            *shape,
307            Vec3::from_array(pos),
308            Quat::from_euler_deg(euler_deg),
309            friction,
310            mask,
311        ))
312    }
313
314    /// Add a body driven to a position rather than by forces: infinite mass,
315    /// untouched by gravity or impulses, but it pushes what it is driven
316    /// into. Move it with [`Simulation::set_kinematic_translation`]. `None`
317    /// when the pool is full.
318    pub fn add_kinematic(
319        &mut self,
320        shape: &ColliderShape,
321        pos: [f32; 3],
322        euler_deg: [f32; 3],
323        friction: f32,
324        mask: LayerMask,
325    ) -> Option<BodyHandle> {
326        self.add(Body::kinematic(
327            *shape,
328            Vec3::from_array(pos),
329            Quat::from_euler_deg(euler_deg),
330            friction,
331            mask,
332        ))
333    }
334
335    /// Add a position-driven character capsule centred on `center`: a
336    /// cylinder of `2 * half_height` capped by hemispheres of `radius`.
337    ///
338    /// Gravity does not move it and the solver does not push it. Resolve a
339    /// desired move with [`Simulation::move_character`] and apply the answer
340    /// with [`Simulation::set_kinematic_translation`]. `None` when the pool is
341    /// full.
342    pub fn add_character(
343        &mut self,
344        half_height: f32,
345        radius: f32,
346        center: [f32; 3],
347        mask: LayerMask,
348    ) -> Option<BodyHandle> {
349        self.add_kinematic(
350            &ColliderShape::Capsule {
351                half_height,
352                radius,
353            },
354            center,
355            [0.0; 3],
356            CHARACTER_FRICTION,
357            mask,
358        )
359    }
360
361    /// Add a region that records what overlaps it and resists nothing.
362    ///
363    /// It never collides, never blocks a query, and never moves. What crosses
364    /// its boundary is reported as a [`SensorCrossing`] carrying `tag`,
365    /// collected with [`Simulation::drain_sensor_crossings_into`]. Freely
366    /// simulated and position-driven bodies cross it; immovable geometry does
367    /// not, and two overlapping regions record a crossing each.
368    ///
369    /// `None` when the pool is full.
370    ///
371    /// # Examples
372    ///
373    /// ```
374    /// use concinnity_core::physics::{ColliderShape, DynamicParams, LayerMask, Simulation};
375    ///
376    /// let mut sim = Simulation::with_capacity(2);
377    /// sim.add_sensor(
378    ///     &ColliderShape::Cuboid { half_extents: [1.0, 1.0, 1.0] },
379    ///     [0.0, 2.0, 0.0],
380    ///     [0.0; 3],
381    ///     7,
382    ///     LayerMask::ALL,
383    /// )
384    /// .expect("room in the pool");
385    ///
386    /// // Nothing holds the ball up, so it falls through the region.
387    /// sim.add_dynamic(
388    ///     &ColliderShape::Ball { radius: 0.25 },
389    ///     [0.0, 6.0, 0.0],
390    ///     [0.0; 3],
391    ///     DynamicParams {
392    ///         mass: 1.0,
393    ///         friction: 0.5,
394    ///         restitution: 0.0,
395    ///         gravity_scale: 1.0,
396    ///         linear_damping: 0.0,
397    ///     },
398    ///     LayerMask::ALL,
399    /// )
400    /// .expect("room in the pool");
401    ///
402    /// let mut crossings = Vec::new();
403    /// let (mut entered, mut left) = (false, false);
404    /// for _ in 0..300 {
405    ///     sim.step(1.0 / 60.0);
406    ///     sim.drain_sensor_crossings_into(&mut crossings);
407    ///     for crossing in &crossings {
408    ///         assert_eq!(crossing.tag, 7);
409    ///         if crossing.entered { entered = true } else { left = true }
410    ///     }
411    /// }
412    /// assert!(entered && left, "the ball went in and came out again");
413    /// ```
414    pub fn add_sensor(
415        &mut self,
416        shape: &ColliderShape,
417        pos: [f32; 3],
418        euler_deg: [f32; 3],
419        tag: u64,
420        mask: LayerMask,
421    ) -> Option<BodyHandle> {
422        self.add(Body::sensor(
423            *shape,
424            Vec3::from_array(pos),
425            Quat::from_euler_deg(euler_deg),
426            tag,
427            mask,
428        ))
429    }
430
431    /// Move the boundary crossings recorded since the last drain into `out`,
432    /// oldest first. `out` is cleared first, and both it and the queue keep
433    /// their capacity, so a per-tick drain never reallocates.
434    pub fn drain_sensor_crossings_into(&mut self, out: &mut Vec<SensorCrossing>) {
435        self.sensors.drain_into(out);
436    }
437
438    #[cfg(test)]
439    /// Crossings and overlaps the reservation had no room for, since the
440    /// count was last cleared.
441    ///
442    /// Both are reserved against the body capacity, which covers a caller
443    /// draining every step and a world whose regions hold fewer bodies than
444    /// it has. A non-zero count means something crossed a region and no
445    /// caller was told about it.
446    pub(crate) fn sensor_overflows(&self) -> u32 {
447        self.sensors.overflows()
448    }
449
450    #[cfg(test)]
451    /// Reset the count of declined crossings.
452    pub(crate) fn clear_sensor_overflows(&mut self) {
453        self.sensors.clear_overflows();
454    }
455
456    /// Set the smallest contact impulse worth reporting as a
457    /// [`ContactHit`], measured at a step of `tick_dt`.
458    ///
459    /// The simulation gates on the force that impulse stands for, so a pair
460    /// leaning on another at rest stays silent while the same pair colliding
461    /// does not. It applies to every body, whenever it is called.
462    pub fn set_contact_min_impulse(&mut self, min_impulse: f32, tick_dt: f32) {
463        self.impacts.set_min_impulse(min_impulse, tick_dt);
464    }
465
466    /// Move the contact hits recorded since the last drain into `out`, oldest
467    /// first. `out` is cleared first, and both it and the queue keep their
468    /// capacity.
469    ///
470    /// Only pairs with a freely simulated body on at least one side, carrying
471    /// more than the force [`Simulation::set_contact_min_impulse`] set, appear.
472    pub fn drain_contact_hits_into(&mut self, out: &mut Vec<ContactHit>) {
473        self.impacts.drain_into(out);
474    }
475
476    #[cfg(test)]
477    /// Contact hits the reservation had no room for, since the count was last
478    /// cleared.
479    ///
480    /// The queue is reserved for one hit per contact pair, which covers a
481    /// caller draining every step. A non-zero count means a collision
482    /// happened that no caller was told about.
483    pub(crate) fn contact_hit_overflows(&self) -> u32 {
484        self.impacts.overflows()
485    }
486
487    /// Add a static height grid: terrain, addressed like any other body.
488    ///
489    /// `heights` is a `rows * cols` row-major grid of world-space `y` values,
490    /// with rows running along `z` and columns along `x`. `scale` is the whole
491    /// extent `[width, height_multiplier, depth]`, and the grid is centred on
492    /// `pos`.
493    ///
494    /// `None` when the pool is full or the grid names no surface: fewer than
495    /// two rows or columns, the wrong number of heights, or no footprint.
496    ///
497    /// The body is immovable and never rotates. Contacts against it are
498    /// answered along the surface's own face normals, so a shape crossing the
499    /// boundary between two cells is not caught by the edge they share.
500    ///
501    /// # Examples
502    ///
503    /// ```
504    /// use concinnity_core::physics::{ColliderShape, DynamicParams, LayerMask, Simulation};
505    ///
506    /// let mut sim = Simulation::with_capacity(2);
507    /// // A flat five-by-five grid twenty units square, its surface at y = 0.
508    /// sim.add_heightfield(
509    ///     5,
510    ///     5,
511    ///     vec![0.0; 25],
512    ///     [20.0, 1.0, 20.0],
513    ///     [0.0; 3],
514    ///     LayerMask::ALL,
515    /// )
516    /// .expect("room in the pool");
517    ///
518    /// let ball = sim
519    ///     .add_dynamic(
520    ///         &ColliderShape::Ball { radius: 0.5 },
521    ///         [1.0, 5.0, -2.0],
522    ///         [0.0; 3],
523    ///         DynamicParams {
524    ///             mass: 1.0,
525    ///             friction: 0.5,
526    ///             restitution: 0.0,
527    ///             gravity_scale: 1.0,
528    ///             linear_damping: 0.0,
529    ///         },
530    ///         LayerMask::ALL,
531    ///     )
532    ///     .expect("room in the pool");
533    ///
534    /// for _ in 0..240 {
535    ///     sim.step(1.0 / 60.0);
536    /// }
537    ///
538    /// let (position, _) = sim.body_pose_quat(ball).expect("a live body");
539    /// assert!(
540    ///     (position[1] - 0.5).abs() < 0.02,
541    ///     "the ball rests on the terrain, at y = {}",
542    ///     position[1]
543    /// );
544    /// ```
545    pub fn add_heightfield(
546        &mut self,
547        rows: usize,
548        cols: usize,
549        heights: Vec<f32>,
550        scale: [f32; 3],
551        pos: [f32; 3],
552        mask: LayerMask,
553    ) -> Option<BodyHandle> {
554        if self.bodies.len() >= self.bodies.capacity() {
555            return None;
556        }
557        let origin = Vec3::from_array(pos);
558        let field = Heightfield::new(rows, cols, heights, Vec3::from_array(scale), origin)?;
559        let bounds = field.bounds();
560        let index = self.fields.push(field);
561        // Terrain is fully rough: a slope holds whatever the body resting on
562        // it brings, rather than the surface capping it.
563        self.add(Body::terrain(index, bounds, origin, 1.0, mask))
564    }
565
566    #[cfg(test)]
567    /// Queries that gave up with terrain still to look at, since the count was
568    /// last cleared.
569    ///
570    /// A query walks a bounded number of the grid's triangles and stops rather
571    /// than growing a buffer. A non-zero count means some question was asked
572    /// of more surface than that, and the answer covered only part of it.
573    pub(crate) fn heightfield_overflows(&self) -> u32 {
574        self.fields.overflows()
575    }
576
577    #[cfg(test)]
578    /// Reset the count of terrain queries that gave up.
579    pub(crate) fn clear_heightfield_overflows(&mut self) {
580        self.fields.clear_overflows();
581    }
582
583    #[cfg(test)]
584    /// Fast bodies and swept region crossings the continuous-collision
585    /// reservation had no room for, since the count was last cleared.
586    ///
587    /// The reservation holds one entry per body, which covers every body in
588    /// the world moving fast at once. A non-zero count means a body took a
589    /// step long enough to pass through something and was not swept.
590    pub(crate) fn ccd_overflows(&self) -> u32 {
591        self.ccd.overflows()
592    }
593
594    #[cfg(test)]
595    /// Bodies the last step swept along their own path because they moved
596    /// too far for its contact test to have seen what they crossed.
597    ///
598    /// Zero for a world at ordinary speeds, which is what the gate is for:
599    /// a non-zero count is the number of bodies that paid for the expensive
600    /// path on the last step.
601    pub(crate) fn swept_body_count(&self) -> usize {
602        self.ccd.mover_count()
603    }
604
605    /// Add a freely simulated body. `None` when the pool is full.
606    pub fn add_dynamic(
607        &mut self,
608        shape: &ColliderShape,
609        pos: [f32; 3],
610        euler_deg: [f32; 3],
611        params: DynamicParams,
612        mask: LayerMask,
613    ) -> Option<BodyHandle> {
614        self.add(Body::dynamic(
615            *shape,
616            Vec3::from_array(pos),
617            Quat::from_euler_deg(euler_deg),
618            params,
619            mask,
620        ))
621    }
622
623    /// Constrain two bodies to each other. Anchors are in each body's own
624    /// frame, and the joint holds the relative pose the bodies are in when it
625    /// is made.
626    ///
627    /// Returns whether the joint was made. It needs two different live bodies;
628    /// past that, degenerate input is repaired rather than refused, so a
629    /// zero-length axis becomes `+Y` and limits given the wrong way round are
630    /// read low to high.
631    ///
632    /// A joint is removed by removing either of the bodies it holds.
633    ///
634    /// # Examples
635    ///
636    /// ```
637    /// use concinnity_core::physics::{
638    ///     ColliderShape, DynamicParams, JointSpec, LayerMask, Simulation,
639    /// };
640    ///
641    /// let mut sim = Simulation::with_capacity(2);
642    /// let post = sim
643    ///     .add_fixed(
644    ///         &ColliderShape::Ball { radius: 0.1 },
645    ///         [0.0, 4.0, 0.0],
646    ///         [0.0; 3],
647    ///         0.5,
648    ///         LayerMask::ALL,
649    ///     )
650    ///     .expect("room in the pool");
651    /// let bob = sim
652    ///     .add_dynamic(
653    ///         &ColliderShape::Ball { radius: 0.2 },
654    ///         [1.0, 4.0, 0.0],
655    ///         [0.0; 3],
656    ///         DynamicParams {
657    ///             mass: 1.0,
658    ///             friction: 0.5,
659    ///             restitution: 0.0,
660    ///             gravity_scale: 1.0,
661    ///             linear_damping: 0.0,
662    ///         },
663    ///         LayerMask::ALL,
664    ///     )
665    ///     .expect("room in the pool");
666    ///
667    /// assert!(sim.add_joint(post, bob, [0.0; 3], [-1.0, 0.0, 0.0], JointSpec::Spherical));
668    ///
669    /// for _ in 0..120 {
670    ///     sim.step(1.0 / 60.0);
671    /// }
672    ///
673    /// // The bob swings, but it stays one unit from the post it hangs off.
674    /// let (position, _) = sim.body_pose_quat(bob).expect("a live body");
675    /// let reach = ((position[0] - 0.0).powi(2)
676    ///     + (position[1] - 4.0).powi(2)
677    ///     + (position[2] - 0.0).powi(2))
678    /// .sqrt();
679    /// assert!((reach - 1.0).abs() < 0.01, "hanging {reach} from the post");
680    /// ```
681    pub fn add_joint(
682        &mut self,
683        body_a: BodyHandle,
684        body_b: BodyHandle,
685        anchor_a: [f32; 3],
686        anchor_b: [f32; 3],
687        spec: JointSpec,
688    ) -> bool {
689        let (slot_a, slot_b) = (body_a.index(), body_b.index());
690        if slot_a == slot_b {
691            return false;
692        }
693        let (anchor_a, anchor_b) = (Vec3::from_array(anchor_a), Vec3::from_array(anchor_b));
694        if !anchor_a.is_finite() || !anchor_b.is_finite() {
695            return false;
696        }
697        let (Some(a), Some(b)) = (
698            self.bodies.get(pool_handle(body_a)),
699            self.bodies.get(pool_handle(body_b)),
700        ) else {
701            return false;
702        };
703        let frame = JointFrame::new(spec, a.orientation, b.orientation);
704        self.joints.push(Joint {
705            a: slot_a,
706            b: slot_b,
707            anchor_a,
708            anchor_b,
709            frame,
710            impulses: Default::default(),
711        });
712        // A joint arriving on a settled body has to be able to disturb it.
713        for slot in [slot_a, slot_b] {
714            if let Some(body) = self.bodies.get_at_mut(slot as usize) {
715                body.wake();
716            }
717        }
718        true
719    }
720
721    /// Send a position-driven body to `pos` over the next step. It arrives
722    /// exactly there, pushing whatever it meets on the way. Returns whether
723    /// the handle named a live position-driven body.
724    ///
725    /// The target is consumed by the step, so a body that is to keep moving
726    /// is given a fresh one each tick and a body that is left alone stops.
727    pub fn set_kinematic_translation(&mut self, handle: BodyHandle, pos: [f32; 3]) -> bool {
728        let Some(body) = self.bodies.get_mut(pool_handle(handle)) else {
729            return false;
730        };
731        if !body.is_kinematic() {
732            return false;
733        }
734        body.kinematic_target = Some(Vec3::from_array(pos));
735        true
736    }
737
738    /// Switch a body to position-driven control, keeping its handle and the
739    /// mass it was authored with. Returns whether the handle named a live
740    /// body.
741    pub fn make_kinematic(&mut self, handle: BodyHandle) -> bool {
742        self.reclassify(handle, |body| body.make_kinematic())
743    }
744
745    /// Hand a body back to the solver with a launch velocity, restoring the
746    /// mass it was authored with. Returns whether the handle named a live
747    /// body.
748    pub fn make_dynamic(&mut self, handle: BodyHandle, linear_velocity: [f32; 3]) -> bool {
749        let velocity = Vec3::from_array(linear_velocity);
750        self.reclassify(handle, move |body| body.make_dynamic(velocity))
751    }
752
753    /// Whether a body is driven by position rather than by forces.
754    #[cfg(test)]
755    pub(crate) fn is_kinematic(&self, handle: BodyHandle) -> Option<bool> {
756        Some(self.bodies.get(pool_handle(handle))?.is_kinematic())
757    }
758
759    /// What a query reads: the bodies, the sweep order, and the height grids.
760    fn scene(&self) -> Scene<'_> {
761        Scene {
762            bodies: &self.bodies,
763            broadphase: &self.broadphase,
764            fields: &self.fields,
765        }
766    }
767
768    /// Cast a ray, returning the nearest hit within `max_dist`.
769    ///
770    /// `dir` need not be unit length; a zero direction misses. `exclude`
771    /// leaves one body out, and `mask` restricts the hit set to layers the
772    /// query interacts with. A ray that begins inside a body hits it at zero
773    /// distance with the normal turned back along the ray.
774    pub fn raycast(
775        &self,
776        origin: [f32; 3],
777        dir: [f32; 3],
778        max_dist: f32,
779        exclude: Option<BodyHandle>,
780        mask: LayerMask,
781    ) -> Option<RayHit> {
782        query::raycast(
783            self.scene(),
784            &RayQuery {
785                origin,
786                dir,
787                max_dist,
788                exclude,
789                mask,
790            },
791        )
792    }
793
794    /// Sweep a shape through the world, returning the nearest body it runs
795    /// into and how far along the cast's motion that happens.
796    #[cfg(test)]
797    pub(crate) fn shape_cast(&self, cast: &ShapeCast) -> Option<ShapeCastHit> {
798        query::shape_cast(self.scene(), cast)
799    }
800
801    /// Resolve a desired character move against the world without moving
802    /// anything in it, returning the translation to apply and whether the
803    /// capsule ends up on the ground.
804    ///
805    /// The capsule sweeps along the desired translation and slides along
806    /// whatever it meets rather than stopping dead, up to a bounded number of
807    /// deflections. `input.exclude` is the mover's own body, left out of the
808    /// query so it does not collide with itself; other characters' capsules
809    /// stay solid to it. What counts as ground, how high an obstacle is
810    /// climbed, and whether the mover is gravity-bound at all come from
811    /// [`Simulation::configure_character`].
812    ///
813    /// Apply the result with [`Simulation::set_kinematic_translation`].
814    ///
815    /// # Examples
816    ///
817    /// ```
818    /// use concinnity_core::physics::{CharacterMoveInput, ColliderShape, LayerMask, Simulation};
819    ///
820    /// let mut sim = Simulation::with_capacity(3);
821    /// sim.add_fixed(
822    ///     &ColliderShape::Cuboid { half_extents: [10.0, 0.5, 10.0] },
823    ///     [0.0, -0.5, 0.0],
824    ///     [0.0; 3],
825    ///     0.8,
826    ///     LayerMask::ALL,
827    /// );
828    /// // A wall whose near face is at z = 1.
829    /// sim.add_fixed(
830    ///     &ColliderShape::Cuboid { half_extents: [4.0, 2.0, 0.5] },
831    ///     [0.0, 2.0, 1.5],
832    ///     [0.0; 3],
833    ///     0.8,
834    ///     LayerMask::ALL,
835    /// );
836    ///
837    /// // The capsule stands on the floor: half height plus radius above it.
838    /// let center = [0.0, 0.9, 0.0];
839    /// let capsule = sim
840    ///     .add_kinematic(
841    ///         &ColliderShape::Capsule { half_height: 0.6, radius: 0.3 },
842    ///         center,
843    ///         [0.0; 3],
844    ///         0.8,
845    ///         LayerMask::ALL,
846    ///     )
847    ///     .expect("room in the pool");
848    ///
849    /// let shape = Simulation::character_shape(0.6, 0.3);
850    /// let moved = sim.move_character(
851    ///     &shape,
852    ///     &CharacterMoveInput {
853    ///         center,
854    ///         desired: [0.0, -0.01, 2.0],
855    ///         dt: 1.0 / 60.0,
856    ///         exclude: capsule,
857    ///         mask: LayerMask::ALL,
858    ///     },
859    /// );
860    ///
861    /// // Two units of walk, and the wall stops it a radius short of its face.
862    /// assert!(
863    ///     (moved.translation[2] - 0.7).abs() < 0.01,
864    ///     "walked {}",
865    ///     moved.translation[2]
866    /// );
867    /// assert!(moved.grounded, "the floor is still underfoot");
868    /// ```
869    pub fn move_character(
870        &self,
871        shape: &CharacterCapsule,
872        input: &CharacterMoveInput,
873    ) -> CharacterMove {
874        character::resolve(self.scene(), &self.character, shape, input)
875    }
876
877    /// Remove a body, along with every joint it was in. Returns whether the
878    /// handle named a live one.
879    ///
880    /// Purging the joints is part of the removal rather than something a
881    /// caller does afterwards: a joint naming a slot whose body has gone would
882    /// constrain whatever occupied that slot next.
883    pub fn remove_body(&mut self, handle: BodyHandle) -> bool {
884        let slot = handle.index();
885        if self.bodies.remove(pool_handle(handle)).is_none() {
886            return false;
887        }
888        self.broadphase.remove(slot);
889        // Whatever this body was touching or holding has to be re-examined
890        // without it, which has to happen before the joints are dropped.
891        self.wake_neighbours(slot);
892        self.joints.remove_incident(slot);
893        true
894    }
895
896    #[cfg(test)]
897    /// A body's world-space position and Euler-degree rotation.
898    pub(crate) fn body_pose(&self, handle: BodyHandle) -> Option<([f32; 3], [f32; 3])> {
899        let body = self.bodies.get(pool_handle(handle))?;
900        Some((body.position.to_array(), body.orientation.to_euler_deg()))
901    }
902
903    /// A body's world-space position and `[x, y, z, w]` rotation quaternion.
904    pub fn body_pose_quat(&self, handle: BodyHandle) -> Option<([f32; 3], [f32; 4])> {
905        let body = self.bodies.get(pool_handle(handle))?;
906        Some((body.position.to_array(), body.orientation.to_xyzw()))
907    }
908
909    #[cfg(test)]
910    /// A body's linear velocity in world units per second.
911    pub(crate) fn linear_velocity(&self, handle: BodyHandle) -> Option<[f32; 3]> {
912        Some(
913            self.bodies
914                .get(pool_handle(handle))?
915                .linear_velocity
916                .to_array(),
917        )
918    }
919
920    #[cfg(test)]
921    /// A body's angular velocity in radians per second.
922    pub(crate) fn angular_velocity(&self, handle: BodyHandle) -> Option<[f32; 3]> {
923        Some(
924            self.bodies
925                .get(pool_handle(handle))?
926                .angular_velocity
927                .to_array(),
928        )
929    }
930
931    /// A body's mass in kilograms. Immovable bodies report `0`.
932    pub fn mass(&self, handle: BodyHandle) -> Option<f32> {
933        Some(self.bodies.get(pool_handle(handle))?.mass)
934    }
935
936    #[cfg(test)]
937    /// Whether a settled body has stopped being simulated.
938    pub(crate) fn is_sleeping(&self, handle: BodyHandle) -> Option<bool> {
939        Some(self.bodies.get(pool_handle(handle))?.sleeping)
940    }
941
942    #[cfg(test)]
943    /// Set a body's linear velocity, waking it.
944    pub(crate) fn set_linear_velocity(&mut self, handle: BodyHandle, velocity: [f32; 3]) {
945        if let Some(body) = self.bodies.get_mut(pool_handle(handle)) {
946            body.linear_velocity = Vec3::from_array(velocity);
947            body.wake();
948        }
949    }
950
951    #[cfg(test)]
952    /// Set a body's angular velocity in radians per second, waking it.
953    pub(crate) fn set_angular_velocity(&mut self, handle: BodyHandle, velocity: [f32; 3]) {
954        if let Some(body) = self.bodies.get_mut(pool_handle(handle)) {
955            body.angular_velocity = Vec3::from_array(velocity);
956            body.wake();
957        }
958    }
959
960    #[cfg(test)]
961    /// Apply an impulse through a body's centre of mass, waking it.
962    pub(crate) fn apply_impulse(&mut self, handle: BodyHandle, impulse: [f32; 3]) {
963        if let Some(body) = self.bodies.get_mut(pool_handle(handle)) {
964            body.linear_velocity += Vec3::from_array(impulse) * body.inv_mass;
965            body.wake();
966        }
967    }
968
969    #[cfg(test)]
970    /// Kinetic plus gravitational potential energy over every dynamic body,
971    /// with potential measured from `y = 0`.
972    ///
973    /// A settled world's total must not climb: a solver that injects energy
974    /// says so here before it says so as a stack that will not stand.
975    pub(crate) fn total_energy(&self) -> f32 {
976        let mut total = 0.0;
977        for (_, body) in self.bodies.iter() {
978            if !body.is_dynamic() {
979                continue;
980            }
981            let momentum = Mat3::diagonal_conjugated(body.orientation, body.inertia_local)
982                .mul_vec3(body.angular_velocity);
983            total += 0.5 * body.mass * body.linear_velocity.length_squared()
984                + 0.5 * body.angular_velocity.dot(momentum)
985                + body.mass * self.config.gravity * body.gravity_scale * body.position.y;
986        }
987        total
988    }
989
990    /// Advance the simulation by `dt` seconds on the calling thread.
991    pub fn step(&mut self, dt: f32) {
992        self.step_with(dt, &crate::physics::Inline);
993    }
994
995    /// Advance the simulation by `dt` seconds, offering the step's independent
996    /// work to `fanout`.
997    ///
998    /// A step splits the same way whatever it is handed, so a world stepped
999    /// across a thread pool and the same world stepped on one thread land in
1000    /// exactly the same place. Nothing is reserved for a fan-out that
1001    /// [`Simulation::reserve_workers`] was not told about, so a wider one is
1002    /// used only as far as the reservation goes.
1003    ///
1004    /// # Examples
1005    ///
1006    /// ```
1007    /// use concinnity_core::physics::{ColliderShape, DynamicParams, Inline, LayerMask, Simulation};
1008    ///
1009    /// let mut sim = Simulation::with_capacity(1);
1010    /// let ball = sim
1011    ///     .add_dynamic(
1012    ///         &ColliderShape::Ball { radius: 0.5 },
1013    ///         [0.0, 10.0, 0.0],
1014    ///         [0.0; 3],
1015    ///         DynamicParams {
1016    ///             mass: 1.0,
1017    ///             friction: 0.4,
1018    ///             restitution: 0.2,
1019    ///             gravity_scale: 1.0,
1020    ///             linear_damping: 0.0,
1021    ///         },
1022    ///         LayerMask::ALL,
1023    ///     )
1024    ///     .expect("room for one body");
1025    /// sim.step_with(1.0 / 60.0, &Inline);
1026    /// assert!(sim.body_pose_quat(ball).expect("a live body").0[1] < 10.0, "it fell");
1027    /// ```
1028    pub fn step_with(&mut self, dt: f32, fanout: &impl Fanout) {
1029        if !dt.is_finite() || dt <= 0.0 {
1030            return;
1031        }
1032        let asked = fanout.workers().max(1);
1033        if asked > self.workers {
1034            self.worker_overflows = self.worker_overflows.saturating_add(1);
1035        }
1036        let workers = asked.min(self.workers);
1037        self.drive_kinematics(dt);
1038        let awake = self.refresh_bounds();
1039        // Reaching a fan-out's workers costs the same whatever is handed to
1040        // them, so a step with little to do is worth more on the thread it is
1041        // already on.
1042        if workers > 1 && self.step_cost(awake) >= MIN_FANOUT_COST {
1043            fanout.scope(|| self.advance(dt, fanout, workers));
1044        } else {
1045            self.advance(dt, &crate::physics::Inline, 1);
1046        }
1047    }
1048
1049    /// Roughly what the step ahead will cost, from what the world was holding
1050    /// last step.
1051    ///
1052    /// Only a moving body's contacts are work: a settled world keeps a full
1053    /// pair list and solves none of it, so the pairs are counted against what
1054    /// is still awake rather than taken at face value.
1055    fn step_cost(&self, awake: usize) -> usize {
1056        if awake == 0 {
1057            return 0;
1058        }
1059        let contacts = self.broadphase.pair_count().min(awake * 4);
1060        let joints = self.joints.len().min(awake * 2);
1061        awake + contacts * CONTACT_COST + joints * JOINT_COST
1062    }
1063
1064    /// Everything a step does once the bounds are current and the workers, if
1065    /// any, have been gathered.
1066    fn advance(&mut self, dt: f32, fanout: &impl Fanout, workers: usize) {
1067        let Simulation {
1068            config,
1069            bodies,
1070            broadphase,
1071            contacts,
1072            fields,
1073            narrow,
1074            joints,
1075            islands,
1076            solver,
1077            sensors,
1078            impacts,
1079            ccd,
1080            ..
1081        } = self;
1082        let sweeping = ccd::enabled(config);
1083
1084        let pairs = broadphase.sweep(fanout, workers);
1085        sensors.resolve(bodies, pairs.sensors);
1086        let (current, previous) = contacts.begin();
1087        narrow.build(
1088            narrow::Work {
1089                bodies,
1090                fields,
1091                pairs: pairs.contacts,
1092                previous,
1093                out: current,
1094                margin: config.speculative_margin,
1095            },
1096            fanout,
1097            workers,
1098        );
1099        carry_impulses(previous, current);
1100        wake_driven_contacts(bodies, current);
1101
1102        solver.begin();
1103        gather(bodies, solver, current, joints.as_slice());
1104        solver.run(
1105            solver::Work {
1106                manifolds: current,
1107                joints: joints.as_mut_slice(),
1108                islands,
1109                config,
1110                dt,
1111            },
1112            fanout,
1113            workers,
1114        );
1115        // The solver is asked what it delivered, so a pair whose manifold was
1116        // carried forward untouched is not read as a fresh collision.
1117        impacts.collect(bodies, current, solver.loads(), dt);
1118        ccd.begin();
1119        for (handle, body) in bodies.iter_mut() {
1120            if !body.is_simulated() {
1121                continue;
1122            }
1123            let slot = handle.index() as u32;
1124            let solved = solver.body(slot);
1125            let began_at = body.position;
1126            body.linear_velocity = solved.linear_velocity;
1127            body.angular_velocity = solved.angular_velocity;
1128            body.position = solved.position;
1129            body.orientation = solved.rotation;
1130            // A position-driven body lands exactly where it was sent, not
1131            // wherever the substeps' arithmetic left it.
1132            if let Some(target) = body.kinematic_target.take() {
1133                body.position = target;
1134            }
1135            if sweeping {
1136                ccd.observe(slot, body, began_at, config.ccd_motion_ratio);
1137            }
1138        }
1139
1140        // After the write-back, so a mover is swept along the path the step
1141        // actually took it on, and before sleep, so what it ran into is awake
1142        // when the islands are decided.
1143        if sweeping {
1144            let scene = Scene {
1145                bodies,
1146                broadphase,
1147                fields,
1148            };
1149            ccd.resolve(scene, config, dt);
1150            ccd.report_crossings(bodies, sensors);
1151            ccd.apply(bodies);
1152        }
1153
1154        if !solver.is_idle() {
1155            update_sleep(config, bodies, islands, current, joints.as_slice(), dt);
1156        }
1157    }
1158
1159    /// Change a body's kind and put the broad phase and its neighbours back
1160    /// in step with the change.
1161    fn reclassify(&mut self, handle: BodyHandle, change: impl FnOnce(&mut Body) -> bool) -> bool {
1162        let Some(body) = self.bodies.get_mut(pool_handle(handle)) else {
1163            return false;
1164        };
1165        if !change(body) {
1166            return true;
1167        }
1168        // Whether contact moves this body has changed, so the sweep's pair
1169        // filter and whatever was leaning on it both have to hear about it.
1170        let proxy = proxy_for(body);
1171        let slot = handle.index();
1172        self.broadphase.set_proxy(slot, proxy);
1173        self.wake_neighbours(slot);
1174        true
1175    }
1176
1177    /// Give every driven body the velocity that carries it to its target over
1178    /// this step, so the solver sees the motion as motion.
1179    fn drive_kinematics(&mut self, dt: f32) {
1180        for (_, body) in self.bodies.iter_mut() {
1181            if body.is_kinematic() {
1182                body.drive_to_target(dt);
1183            }
1184        }
1185    }
1186
1187    fn add(&mut self, mut body: Body) -> Option<BodyHandle> {
1188        body.refresh_bounds(self.config.bounds_margin);
1189        let proxy = proxy_for(&body);
1190        let handle = self.bodies.insert(body)?;
1191        let slot = handle.index() as u32;
1192        self.broadphase.insert(slot);
1193        self.broadphase.set_proxy(slot, proxy);
1194        // A body arriving inside a settled stack has to be able to disturb it.
1195        self.wake_neighbours(slot);
1196        Some(body_handle(handle))
1197    }
1198
1199    /// Wake whatever the given slot was in contact with or jointed to, so a
1200    /// change at one body reaches the island it belonged to.
1201    fn wake_neighbours(&mut self, slot: u32) {
1202        let Simulation {
1203            bodies,
1204            contacts,
1205            joints,
1206            ..
1207        } = self;
1208        let mut wake = |other: u32| {
1209            if let Some(body) = bodies.get_at_mut(other as usize) {
1210                body.wake();
1211            }
1212        };
1213        for manifold in contacts.manifolds_mut() {
1214            let other = if manifold.a == slot {
1215                manifold.b
1216            } else if manifold.b == slot {
1217                manifold.a
1218            } else {
1219                continue;
1220            };
1221            wake(other);
1222        }
1223        for joint in joints.as_slice() {
1224            if let Some(other) = joint.other(slot) {
1225                wake(other);
1226            }
1227        }
1228    }
1229
1230    /// Re-fatten the bounds of the bodies that moved, and tell the broad phase
1231    /// about the ones whose bounds actually changed.
1232    fn refresh_bounds(&mut self) -> usize {
1233        let margin = self.config.bounds_margin;
1234        let Simulation {
1235            bodies, broadphase, ..
1236        } = self;
1237        let mut awake = 0;
1238        for (handle, body) in bodies.iter_mut() {
1239            if !body.is_simulated() {
1240                continue;
1241            }
1242            awake += 1;
1243            if body.refresh_bounds(margin) {
1244                broadphase.set_proxy(handle.index() as u32, proxy_for(body));
1245            }
1246        }
1247        awake
1248    }
1249}
1250
1251/// Hand the solver the bodies this step can reach: the ones it moves, and the
1252/// immovable ones a contact leans against. Everything else keeps whatever
1253/// state it had, because nothing will read it, which is what makes a settled
1254/// world cost close to nothing.
1255fn gather(bodies: &Pool<Body>, solver: &mut Solver, manifolds: &[Manifold], joints: &[Joint]) {
1256    for (handle, body) in bodies.iter() {
1257        if body.is_simulated() {
1258            solver.set_body(handle.index() as u32, SolverBody::from_body(body));
1259        }
1260    }
1261    for manifold in manifolds {
1262        gather_partner(bodies, solver, manifold.a, manifold.b);
1263    }
1264    for joint in joints {
1265        gather_partner(bodies, solver, joint.a, joint.b);
1266    }
1267}
1268
1269/// Take the immovable half of a constraint, so the moving half has something
1270/// to lean on.
1271fn gather_partner(bodies: &Pool<Body>, solver: &mut Solver, a: u32, b: u32) {
1272    let simulated = |slot: u32| {
1273        bodies
1274            .get_at(slot as usize)
1275            .is_some_and(|body| body.is_simulated())
1276    };
1277    let (moves_a, moves_b) = (simulated(a), simulated(b));
1278    if moves_a == moves_b {
1279        // Both moving: already taken. Neither moving: the constraint is
1280        // skipped, so neither is needed.
1281        return;
1282    }
1283    let resting = if moves_a { b } else { a };
1284    if let Some(body) = bodies.get_at(resting as usize) {
1285        solver.set_body(resting, SolverBody::from_body(body));
1286    }
1287}
1288
1289fn proxy_for(body: &Body) -> Proxy {
1290    let role = if body.is_sensor() {
1291        Role::Sensor
1292    } else if body.responds_to_contact() {
1293        Role::Dynamic
1294    } else if body.is_kinematic() {
1295        Role::Driven
1296    } else {
1297        Role::Static
1298    };
1299    Proxy {
1300        bounds: body.bounds,
1301        mask: body.mask,
1302        role,
1303    }
1304}
1305
1306fn pool_handle(handle: BodyHandle) -> PoolHandle {
1307    PoolHandle::from_parts(handle.index(), handle.generation())
1308}
1309
1310/// The body a handle still names, or `None` once it has been removed. What a
1311/// caller holding a handle from an earlier step asks.
1312pub(super) fn body_at(bodies: &Pool<Body>, handle: BodyHandle) -> Option<&Body> {
1313    bodies.get(pool_handle(handle))
1314}
1315
1316fn body_handle(handle: PoolHandle) -> BodyHandle {
1317    BodyHandle::from_parts(handle.index() as u32, handle.generation())
1318}
1319
1320/// The handle naming whatever occupies a slot, for a caller that walked the
1321/// broad phase and holds a slot rather than a handle.
1322pub(super) fn handle_at(bodies: &Pool<Body>, slot: u32) -> Option<BodyHandle> {
1323    bodies.handle_at(slot as usize).map(body_handle)
1324}
1325
1326/// Wake whatever a driven body is about to move into, so a platform can
1327/// disturb a stack that has settled on it.
1328///
1329/// Without this a sleeping body would be gathered as immovable and the
1330/// platform would slide straight through it.
1331fn wake_driven_contacts(bodies: &mut Pool<Body>, manifolds: &[Manifold]) {
1332    for manifold in manifolds {
1333        for (slot, other) in [(manifold.a, manifold.b), (manifold.b, manifold.a)] {
1334            let driving = bodies.get_at(slot as usize).is_some_and(|body| {
1335                body.kinematic_target
1336                    .is_some_and(|target| target != body.position)
1337            });
1338            if driving && let Some(body) = bodies.get_at_mut(other as usize) {
1339                body.wake();
1340            }
1341        }
1342    }
1343}
1344
1345/// Decide which islands have settled, and stop simulating the ones that have.
1346fn update_sleep(
1347    config: &SimConfig,
1348    bodies: &mut Pool<Body>,
1349    islands: &mut Islands,
1350    manifolds: &[Manifold],
1351    joints: &[Joint],
1352    dt: f32,
1353) {
1354    for (_, body) in bodies.iter_mut() {
1355        if !body.is_dynamic() {
1356            continue;
1357        }
1358        if !config.allow_sleep {
1359            body.wake();
1360            continue;
1361        }
1362        if body.sleeping {
1363            continue;
1364        }
1365        if body.is_still(config.sleep_linear_velocity, config.sleep_angular_velocity) {
1366            body.sleep_timer += dt;
1367        } else {
1368            body.sleep_timer = 0.0;
1369        }
1370    }
1371    if !config.allow_sleep {
1372        return;
1373    }
1374
1375    islands.clear();
1376    let movable = |slot: u32| {
1377        bodies
1378            .get_at(slot as usize)
1379            .is_some_and(|body| body.is_dynamic())
1380    };
1381    for manifold in manifolds {
1382        if movable(manifold.a) && movable(manifold.b) {
1383            islands.union(manifold.a, manifold.b);
1384        }
1385    }
1386    // Two bodies a joint holds settle together or not at all, exactly as two
1387    // bodies leaning on each other do.
1388    for joint in joints {
1389        if movable(joint.a) && movable(joint.b) {
1390            islands.union(joint.a, joint.b);
1391        }
1392    }
1393    for (handle, body) in bodies.iter() {
1394        if body.is_dynamic() {
1395            islands.mark(
1396                handle.index() as u32,
1397                body.sleep_timer >= config.time_to_sleep,
1398            );
1399        }
1400    }
1401    // A motor with somewhere to go is still doing work however still the
1402    // bodies look, so its island stays awake.
1403    for joint in joints {
1404        if !joint.frame.is_driven() {
1405            continue;
1406        }
1407        for slot in [joint.a, joint.b] {
1408            if movable(slot) {
1409                islands.mark(slot, false);
1410            }
1411        }
1412    }
1413    for (handle, body) in bodies.iter_mut() {
1414        if !body.is_dynamic() {
1415            continue;
1416        }
1417        if islands.island_is_still(handle.index() as u32) {
1418            body.sleep();
1419        } else {
1420            body.sleeping = false;
1421        }
1422    }
1423}
1424
1425#[cfg(test)]
1426mod tests {
1427
1428    // A capsule swept at a floor stops where its lower cap meets it, and the
1429    // hit reports the surface it landed on.
1430    #[test]
1431    fn a_shape_cast_stops_at_the_first_body_in_its_path() {
1432        let mut sim = Simulation::with_capacity(1);
1433        sim.add_fixed(
1434            &ColliderShape::Cuboid {
1435                half_extents: [10.0, 0.5, 10.0],
1436            },
1437            [0.0, -0.5, 0.0],
1438            [0.0; 3],
1439            0.8,
1440            LayerMask::ALL,
1441        );
1442
1443        let capsule = ColliderShape::Capsule {
1444            half_height: 0.6,
1445            radius: 0.3,
1446        };
1447        let hit = sim
1448            .shape_cast(&ShapeCast::new(capsule, [0.0, 4.0, 0.0], [0.0, -8.0, 0.0]))
1449            .expect("the floor is down there");
1450
1451        // The capsule reaches 0.9 below its centre, so it stops with its
1452        // centre 0.9 above the floor.
1453        let landed = 4.0 - hit.toi * 8.0;
1454        assert!((landed - 0.9).abs() < 0.01, "landed at {landed}");
1455        assert!(hit.normal[1] > 0.99, "standing on it: {:?}", hit.normal);
1456    }
1457    use super::*;
1458
1459    const TICK: f32 = 1.0 / 60.0;
1460
1461    fn params(restitution: f32, damping: f32) -> DynamicParams {
1462        DynamicParams {
1463            mass: 1.0,
1464            friction: 0.5,
1465            restitution,
1466            gravity_scale: 1.0,
1467            linear_damping: damping,
1468        }
1469    }
1470
1471    fn floor(sim: &mut Simulation) -> BodyHandle {
1472        sim.add_fixed(
1473            &ColliderShape::Cuboid {
1474                half_extents: [50.0, 1.0, 50.0],
1475            },
1476            [0.0, -1.0, 0.0],
1477            [0.0; 3],
1478            0.8,
1479            LayerMask::ALL,
1480        )
1481        .expect("room")
1482    }
1483
1484    #[test]
1485    fn a_body_falls_under_gravity() {
1486        let mut sim = Simulation::with_capacity(1);
1487        let ball = sim
1488            .add_dynamic(
1489                &ColliderShape::Ball { radius: 0.5 },
1490                [0.0, 10.0, 0.0],
1491                [0.0; 3],
1492                params(0.0, 0.0),
1493                LayerMask::ALL,
1494            )
1495            .expect("room");
1496        for _ in 0..60 {
1497            sim.step(TICK);
1498        }
1499        let (pos, _) = sim.body_pose(ball).expect("live");
1500        // One second of free fall at g = 20 covers about ten units.
1501        assert!((pos[1] - (10.0 - 10.0)).abs() < 0.4, "y = {}", pos[1]);
1502        assert!(sim.linear_velocity(ball).expect("live")[1] < -19.0);
1503    }
1504
1505    #[test]
1506    fn a_zero_or_negative_step_changes_nothing() {
1507        let mut sim = Simulation::with_capacity(1);
1508        let ball = sim
1509            .add_dynamic(
1510                &ColliderShape::Ball { radius: 0.5 },
1511                [0.0, 10.0, 0.0],
1512                [0.0; 3],
1513                params(0.0, 0.0),
1514                LayerMask::ALL,
1515            )
1516            .expect("room");
1517        sim.step(0.0);
1518        sim.step(-1.0);
1519        assert_eq!(sim.body_pose(ball).expect("live").0, [0.0, 10.0, 0.0]);
1520    }
1521
1522    #[test]
1523    fn a_full_pool_declines_rather_than_growing() {
1524        let mut sim = Simulation::with_capacity(1);
1525        assert!(
1526            sim.add_dynamic(
1527                &ColliderShape::Ball { radius: 0.5 },
1528                [0.0, 1.0, 0.0],
1529                [0.0; 3],
1530                params(0.0, 0.0),
1531                LayerMask::ALL
1532            )
1533            .is_some()
1534        );
1535        assert!(
1536            sim.add_dynamic(
1537                &ColliderShape::Ball { radius: 0.5 },
1538                [0.0, 3.0, 0.0],
1539                [0.0; 3],
1540                params(0.0, 0.0),
1541                LayerMask::ALL
1542            )
1543            .is_none()
1544        );
1545        assert_eq!(sim.body_count(), 1);
1546        assert_eq!(sim.capacity(), 1);
1547        assert!(sim.reserved_bytes() > 0);
1548    }
1549
1550    #[test]
1551    fn a_removed_bodys_handle_stops_naming_anything() {
1552        let mut sim = Simulation::with_capacity(2);
1553        let ball = sim
1554            .add_dynamic(
1555                &ColliderShape::Ball { radius: 0.5 },
1556                [0.0, 1.0, 0.0],
1557                [0.0; 3],
1558                params(0.0, 0.0),
1559                LayerMask::ALL,
1560            )
1561            .expect("room");
1562        assert!(sim.remove_body(ball));
1563        assert!(!sim.remove_body(ball));
1564        assert!(sim.body_pose(ball).is_none());
1565        assert_eq!(sim.body_count(), 0);
1566    }
1567
1568    #[test]
1569    fn a_body_lands_on_the_floor_and_stays_on_it() {
1570        let mut sim = Simulation::with_capacity(2);
1571        floor(&mut sim);
1572        let ball = sim
1573            .add_dynamic(
1574                &ColliderShape::Ball { radius: 0.5 },
1575                [0.0, 6.0, 0.0],
1576                [0.0; 3],
1577                params(0.0, 0.0),
1578                LayerMask::ALL,
1579            )
1580            .expect("room");
1581        for _ in 0..240 {
1582            sim.step(TICK);
1583        }
1584        let (pos, _) = sim.body_pose(ball).expect("live");
1585        assert!((pos[1] - 0.5).abs() < 0.02, "y = {}", pos[1]);
1586        assert!(sim.contact_count() > 0);
1587    }
1588
1589    #[test]
1590    fn layers_that_do_not_interact_pass_through_each_other() {
1591        let mut sim = Simulation::with_capacity(2);
1592        sim.add_fixed(
1593            &ColliderShape::Cuboid {
1594                half_extents: [50.0, 1.0, 50.0],
1595            },
1596            [0.0, -1.0, 0.0],
1597            [0.0; 3],
1598            0.8,
1599            LayerMask {
1600                memberships: 0b01,
1601                filter: 0b01,
1602            },
1603        );
1604        let ball = sim
1605            .add_dynamic(
1606                &ColliderShape::Ball { radius: 0.5 },
1607                [0.0, 4.0, 0.0],
1608                [0.0; 3],
1609                params(0.0, 0.0),
1610                LayerMask {
1611                    memberships: 0b10,
1612                    filter: 0b10,
1613                },
1614            )
1615            .expect("room");
1616        for _ in 0..120 {
1617            sim.step(TICK);
1618        }
1619        assert!(sim.body_pose(ball).expect("live").0[1] < -2.0);
1620    }
1621
1622    #[test]
1623    fn an_impulse_moves_a_body_and_wakes_it() {
1624        let mut sim = Simulation::with_capacity(2);
1625        floor(&mut sim);
1626        let ball = sim
1627            .add_dynamic(
1628                &ColliderShape::Ball { radius: 0.5 },
1629                [0.0, 0.5, 0.0],
1630                [0.0; 3],
1631                params(0.0, 0.5),
1632                LayerMask::ALL,
1633            )
1634            .expect("room");
1635        for _ in 0..120 {
1636            sim.step(TICK);
1637        }
1638        assert_eq!(sim.is_sleeping(ball), Some(true));
1639        sim.apply_impulse(ball, [0.0, 8.0, 0.0]);
1640        assert_eq!(sim.is_sleeping(ball), Some(false));
1641        sim.step(TICK);
1642        assert!(sim.body_pose(ball).expect("live").0[1] > 0.55);
1643    }
1644
1645    #[test]
1646    fn velocity_can_be_set_and_read_back() {
1647        let mut sim = Simulation::with_capacity(1);
1648        let ball = sim
1649            .add_dynamic(
1650                &ColliderShape::Ball { radius: 0.5 },
1651                [0.0, 5.0, 0.0],
1652                [0.0; 3],
1653                params(0.0, 0.0),
1654                LayerMask::ALL,
1655            )
1656            .expect("room");
1657        sim.set_linear_velocity(ball, [1.0, 0.0, 0.0]);
1658        sim.set_angular_velocity(ball, [0.0, 2.0, 0.0]);
1659        assert_eq!(sim.linear_velocity(ball), Some([1.0, 0.0, 0.0]));
1660        assert_eq!(sim.angular_velocity(ball), Some([0.0, 2.0, 0.0]));
1661        assert!(sim.mass(ball).expect("live") > 0.0);
1662    }
1663
1664    // The same world stepped twice must land on the same bits, or nothing
1665    // downstream can be reproduced.
1666    #[test]
1667    fn two_identical_runs_agree_bit_for_bit() {
1668        let run = || {
1669            let mut sim = Simulation::with_capacity(16);
1670            floor(&mut sim);
1671            let mut handles = Vec::new();
1672            for i in 0..12 {
1673                handles.push(
1674                    sim.add_dynamic(
1675                        &ColliderShape::Cuboid {
1676                            half_extents: [0.4, 0.4, 0.4],
1677                        },
1678                        [(i % 3) as f32 * 0.9, 1.0 + (i / 3) as f32 * 0.9, 0.0],
1679                        [0.0, i as f32 * 7.0, 0.0],
1680                        params(0.3, 0.0),
1681                        LayerMask::ALL,
1682                    )
1683                    .expect("room"),
1684                );
1685            }
1686            for _ in 0..90 {
1687                sim.step(TICK);
1688            }
1689            handles
1690                .iter()
1691                .map(|&h| {
1692                    let (p, r) = sim.body_pose(h).expect("live");
1693                    (
1694                        [p[0].to_bits(), p[1].to_bits(), p[2].to_bits()],
1695                        [r[0].to_bits(), r[1].to_bits(), r[2].to_bits()],
1696                    )
1697                })
1698                .collect::<Vec<_>>()
1699        };
1700        assert_eq!(run(), run());
1701    }
1702
1703    #[test]
1704    fn a_ray_finds_the_nearest_body_along_it() {
1705        let mut sim = Simulation::with_capacity(4);
1706        for (index, z) in [2.0f32, 5.0, 9.0].into_iter().enumerate() {
1707            sim.add_fixed(
1708                &[
1709                    ColliderShape::Ball { radius: 0.5 },
1710                    ColliderShape::Cuboid {
1711                        half_extents: [0.5, 0.5, 0.5],
1712                    },
1713                    ColliderShape::Capsule {
1714                        half_height: 0.5,
1715                        radius: 0.25,
1716                    },
1717                ][index],
1718                [0.0, 0.0, z],
1719                [0.0; 3],
1720                0.5,
1721                LayerMask::ALL,
1722            )
1723            .expect("room");
1724        }
1725        let hit = sim
1726            .raycast(
1727                [0.0, 0.0, -5.0],
1728                [0.0, 0.0, 1.0],
1729                100.0,
1730                None,
1731                LayerMask::ALL,
1732            )
1733            .expect("a hit");
1734        assert!((hit.distance - 6.5).abs() < 1.0e-4, "{hit:?}");
1735        assert!((hit.normal[2] + 1.0).abs() < 1.0e-4, "{hit:?}");
1736        assert!((hit.point[2] - 1.5).abs() < 1.0e-4, "{hit:?}");
1737    }
1738
1739    #[test]
1740    fn a_ray_answers_the_same_way_from_either_end_of_the_scene() {
1741        let mut sim = Simulation::with_capacity(4);
1742        for z in [2.0f32, 5.0, 9.0] {
1743            sim.add_fixed(
1744                &ColliderShape::Ball { radius: 0.5 },
1745                [0.0, 0.0, z],
1746                [0.0; 3],
1747                0.5,
1748                LayerMask::ALL,
1749            )
1750            .expect("room");
1751        }
1752        sim.step(TICK);
1753        let forward = sim
1754            .raycast(
1755                [0.0, 0.0, -5.0],
1756                [0.0, 0.0, 1.0],
1757                100.0,
1758                None,
1759                LayerMask::ALL,
1760            )
1761            .expect("a hit");
1762        let backward = sim
1763            .raycast(
1764                [0.0, 0.0, 20.0],
1765                [0.0, 0.0, -1.0],
1766                100.0,
1767                None,
1768                LayerMask::ALL,
1769            )
1770            .expect("a hit");
1771        assert!((forward.distance - 6.5).abs() < 1.0e-4, "{forward:?}");
1772        assert!((backward.distance - 10.5).abs() < 1.0e-4, "{backward:?}");
1773    }
1774
1775    #[test]
1776    fn a_ray_skips_layers_it_does_not_interact_with() {
1777        let mut sim = Simulation::with_capacity(2);
1778        let near = LayerMask {
1779            memberships: 0b01,
1780            filter: 0b11,
1781        };
1782        let far = LayerMask {
1783            memberships: 0b10,
1784            filter: 0b11,
1785        };
1786        sim.add_fixed(
1787            &ColliderShape::Ball { radius: 0.5 },
1788            [0.0, 0.0, 0.0],
1789            [0.0; 3],
1790            0.5,
1791            near,
1792        )
1793        .expect("room");
1794        sim.add_fixed(
1795            &ColliderShape::Ball { radius: 0.5 },
1796            [0.0, 0.0, 5.0],
1797            [0.0; 3],
1798            0.5,
1799            far,
1800        )
1801        .expect("room");
1802
1803        let only_far = LayerMask {
1804            memberships: 0b11,
1805            filter: 0b10,
1806        };
1807        let hit = sim
1808            .raycast([0.0, 0.0, -5.0], [0.0, 0.0, 1.0], 100.0, None, only_far)
1809            .expect("a hit");
1810        assert!(
1811            (hit.distance - 9.5).abs() < 1.0e-4,
1812            "the near one is hidden"
1813        );
1814        // With no layer in common there is nothing to hit at all.
1815        assert!(
1816            sim.raycast(
1817                [0.0, 0.0, -5.0],
1818                [0.0, 0.0, 1.0],
1819                100.0,
1820                None,
1821                LayerMask {
1822                    memberships: 0b100,
1823                    filter: 0b100,
1824                },
1825            )
1826            .is_none()
1827        );
1828    }
1829
1830    #[test]
1831    fn a_ray_can_be_told_to_leave_one_body_out() {
1832        let mut sim = Simulation::with_capacity(2);
1833        let near = sim
1834            .add_fixed(
1835                &ColliderShape::Ball { radius: 0.5 },
1836                [0.0, 0.0, 0.0],
1837                [0.0; 3],
1838                0.5,
1839                LayerMask::ALL,
1840            )
1841            .expect("room");
1842        sim.add_fixed(
1843            &ColliderShape::Ball { radius: 0.5 },
1844            [0.0, 0.0, 5.0],
1845            [0.0; 3],
1846            0.5,
1847            LayerMask::ALL,
1848        )
1849        .expect("room");
1850
1851        let cast = |exclude| {
1852            sim.raycast(
1853                [0.0, 0.0, -5.0],
1854                [0.0, 0.0, 1.0],
1855                100.0,
1856                exclude,
1857                LayerMask::ALL,
1858            )
1859        };
1860        assert!((cast(None).expect("a hit").distance - 4.5).abs() < 1.0e-4);
1861        assert!((cast(Some(near)).expect("a hit").distance - 9.5).abs() < 1.0e-4);
1862    }
1863
1864    // A handle to a removed body must not exclude whatever took its slot.
1865    #[test]
1866    fn a_stale_exclusion_does_not_hide_the_slots_new_occupant() {
1867        let mut sim = Simulation::with_capacity(1);
1868        let first = sim
1869            .add_fixed(
1870                &ColliderShape::Ball { radius: 0.5 },
1871                [0.0, 0.0, 0.0],
1872                [0.0; 3],
1873                0.5,
1874                LayerMask::ALL,
1875            )
1876            .expect("room");
1877        assert!(sim.remove_body(first));
1878        sim.add_fixed(
1879            &ColliderShape::Ball { radius: 0.5 },
1880            [0.0, 0.0, 0.0],
1881            [0.0; 3],
1882            0.5,
1883            LayerMask::ALL,
1884        )
1885        .expect("the freed slot");
1886        assert!(
1887            sim.raycast(
1888                [0.0, 0.0, -5.0],
1889                [0.0, 0.0, 1.0],
1890                100.0,
1891                Some(first),
1892                LayerMask::ALL,
1893            )
1894            .is_some(),
1895            "the stale handle names nobody"
1896        );
1897    }
1898
1899    #[test]
1900    fn a_removed_body_stops_being_hit() {
1901        let mut sim = Simulation::with_capacity(1);
1902        let ball = sim
1903            .add_fixed(
1904                &ColliderShape::Ball { radius: 0.5 },
1905                [0.0, 0.0, 0.0],
1906                [0.0; 3],
1907                0.5,
1908                LayerMask::ALL,
1909            )
1910            .expect("room");
1911        let cast = |sim: &Simulation| {
1912            sim.raycast(
1913                [0.0, 0.0, -5.0],
1914                [0.0, 0.0, 1.0],
1915                100.0,
1916                None,
1917                LayerMask::ALL,
1918            )
1919        };
1920        assert!(cast(&sim).is_some());
1921        sim.remove_body(ball);
1922        assert!(cast(&sim).is_none());
1923    }
1924
1925    #[test]
1926    fn a_ray_respects_its_distance_limit_and_needs_a_direction() {
1927        let mut sim = Simulation::with_capacity(1);
1928        sim.add_fixed(
1929            &ColliderShape::Ball { radius: 0.5 },
1930            [0.0, 0.0, 0.0],
1931            [0.0; 3],
1932            0.5,
1933            LayerMask::ALL,
1934        )
1935        .expect("room");
1936        let cast =
1937            |dir: [f32; 3], max| sim.raycast([0.0, 0.0, -5.0], dir, max, None, LayerMask::ALL);
1938        // The surface is exactly 4.5 away.
1939        assert!(cast([0.0, 0.0, 1.0], 4.5).is_some());
1940        assert!(cast([0.0, 0.0, 1.0], 4.4).is_none());
1941        assert!(cast([0.0, 0.0, 0.0], 100.0).is_none(), "no direction");
1942        assert!(cast([0.0, 0.0, 1.0], 0.0).is_none(), "no reach");
1943        assert!(cast([0.0, 0.0, 1.0], -1.0).is_none());
1944        assert!(cast([f32::NAN, 0.0, 0.0], 100.0).is_none());
1945        // An unnormalised direction is the same ray.
1946        assert!((cast([0.0, 0.0, 7.0], 100.0).expect("a hit").distance - 4.5).abs() < 1.0e-4);
1947    }
1948
1949    #[test]
1950    fn a_ray_finds_a_body_added_since_the_last_step() {
1951        let mut sim = Simulation::with_capacity(2);
1952        floor(&mut sim);
1953        sim.step(TICK);
1954        sim.add_fixed(
1955            &ColliderShape::Ball { radius: 0.5 },
1956            [0.0, 5.0, 0.0],
1957            [0.0; 3],
1958            0.5,
1959            LayerMask::ALL,
1960        )
1961        .expect("room");
1962        let hit = sim
1963            .raycast(
1964                [0.0, 9.0, 0.0],
1965                [0.0, -1.0, 0.0],
1966                100.0,
1967                None,
1968                LayerMask::ALL,
1969            )
1970            .expect("a hit");
1971        assert!((hit.distance - 3.5).abs() < 1.0e-4, "{hit:?}");
1972    }
1973
1974    #[test]
1975    fn a_ray_hits_a_position_driven_body() {
1976        let mut sim = Simulation::with_capacity(1);
1977        sim.add_kinematic(
1978            &ColliderShape::Cuboid {
1979                half_extents: [1.0, 0.25, 1.0],
1980            },
1981            [0.0, 0.0, 0.0],
1982            [0.0; 3],
1983            0.5,
1984            LayerMask::ALL,
1985        )
1986        .expect("room");
1987        let hit = sim
1988            .raycast(
1989                [0.0, 5.0, 0.0],
1990                [0.0, -1.0, 0.0],
1991                100.0,
1992                None,
1993                LayerMask::ALL,
1994            )
1995            .expect("a hit");
1996        assert!((hit.distance - 4.75).abs() < 1.0e-4, "{hit:?}");
1997    }
1998
1999    #[test]
2000    fn a_shape_cast_stops_at_the_nearest_body_and_names_it() {
2001        let mut sim = Simulation::with_capacity(3);
2002        let ground = floor(&mut sim);
2003        let ledge = sim
2004            .add_fixed(
2005                &ColliderShape::Cuboid {
2006                    half_extents: [2.0, 0.5, 2.0],
2007                },
2008                [0.0, 3.0, 0.0],
2009                [0.0; 3],
2010                0.5,
2011                LayerMask::ALL,
2012            )
2013            .expect("room");
2014        let capsule = ColliderShape::Capsule {
2015            half_height: 0.5,
2016            radius: 0.25,
2017        };
2018        let hit = sim
2019            .shape_cast(&ShapeCast::new(capsule, [0.0, 9.0, 0.0], [0.0, -12.0, 0.0]))
2020            .expect("a hit");
2021        assert_eq!(hit.body, ledge, "the ledge, not the ground under it");
2022        let landed = 9.0 - hit.toi * 12.0;
2023        assert!((landed - 4.25).abs() < 1.0e-2, "landed at {landed}");
2024        assert!(!hit.started_touching);
2025        assert_ne!(hit.body, ground);
2026    }
2027
2028    #[test]
2029    fn a_shape_cast_that_reaches_nothing_reports_nothing() {
2030        let mut sim = Simulation::with_capacity(2);
2031        floor(&mut sim);
2032        let ball = ColliderShape::Ball { radius: 0.5 };
2033        assert!(
2034            sim.shape_cast(&ShapeCast::new(ball, [0.0, 9.0, 0.0], [0.0, -1.0, 0.0]))
2035                .is_none()
2036        );
2037        assert!(
2038            sim.shape_cast(&ShapeCast::new(ball, [0.0, 9.0, 0.0], [0.0, 5.0, 0.0]))
2039                .is_none(),
2040            "away from everything"
2041        );
2042    }
2043
2044    #[test]
2045    fn a_shape_cast_says_when_it_began_in_contact() {
2046        let mut sim = Simulation::with_capacity(2);
2047        let ground = floor(&mut sim);
2048        let ball = ColliderShape::Ball { radius: 0.5 };
2049        let hit = sim
2050            .shape_cast(&ShapeCast::new(ball, [0.0, 0.4, 0.0], [3.0, 0.0, 0.0]))
2051            .expect("a hit");
2052        assert_eq!(hit.body, ground);
2053        assert_eq!(hit.toi, 0.0);
2054        assert!(hit.started_touching);
2055        assert!(hit.normal[1] > 0.9, "{hit:?}");
2056    }
2057
2058    #[test]
2059    fn a_shape_cast_honours_its_layer_filter_and_its_exclusion() {
2060        let mut sim = Simulation::with_capacity(2);
2061        let near = sim
2062            .add_fixed(
2063                &ColliderShape::Cuboid {
2064                    half_extents: [2.0, 0.5, 2.0],
2065                },
2066                [0.0, 2.0, 0.0],
2067                [0.0; 3],
2068                0.5,
2069                LayerMask {
2070                    memberships: 0b01,
2071                    filter: 0b11,
2072                },
2073            )
2074            .expect("room");
2075        let far = sim
2076            .add_fixed(
2077                &ColliderShape::Cuboid {
2078                    half_extents: [2.0, 0.5, 2.0],
2079                },
2080                [0.0, 0.0, 0.0],
2081                [0.0; 3],
2082                0.5,
2083                LayerMask {
2084                    memberships: 0b10,
2085                    filter: 0b11,
2086                },
2087            )
2088            .expect("room");
2089        let ball = ColliderShape::Ball { radius: 0.5 };
2090        let straight = ShapeCast::new(ball, [0.0, 6.0, 0.0], [0.0, -8.0, 0.0]);
2091        assert_eq!(sim.shape_cast(&straight).expect("a hit").body, near);
2092        assert_eq!(
2093            sim.shape_cast(&ShapeCast {
2094                exclude: Some(near),
2095                ..straight
2096            })
2097            .expect("a hit")
2098            .body,
2099            far
2100        );
2101        assert_eq!(
2102            sim.shape_cast(&ShapeCast {
2103                mask: LayerMask {
2104                    memberships: 0b11,
2105                    filter: 0b10,
2106                },
2107                ..straight
2108            })
2109            .expect("a hit")
2110            .body,
2111            far
2112        );
2113    }
2114
2115    #[test]
2116    fn a_driven_body_arrives_exactly_where_it_was_sent() {
2117        let mut sim = Simulation::with_capacity(1);
2118        let platform = sim
2119            .add_kinematic(
2120                &ColliderShape::Cuboid {
2121                    half_extents: [1.0, 0.25, 1.0],
2122                },
2123                [0.0, 0.0, 0.0],
2124                [0.0; 3],
2125                0.5,
2126                LayerMask::ALL,
2127            )
2128            .expect("room");
2129        assert!(sim.set_kinematic_translation(platform, [1.5, 2.0, -3.0]));
2130        sim.step(TICK);
2131        assert_eq!(sim.body_pose(platform).expect("live").0, [1.5, 2.0, -3.0]);
2132        // The target is spent, so a body left alone stops where it arrived.
2133        sim.step(TICK);
2134        assert_eq!(sim.body_pose(platform).expect("live").0, [1.5, 2.0, -3.0]);
2135        assert_eq!(sim.linear_velocity(platform), Some([0.0; 3]));
2136    }
2137
2138    #[test]
2139    fn a_driven_body_ignores_gravity_and_impulses() {
2140        let mut sim = Simulation::with_capacity(1);
2141        let platform = sim
2142            .add_kinematic(
2143                &ColliderShape::Ball { radius: 0.5 },
2144                [0.0, 5.0, 0.0],
2145                [0.0; 3],
2146                0.5,
2147                LayerMask::ALL,
2148            )
2149            .expect("room");
2150        sim.apply_impulse(platform, [0.0, 100.0, 0.0]);
2151        for _ in 0..120 {
2152            sim.step(TICK);
2153        }
2154        assert_eq!(sim.body_pose(platform).expect("live").0, [0.0, 5.0, 0.0]);
2155        assert_eq!(sim.mass(platform), Some(0.0));
2156        assert_eq!(sim.is_kinematic(platform), Some(true));
2157    }
2158
2159    // A character capsule is a position-driven body: it holds its place under
2160    // gravity and is moved by a target rather than by the solver.
2161    #[test]
2162    fn a_character_capsule_is_a_position_driven_body() {
2163        let mut sim = Simulation::with_capacity(1);
2164        let handle = sim
2165            .add_character(0.6, 0.3, [0.0, 4.0, 0.0], LayerMask::ALL)
2166            .expect("room in the pool");
2167        assert_eq!(sim.is_kinematic(handle), Some(true));
2168
2169        for _ in 0..60 {
2170            sim.step(TICK);
2171        }
2172        let (position, _) = sim.body_pose(handle).expect("a live body");
2173        assert_eq!(position[1], 4.0, "gravity does not move a driven capsule");
2174
2175        assert!(sim.set_kinematic_translation(handle, [0.0, 3.0, 0.0]));
2176        sim.step(TICK);
2177        let (position, _) = sim.body_pose(handle).expect("a live body");
2178        assert!((position[1] - 3.0).abs() < 1.0e-5, "{position:?}");
2179    }
2180
2181    // A body set to a position it already holds must not read as motion, and
2182    // a body that is not driven by position must refuse the instruction.
2183    #[test]
2184    fn only_a_position_driven_body_takes_a_translation_target() {
2185        let mut sim = Simulation::with_capacity(2);
2186        let fixed = floor(&mut sim);
2187        let ball = sim
2188            .add_dynamic(
2189                &ColliderShape::Ball { radius: 0.5 },
2190                [0.0, 5.0, 0.0],
2191                [0.0; 3],
2192                params(0.0, 0.0),
2193                LayerMask::ALL,
2194            )
2195            .expect("room");
2196        assert!(!sim.set_kinematic_translation(fixed, [0.0, 9.0, 0.0]));
2197        assert!(!sim.set_kinematic_translation(ball, [0.0, 9.0, 0.0]));
2198        sim.step(TICK);
2199        assert!(sim.body_pose(ball).expect("live").0[1] < 5.0, "still falls");
2200    }
2201
2202    #[test]
2203    fn a_driven_body_pushes_a_dynamic_one_it_is_moved_into() {
2204        let mut sim = Simulation::with_capacity(3);
2205        floor(&mut sim);
2206        let crate_body = sim
2207            .add_dynamic(
2208                &ColliderShape::Cuboid {
2209                    half_extents: [0.5, 0.5, 0.5],
2210                },
2211                [0.0, 0.5, 0.0],
2212                [0.0; 3],
2213                params(0.0, 0.0),
2214                LayerMask::ALL,
2215            )
2216            .expect("room");
2217        let blade = sim
2218            .add_kinematic(
2219                &ColliderShape::Cuboid {
2220                    half_extents: [0.5, 0.5, 0.5],
2221                },
2222                [-3.0, 0.5, 0.0],
2223                [0.0; 3],
2224                0.5,
2225                LayerMask::ALL,
2226            )
2227            .expect("room");
2228
2229        // Long enough for the crate to settle and doze off before the blade
2230        // arrives, so this also proves the push wakes it.
2231        let mut x = -3.0f32;
2232        for step in 0..180 {
2233            if step == 60 {
2234                assert_eq!(sim.is_sleeping(crate_body), Some(true), "settled first");
2235            }
2236            if step >= 60 {
2237                x += 0.03;
2238                assert!(sim.set_kinematic_translation(blade, [x, 0.5, 0.0]));
2239            }
2240            sim.step(TICK);
2241        }
2242        assert!((sim.body_pose(blade).expect("live").0[0] - x).abs() < 1.0e-5);
2243        let pushed = sim.body_pose(crate_body).expect("live").0[0];
2244        assert!(pushed > 1.5, "the crate was shoved along: {pushed}");
2245        assert!(
2246            pushed > x,
2247            "and it stays ahead of the blade: {pushed} vs {x}"
2248        );
2249    }
2250
2251    #[test]
2252    fn switching_a_bodys_kind_keeps_its_handle_and_leaves_the_world_standing() {
2253        let mut sim = Simulation::with_capacity(2);
2254        floor(&mut sim);
2255        let ball = sim
2256            .add_dynamic(
2257                &ColliderShape::Ball { radius: 0.5 },
2258                [0.0, 4.0, 0.0],
2259                [0.0; 3],
2260                params(0.0, 0.0),
2261                LayerMask::ALL,
2262            )
2263            .expect("room");
2264        for _ in 0..180 {
2265            sim.step(TICK);
2266        }
2267        let resting = sim.body_pose(ball).expect("live").0;
2268        assert!((resting[1] - 0.5).abs() < 0.02, "{resting:?}");
2269
2270        // Picked up: held in the air by position alone.
2271        assert!(sim.make_kinematic(ball));
2272        assert_eq!(sim.is_kinematic(ball), Some(true));
2273        assert!(sim.set_kinematic_translation(ball, [0.0, 6.0, 0.0]));
2274        sim.step(TICK);
2275        for _ in 0..60 {
2276            sim.step(TICK);
2277        }
2278        assert_eq!(sim.body_pose(ball).expect("live").0, [0.0, 6.0, 0.0]);
2279
2280        // Thrown: the same handle, back under gravity, and it lands again.
2281        assert!(sim.make_dynamic(ball, [0.0, 0.0, 2.0]));
2282        assert_eq!(sim.is_kinematic(ball), Some(false));
2283        assert!(sim.mass(ball).expect("live") > 0.0);
2284        for _ in 0..300 {
2285            sim.step(TICK);
2286        }
2287        let landed = sim.body_pose(ball).expect("live").0;
2288        assert!(
2289            (landed[1] - 0.5).abs() < 0.02,
2290            "back on the floor: {landed:?}"
2291        );
2292        assert!(landed[2] > 0.5, "and it travelled: {landed:?}");
2293        assert_eq!(sim.body_count(), 2);
2294        assert_eq!(sim.collider_count(), sim.body_count());
2295    }
2296
2297    // The broad phase filters pairs by whether contact can move either body,
2298    // so a switch that did not reach it would leave a stack leaning on
2299    // nothing.
2300    #[test]
2301    fn a_stack_stays_up_when_the_body_under_it_is_switched() {
2302        let mut sim = Simulation::with_capacity(3);
2303        floor(&mut sim);
2304        let cube = ColliderShape::Cuboid {
2305            half_extents: [0.5, 0.5, 0.5],
2306        };
2307        let lower = sim
2308            .add_dynamic(
2309                &cube,
2310                [0.0, 0.5, 0.0],
2311                [0.0; 3],
2312                params(0.0, 0.0),
2313                LayerMask::ALL,
2314            )
2315            .expect("room");
2316        let upper = sim
2317            .add_dynamic(
2318                &cube,
2319                [0.0, 1.5, 0.0],
2320                [0.0; 3],
2321                params(0.0, 0.0),
2322                LayerMask::ALL,
2323            )
2324            .expect("room");
2325        for _ in 0..180 {
2326            sim.step(TICK);
2327        }
2328        let held = sim.body_pose(lower).expect("live").0;
2329        assert!(sim.make_kinematic(lower));
2330        for _ in 0..180 {
2331            sim.step(TICK);
2332        }
2333        let top = sim.body_pose(upper).expect("live").0;
2334        assert!(
2335            (top[1] - 1.5).abs() < 0.05,
2336            "the top box still rests: {top:?}"
2337        );
2338        assert_eq!(
2339            sim.body_pose(lower).expect("live").0,
2340            held,
2341            "and the one below it has not stirred"
2342        );
2343
2344        // And a lift carries the box above it up too.
2345        assert!(sim.set_kinematic_translation(lower, [held[0], 1.5, held[2]]));
2346        for _ in 0..120 {
2347            sim.step(TICK);
2348        }
2349        let lifted = sim.body_pose(upper).expect("live").0;
2350        assert!(lifted[1] > 2.0, "carried up: {lifted:?}");
2351    }
2352
2353    #[test]
2354    fn switching_the_kind_of_a_body_that_is_not_there_reports_so() {
2355        let mut sim = Simulation::with_capacity(1);
2356        let ball = sim
2357            .add_dynamic(
2358                &ColliderShape::Ball { radius: 0.5 },
2359                [0.0, 1.0, 0.0],
2360                [0.0; 3],
2361                params(0.0, 0.0),
2362                LayerMask::ALL,
2363            )
2364            .expect("room");
2365        assert!(sim.remove_body(ball));
2366        assert!(!sim.make_kinematic(ball));
2367        assert!(!sim.make_dynamic(ball, [0.0; 3]));
2368        assert!(!sim.set_kinematic_translation(ball, [0.0; 3]));
2369        assert_eq!(sim.is_kinematic(ball), None);
2370    }
2371
2372    // Two runs of the same query on the same world must agree bit for bit,
2373    // or nothing built on a query can be reproduced.
2374    #[test]
2375    fn queries_answer_the_same_bits_twice_running() {
2376        let mut sim = Simulation::with_capacity(17);
2377        floor(&mut sim);
2378        // Terrain answers along its own faces rather than through the sweep
2379        // the convex shapes use, so it needs its own place in this check.
2380        let side = 9usize;
2381        let mut heights = Vec::with_capacity(side * side);
2382        for row in 0..side {
2383            for col in 0..side {
2384                heights.push((row as f32 * 0.37).sin() * 0.4 + (col as f32 * 0.21).cos() * 0.3);
2385            }
2386        }
2387        sim.add_heightfield(
2388            side,
2389            side,
2390            heights,
2391            [24.0, 1.0, 24.0],
2392            [0.0, -3.0, 0.0],
2393            LayerMask::ALL,
2394        )
2395        .expect("room");
2396        for i in 0..12 {
2397            sim.add_dynamic(
2398                &ColliderShape::Cuboid {
2399                    half_extents: [0.4, 0.4, 0.4],
2400                },
2401                [(i % 3) as f32 * 0.9, 1.0 + (i / 3) as f32 * 0.9, 0.0],
2402                [0.0, i as f32 * 7.0, 0.0],
2403                params(0.3, 0.0),
2404                LayerMask::ALL,
2405            )
2406            .expect("room");
2407        }
2408        for _ in 0..90 {
2409            sim.step(TICK);
2410        }
2411        let ray = |sim: &Simulation| {
2412            sim.raycast(
2413                [-6.0, 1.3, 0.1],
2414                [1.0, -0.1, 0.0],
2415                40.0,
2416                None,
2417                LayerMask::ALL,
2418            )
2419            .map(|hit| (hit.distance.to_bits(), hit.point, hit.normal))
2420        };
2421        let sweep = |sim: &Simulation| {
2422            sim.shape_cast(&ShapeCast::new(
2423                ColliderShape::Capsule {
2424                    half_height: 0.3,
2425                    radius: 0.2,
2426                },
2427                [-6.0, 1.3, 0.1],
2428                [12.0, 0.0, 0.0],
2429            ))
2430            .map(|hit| (hit.toi.to_bits(), hit.point, hit.normal, hit.body))
2431        };
2432        let shape = Simulation::character_shape(0.3, 0.2);
2433        let capsule = sim
2434            .add_kinematic(
2435                &ColliderShape::Capsule {
2436                    half_height: 0.3,
2437                    radius: 0.2,
2438                },
2439                [-6.0, 1.3, 0.1],
2440                [0.0; 3],
2441                0.8,
2442                LayerMask::ALL,
2443            )
2444            .expect("room");
2445        let walk = |sim: &Simulation| {
2446            let moved = sim.move_character(
2447                &shape,
2448                &CharacterMoveInput {
2449                    center: [-6.0, 1.3, 0.1],
2450                    desired: [12.0, -0.05, 0.0],
2451                    dt: TICK,
2452                    exclude: capsule,
2453                    mask: LayerMask::ALL,
2454                },
2455            );
2456            (moved.translation.map(f32::to_bits), moved.grounded)
2457        };
2458        // Fired into the terrain rather than at the stack, so the two answers
2459        // below are about the height grid and not about the boxes on it.
2460        let terrain_ray = |sim: &Simulation| {
2461            sim.raycast(
2462                [-1.7, 6.0, 2.3],
2463                [0.0, -1.0, 0.0],
2464                40.0,
2465                None,
2466                LayerMask::ALL,
2467            )
2468            .map(|hit| (hit.distance.to_bits(), hit.point, hit.normal))
2469        };
2470        let terrain_sweep = |sim: &Simulation| {
2471            sim.shape_cast(&ShapeCast::new(
2472                ColliderShape::Ball { radius: 0.4 },
2473                [-9.0, -2.2, 2.3],
2474                [18.0, 0.0, 0.0],
2475            ))
2476            .map(|hit| (hit.toi.to_bits(), hit.point, hit.normal, hit.body))
2477        };
2478        assert!(
2479            ray(&sim).is_some() && sweep(&sim).is_some(),
2480            "the scene is in the way"
2481        );
2482        assert!(
2483            terrain_ray(&sim).is_some() && terrain_sweep(&sim).is_some(),
2484            "and so is the terrain"
2485        );
2486        assert_eq!(ray(&sim), ray(&sim));
2487        assert_eq!(sweep(&sim), sweep(&sim));
2488        assert_eq!(walk(&sim), walk(&sim));
2489        assert_eq!(terrain_ray(&sim), terrain_ray(&sim));
2490        assert_eq!(terrain_sweep(&sim), terrain_sweep(&sim));
2491    }
2492
2493    #[test]
2494    fn config_round_trips_and_takes_effect() {
2495        // Sleeping is off for the weightless stretch: a body that settles
2496        // there would stop being simulated, and the test would be measuring
2497        // that rather than the gravity change.
2498        let mut sim = Simulation::new(
2499            SimConfig {
2500                gravity: 0.0,
2501                allow_sleep: false,
2502                ..SimConfig::default()
2503            },
2504            1,
2505        );
2506        assert_eq!(sim.config().gravity, 0.0);
2507        let ball = sim
2508            .add_dynamic(
2509                &ColliderShape::Ball { radius: 0.5 },
2510                [0.0, 5.0, 0.0],
2511                [0.0; 3],
2512                params(0.0, 0.0),
2513                LayerMask::ALL,
2514            )
2515            .expect("room");
2516        for _ in 0..60 {
2517            sim.step(TICK);
2518        }
2519        assert_eq!(sim.body_pose(ball).expect("live").0[1], 5.0);
2520        sim.set_config(SimConfig::default());
2521        assert_eq!(sim.config().gravity, crate::physics::GRAVITY);
2522        sim.step(TICK);
2523        assert!(sim.body_pose(ball).expect("live").0[1] < 5.0);
2524    }
2525
2526    #[test]
2527    fn gravity_scale_and_damping_do_what_they_say() {
2528        let mut sim = Simulation::with_capacity(2);
2529        let floating = sim
2530            .add_dynamic(
2531                &ColliderShape::Ball { radius: 0.5 },
2532                [0.0, 5.0, 0.0],
2533                [0.0; 3],
2534                DynamicParams {
2535                    gravity_scale: 0.0,
2536                    ..params(0.0, 0.0)
2537                },
2538                LayerMask::ALL,
2539            )
2540            .expect("room");
2541        let damped = sim
2542            .add_dynamic(
2543                &ColliderShape::Ball { radius: 0.5 },
2544                [5.0, 5.0, 0.0],
2545                [0.0; 3],
2546                DynamicParams {
2547                    gravity_scale: 0.0,
2548                    ..params(0.0, 4.0)
2549                },
2550                LayerMask::ALL,
2551            )
2552            .expect("room");
2553        sim.set_linear_velocity(floating, [3.0, 0.0, 0.0]);
2554        sim.set_linear_velocity(damped, [3.0, 0.0, 0.0]);
2555        for _ in 0..60 {
2556            sim.step(TICK);
2557        }
2558        assert_eq!(sim.body_pose(floating).expect("live").0[1], 5.0);
2559        let free = sim.linear_velocity(floating).expect("live")[0];
2560        let slowed = sim.linear_velocity(damped).expect("live")[0];
2561        assert!((free - 3.0).abs() < 1.0e-5, "{free}");
2562        assert!(slowed < 0.5, "damping must bleed the speed off: {slowed}");
2563    }
2564}