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

euv_engine/physics/
impl.rs
