euv_engine/physics/
impl.rs

1use crate::*;
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        PhysicsWorld2D::new(config)
167    }
168
169    /// Adds a rigid body to the world.
170    ///
171    /// # Arguments
172    ///
173    /// - `RigidBody2D` - The body to add.
174    pub fn add_body(&mut self, body: RigidBody2D) {
175        self.get_mut_bodies().push(body);
176    }
177
178    /// Removes the body with the given ID.
179    ///
180    /// # Arguments
181    ///
182    /// - `u64` - The ID of the body to remove.
183    pub fn remove_body(&mut self, id: u64) {
184        self.get_mut_bodies()
185            .retain(|body: &RigidBody2D| body.get_id() != id);
186    }
187
188    /// Returns a reference to the body with the given ID.
189    ///
190    /// # Arguments
191    ///
192    /// - `u64` - The body ID.
193    ///
194    /// # Returns
195    ///
196    /// - `Option<&RigidBody2D>` - The body reference, if found.
197    pub fn get_body(&self, id: u64) -> Option<&RigidBody2D> {
198        self.get_bodies()
199            .iter()
200            .find(|body: &&RigidBody2D| body.get_id() == id)
201    }
202
203    /// Returns a mutable reference to the body with the given ID.
204    ///
205    /// # Arguments
206    ///
207    /// - `u64` - The body ID.
208    ///
209    /// # Returns
210    ///
211    /// - `Option<&mut RigidBody2D>` - The mutable body reference, if found.
212    pub fn get_body_mut(&mut self, id: u64) -> Option<&mut RigidBody2D> {
213        self.get_mut_bodies()
214            .iter_mut()
215            .find(|body: &&mut RigidBody2D| body.get_id() == id)
216    }
217}
218
219/// Implements `Default` for `PhysicsWorld2D` as an empty world.
220impl Default for PhysicsWorld2D {
221    fn default() -> PhysicsWorld2D {
222        PhysicsWorld2D::new(PhysicsConfig::default())
223    }
224}
225
226/// Implements collision detection and resolution for `RigidBody2D`.
227impl RigidBody2D {
228    /// Checks collision with another body based on both bodies' collider shapes.
229    ///
230    /// # Arguments
231    ///
232    /// - `&RigidBody2D` - The other body to check against.
233    ///
234    /// # Returns
235    ///
236    /// - `Option<CollisionResult>` - The collision result, or `None`.
237    fn check_collision_with(&self, other: &RigidBody2D) -> Option<CollisionResult> {
238        let a_bbox: Rect = self.bounding_box()?;
239        let b_bbox: Rect = other.bounding_box()?;
240        if !Rect::broad_phase_check(a_bbox, b_bbox) {
241            return None;
242        }
243        let self_collider: Option<BodyCollider> = self.get_collider();
244        let other_collider: Option<BodyCollider> = other.get_collider();
245        let position_delta: Vector2D = other.get_position() - self.get_position();
246        match (self_collider, other_collider) {
247            (Some(BodyCollider::Aabb(aabb_a)), Some(BodyCollider::Aabb(aabb_b))) => {
248                let aabb_b_rect: Rect = aabb_b.get_rect();
249                let offset_aabb_b: AabbCollider = AabbCollider::new(Rect::new(
250                    aabb_b_rect.get_x() + position_delta.get_x(),
251                    aabb_b_rect.get_y() + position_delta.get_y(),
252                    aabb_b_rect.get_width(),
253                    aabb_b_rect.get_height(),
254                ));
255                aabb_a.collide_with_aabb(&offset_aabb_b)
256            }
257            (Some(BodyCollider::Circle(circle_a)), Some(BodyCollider::Circle(circle_b))) => {
258                let circle_b_inner: Circle = circle_b.get_circle();
259                let offset_circle_b: CircleCollider = CircleCollider::new(Circle::new(
260                    circle_b_inner.get_center() + position_delta,
261                    circle_b_inner.get_radius(),
262                ));
263                circle_a.collide_with_circle(&offset_circle_b)
264            }
265            (Some(BodyCollider::Aabb(aabb)), Some(BodyCollider::Circle(circle))) => {
266                let circle_inner: Circle = circle.get_circle();
267                let offset_circle: CircleCollider = CircleCollider::new(Circle::new(
268                    circle_inner.get_center() + position_delta,
269                    circle_inner.get_radius(),
270                ));
271                aabb.collide_with_circle(&offset_circle)
272            }
273            (Some(BodyCollider::Circle(circle)), Some(BodyCollider::Aabb(aabb))) => {
274                let aabb_rect: Rect = aabb.get_rect();
275                let offset_aabb: AabbCollider = AabbCollider::new(Rect::new(
276                    aabb_rect.get_x() + position_delta.get_x(),
277                    aabb_rect.get_y() + position_delta.get_y(),
278                    aabb_rect.get_width(),
279                    aabb_rect.get_height(),
280                ));
281                offset_aabb
282                    .collide_with_circle(&circle)
283                    .map(|mut result: CollisionResult| {
284                        result.set_normal(-result.get_normal());
285                        result
286                    })
287            }
288            _ => None,
289        }
290    }
291
292    /// Resolves a collision with another body using impulse-based response
293    /// and position correction.
294    ///
295    /// # Arguments
296    ///
297    /// - `&mut RigidBody2D` - The other body involved in the collision.
298    /// - `&CollisionResult` - The collision data.
299    fn resolve_collision_with(&mut self, other: &mut RigidBody2D, result: &CollisionResult) {
300        let self_inverse_mass: f64 = self.get_inverse_mass();
301        let other_inverse_mass: f64 = other.get_inverse_mass();
302        let relative_velocity: Vector2D = other.get_velocity() - self.get_velocity();
303        let velocity_along_normal: f64 = relative_velocity.dot(result.get_normal());
304        if velocity_along_normal > 0.0 {
305            return;
306        }
307        let restitution: f64 = self.get_restitution().min(other.get_restitution());
308        let inverse_mass_sum: f64 = self_inverse_mass + other_inverse_mass;
309        if inverse_mass_sum == 0.0 {
310            return;
311        }
312        let impulse_magnitude: f64 =
313            -(1.0 + restitution) * velocity_along_normal / inverse_mass_sum;
314        let impulse: Vector2D = result.get_normal().scaled(impulse_magnitude);
315        *self.get_mut_velocity() -= impulse.scaled(self_inverse_mass);
316        *other.get_mut_velocity() += impulse.scaled(other_inverse_mass);
317        let correction: Vector2D = result
318            .get_normal()
319            .scaled((result.get_depth() * PHYSICS_POSITION_PERCENT / inverse_mass_sum).max(0.0));
320        *self.get_mut_position() -= correction.scaled(self_inverse_mass);
321        *other.get_mut_position() += correction.scaled(other_inverse_mass);
322    }
323}
324
325/// Implements simulation stepping and collision resolution for `PhysicsWorld2D`.
326impl PhysicsWorld2D {
327    /// Performs one physics simulation step using semi-implicit Euler integration.
328    ///
329    /// Applies gravity to dynamic bodies, integrates velocity from accumulated forces,
330    /// applies damping, integrates position, and resolves collisions.
331    ///
332    /// # Arguments
333    ///
334    /// - `f64` - The fixed delta time in seconds.
335    pub fn step(&mut self, delta_time: f64) {
336        let config: PhysicsConfig = self.get_config();
337        for body in self.get_mut_bodies() {
338            if !body.is_dynamic() {
339                continue;
340            }
341            let body_mass: f64 = body.get_mass();
342            let body_inverse_mass: f64 = body.get_inverse_mass();
343            *body.get_mut_force_accumulator() += config.get_gravity().scaled(body_mass);
344            let force: Vector2D = body.get_force_accumulator();
345            *body.get_mut_velocity() += force.scaled(body_inverse_mass * delta_time);
346            let damping_factor: f64 = (1.0 - config.get_linear_damping() * delta_time).max(0.0);
347            let velocity: Vector2D = body.get_velocity();
348            body.set_velocity(velocity.scaled(damping_factor));
349            let current_velocity: Vector2D = body.get_velocity();
350            *body.get_mut_position() += current_velocity.scaled(delta_time);
351            body.set_force_accumulator(Vector2D::zero());
352            let angular_damping: f64 = (1.0 - config.get_angular_damping() * delta_time).max(0.0);
353            let angular_velocity: f64 = body.get_angular_velocity();
354            *body.get_mut_angular_velocity() = angular_velocity * angular_damping;
355            let current_angular_velocity: f64 = body.get_angular_velocity();
356            *body.get_mut_rotation() += current_angular_velocity * delta_time;
357        }
358        self.resolve_collisions();
359    }
360
361    /// Detects and resolves all collisions between bodies in the world.
362    ///
363    /// Uses a spatial hash grid for broad-phase culling followed by narrow-phase
364    /// shape-specific collision detection, then applies impulse-based resolution.
365    /// This reduces the broad-phase from O(n²) to near O(n) for typical scenes.
366    fn resolve_collisions(&mut self) {
367        let body_count: usize = self.get_bodies().len();
368        if body_count < 2 {
369            return;
370        }
371        let mut grid: SpatialHashGrid2D = SpatialHashGrid2D::with_default_size();
372        for (index, body) in self.get_bodies().iter().enumerate() {
373            if let Some(bbox) = body.bounding_box() {
374                grid.insert(index, bbox.min(), bbox.max());
375            }
376        }
377        for iteration in 0..PHYSICS_MAX_ITERATIONS {
378            let mut any_collision: bool = false;
379            for i in 0..body_count {
380                let candidates: Vec<usize> = {
381                    let body: &RigidBody2D = &self.get_bodies()[i];
382                    match body.bounding_box() {
383                        Some(bbox) => grid.query(bbox.min(), bbox.max()),
384                        None => Vec::new(),
385                    }
386                };
387                for j in candidates {
388                    if j <= i {
389                        continue;
390                    }
391                    let (left, right) = self.get_mut_bodies().split_at_mut(j);
392                    let body_a: &mut RigidBody2D = &mut left[i];
393                    let body_b: &mut RigidBody2D = &mut right[0];
394                    if body_a.get_inverse_mass() == 0.0 && body_b.get_inverse_mass() == 0.0 {
395                        continue;
396                    }
397                    if let Some(result) = body_a.check_collision_with(body_b) {
398                        body_a.resolve_collision_with(body_b, &result);
399                        any_collision = true;
400                    }
401                }
402            }
403            if !any_collision {
404                break;
405            }
406            let _ = iteration;
407        }
408    }
409}
410
411/// Implements default configuration for `PhysicsConfig3D`.
412impl Default for PhysicsConfig3D {
413    fn default() -> PhysicsConfig3D {
414        PhysicsConfig3D::new(
415            Vector3D::new(0.0, DEFAULT_GRAVITY_3D, 0.0),
416            DEFAULT_LINEAR_DAMPING,
417            DEFAULT_ANGULAR_DAMPING,
418        )
419    }
420}
421
422/// Implements body creation and force management for `RigidBody3D`.
423impl RigidBody3D {
424    /// Creates a new dynamic 3D rigid body with default mass and the given position.
425    ///
426    /// # Arguments
427    ///
428    /// - `u64` - The unique ID.
429    /// - `Vector3D` - The initial position.
430    ///
431    /// # Returns
432    ///
433    /// - `RigidBody3D` - The new body.
434    pub fn new_dynamic(id: u64, position: Vector3D) -> RigidBody3D {
435        let mass: f64 = PHYSICS_DEFAULT_MASS;
436        RigidBody3D::new(
437            id,
438            position,
439            mass,
440            1.0 / mass,
441            DEFAULT_RESTITUTION,
442            DEFAULT_FRICTION,
443            BodyType::Dynamic,
444        )
445    }
446
447    /// Creates a new static 3D rigid body at the given position with infinite mass.
448    ///
449    /// # Arguments
450    ///
451    /// - `u64` - The unique ID.
452    /// - `Vector3D` - The position.
453    ///
454    /// # Returns
455    ///
456    /// - `RigidBody3D` - The new static body.
457    pub fn new_static(id: u64, position: Vector3D) -> RigidBody3D {
458        RigidBody3D::new(
459            id,
460            position,
461            PHYSICS_STATIC_MASS,
462            0.0,
463            DEFAULT_RESTITUTION,
464            DEFAULT_FRICTION,
465            BodyType::Static,
466        )
467    }
468
469    /// Applies a force to the body's force accumulator.
470    ///
471    /// # Arguments
472    ///
473    /// - `Vector3D` - The force vector.
474    pub fn apply_force(&mut self, force: Vector3D) {
475        *self.get_mut_force_accumulator() += force;
476    }
477
478    /// Applies a torque to the body's torque accumulator.
479    ///
480    /// # Arguments
481    ///
482    /// - `Vector3D` - The torque vector.
483    pub fn apply_torque(&mut self, torque: Vector3D) {
484        *self.get_mut_torque_accumulator() += torque;
485    }
486
487    /// Applies an instantaneous impulse, directly changing velocity.
488    ///
489    /// # Arguments
490    ///
491    /// - `Vector3D` - The impulse vector.
492    pub fn apply_impulse(&mut self, impulse: Vector3D) {
493        let inverse_mass: f64 = self.get_inverse_mass();
494        if inverse_mass == 0.0 {
495            return;
496        }
497        *self.get_mut_velocity() += impulse.scaled(inverse_mass);
498    }
499
500    /// Sets the mass of the body, updating the inverse mass.
501    /// A mass of 0 makes the body static (infinite mass).
502    ///
503    /// # Arguments
504    ///
505    /// - `f64` - The new mass.
506    pub fn update_mass(&mut self, mass: f64) {
507        self.set_mass(mass);
508        self.set_inverse_mass(if mass > 0.0 { 1.0 / mass } else { 0.0 });
509    }
510
511    /// Returns `true` if this body is affected by forces and collisions.
512    ///
513    /// # Returns
514    ///
515    /// - `bool` - True if the body is dynamic.
516    pub fn is_dynamic(&self) -> bool {
517        self.get_body_type() == BodyType::Dynamic
518    }
519
520    /// Attaches a 3D collider shape to this body.
521    ///
522    /// # Arguments
523    ///
524    /// - `BodyCollider3D` - The collider to attach.
525    pub fn update_collider(&mut self, collider: BodyCollider3D) {
526        self.set_collider(Some(collider));
527    }
528
529    /// Returns the world-space 3D bounding box of the attached collider, if any.
530    ///
531    /// # Returns
532    ///
533    /// - `Option<AABB3D>` - The bounding box, or `None` if no collider is attached.
534    pub fn bounding_box(&self) -> Option<AABB3D> {
535        let collider: Option<BodyCollider3D> = self.get_collider();
536        let position: Vector3D = self.get_position();
537        match collider? {
538            BodyCollider3D::Aabb(aabb) => {
539                let center: Vector3D = aabb.get_aabb().center();
540                let size: Vector3D = aabb.get_aabb().size();
541                Some(AABB3D::from_center(
542                    position + center,
543                    size.get_x(),
544                    size.get_y(),
545                    size.get_z(),
546                ))
547            }
548            BodyCollider3D::Sphere(sphere) => {
549                let sphere_inner: Sphere = sphere.get_sphere();
550                let diameter: f64 = sphere_inner.get_radius() * 2.0;
551                Some(AABB3D::from_center(
552                    position + sphere_inner.get_center(),
553                    diameter,
554                    diameter,
555                    diameter,
556                ))
557            }
558        }
559    }
560}
561
562/// Implements body management and simulation for `PhysicsWorld3D`.
563impl PhysicsWorld3D {
564    /// Creates a new 3D physics world with the given configuration.
565    ///
566    /// # Arguments
567    ///
568    /// - `PhysicsConfig3D` - The simulation configuration.
569    ///
570    /// # Returns
571    ///
572    /// - `PhysicsWorld3D` - The new world.
573    pub fn with_config(config: PhysicsConfig3D) -> PhysicsWorld3D {
574        PhysicsWorld3D::new(config)
575    }
576
577    /// Adds a rigid body to the world.
578    ///
579    /// # Arguments
580    ///
581    /// - `RigidBody3D` - The body to add.
582    pub fn add_body(&mut self, body: RigidBody3D) {
583        self.get_mut_bodies().push(body);
584    }
585
586    /// Removes the body with the given ID.
587    ///
588    /// # Arguments
589    ///
590    /// - `u64` - The ID of the body to remove.
591    pub fn remove_body(&mut self, id: u64) {
592        self.get_mut_bodies()
593            .retain(|body: &RigidBody3D| body.get_id() != id);
594    }
595
596    /// Returns a reference to the body with the given ID.
597    ///
598    /// # Arguments
599    ///
600    /// - `u64` - The body ID.
601    ///
602    /// # Returns
603    ///
604    /// - `Option<&RigidBody3D>` - The body reference, if found.
605    pub fn get_body(&self, id: u64) -> Option<&RigidBody3D> {
606        self.get_bodies()
607            .iter()
608            .find(|body: &&RigidBody3D| body.get_id() == id)
609    }
610
611    /// Returns a mutable reference to the body with the given ID.
612    ///
613    /// # Arguments
614    ///
615    /// - `u64` - The body ID.
616    ///
617    /// # Returns
618    ///
619    /// - `Option<&mut RigidBody3D>` - The mutable body reference, if found.
620    pub fn get_body_mut(&mut self, id: u64) -> Option<&mut RigidBody3D> {
621        self.get_mut_bodies()
622            .iter_mut()
623            .find(|body: &&mut RigidBody3D| body.get_id() == id)
624    }
625
626    /// Performs one physics simulation step using semi-implicit Euler integration.
627    ///
628    /// Applies gravity to dynamic bodies, integrates velocity from accumulated forces,
629    /// applies damping, integrates position, and resolves collisions.
630    ///
631    /// # Arguments
632    ///
633    /// - `f64` - The fixed delta time in seconds.
634    pub fn step(&mut self, delta_time: f64) {
635        let config: PhysicsConfig3D = self.get_config();
636        for body in self.get_mut_bodies() {
637            if !body.is_dynamic() {
638                continue;
639            }
640            let body_mass: f64 = body.get_mass();
641            let body_inverse_mass: f64 = body.get_inverse_mass();
642            *body.get_mut_force_accumulator() += config.get_gravity().scaled(body_mass);
643            let force: Vector3D = body.get_force_accumulator();
644            *body.get_mut_velocity() += force.scaled(body_inverse_mass * delta_time);
645            let damping_factor: f64 = (1.0 - config.get_linear_damping() * delta_time).max(0.0);
646            let velocity: Vector3D = body.get_velocity();
647            body.set_velocity(velocity.scaled(damping_factor));
648            let current_velocity: Vector3D = body.get_velocity();
649            *body.get_mut_position() += current_velocity.scaled(delta_time);
650            body.set_force_accumulator(Vector3D::zero());
651            let angular_damping: f64 = (1.0 - config.get_angular_damping() * delta_time).max(0.0);
652            let angular_velocity: Vector3D = body.get_angular_velocity().scaled(angular_damping);
653            body.set_angular_velocity(angular_velocity);
654            let rotation_delta: Quaternion = Quaternion::new(
655                angular_velocity.get_x() * delta_time * 0.5,
656                angular_velocity.get_y() * delta_time * 0.5,
657                angular_velocity.get_z() * delta_time * 0.5,
658                1.0,
659            );
660            body.set_rotation((rotation_delta * body.get_rotation()).normalized());
661            body.set_torque_accumulator(Vector3D::zero());
662        }
663        self.resolve_collisions();
664    }
665
666    /// Detects and resolves all collisions between bodies in the 3D world.
667    ///
668    /// Uses a spatial hash grid for broad-phase culling followed by narrow-phase
669    /// shape-specific collision detection, then applies impulse-based resolution.
670    /// This reduces the broad-phase from O(n²) to near O(n) for typical scenes.
671    fn resolve_collisions(&mut self) {
672        let body_count: usize = self.get_bodies().len();
673        if body_count < 2 {
674            return;
675        }
676        let mut grid: SpatialHashGrid3D = SpatialHashGrid3D::with_default_size();
677        for (index, body) in self.get_bodies().iter().enumerate() {
678            if let Some(bbox) = body.bounding_box() {
679                grid.insert(index, bbox.get_min(), bbox.get_max());
680            }
681        }
682        for iteration in 0..PHYSICS_MAX_ITERATIONS {
683            let mut any_collision: bool = false;
684            for i in 0..body_count {
685                let candidates: Vec<usize> = {
686                    let body: &RigidBody3D = &self.get_bodies()[i];
687                    match body.bounding_box() {
688                        Some(bbox) => grid.query(bbox.get_min(), bbox.get_max()),
689                        None => Vec::new(),
690                    }
691                };
692                for j in candidates {
693                    if j <= i {
694                        continue;
695                    }
696                    let (left, right) = self.get_mut_bodies().split_at_mut(j);
697                    let body_a: &mut RigidBody3D = &mut left[i];
698                    let body_b: &mut RigidBody3D = &mut right[0];
699                    if body_a.get_inverse_mass() == 0.0 && body_b.get_inverse_mass() == 0.0 {
700                        continue;
701                    }
702                    if let Some(result) = Self::check_collision_3d(body_a, body_b) {
703                        Self::resolve_collision_3d(body_a, body_b, &result);
704                        any_collision = true;
705                    }
706                }
707            }
708            if !any_collision {
709                break;
710            }
711            let _ = iteration;
712        }
713    }
714
715    /// Checks collision between two 3D bodies based on both bodies' collider shapes.
716    ///
717    /// # Arguments
718    ///
719    /// - `&RigidBody3D` - The first body.
720    /// - `&RigidBody3D` - The second body.
721    ///
722    /// # Returns
723    ///
724    /// - `Option<CollisionResult3D>` - The collision result, or `None`.
725    fn check_collision_3d(a: &RigidBody3D, b: &RigidBody3D) -> Option<CollisionResult3D> {
726        let a_bbox: AABB3D = a.bounding_box()?;
727        let b_bbox: AABB3D = b.bounding_box()?;
728        if !AABB3D::broad_phase_check(a_bbox, b_bbox) {
729            return None;
730        }
731        let a_collider: Option<BodyCollider3D> = a.get_collider();
732        let b_collider: Option<BodyCollider3D> = b.get_collider();
733        let position_delta: Vector3D = b.get_position() - a.get_position();
734        match (a_collider, b_collider) {
735            (Some(BodyCollider3D::Aabb(aabb_a)), Some(BodyCollider3D::Aabb(aabb_b))) => {
736                let aabb_b_inner: AABB3D = aabb_b.get_aabb();
737                let offset_aabb: AabbCollider3D = AabbCollider3D::new(AABB3D::new(
738                    aabb_b_inner.get_min() + position_delta,
739                    aabb_b_inner.get_max() + position_delta,
740                ));
741                aabb_a.collide_with_aabb(&offset_aabb)
742            }
743            (Some(BodyCollider3D::Sphere(sphere_a)), Some(BodyCollider3D::Sphere(sphere_b))) => {
744                let sphere_b_inner: Sphere = sphere_b.get_sphere();
745                let offset_sphere: SphereCollider3D = SphereCollider3D::new(Sphere::new(
746                    sphere_b_inner.get_center() + position_delta,
747                    sphere_b_inner.get_radius(),
748                ));
749                sphere_a.collide_with_sphere(&offset_sphere)
750            }
751            (Some(BodyCollider3D::Aabb(aabb)), Some(BodyCollider3D::Sphere(sphere))) => {
752                let sphere_inner: Sphere = sphere.get_sphere();
753                let offset_sphere: SphereCollider3D = SphereCollider3D::new(Sphere::new(
754                    sphere_inner.get_center() + position_delta,
755                    sphere_inner.get_radius(),
756                ));
757                aabb.collide_with_sphere(&offset_sphere)
758            }
759            (Some(BodyCollider3D::Sphere(sphere)), Some(BodyCollider3D::Aabb(aabb))) => {
760                let aabb_inner: AABB3D = aabb.get_aabb();
761                let offset_aabb: AabbCollider3D = AabbCollider3D::new(AABB3D::new(
762                    aabb_inner.get_min() + position_delta,
763                    aabb_inner.get_max() + position_delta,
764                ));
765                offset_aabb
766                    .collide_with_sphere(&sphere)
767                    .map(|mut result: CollisionResult3D| {
768                        result.set_normal(-result.get_normal());
769                        result
770                    })
771            }
772            _ => None,
773        }
774    }
775
776    /// Resolves a collision between two 3D bodies using impulse-based response
777    /// and position correction.
778    ///
779    /// # Arguments
780    ///
781    /// - `&mut RigidBody3D` - The first body.
782    /// - `&mut RigidBody3D` - The second body.
783    /// - `&CollisionResult3D` - The collision data.
784    fn resolve_collision_3d(a: &mut RigidBody3D, b: &mut RigidBody3D, result: &CollisionResult3D) {
785        let a_inverse_mass: f64 = a.get_inverse_mass();
786        let b_inverse_mass: f64 = b.get_inverse_mass();
787        let relative_velocity: Vector3D = b.get_velocity() - a.get_velocity();
788        let velocity_along_normal: f64 = relative_velocity.dot(result.get_normal());
789        if velocity_along_normal > 0.0 {
790            return;
791        }
792        let restitution: f64 = a.get_restitution().min(b.get_restitution());
793        let inverse_mass_sum: f64 = a_inverse_mass + b_inverse_mass;
794        if inverse_mass_sum == 0.0 {
795            return;
796        }
797        let impulse_magnitude: f64 =
798            -(1.0 + restitution) * velocity_along_normal / inverse_mass_sum;
799        let impulse: Vector3D = result.get_normal().scaled(impulse_magnitude);
800        *a.get_mut_velocity() -= impulse.scaled(a_inverse_mass);
801        *b.get_mut_velocity() += impulse.scaled(b_inverse_mass);
802        let correction: Vector3D = result
803            .get_normal()
804            .scaled((result.get_depth() * PHYSICS_POSITION_PERCENT / inverse_mass_sum).max(0.0));
805        *a.get_mut_position() -= correction.scaled(a_inverse_mass);
806        *b.get_mut_position() += correction.scaled(b_inverse_mass);
807    }
808}
809
810/// Implements `Default` for `PhysicsWorld3D` as an empty world.
811impl Default for PhysicsWorld3D {
812    fn default() -> PhysicsWorld3D {
813        PhysicsWorld3D::new(PhysicsConfig3D::default())
814    }
815}
euv_engine/physics/impl.rs

euv_engine/physics/
impl.rs
