euv_engine/physics/
impl.rs

1use super::*;
2
3/// Implements `Default` for `BodyCollider`, returning an AABB collider with default values.
4impl Default for BodyCollider {
5    fn default() -> BodyCollider {
6        BodyCollider::Aabb(AabbCollider::default())
7    }
8}
9
10/// Implements `Default` for `BodyCollider3D`, returning a 3D AABB collider with default values.
11impl Default for BodyCollider3D {
12    fn default() -> BodyCollider3D {
13        BodyCollider3D::Aabb(AabbCollider3D::default())
14    }
15}
16
17/// Implements default configuration for `PhysicsConfig`.
18impl Default for PhysicsConfig {
19    fn default() -> PhysicsConfig {
20        PhysicsConfig::new(
21            Vector2D::new(0.0, DEFAULT_GRAVITY),
22            DEFAULT_LINEAR_DAMPING,
23            DEFAULT_ANGULAR_DAMPING,
24        )
25    }
26}
27
28/// Implements body creation and force management for `RigidBody2D`.
29impl RigidBody2D {
30    /// Creates a new dynamic rigid body with default mass and the given position.
31    ///
32    /// # Arguments
33    ///
34    /// - `u64` - The unique ID.
35    /// - `Vector2D` - The initial position.
36    ///
37    /// # Returns
38    ///
39    /// - `RigidBody2D` - The new body.
40    pub fn new_dynamic(id: u64, position: Vector2D) -> RigidBody2D {
41        let mass: f64 = PHYSICS_DEFAULT_MASS;
42        RigidBody2D::new(
43            id,
44            position,
45            mass,
46            1.0 / mass,
47            DEFAULT_RESTITUTION,
48            DEFAULT_FRICTION,
49            BodyType::Dynamic,
50        )
51    }
52
53    /// Creates a new static rigid body at the given position with infinite mass.
54    ///
55    /// # Arguments
56    ///
57    /// - `u64` - The unique ID.
58    /// - `Vector2D` - The position.
59    ///
60    /// # Returns
61    ///
62    /// - `RigidBody2D` - The new static body.
63    pub fn new_static(id: u64, position: Vector2D) -> RigidBody2D {
64        RigidBody2D::new(
65            id,
66            position,
67            PHYSICS_STATIC_MASS,
68            0.0,
69            DEFAULT_RESTITUTION,
70            DEFAULT_FRICTION,
71            BodyType::Static,
72        )
73    }
74
75    /// Applies a force to the body's force accumulator.
76    ///
77    /// # Arguments
78    ///
79    /// - `Vector2D` - The force vector.
80    pub fn apply_force(&mut self, force: Vector2D) {
81        *self.get_mut_force_accumulator() += force;
82    }
83
84    /// Applies an instantaneous impulse, directly changing velocity.
85    ///
86    /// # Arguments
87    ///
88    /// - `Vector2D` - The impulse vector.
89    pub fn apply_impulse(&mut self, impulse: Vector2D) {
90        let inverse_mass: f64 = self.get_inverse_mass();
91        if inverse_mass == 0.0 {
92            return;
93        }
94        *self.get_mut_velocity() += impulse.scaled(inverse_mass);
95    }
96
97    /// Sets the mass of the body, updating the inverse mass.
98    /// A mass of 0 makes the body static (infinite mass).
99    ///
100    /// # Arguments
101    ///
102    /// - `f64` - The new mass.
103    pub fn update_mass(&mut self, mass: f64) {
104        self.set_mass(mass);
105        self.set_inverse_mass(if mass > 0.0 { 1.0 / mass } else { 0.0 });
106    }
107
108    /// Returns `true` if this body is affected by forces and collisions.
109    ///
110    /// # Returns
111    ///
112    /// - `bool` - True if the body is dynamic.
113    pub fn is_dynamic(&self) -> bool {
114        self.get_body_type() == BodyType::Dynamic
115    }
116
117    /// Attaches a collider shape to this body.
118    ///
119    /// # Arguments
120    ///
121    /// - `BodyCollider` - The collider to attach.
122    pub fn update_collider(&mut self, collider: BodyCollider) {
123        self.set_collider(Some(collider));
124    }
125
126    /// Returns the world-space bounding box of the attached collider, if any.
127    ///
128    /// # Returns
129    ///
130    /// - `Option<Rect>` - The bounding box, or `None` if no collider is attached.
131    pub fn bounding_box(&self) -> Option<Rect> {
132        let collider: Option<BodyCollider> = self.get_collider();
133        match collider? {
134            BodyCollider::Aabb(aabb) => {
135                let aabb_rect: Rect = aabb.get_rect();
136                let mut offset_rect: Rect = aabb_rect;
137                offset_rect.set_x(
138                    offset_rect.get_x() + self.get_position().get_x() - aabb_rect.get_width() * 0.5,
139                );
140                offset_rect.set_y(
141                    offset_rect.get_y() + self.get_position().get_y()
142                        - aabb_rect.get_height() * 0.5,
143                );
144                Some(offset_rect)
145            }
146            BodyCollider::Circle(circle) => {
147                let diameter: f64 = circle.get_circle().get_radius() * 2.0;
148                Some(Rect::from_center(self.get_position(), diameter, diameter))
149            }
150        }
151    }
152}
153
154/// Implements body management and simulation for `PhysicsWorld2D`.
155impl PhysicsWorld2D {
156    /// Creates a new physics world with the given configuration.
157    ///
158    /// # Arguments
159    ///
160    /// - `PhysicsConfig` - The simulation configuration.
161    ///
162    /// # Returns
163    ///
164    /// - `PhysicsWorld2D` - The new world.
165    pub fn with_config(config: PhysicsConfig) -> PhysicsWorld2D {
166        let mut world: PhysicsWorld2D = PhysicsWorld2D::new(config);
167        world.set_grid(SpatialHashGrid2D::with_default_size());
168        world
169    }
170
171    /// Adds a rigid body to the world.
172    ///
173    /// # Arguments
174    ///
175    /// - `RigidBody2D` - The body to add.
176    pub fn add_body(&mut self, body: RigidBody2D) {
177        self.get_mut_bodies().push(body);
178    }
179
180    /// Removes the body with the given ID.
181    ///
182    /// # Arguments
183    ///
184    /// - `u64` - The ID of the body to remove.
185    pub fn remove_body(&mut self, id: u64) {
186        self.get_mut_bodies()
187            .retain(|body: &RigidBody2D| body.get_id() != id);
188    }
189
190    /// Returns a reference to the body with the given ID.
191    ///
192    /// # Arguments
193    ///
194    /// - `u64` - The body ID.
195    ///
196    /// # Returns
197    ///
198    /// - `Option<&RigidBody2D>` - The body reference, if found.
199    pub fn get_body(&self, id: u64) -> Option<&RigidBody2D> {
200        self.get_bodies()
201            .iter()
202            .find(|body: &&RigidBody2D| body.get_id() == id)
203    }
204
205    /// Returns a mutable reference to the body with the given ID.
206    ///
207    /// # Arguments
208    ///
209    /// - `u64` - The body ID.
210    ///
211    /// # Returns
212    ///
213    /// - `Option<&mut RigidBody2D>` - The mutable body reference, if found.
214    pub fn get_body_mut(&mut self, id: u64) -> Option<&mut RigidBody2D> {
215        self.get_mut_bodies()
216            .iter_mut()
217            .find(|body: &&mut RigidBody2D| body.get_id() == id)
218    }
219}
220
221/// Implements `Default` for `PhysicsWorld2D` as an empty world.
222impl Default for PhysicsWorld2D {
223    fn default() -> PhysicsWorld2D {
224        PhysicsWorld2D::with_config(PhysicsConfig::default())
225    }
226}
227
228/// Implements collision detection and resolution for `RigidBody2D`.
229impl RigidBody2D {
230    /// Checks collision with another body based on both bodies' collider shapes.
231    ///
232    /// # Arguments
233    ///
234    /// - `&RigidBody2D` - The other body to check against.
235    ///
236    /// # Returns
237    ///
238    /// - `Option<CollisionResult>` - The collision result, or `None`.
239    fn check_collision_with(&self, other: &RigidBody2D) -> Option<CollisionResult> {
240        let a_bbox: Rect = self.bounding_box()?;
241        let b_bbox: Rect = other.bounding_box()?;
242        if !Rect::broad_phase_alias(a_bbox, b_bbox) {
243            return None;
244        }
245        let self_collider: Option<BodyCollider> = self.get_collider();
246        let other_collider: Option<BodyCollider> = other.get_collider();
247        let position_delta: Vector2D = other.get_position() - self.get_position();
248        match (self_collider, other_collider) {
249            (Some(BodyCollider::Aabb(aabb_a)), Some(BodyCollider::Aabb(aabb_b))) => {
250                let aabb_b_rect: Rect = aabb_b.get_rect();
251                let offset_aabb_b: AabbCollider = AabbCollider::new(Rect::new(
252                    aabb_b_rect.get_x() + position_delta.get_x(),
253                    aabb_b_rect.get_y() + position_delta.get_y(),
254                    aabb_b_rect.get_width(),
255                    aabb_b_rect.get_height(),
256                ));
257                aabb_a.collide_with_aabb(&offset_aabb_b)
258            }
259            (Some(BodyCollider::Circle(circle_a)), Some(BodyCollider::Circle(circle_b))) => {
260                let circle_b_inner: Circle = circle_b.get_circle();
261                let offset_circle_b: CircleCollider = CircleCollider::new(Circle::new(
262                    circle_b_inner.get_center() + position_delta,
263                    circle_b_inner.get_radius(),
264                ));
265                circle_a.collide_with_circle(&offset_circle_b)
266            }
267            (Some(BodyCollider::Aabb(aabb)), Some(BodyCollider::Circle(circle))) => {
268                let circle_inner: Circle = circle.get_circle();
269                let offset_circle: CircleCollider = CircleCollider::new(Circle::new(
270                    circle_inner.get_center() + position_delta,
271                    circle_inner.get_radius(),
272                ));
273                aabb.collide_with_circle(&offset_circle)
274            }
275            (Some(BodyCollider::Circle(circle)), Some(BodyCollider::Aabb(aabb))) => {
276                let aabb_rect: Rect = aabb.get_rect();
277                let offset_aabb: AabbCollider = AabbCollider::new(Rect::new(
278                    aabb_rect.get_x() + position_delta.get_x(),
279                    aabb_rect.get_y() + position_delta.get_y(),
280                    aabb_rect.get_width(),
281                    aabb_rect.get_height(),
282                ));
283                offset_aabb
284                    .collide_with_circle(&circle)
285                    .map(|mut result: CollisionResult| {
286                        result.set_normal(-result.get_normal());
287                        result
288                    })
289            }
290            _ => None,
291        }
292    }
293
294    /// Resolves a collision with another body using impulse-based response
295    /// and position correction.
296    ///
297    /// # Arguments
298    ///
299    /// - `&mut RigidBody2D` - The other body involved in the collision.
300    /// - `&CollisionResult` - The collision data.
301    fn resolve_collision_with(&mut self, other: &mut RigidBody2D, result: &CollisionResult) {
302        let self_inverse_mass: f64 = self.get_inverse_mass();
303        let other_inverse_mass: f64 = other.get_inverse_mass();
304        let relative_velocity: Vector2D = other.get_velocity() - self.get_velocity();
305        let velocity_along_normal: f64 = relative_velocity.dot(result.get_normal());
306        if velocity_along_normal > 0.0 {
307            return;
308        }
309        let restitution: f64 = self.get_restitution().min(other.get_restitution());
310        let inverse_mass_sum: f64 = self_inverse_mass + other_inverse_mass;
311        if inverse_mass_sum == 0.0 {
312            return;
313        }
314        let impulse_magnitude: f64 =
315            -(1.0 + restitution) * velocity_along_normal / inverse_mass_sum;
316        let impulse: Vector2D = result.get_normal().scaled(impulse_magnitude);
317        *self.get_mut_velocity() -= impulse.scaled(self_inverse_mass);
318        *other.get_mut_velocity() += impulse.scaled(other_inverse_mass);
319        let correction: Vector2D = result
320            .get_normal()
321            .scaled((result.get_depth() * PHYSICS_POSITION_PERCENT / inverse_mass_sum).max(0.0));
322        *self.get_mut_position() -= correction.scaled(self_inverse_mass);
323        *other.get_mut_position() += correction.scaled(other_inverse_mass);
324    }
325}
326
327/// Implements simulation stepping and collision resolution for `PhysicsWorld2D`.
328impl PhysicsWorld2D {
329    /// Performs one physics simulation step using semi-implicit Euler integration.
330    ///
331    /// Applies gravity to dynamic bodies, integrates velocity from accumulated forces,
332    /// applies damping, integrates position, and resolves collisions.
333    ///
334    /// # Arguments
335    ///
336    /// - `f64` - The fixed delta time in seconds.
337    pub fn step(&mut self, delta_time: f64) {
338        let config: PhysicsConfig = self.get_config();
339        // Hoist loop-invariant damping factors out of the per-body loop.
340        let damping_factor: f64 = (1.0 - config.get_linear_damping() * delta_time).max(0.0);
341        let angular_damping: f64 = (1.0 - config.get_angular_damping() * delta_time).max(0.0);
342        let gravity: Vector2D = config.get_gravity();
343        for body in self.get_mut_bodies() {
344            if !body.is_dynamic() {
345                continue;
346            }
347            let body_mass: f64 = body.get_mass();
348            let body_inverse_mass: f64 = body.get_inverse_mass();
349            *body.get_mut_force_accumulator() += gravity.scaled(body_mass);
350            let force: Vector2D = body.get_force_accumulator();
351            *body.get_mut_velocity() += force.scaled(body_inverse_mass * delta_time);
352            // In-place damping and integration avoid temporary vector copies.
353            *body.get_mut_velocity() *= damping_factor;
354            let current_velocity: Vector2D = body.get_velocity();
355            *body.get_mut_position() += current_velocity.scaled(delta_time);
356            body.set_force_accumulator(Vector2D::zero());
357            *body.get_mut_angular_velocity() *= angular_damping;
358            let current_angular_velocity: f64 = body.get_angular_velocity();
359            *body.get_mut_rotation() += current_angular_velocity * delta_time;
360        }
361        self.resolve_collisions();
362    }
363
364    /// Detects and resolves all collisions between bodies in the world.
365    ///
366    /// Uses a spatial hash grid for broad-phase culling followed by narrow-phase
367    /// shape-specific collision detection, then applies impulse-based resolution.
368    /// This reduces the broad-phase from O(n²) to near O(n) for typical scenes.
369    fn resolve_collisions(&mut self) {
370        let body_count: usize = self.get_bodies().len();
371        if body_count < 2 {
372            return;
373        }
374        // Rebuild the persistent grid once per step and collect the candidate
375        // pair list once; every solver iteration then reuses both (the grid is
376        // unchanged between iterations), eliminating per-iteration re-queries and
377        // per-query allocations.
378        let mut pairs: Vec<(usize, usize)> = Vec::new();
379        {
380            let (bodies, grid, query_buffer, query_seen) = (
381                &self.bodies,
382                &mut self.grid,
383                &mut self.query_buffer,
384                &mut self.query_seen,
385            );
386            grid.clear();
387            for (index, body) in bodies.iter().enumerate() {
388                if let Some(bbox) = body.bounding_box() {
389                    grid.insert(index, bbox.min(), bbox.max());
390                }
391            }
392            for (i, body) in bodies.iter().enumerate() {
393                let Some(bbox) = body.bounding_box() else {
394                    continue;
395                };
396                grid.query_into(bbox.min(), bbox.max(), query_buffer, query_seen);
397                for &j in query_buffer.iter() {
398                    if j > i {
399                        pairs.push((i, j));
400                    }
401                }
402            }
403        }
404        for iteration in 0..PHYSICS_MAX_ITERATIONS {
405            let mut any_collision: bool = false;
406            for &(i, j) in pairs.iter() {
407                let (left, right) = self.get_mut_bodies().split_at_mut(j);
408                let body_a: &mut RigidBody2D = &mut left[i];
409                let body_b: &mut RigidBody2D = &mut right[0];
410                if body_a.get_inverse_mass() == 0.0 && body_b.get_inverse_mass() == 0.0 {
411                    continue;
412                }
413                if let Some(result) = body_a.check_collision_with(body_b) {
414                    body_a.resolve_collision_with(body_b, &result);
415                    any_collision = true;
416                }
417            }
418            if !any_collision {
419                break;
420            }
421            let _: u32 = iteration;
422        }
423    }
424}
425
426/// Forwards `PhysicsWorld2D::step` through the [`Updatable`] trait so that
427/// physics worlds participate in the same update loop as entities, animators,
428/// and scene managers. The inherent [`PhysicsWorld2D::step`] method is the
429/// canonical implementation; this impl exists purely for trait dispatch.
430/// The inherent call resolves first when both are in scope, so there is no
431/// recursion.
432impl Updatable for PhysicsWorld2D {
433    fn update(&mut self, delta_time: f64) {
434        PhysicsWorld2D::step(self, delta_time);
435    }
436}
437
438/// Implements default configuration for `PhysicsConfig3D`.
439impl Default for PhysicsConfig3D {
440    fn default() -> PhysicsConfig3D {
441        PhysicsConfig3D::new(
442            Vector3D::new(0.0, DEFAULT_GRAVITY_3D, 0.0),
443            DEFAULT_LINEAR_DAMPING,
444            DEFAULT_ANGULAR_DAMPING,
445        )
446    }
447}
448
449/// Implements body creation and force management for `RigidBody3D`.
450impl RigidBody3D {
451    /// Creates a new dynamic 3D rigid body with default mass and the given position.
452    ///
453    /// # Arguments
454    ///
455    /// - `u64` - The unique ID.
456    /// - `Vector3D` - The initial position.
457    ///
458    /// # Returns
459    ///
460    /// - `RigidBody3D` - The new body.
461    pub fn new_dynamic(id: u64, position: Vector3D) -> RigidBody3D {
462        let mass: f64 = PHYSICS_DEFAULT_MASS;
463        RigidBody3D::new(
464            id,
465            position,
466            mass,
467            1.0 / mass,
468            DEFAULT_RESTITUTION,
469            DEFAULT_FRICTION,
470            BodyType::Dynamic,
471        )
472    }
473
474    /// Creates a new static 3D rigid body at the given position with infinite mass.
475    ///
476    /// # Arguments
477    ///
478    /// - `u64` - The unique ID.
479    /// - `Vector3D` - The position.
480    ///
481    /// # Returns
482    ///
483    /// - `RigidBody3D` - The new static body.
484    pub fn new_static(id: u64, position: Vector3D) -> RigidBody3D {
485        RigidBody3D::new(
486            id,
487            position,
488            PHYSICS_STATIC_MASS,
489            0.0,
490            DEFAULT_RESTITUTION,
491            DEFAULT_FRICTION,
492            BodyType::Static,
493        )
494    }
495
496    /// Applies a force to the body's force accumulator.
497    ///
498    /// # Arguments
499    ///
500    /// - `Vector3D` - The force vector.
501    pub fn apply_force(&mut self, force: Vector3D) {
502        *self.get_mut_force_accumulator() += force;
503    }
504
505    /// Applies a torque to the body's torque accumulator.
506    ///
507    /// # Arguments
508    ///
509    /// - `Vector3D` - The torque vector.
510    pub fn apply_torque(&mut self, torque: Vector3D) {
511        *self.get_mut_torque_accumulator() += torque;
512    }
513
514    /// Applies an instantaneous impulse, directly changing velocity.
515    ///
516    /// # Arguments
517    ///
518    /// - `Vector3D` - The impulse vector.
519    pub fn apply_impulse(&mut self, impulse: Vector3D) {
520        let inverse_mass: f64 = self.get_inverse_mass();
521        if inverse_mass == 0.0 {
522            return;
523        }
524        *self.get_mut_velocity() += impulse.scaled(inverse_mass);
525    }
526
527    /// Sets the mass of the body, updating the inverse mass.
528    /// A mass of 0 makes the body static (infinite mass).
529    ///
530    /// # Arguments
531    ///
532    /// - `f64` - The new mass.
533    pub fn update_mass(&mut self, mass: f64) {
534        self.set_mass(mass);
535        self.set_inverse_mass(if mass > 0.0 { 1.0 / mass } else { 0.0 });
536    }
537
538    /// Returns `true` if this body is affected by forces and collisions.
539    ///
540    /// # Returns
541    ///
542    /// - `bool` - True if the body is dynamic.
543    pub fn is_dynamic(&self) -> bool {
544        self.get_body_type() == BodyType::Dynamic
545    }
546
547    /// Attaches a 3D collider shape to this body.
548    ///
549    /// # Arguments
550    ///
551    /// - `BodyCollider3D` - The collider to attach.
552    pub fn update_collider(&mut self, collider: BodyCollider3D) {
553        self.set_collider(Some(collider));
554    }
555
556    /// Returns the world-space 3D bounding box of the attached collider, if any.
557    ///
558    /// # Returns
559    ///
560    /// - `Option<AABB3D>` - The bounding box, or `None` if no collider is attached.
561    pub fn bounding_box(&self) -> Option<AABB3D> {
562        let collider: Option<BodyCollider3D> = self.get_collider();
563        let position: Vector3D = self.get_position();
564        match collider? {
565            BodyCollider3D::Aabb(aabb) => {
566                let center: Vector3D = aabb.get_aabb().center();
567                let size: Vector3D = aabb.get_aabb().size();
568                Some(AABB3D::from_center(
569                    position + center,
570                    size.get_x(),
571                    size.get_y(),
572                    size.get_z(),
573                ))
574            }
575            BodyCollider3D::Sphere(sphere) => {
576                let sphere_inner: Sphere = sphere.get_sphere();
577                let diameter: f64 = sphere_inner.get_radius() * 2.0;
578                Some(AABB3D::from_center(
579                    position + sphere_inner.get_center(),
580                    diameter,
581                    diameter,
582                    diameter,
583                ))
584            }
585        }
586    }
587}
588
589/// Implements body management and simulation for `PhysicsWorld3D`.
590impl PhysicsWorld3D {
591    /// Creates a new 3D physics world with the given configuration.
592    ///
593    /// # Arguments
594    ///
595    /// - `PhysicsConfig3D` - The simulation configuration.
596    ///
597    /// # Returns
598    ///
599    /// - `PhysicsWorld3D` - The new world.
600    pub fn with_config(config: PhysicsConfig3D) -> PhysicsWorld3D {
601        let mut world: PhysicsWorld3D = PhysicsWorld3D::new(config);
602        world.set_grid(SpatialHashGrid3D::with_default_size());
603        world
604    }
605
606    /// Adds a rigid body to the world.
607    ///
608    /// # Arguments
609    ///
610    /// - `RigidBody3D` - The body to add.
611    pub fn add_body(&mut self, body: RigidBody3D) {
612        self.get_mut_bodies().push(body);
613    }
614
615    /// Removes the body with the given ID.
616    ///
617    /// # Arguments
618    ///
619    /// - `u64` - The ID of the body to remove.
620    pub fn remove_body(&mut self, id: u64) {
621        self.get_mut_bodies()
622            .retain(|body: &RigidBody3D| body.get_id() != id);
623    }
624
625    /// Returns a reference to the body with the given ID.
626    ///
627    /// # Arguments
628    ///
629    /// - `u64` - The body ID.
630    ///
631    /// # Returns
632    ///
633    /// - `Option<&RigidBody3D>` - The body reference, if found.
634    pub fn get_body(&self, id: u64) -> Option<&RigidBody3D> {
635        self.get_bodies()
636            .iter()
637            .find(|body: &&RigidBody3D| body.get_id() == id)
638    }
639
640    /// Returns a mutable reference to the body with the given ID.
641    ///
642    /// # Arguments
643    ///
644    /// - `u64` - The body ID.
645    ///
646    /// # Returns
647    ///
648    /// - `Option<&mut RigidBody3D>` - The mutable body reference, if found.
649    pub fn get_body_mut(&mut self, id: u64) -> Option<&mut RigidBody3D> {
650        self.get_mut_bodies()
651            .iter_mut()
652            .find(|body: &&mut RigidBody3D| body.get_id() == id)
653    }
654
655    /// Performs one physics simulation step using semi-implicit Euler integration.
656    ///
657    /// Applies gravity to dynamic bodies, integrates velocity from accumulated forces,
658    /// applies damping, integrates position, and resolves collisions.
659    ///
660    /// # Arguments
661    ///
662    /// - `f64` - The fixed delta time in seconds.
663    pub fn step(&mut self, delta_time: f64) {
664        let config: PhysicsConfig3D = self.get_config();
665        // Hoist loop-invariant damping factors out of the per-body loop.
666        let damping_factor: f64 = (1.0 - config.get_linear_damping() * delta_time).max(0.0);
667        let angular_damping: f64 = (1.0 - config.get_angular_damping() * delta_time).max(0.0);
668        let gravity: Vector3D = config.get_gravity();
669        for body in self.get_mut_bodies() {
670            if !body.is_dynamic() {
671                continue;
672            }
673            let body_mass: f64 = body.get_mass();
674            let body_inverse_mass: f64 = body.get_inverse_mass();
675            *body.get_mut_force_accumulator() += gravity.scaled(body_mass);
676            let force: Vector3D = body.get_force_accumulator();
677            *body.get_mut_velocity() += force.scaled(body_inverse_mass * delta_time);
678            // In-place damping and integration avoid temporary vector copies.
679            *body.get_mut_velocity() *= damping_factor;
680            let current_velocity: Vector3D = body.get_velocity();
681            *body.get_mut_position() += current_velocity.scaled(delta_time);
682            body.set_force_accumulator(Vector3D::zero());
683            *body.get_mut_angular_velocity() *= angular_damping;
684            let angular_velocity: Vector3D = body.get_angular_velocity();
685            let rotation_delta: Quaternion = Quaternion::new(
686                angular_velocity.get_x() * delta_time * 0.5,
687                angular_velocity.get_y() * delta_time * 0.5,
688                angular_velocity.get_z() * delta_time * 0.5,
689                1.0,
690            );
691            body.set_rotation((rotation_delta * body.get_rotation()).normalized());
692            body.set_torque_accumulator(Vector3D::zero());
693        }
694        self.resolve_collisions();
695    }
696
697    /// Detects and resolves all collisions between bodies in the 3D world.
698    ///
699    /// Uses a spatial hash grid for broad-phase culling followed by narrow-phase
700    /// shape-specific collision detection, then applies impulse-based resolution.
701    /// This reduces the broad-phase from O(n²) to near O(n) for typical scenes.
702    fn resolve_collisions(&mut self) {
703        let body_count: usize = self.get_bodies().len();
704        if body_count < 2 {
705            return;
706        }
707        // Rebuild the persistent grid once per step and collect the candidate
708        // pair list once; every solver iteration then reuses both (the grid is
709        // unchanged between iterations), eliminating per-iteration re-queries and
710        // per-query allocations.
711        let mut pairs: Vec<(usize, usize)> = Vec::new();
712        {
713            let (bodies, grid, query_buffer, query_seen) = (
714                &self.bodies,
715                &mut self.grid,
716                &mut self.query_buffer,
717                &mut self.query_seen,
718            );
719            grid.clear();
720            for (index, body) in bodies.iter().enumerate() {
721                if let Some(bbox) = body.bounding_box() {
722                    grid.insert(index, bbox.get_min(), bbox.get_max());
723                }
724            }
725            for (i, body) in bodies.iter().enumerate() {
726                let Some(bbox) = body.bounding_box() else {
727                    continue;
728                };
729                grid.query_into(bbox.get_min(), bbox.get_max(), query_buffer, query_seen);
730                for &j in query_buffer.iter() {
731                    if j > i {
732                        pairs.push((i, j));
733                    }
734                }
735            }
736        }
737        for iteration in 0..PHYSICS_MAX_ITERATIONS {
738            let mut any_collision: bool = false;
739            for &(i, j) in pairs.iter() {
740                let (left, right) = self.get_mut_bodies().split_at_mut(j);
741                let body_a: &mut RigidBody3D = &mut left[i];
742                let body_b: &mut RigidBody3D = &mut right[0];
743                if body_a.get_inverse_mass() == 0.0 && body_b.get_inverse_mass() == 0.0 {
744                    continue;
745                }
746                if let Some(result) = Self::check_collision_3d(body_a, body_b) {
747                    Self::resolve_collision_3d(body_a, body_b, &result);
748                    any_collision = true;
749                }
750            }
751            if !any_collision {
752                break;
753            }
754            let _: u32 = iteration;
755        }
756    }
757
758    /// Checks collision between two 3D bodies based on both bodies' collider shapes.
759    ///
760    /// # Arguments
761    ///
762    /// - `&RigidBody3D` - The first body.
763    /// - `&RigidBody3D` - The second body.
764    ///
765    /// # Returns
766    ///
767    /// - `Option<CollisionResult3D>` - The collision result, or `None`.
768    fn check_collision_3d(a: &RigidBody3D, b: &RigidBody3D) -> Option<CollisionResult3D> {
769        let a_bbox: AABB3D = a.bounding_box()?;
770        let b_bbox: AABB3D = b.bounding_box()?;
771        if !AABB3D::broad_phase(a_bbox, b_bbox) {
772            return None;
773        }
774        let a_collider: Option<BodyCollider3D> = a.get_collider();
775        let b_collider: Option<BodyCollider3D> = b.get_collider();
776        let position_delta: Vector3D = b.get_position() - a.get_position();
777        match (a_collider, b_collider) {
778            (Some(BodyCollider3D::Aabb(aabb_a)), Some(BodyCollider3D::Aabb(aabb_b))) => {
779                let aabb_b_inner: AABB3D = aabb_b.get_aabb();
780                let offset_aabb: AabbCollider3D = AabbCollider3D::new(AABB3D::new(
781                    aabb_b_inner.get_min() + position_delta,
782                    aabb_b_inner.get_max() + position_delta,
783                ));
784                aabb_a.collide_with_aabb(&offset_aabb)
785            }
786            (Some(BodyCollider3D::Sphere(sphere_a)), Some(BodyCollider3D::Sphere(sphere_b))) => {
787                let sphere_b_inner: Sphere = sphere_b.get_sphere();
788                let offset_sphere: SphereCollider3D = SphereCollider3D::new(Sphere::new(
789                    sphere_b_inner.get_center() + position_delta,
790                    sphere_b_inner.get_radius(),
791                ));
792                sphere_a.collide_with_sphere(&offset_sphere)
793            }
794            (Some(BodyCollider3D::Aabb(aabb)), Some(BodyCollider3D::Sphere(sphere))) => {
795                let sphere_inner: Sphere = sphere.get_sphere();
796                let offset_sphere: SphereCollider3D = SphereCollider3D::new(Sphere::new(
797                    sphere_inner.get_center() + position_delta,
798                    sphere_inner.get_radius(),
799                ));
800                aabb.collide_with_sphere(&offset_sphere)
801            }
802            (Some(BodyCollider3D::Sphere(sphere)), Some(BodyCollider3D::Aabb(aabb))) => {
803                let aabb_inner: AABB3D = aabb.get_aabb();
804                let offset_aabb: AabbCollider3D = AabbCollider3D::new(AABB3D::new(
805                    aabb_inner.get_min() + position_delta,
806                    aabb_inner.get_max() + position_delta,
807                ));
808                offset_aabb
809                    .collide_with_sphere(&sphere)
810                    .map(|mut result: CollisionResult3D| {
811                        result.set_normal(-result.get_normal());
812                        result
813                    })
814            }
815            _ => None,
816        }
817    }
818
819    /// Resolves a collision between two 3D bodies using impulse-based response
820    /// and position correction.
821    ///
822    /// # Arguments
823    ///
824    /// - `&mut RigidBody3D` - The first body.
825    /// - `&mut RigidBody3D` - The second body.
826    /// - `&CollisionResult3D` - The collision data.
827    fn resolve_collision_3d(a: &mut RigidBody3D, b: &mut RigidBody3D, result: &CollisionResult3D) {
828        let a_inverse_mass: f64 = a.get_inverse_mass();
829        let b_inverse_mass: f64 = b.get_inverse_mass();
830        let relative_velocity: Vector3D = b.get_velocity() - a.get_velocity();
831        let velocity_along_normal: f64 = relative_velocity.dot(result.get_normal());
832        if velocity_along_normal > 0.0 {
833            return;
834        }
835        let restitution: f64 = a.get_restitution().min(b.get_restitution());
836        let inverse_mass_sum: f64 = a_inverse_mass + b_inverse_mass;
837        if inverse_mass_sum == 0.0 {
838            return;
839        }
840        let impulse_magnitude: f64 =
841            -(1.0 + restitution) * velocity_along_normal / inverse_mass_sum;
842        let impulse: Vector3D = result.get_normal().scaled(impulse_magnitude);
843        *a.get_mut_velocity() -= impulse.scaled(a_inverse_mass);
844        *b.get_mut_velocity() += impulse.scaled(b_inverse_mass);
845        let correction: Vector3D = result
846            .get_normal()
847            .scaled((result.get_depth() * PHYSICS_POSITION_PERCENT / inverse_mass_sum).max(0.0));
848        *a.get_mut_position() -= correction.scaled(a_inverse_mass);
849        *b.get_mut_position() += correction.scaled(b_inverse_mass);
850    }
851}
852
853/// Forwards `PhysicsWorld3D::step` through the [`Updatable`] trait so that
854/// 3D physics worlds participate in the same update loop as their 2D
855/// counterparts, entities, animators, and scene managers. The inherent
856/// [`PhysicsWorld3D::step`] method is the canonical implementation; this impl
857/// exists purely for trait dispatch. The inherent call resolves first when
858/// both are in scope, so there is no recursion.
859impl Updatable for PhysicsWorld3D {
860    fn update(&mut self, delta_time: f64) {
861        PhysicsWorld3D::step(self, delta_time);
862    }
863}
864
865/// Implements `Default` for `PhysicsWorld3D` as an empty world.
866impl Default for PhysicsWorld3D {
867    fn default() -> PhysicsWorld3D {
868        PhysicsWorld3D::with_config(PhysicsConfig3D::default())
869    }
870}
euv_engine/physics/impl.rs

euv_engine/physics/
impl.rs
