Skip to main content

concinnity_physics/sim/
world.rs

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