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 broad-phase bounding box checks followed by narrow-phase shape-specific
364    /// collision detection, then applies impulse-based resolution.
365    fn resolve_collisions(&mut self) {
366        let body_count: usize = self.get_bodies().len();
367        for iteration in 0..PHYSICS_MAX_ITERATIONS {
368            let mut any_collision: bool = false;
369            for i in 0..body_count {
370                for j in (i + 1)..body_count {
371                    let (left, right) = self.get_mut_bodies().split_at_mut(j);
372                    let body_a: &mut RigidBody2D = &mut left[i];
373                    let body_b: &mut RigidBody2D = &mut right[0];
374                    if body_a.get_inverse_mass() == 0.0 && body_b.get_inverse_mass() == 0.0 {
375                        continue;
376                    }
377                    if let Some(result) = body_a.check_collision_with(body_b) {
378                        body_a.resolve_collision_with(body_b, &result);
379                        any_collision = true;
380                    }
381                }
382            }
383            if !any_collision {
384                break;
385            }
386            let _ = iteration;
387        }
388    }
389}
390
391/// Implements default configuration for `PhysicsConfig3D`.
392impl Default for PhysicsConfig3D {
393    fn default() -> PhysicsConfig3D {
394        PhysicsConfig3D::new(
395            Vector3D::new(0.0, DEFAULT_GRAVITY_3D, 0.0),
396            DEFAULT_LINEAR_DAMPING,
397            DEFAULT_ANGULAR_DAMPING,
398        )
399    }
400}
401
402/// Implements body creation and force management for `RigidBody3D`.
403impl RigidBody3D {
404    /// Creates a new dynamic 3D rigid body with default mass and the given position.
405    ///
406    /// # Arguments
407    ///
408    /// - `u64` - The unique ID.
409    /// - `Vector3D` - The initial position.
410    ///
411    /// # Returns
412    ///
413    /// - `RigidBody3D` - The new body.
414    pub fn new_dynamic(id: u64, position: Vector3D) -> RigidBody3D {
415        let mass: f64 = PHYSICS_DEFAULT_MASS;
416        RigidBody3D::new(
417            id,
418            position,
419            mass,
420            1.0 / mass,
421            DEFAULT_RESTITUTION,
422            DEFAULT_FRICTION,
423            BodyType::Dynamic,
424        )
425    }
426
427    /// Creates a new static 3D rigid body at the given position with infinite mass.
428    ///
429    /// # Arguments
430    ///
431    /// - `u64` - The unique ID.
432    /// - `Vector3D` - The position.
433    ///
434    /// # Returns
435    ///
436    /// - `RigidBody3D` - The new static body.
437    pub fn new_static(id: u64, position: Vector3D) -> RigidBody3D {
438        RigidBody3D::new(
439            id,
440            position,
441            PHYSICS_STATIC_MASS,
442            0.0,
443            DEFAULT_RESTITUTION,
444            DEFAULT_FRICTION,
445            BodyType::Static,
446        )
447    }
448
449    /// Applies a force to the body's force accumulator.
450    ///
451    /// # Arguments
452    ///
453    /// - `Vector3D` - The force vector.
454    pub fn apply_force(&mut self, force: Vector3D) {
455        *self.get_mut_force_accumulator() += force;
456    }
457
458    /// Applies a torque to the body's torque accumulator.
459    ///
460    /// # Arguments
461    ///
462    /// - `Vector3D` - The torque vector.
463    pub fn apply_torque(&mut self, torque: Vector3D) {
464        *self.get_mut_torque_accumulator() += torque;
465    }
466
467    /// Applies an instantaneous impulse, directly changing velocity.
468    ///
469    /// # Arguments
470    ///
471    /// - `Vector3D` - The impulse vector.
472    pub fn apply_impulse(&mut self, impulse: Vector3D) {
473        let inverse_mass: f64 = self.get_inverse_mass();
474        if inverse_mass == 0.0 {
475            return;
476        }
477        *self.get_mut_velocity() += impulse.scaled(inverse_mass);
478    }
479
480    /// Sets the mass of the body, updating the inverse mass.
481    /// A mass of 0 makes the body static (infinite mass).
482    ///
483    /// # Arguments
484    ///
485    /// - `f64` - The new mass.
486    pub fn update_mass(&mut self, mass: f64) {
487        self.set_mass(mass);
488        self.set_inverse_mass(if mass > 0.0 { 1.0 / mass } else { 0.0 });
489    }
490
491    /// Returns `true` if this body is affected by forces and collisions.
492    ///
493    /// # Returns
494    ///
495    /// - `bool` - True if the body is dynamic.
496    pub fn is_dynamic(&self) -> bool {
497        self.get_body_type() == BodyType::Dynamic
498    }
499
500    /// Attaches a 3D collider shape to this body.
501    ///
502    /// # Arguments
503    ///
504    /// - `BodyCollider3D` - The collider to attach.
505    pub fn update_collider(&mut self, collider: BodyCollider3D) {
506        self.set_collider(Some(collider));
507    }
508
509    /// Returns the world-space 3D bounding box of the attached collider, if any.
510    ///
511    /// # Returns
512    ///
513    /// - `Option<AABB3D>` - The bounding box, or `None` if no collider is attached.
514    pub fn bounding_box(&self) -> Option<AABB3D> {
515        let collider: Option<BodyCollider3D> = self.get_collider();
516        let position: Vector3D = self.get_position();
517        match collider? {
518            BodyCollider3D::Aabb(aabb) => {
519                let center: Vector3D = aabb.get_aabb().center();
520                let size: Vector3D = aabb.get_aabb().size();
521                Some(AABB3D::from_center(
522                    position + center,
523                    size.get_x(),
524                    size.get_y(),
525                    size.get_z(),
526                ))
527            }
528            BodyCollider3D::Sphere(sphere) => {
529                let sphere_inner: Sphere = sphere.get_sphere();
530                let diameter: f64 = sphere_inner.get_radius() * 2.0;
531                Some(AABB3D::from_center(
532                    position + sphere_inner.get_center(),
533                    diameter,
534                    diameter,
535                    diameter,
536                ))
537            }
538        }
539    }
540}
541
542/// Implements body management and simulation for `PhysicsWorld3D`.
543impl PhysicsWorld3D {
544    /// Creates a new 3D physics world with the given configuration.
545    ///
546    /// # Arguments
547    ///
548    /// - `PhysicsConfig3D` - The simulation configuration.
549    ///
550    /// # Returns
551    ///
552    /// - `PhysicsWorld3D` - The new world.
553    pub fn with_config(config: PhysicsConfig3D) -> PhysicsWorld3D {
554        PhysicsWorld3D::new(config)
555    }
556
557    /// Adds a rigid body to the world.
558    ///
559    /// # Arguments
560    ///
561    /// - `RigidBody3D` - The body to add.
562    pub fn add_body(&mut self, body: RigidBody3D) {
563        self.get_mut_bodies().push(body);
564    }
565
566    /// Removes the body with the given ID.
567    ///
568    /// # Arguments
569    ///
570    /// - `u64` - The ID of the body to remove.
571    pub fn remove_body(&mut self, id: u64) {
572        self.get_mut_bodies()
573            .retain(|body: &RigidBody3D| body.get_id() != id);
574    }
575
576    /// Returns a reference to the body with the given ID.
577    ///
578    /// # Arguments
579    ///
580    /// - `u64` - The body ID.
581    ///
582    /// # Returns
583    ///
584    /// - `Option<&RigidBody3D>` - The body reference, if found.
585    pub fn get_body(&self, id: u64) -> Option<&RigidBody3D> {
586        self.get_bodies()
587            .iter()
588            .find(|body: &&RigidBody3D| body.get_id() == id)
589    }
590
591    /// Returns a mutable reference to the body with the given ID.
592    ///
593    /// # Arguments
594    ///
595    /// - `u64` - The body ID.
596    ///
597    /// # Returns
598    ///
599    /// - `Option<&mut RigidBody3D>` - The mutable body reference, if found.
600    pub fn get_body_mut(&mut self, id: u64) -> Option<&mut RigidBody3D> {
601        self.get_mut_bodies()
602            .iter_mut()
603            .find(|body: &&mut RigidBody3D| body.get_id() == id)
604    }
605
606    /// Performs one physics simulation step using semi-implicit Euler integration.
607    ///
608    /// Applies gravity to dynamic bodies, integrates velocity from accumulated forces,
609    /// applies damping, integrates position, and resolves collisions.
610    ///
611    /// # Arguments
612    ///
613    /// - `f64` - The fixed delta time in seconds.
614    pub fn step(&mut self, delta_time: f64) {
615        let config: PhysicsConfig3D = self.get_config();
616        for body in self.get_mut_bodies() {
617            if !body.is_dynamic() {
618                continue;
619            }
620            let body_mass: f64 = body.get_mass();
621            let body_inverse_mass: f64 = body.get_inverse_mass();
622            *body.get_mut_force_accumulator() += config.get_gravity().scaled(body_mass);
623            let force: Vector3D = body.get_force_accumulator();
624            *body.get_mut_velocity() += force.scaled(body_inverse_mass * delta_time);
625            let damping_factor: f64 = (1.0 - config.get_linear_damping() * delta_time).max(0.0);
626            let velocity: Vector3D = body.get_velocity();
627            body.set_velocity(velocity.scaled(damping_factor));
628            let current_velocity: Vector3D = body.get_velocity();
629            *body.get_mut_position() += current_velocity.scaled(delta_time);
630            body.set_force_accumulator(Vector3D::zero());
631            let angular_damping: f64 = (1.0 - config.get_angular_damping() * delta_time).max(0.0);
632            let angular_velocity: Vector3D = body.get_angular_velocity().scaled(angular_damping);
633            body.set_angular_velocity(angular_velocity);
634            let rotation_delta: Quaternion = Quaternion::new(
635                angular_velocity.get_x() * delta_time * 0.5,
636                angular_velocity.get_y() * delta_time * 0.5,
637                angular_velocity.get_z() * delta_time * 0.5,
638                1.0,
639            );
640            body.set_rotation((rotation_delta * body.get_rotation()).normalized());
641            body.set_torque_accumulator(Vector3D::zero());
642        }
643        self.resolve_collisions();
644    }
645
646    /// Detects and resolves all collisions between bodies in the 3D world.
647    ///
648    /// Uses broad-phase bounding box checks followed by narrow-phase shape-specific
649    /// collision detection, then applies impulse-based resolution.
650    fn resolve_collisions(&mut self) {
651        let body_count: usize = self.get_bodies().len();
652        for iteration in 0..PHYSICS_MAX_ITERATIONS {
653            let mut any_collision: bool = false;
654            for i in 0..body_count {
655                for j in (i + 1)..body_count {
656                    let (left, right) = self.get_mut_bodies().split_at_mut(j);
657                    let body_a: &mut RigidBody3D = &mut left[i];
658                    let body_b: &mut RigidBody3D = &mut right[0];
659                    if body_a.get_inverse_mass() == 0.0 && body_b.get_inverse_mass() == 0.0 {
660                        continue;
661                    }
662                    if let Some(result) = Self::check_collision_3d(body_a, body_b) {
663                        Self::resolve_collision_3d(body_a, body_b, &result);
664                        any_collision = true;
665                    }
666                }
667            }
668            if !any_collision {
669                break;
670            }
671            let _ = iteration;
672        }
673    }
674
675    /// Checks collision between two 3D bodies based on both bodies' collider shapes.
676    ///
677    /// # Arguments
678    ///
679    /// - `&RigidBody3D` - The first body.
680    /// - `&RigidBody3D` - The second body.
681    ///
682    /// # Returns
683    ///
684    /// - `Option<CollisionResult3D>` - The collision result, or `None`.
685    fn check_collision_3d(a: &RigidBody3D, b: &RigidBody3D) -> Option<CollisionResult3D> {
686        let a_bbox: AABB3D = a.bounding_box()?;
687        let b_bbox: AABB3D = b.bounding_box()?;
688        if !AABB3D::broad_phase_check(a_bbox, b_bbox) {
689            return None;
690        }
691        let a_collider: Option<BodyCollider3D> = a.get_collider();
692        let b_collider: Option<BodyCollider3D> = b.get_collider();
693        let position_delta: Vector3D = b.get_position() - a.get_position();
694        match (a_collider, b_collider) {
695            (Some(BodyCollider3D::Aabb(aabb_a)), Some(BodyCollider3D::Aabb(aabb_b))) => {
696                let aabb_b_inner: AABB3D = aabb_b.get_aabb();
697                let offset_aabb: AabbCollider3D = AabbCollider3D::new(AABB3D::new(
698                    aabb_b_inner.get_min() + position_delta,
699                    aabb_b_inner.get_max() + position_delta,
700                ));
701                aabb_a.collide_with_aabb(&offset_aabb)
702            }
703            (Some(BodyCollider3D::Sphere(sphere_a)), Some(BodyCollider3D::Sphere(sphere_b))) => {
704                let sphere_b_inner: Sphere = sphere_b.get_sphere();
705                let offset_sphere: SphereCollider3D = SphereCollider3D::new(Sphere::new(
706                    sphere_b_inner.get_center() + position_delta,
707                    sphere_b_inner.get_radius(),
708                ));
709                sphere_a.collide_with_sphere(&offset_sphere)
710            }
711            (Some(BodyCollider3D::Aabb(aabb)), Some(BodyCollider3D::Sphere(sphere))) => {
712                let sphere_inner: Sphere = sphere.get_sphere();
713                let offset_sphere: SphereCollider3D = SphereCollider3D::new(Sphere::new(
714                    sphere_inner.get_center() + position_delta,
715                    sphere_inner.get_radius(),
716                ));
717                aabb.collide_with_sphere(&offset_sphere)
718            }
719            (Some(BodyCollider3D::Sphere(sphere)), Some(BodyCollider3D::Aabb(aabb))) => {
720                let aabb_inner: AABB3D = aabb.get_aabb();
721                let offset_aabb: AabbCollider3D = AabbCollider3D::new(AABB3D::new(
722                    aabb_inner.get_min() + position_delta,
723                    aabb_inner.get_max() + position_delta,
724                ));
725                offset_aabb
726                    .collide_with_sphere(&sphere)
727                    .map(|mut result: CollisionResult3D| {
728                        result.set_normal(-result.get_normal());
729                        result
730                    })
731            }
732            _ => None,
733        }
734    }
735
736    /// Resolves a collision between two 3D bodies using impulse-based response
737    /// and position correction.
738    ///
739    /// # Arguments
740    ///
741    /// - `&mut RigidBody3D` - The first body.
742    /// - `&mut RigidBody3D` - The second body.
743    /// - `&CollisionResult3D` - The collision data.
744    fn resolve_collision_3d(a: &mut RigidBody3D, b: &mut RigidBody3D, result: &CollisionResult3D) {
745        let a_inverse_mass: f64 = a.get_inverse_mass();
746        let b_inverse_mass: f64 = b.get_inverse_mass();
747        let relative_velocity: Vector3D = b.get_velocity() - a.get_velocity();
748        let velocity_along_normal: f64 = relative_velocity.dot(result.get_normal());
749        if velocity_along_normal > 0.0 {
750            return;
751        }
752        let restitution: f64 = a.get_restitution().min(b.get_restitution());
753        let inverse_mass_sum: f64 = a_inverse_mass + b_inverse_mass;
754        if inverse_mass_sum == 0.0 {
755            return;
756        }
757        let impulse_magnitude: f64 =
758            -(1.0 + restitution) * velocity_along_normal / inverse_mass_sum;
759        let impulse: Vector3D = result.get_normal().scaled(impulse_magnitude);
760        *a.get_mut_velocity() -= impulse.scaled(a_inverse_mass);
761        *b.get_mut_velocity() += impulse.scaled(b_inverse_mass);
762        let correction: Vector3D = result
763            .get_normal()
764            .scaled((result.get_depth() * PHYSICS_POSITION_PERCENT / inverse_mass_sum).max(0.0));
765        *a.get_mut_position() -= correction.scaled(a_inverse_mass);
766        *b.get_mut_position() += correction.scaled(b_inverse_mass);
767    }
768}
769
770/// Implements `Default` for `PhysicsWorld3D` as an empty world.
771impl Default for PhysicsWorld3D {
772    fn default() -> PhysicsWorld3D {
773        PhysicsWorld3D::new(PhysicsConfig3D::default())
774    }
775}
euv_engine/physics/impl.rs

euv_engine/physics/
impl.rs
