khora_core/physics/
collision.rs

1// Copyright 2025 eraflo
2//
3// Licensed under the Apache License, Version 2.0 (the "License");
4// you may not use this file except in compliance with the License.
5// You may obtain a copy of the License at
6//
7//     http://www.apache.org/licenses/LICENSE-2.0
8//
9// Unless required by applicable law or agreed to in writing, software
10// distributed under the License is distributed on an "AS IS" BASIS,
11// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12// See the License for the specific language governing permissions and
13// limitations under the License.
14
15//! Internal narrow-phase collision detection.
16
17use super::{ColliderShape, ContactManifold};
18use crate::math::{AffineTransform, Vec3, Vec4};
19
20/// Narrow-phase collision detection system.
21pub struct NarrowPhase;
22
23impl NarrowPhase {
24    /// Creates a new `NarrowPhase` instance.
25    pub fn new() -> Self {
26        Self
27    }
28
29    /// Detects collision between two shapes.
30    ///
31    /// Returns a `ContactManifold` if a collision is detected.
32    pub fn detect(
33        &self,
34        shape_a: &ColliderShape,
35        trans_a: &AffineTransform,
36        shape_b: &ColliderShape,
37        trans_b: &AffineTransform,
38    ) -> Option<ContactManifold> {
39        match (shape_a, shape_b) {
40            (ColliderShape::Sphere(ra), ColliderShape::Sphere(rb)) => {
41                let pa = trans_a.translation();
42                let pb = trans_b.translation();
43                let delta = pb - pa;
44                let dist_sq = delta.length_squared();
45                let total_r = ra + rb;
46                if dist_sq < total_r * total_r {
47                    let dist = dist_sq.sqrt();
48                    let normal = if dist > 0.0001 {
49                        delta / dist
50                    } else {
51                        Vec3::new(0.0, 1.0, 0.0)
52                    };
53                    Some(ContactManifold {
54                        normal,
55                        depth: total_r - dist,
56                        point: pa + normal * (*ra),
57                    })
58                } else {
59                    None
60                }
61            }
62            (ColliderShape::Sphere(ra), ColliderShape::Box(half_b)) => {
63                let pa = trans_a.translation();
64                // Transform sphere center to box local space
65                let inv_b = trans_b.inverse()?;
66                let local_pa = (inv_b.0 * Vec4::from_vec3(pa, 1.0)).truncate();
67
68                // Closest point on box
69                let closest = Vec3::new(
70                    local_pa.x.clamp(-half_b.x, half_b.x),
71                    local_pa.y.clamp(-half_b.y, half_b.y),
72                    local_pa.z.clamp(-half_b.z, half_b.z),
73                );
74
75                let delta = local_pa - closest;
76                let dist_sq = delta.length_squared();
77                if dist_sq < ra * ra {
78                    let dist = dist_sq.sqrt();
79                    let local_normal = if dist > 0.0001 {
80                        delta / dist
81                    } else {
82                        Vec3::new(0.0, 1.0, 0.0)
83                    };
84                    // Normal back to world space
85                    let normal = trans_b.rotation().rotate_vec3(local_normal);
86                    Some(ContactManifold {
87                        normal,
88                        depth: ra - dist,
89                        point: (trans_b.0 * Vec4::from_vec3(closest, 1.0)).truncate(),
90                    })
91                } else {
92                    None
93                }
94            }
95            // Mirror Sphere-Box for Box-Sphere
96            (ColliderShape::Box(_half_a), ColliderShape::Sphere(_rb)) => self
97                .detect(shape_b, trans_b, shape_a, trans_a)
98                .map(|m| m.inverted()),
99            _ => None,
100        }
101    }
102}
103
104impl Default for NarrowPhase {
105    fn default() -> Self {
106        Self::new()
107    }
108}
109
110#[cfg(test)]
111mod tests {
112    use super::*;
113    use crate::math::Vec3;
114
115    #[test]
116    fn test_sphere_sphere_collision() {
117        let narrow = NarrowPhase::new();
118        let sphere_a = ColliderShape::Sphere(1.0);
119        let sphere_b = ColliderShape::Sphere(1.0);
120        let trans_a = AffineTransform::from_translation(Vec3::new(0.0, 0.0, 0.0));
121        let trans_b = AffineTransform::from_translation(Vec3::new(1.5, 0.0, 0.0));
122
123        let manifold = narrow
124            .detect(&sphere_a, &trans_a, &sphere_b, &trans_b)
125            .unwrap();
126        assert!(manifold.depth > 0.0);
127        assert!((manifold.normal.x - 1.0).abs() < 0.001);
128        assert!((manifold.depth - 0.5).abs() < 0.001);
129    }
130}
khora_core/physics/collision.rs

khora_core/physics/
collision.rs
