1use crate::types::{CollisionPair, Contact, ContactManifold};
6use oxiphysics_core::Transform;
7use oxiphysics_geometry::Shape;
8use std::collections::HashMap;
9
10use super::functions::{NarrowPhaseFn, gjk_epa, shape_type_ordinal, try_specialized};
11
12#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
14pub enum ShapeType {
15 Sphere,
17 Box,
19 Capsule,
21 Cylinder,
23 Cone,
25 ConvexHull,
27 TriangleMesh,
29 Compound,
31 HeightField,
33 Plane,
35 Custom(u32),
37}
38#[derive(Debug, Clone)]
40pub struct DispatchConfig {
41 pub use_gjk_fallback: bool,
43 pub max_gjk_iterations: usize,
45 pub max_epa_iterations: usize,
47 pub contact_tolerance: f64,
49}
50#[derive(Debug, Clone, Copy)]
52pub struct MeshTriangle {
53 pub vertices: [[f64; 3]; 3],
55 pub index: usize,
57 pub aabb: Aabb,
59}
60impl MeshTriangle {
61 pub fn new(vertices: [[f64; 3]; 3], index: usize) -> Self {
63 let min = [
64 vertices[0][0].min(vertices[1][0]).min(vertices[2][0]),
65 vertices[0][1].min(vertices[1][1]).min(vertices[2][1]),
66 vertices[0][2].min(vertices[1][2]).min(vertices[2][2]),
67 ];
68 let max = [
69 vertices[0][0].max(vertices[1][0]).max(vertices[2][0]),
70 vertices[0][1].max(vertices[1][1]).max(vertices[2][1]),
71 vertices[0][2].max(vertices[1][2]).max(vertices[2][2]),
72 ];
73 Self {
74 vertices,
75 index,
76 aabb: Aabb::new(min, max),
77 }
78 }
79}
80#[derive(Debug, Clone)]
85pub struct ContactPatch {
86 pub points: Vec<[f64; 3]>,
88 pub normal: [f64; 3],
90 pub depth: f64,
92}
93impl ContactPatch {
94 pub fn new(normal: [f64; 3], depth: f64) -> Self {
96 Self {
97 points: Vec::new(),
98 normal,
99 depth,
100 }
101 }
102 pub fn add_point(&mut self, p: [f64; 3]) {
104 self.points.push(p);
105 }
106 pub fn len(&self) -> usize {
108 self.points.len()
109 }
110 pub fn is_empty(&self) -> bool {
112 self.points.is_empty()
113 }
114}
115#[derive(Debug)]
117pub struct ConvexPiece {
118 pub vertices: Vec<[f64; 3]>,
120 pub shape_type: ShapeType,
122 pub aabb_min: [f64; 3],
124 pub aabb_max: [f64; 3],
126}
127impl ConvexPiece {
128 pub fn new(vertices: Vec<[f64; 3]>) -> Self {
130 let aabb_min = vertices.iter().fold([f64::MAX; 3], |mut acc, v| {
131 acc[0] = acc[0].min(v[0]);
132 acc[1] = acc[1].min(v[1]);
133 acc[2] = acc[2].min(v[2]);
134 acc
135 });
136 let aabb_max = vertices.iter().fold([f64::MIN; 3], |mut acc, v| {
137 acc[0] = acc[0].max(v[0]);
138 acc[1] = acc[1].max(v[1]);
139 acc[2] = acc[2].max(v[2]);
140 acc
141 });
142 Self {
143 vertices,
144 shape_type: ShapeType::ConvexHull,
145 aabb_min,
146 aabb_max,
147 }
148 }
149 pub fn aabb_overlaps(&self, other: &ConvexPiece, expand: f64) -> bool {
151 (0..3).all(|i| {
152 self.aabb_max[i] + expand >= other.aabb_min[i] - expand
153 && other.aabb_max[i] + expand >= self.aabb_min[i] - expand
154 })
155 }
156}
157pub type BatchCollisionPair<'a> = (
161 &'a dyn Shape,
162 ShapeType,
163 &'a Transform,
164 &'a dyn Shape,
165 ShapeType,
166 &'a Transform,
167 CollisionPair,
168);
169
170pub struct NarrowPhaseDispatcher {
172 pub(super) table: HashMap<DispatchKey, NarrowPhaseFn>,
174 pub(super) _config: DispatchConfig,
176}
177impl NarrowPhaseDispatcher {
178 pub fn empty() -> Self {
180 NarrowPhaseDispatcher {
181 table: HashMap::new(),
182 _config: DispatchConfig::default(),
183 }
184 }
185 pub fn with_config(config: DispatchConfig) -> Self {
187 NarrowPhaseDispatcher {
188 table: HashMap::new(),
189 _config: config,
190 }
191 }
192 pub fn register_pair(&mut self, type_a: ShapeType, type_b: ShapeType, func: NarrowPhaseFn) {
194 let key = DispatchKey::new(type_a, type_b);
195 self.table.insert(key, func);
196 }
197 pub fn unregister_pair(&mut self, type_a: ShapeType, type_b: ShapeType) -> bool {
200 let key = DispatchKey::new(type_a, type_b);
201 self.table.remove(&key).is_some()
202 }
203 pub fn has_pair(&self, type_a: ShapeType, type_b: ShapeType) -> bool {
205 let key = DispatchKey::new(type_a, type_b);
206 self.table.contains_key(&key)
207 }
208 pub fn registered_count(&self) -> usize {
210 self.table.len()
211 }
212 pub fn registered_keys(&self) -> Vec<DispatchKey> {
214 self.table.keys().copied().collect()
215 }
216 pub fn dispatch(
218 &self,
219 shape_a: &dyn Shape,
220 type_a: ShapeType,
221 transform_a: &Transform,
222 shape_b: &dyn Shape,
223 type_b: ShapeType,
224 transform_b: &Transform,
225 pair: CollisionPair,
226 ) -> NarrowPhaseResult {
227 let key = DispatchKey::new(type_a, type_b);
228 if let Some(func) = self.table.get(&key) {
229 return func(shape_a, transform_a, shape_b, transform_b, pair);
230 }
231 match gjk_epa(shape_a, transform_a, shape_b, transform_b, pair) {
232 Some(manifold) => NarrowPhaseResult::contact(manifold),
233 None => NarrowPhaseResult::separated(),
234 }
235 }
236 pub fn dispatch_symmetric(
239 &self,
240 shape_a: &dyn Shape,
241 type_a: ShapeType,
242 transform_a: &Transform,
243 shape_b: &dyn Shape,
244 type_b: ShapeType,
245 transform_b: &Transform,
246 pair: CollisionPair,
247 ) -> NarrowPhaseResult {
248 let key = DispatchKey::new(type_a, type_b);
249 if let Some(func) = self.table.get(&key) {
250 let need_swap = shape_type_ordinal(type_a) > shape_type_ordinal(type_b);
251 if need_swap {
252 let swapped_pair = CollisionPair::new(pair.b, pair.a);
253 let mut result = func(shape_b, transform_b, shape_a, transform_a, swapped_pair);
254 if let Some(ref mut manifold) = result.manifold {
255 for contact in &mut manifold.contacts {
256 contact.normal = -contact.normal;
257 std::mem::swap(&mut contact.point_a, &mut contact.point_b);
258 }
259 manifold.pair = pair;
260 }
261 return result;
262 }
263 return func(shape_a, transform_a, shape_b, transform_b, pair);
264 }
265 match gjk_epa(shape_a, transform_a, shape_b, transform_b, pair) {
266 Some(manifold) => NarrowPhaseResult::contact(manifold),
267 None => NarrowPhaseResult::separated(),
268 }
269 }
270 pub fn generate_contacts(
272 shape_a: &dyn Shape,
273 transform_a: &Transform,
274 shape_b: &dyn Shape,
275 transform_b: &Transform,
276 pair: CollisionPair,
277 ) -> Option<ContactManifold> {
278 if let Some(contact) = try_specialized(shape_a, transform_a, shape_b, transform_b) {
279 let mut manifold = ContactManifold::new(pair);
280 manifold.add_contact(contact);
281 return Some(manifold);
282 }
283 gjk_epa(shape_a, transform_a, shape_b, transform_b, pair)
284 }
285 pub fn dispatch_batch<'a>(&self, pairs: &[BatchCollisionPair<'a>]) -> Vec<NarrowPhaseResult> {
287 pairs
288 .iter()
289 .map(|&(sa, ta_type, ta, sb, tb_type, tb, pair)| {
290 self.dispatch(sa, ta_type, ta, sb, tb_type, tb, pair)
291 })
292 .collect()
293 }
294}
295#[derive(Debug, Clone, Copy)]
297pub struct Aabb {
298 pub min: [f64; 3],
300 pub max: [f64; 3],
302}
303impl Aabb {
304 pub fn new(min: [f64; 3], max: [f64; 3]) -> Self {
306 Self { min, max }
307 }
308 pub fn overlaps(&self, other: &Aabb) -> bool {
310 self.min[0] <= other.max[0]
311 && self.max[0] >= other.min[0]
312 && self.min[1] <= other.max[1]
313 && self.max[1] >= other.min[1]
314 && self.min[2] <= other.max[2]
315 && self.max[2] >= other.min[2]
316 }
317 pub fn surface_area(&self) -> f64 {
319 let dx = self.max[0] - self.min[0];
320 let dy = self.max[1] - self.min[1];
321 let dz = self.max[2] - self.min[2];
322 2.0 * (dx * dy + dy * dz + dx * dz)
323 }
324}
325#[derive(Debug, Clone)]
327pub struct SpeculativeConfig {
328 pub margin: f64,
330 pub velocity_scale: f64,
332}
333#[derive(Debug, Clone, Default)]
335pub struct DispatchStats {
336 pub total_dispatches: usize,
338 pub contacts_found: usize,
340 pub gjk_fallbacks: usize,
342 pub specialized_calls: usize,
344 pub broad_phase_pruned: usize,
346}
347impl DispatchStats {
348 pub fn record(&mut self, had_contact: bool, used_gjk: bool) {
350 self.total_dispatches += 1;
351 if had_contact {
352 self.contacts_found += 1;
353 }
354 if used_gjk {
355 self.gjk_fallbacks += 1;
356 } else {
357 self.specialized_calls += 1;
358 }
359 }
360 pub fn record_pruned(&mut self) {
362 self.broad_phase_pruned += 1;
363 }
364 pub fn contact_rate(&self) -> f64 {
366 if self.total_dispatches == 0 {
367 0.0
368 } else {
369 self.contacts_found as f64 / self.total_dispatches as f64
370 }
371 }
372 pub fn specialization_rate(&self) -> f64 {
374 if self.total_dispatches == 0 {
375 0.0
376 } else {
377 self.specialized_calls as f64 / self.total_dispatches as f64
378 }
379 }
380}
381#[derive(Debug, Clone, Copy)]
383pub struct HeightFieldSample {
384 pub position: [f64; 3],
386 pub normal: [f64; 3],
388 pub height: f64,
390}
391pub struct SimpleHeightField {
393 pub heights: Vec<f64>,
395 pub rows: usize,
397 pub cols: usize,
399 pub spacing: f64,
401}
402impl SimpleHeightField {
403 pub fn new(heights: Vec<f64>, rows: usize, cols: usize, spacing: f64) -> Self {
405 Self {
406 heights,
407 rows,
408 cols,
409 spacing,
410 }
411 }
412 pub fn height_at(&self, row: usize, col: usize) -> f64 {
414 let r = row.min(self.rows - 1);
415 let c = col.min(self.cols - 1);
416 self.heights[r * self.cols + c]
417 }
418 pub fn normal_at(&self, row: usize, col: usize) -> [f64; 3] {
420 let h = self.height_at(row, col);
421 let hx = if col + 1 < self.cols {
422 self.height_at(row, col + 1)
423 } else {
424 h
425 };
426 let hz = if row + 1 < self.rows {
427 self.height_at(row + 1, col)
428 } else {
429 h
430 };
431 let dx = hx - h;
432 let dz = hz - h;
433 let nx = -dx;
434 let ny = self.spacing;
435 let nz = -dz;
436 let len = (nx * nx + ny * ny + nz * nz).sqrt();
437 if len < 1e-12 {
438 [0.0, 1.0, 0.0]
439 } else {
440 [nx / len, ny / len, nz / len]
441 }
442 }
443 pub fn test_sphere(
445 &self,
446 sphere_center: [f64; 3],
447 sphere_radius: f64,
448 ) -> Option<([f64; 3], [f64; 3], f64)> {
449 let col_f = sphere_center[0] / self.spacing;
450 let row_f = sphere_center[2] / self.spacing;
451 if col_f < 0.0 || row_f < 0.0 {
452 return None;
453 }
454 let col = col_f as usize;
455 let row = row_f as usize;
456 if col >= self.cols || row >= self.rows {
457 return None;
458 }
459 let h = self.height_at(row, col);
460 let penetration = h + sphere_radius - sphere_center[1];
461 if penetration > 0.0 {
462 let normal = self.normal_at(row, col);
463 let point = [sphere_center[0], h, sphere_center[2]];
464 Some((point, normal, penetration))
465 } else {
466 None
467 }
468 }
469}
470pub struct DispatchQueue {
475 pub(super) entries: Vec<DispatchQueueEntry>,
476}
477impl DispatchQueue {
478 pub fn new() -> Self {
480 Self {
481 entries: Vec::new(),
482 }
483 }
484 pub fn push(&mut self, pair: CollisionPair, priority: f64) {
486 self.entries.push(DispatchQueueEntry { pair, priority });
487 }
488 pub fn pop(&mut self) -> Option<DispatchQueueEntry> {
490 if self.entries.is_empty() {
491 return None;
492 }
493 let idx = self
494 .entries
495 .iter()
496 .enumerate()
497 .max_by(|(_, a), (_, b)| {
498 a.priority
499 .partial_cmp(&b.priority)
500 .unwrap_or(std::cmp::Ordering::Equal)
501 })
502 .map(|(i, _)| i)?;
503 Some(self.entries.swap_remove(idx))
504 }
505 pub fn len(&self) -> usize {
507 self.entries.len()
508 }
509 pub fn is_empty(&self) -> bool {
511 self.entries.is_empty()
512 }
513 pub fn clear(&mut self) {
515 self.entries.clear();
516 }
517}
518#[derive(Debug, Clone)]
520pub struct DispatchQueueEntry {
521 pub pair: CollisionPair,
523 pub priority: f64,
525}
526#[derive(Debug, Clone)]
528pub struct CompoundDispatchResult {
529 pub manifolds: Vec<ContactManifold>,
531 pub tests_performed: usize,
533 pub pairs_pruned: usize,
535}
536impl CompoundDispatchResult {
537 pub fn has_contacts(&self) -> bool {
539 !self.manifolds.is_empty()
540 }
541 pub fn total_contacts(&self) -> usize {
543 self.manifolds.iter().map(|m| m.contacts.len()).sum()
544 }
545}
546pub struct ContactPatchReducer {
551 pub max_contacts: usize,
553}
554impl ContactPatchReducer {
555 pub fn new(max_contacts: usize) -> Self {
557 Self { max_contacts }
558 }
559 pub fn reduce(&self, manifold: &mut ContactManifold) {
566 if manifold.contacts.len() <= self.max_contacts {
567 return;
568 }
569 let mut kept: Vec<Contact> = Vec::with_capacity(self.max_contacts);
570 if let Some(idx) = manifold
571 .contacts
572 .iter()
573 .enumerate()
574 .max_by(|(_, a), (_, b)| {
575 a.depth
576 .partial_cmp(&b.depth)
577 .unwrap_or(std::cmp::Ordering::Equal)
578 })
579 .map(|(i, _)| i)
580 {
581 kept.push(manifold.contacts[idx].clone());
582 }
583 if kept.len() < self.max_contacts && !manifold.contacts.is_empty() {
584 let ref_pt = kept[0].point_a;
585 if let Some(idx) = manifold
586 .contacts
587 .iter()
588 .enumerate()
589 .max_by(|(_, a), (_, b)| {
590 let da = (a.point_a - ref_pt).norm_squared();
591 let db = (b.point_a - ref_pt).norm_squared();
592 da.partial_cmp(&db).unwrap_or(std::cmp::Ordering::Equal)
593 })
594 .map(|(i, _)| i)
595 {
596 kept.push(manifold.contacts[idx].clone());
597 }
598 }
599 while kept.len() < self.max_contacts {
600 let mut best_area = -1.0;
601 let mut best_idx = 0;
602 'outer: for (i, c) in manifold.contacts.iter().enumerate() {
603 for k in &kept {
604 if (c.point_a - k.point_a).norm_squared() < 1e-10 {
605 continue 'outer;
606 }
607 }
608 let area = if kept.len() >= 2 {
609 let ab = kept[1].point_a - kept[0].point_a;
610 let ac = c.point_a - kept[0].point_a;
611 ab.cross(&ac).norm()
612 } else {
613 (c.point_a - kept[0].point_a).norm()
614 };
615 if area > best_area {
616 best_area = area;
617 best_idx = i;
618 }
619 }
620 if best_area <= 0.0 {
621 break;
622 }
623 kept.push(manifold.contacts[best_idx].clone());
624 }
625 manifold.contacts = kept;
626 }
627}
628pub struct CompoundShape {
630 pub children: Vec<Box<dyn Shape>>,
632 pub child_types: Vec<ShapeType>,
634 pub local_transforms: Vec<Transform>,
636}
637impl CompoundShape {
638 pub fn new() -> Self {
640 Self {
641 children: Vec::new(),
642 child_types: Vec::new(),
643 local_transforms: Vec::new(),
644 }
645 }
646 pub fn add_child(
648 &mut self,
649 shape: Box<dyn Shape>,
650 shape_type: ShapeType,
651 local_transform: Transform,
652 ) {
653 self.children.push(shape);
654 self.child_types.push(shape_type);
655 self.local_transforms.push(local_transform);
656 }
657 pub fn len(&self) -> usize {
659 self.children.len()
660 }
661 pub fn is_empty(&self) -> bool {
663 self.children.is_empty()
664 }
665}
666#[derive(Debug, Clone)]
668pub struct NarrowPhaseResult {
669 pub manifold: Option<ContactManifold>,
671}
672impl NarrowPhaseResult {
673 pub fn separated() -> Self {
675 NarrowPhaseResult { manifold: None }
676 }
677 pub fn contact(manifold: ContactManifold) -> Self {
679 NarrowPhaseResult {
680 manifold: Some(manifold),
681 }
682 }
683 pub fn has_contact(&self) -> bool {
685 self.manifold.is_some()
686 }
687 pub fn penetration_depth(&self) -> Option<f64> {
689 self.manifold
690 .as_ref()
691 .and_then(|m| m.contacts.first())
692 .map(|c| c.depth)
693 }
694}
695pub struct ConcaveMesh {
697 pub pieces: Vec<ConvexPiece>,
699}
700impl ConcaveMesh {
701 pub fn new(pieces: Vec<ConvexPiece>) -> Self {
703 Self { pieces }
704 }
705 pub fn num_pieces(&self) -> usize {
707 self.pieces.len()
708 }
709}
710#[derive(Debug, Clone)]
712pub struct ShapeFeatureCache {
713 pub(super) support_cache: std::collections::HashMap<u64, [f64; 3]>,
715 pub hits: usize,
717 pub misses: usize,
719}
720impl ShapeFeatureCache {
721 pub fn new() -> Self {
723 Self {
724 support_cache: std::collections::HashMap::new(),
725 hits: 0,
726 misses: 0,
727 }
728 }
729 pub fn get_or_insert(&mut self, dir_key: u64, compute: impl FnOnce() -> [f64; 3]) -> [f64; 3] {
733 if let Some(&cached) = self.support_cache.get(&dir_key) {
734 self.hits += 1;
735 cached
736 } else {
737 self.misses += 1;
738 let val = compute();
739 self.support_cache.insert(dir_key, val);
740 val
741 }
742 }
743 pub fn hit_ratio(&self) -> f64 {
745 let total = self.hits + self.misses;
746 if total == 0 {
747 0.0
748 } else {
749 self.hits as f64 / total as f64
750 }
751 }
752 pub fn clear(&mut self) {
754 self.support_cache.clear();
755 self.hits = 0;
756 self.misses = 0;
757 }
758}
759#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
762pub struct DispatchKey(pub ShapeType, pub ShapeType);
763impl DispatchKey {
764 pub fn new(a: ShapeType, b: ShapeType) -> Self {
766 if shape_type_ordinal(a) <= shape_type_ordinal(b) {
767 DispatchKey(a, b)
768 } else {
769 DispatchKey(b, a)
770 }
771 }
772}