1use oxiphysics_core::Transform;
6use oxiphysics_core::math::{Real, Vec3};
7use oxiphysics_geometry::Shape;
8
9use super::functions::*;
10
11#[derive(Debug, Clone)]
13pub struct GjkIterationRecord {
14 pub simplex_size: usize,
16 pub direction: Vec3,
18 pub new_support: Vec3,
20 pub dot_positive: bool,
22}
23#[derive(Debug, Clone)]
25pub struct Simplex {
26 pub points: Vec<SupportPoint>,
28}
29impl Simplex {
30 pub fn new() -> Self {
32 Self {
33 points: Vec::with_capacity(4),
34 }
35 }
36 pub fn push(&mut self, point: SupportPoint) {
38 self.points.push(point);
39 }
40 pub fn len(&self) -> usize {
42 self.points.len()
43 }
44 pub fn is_empty(&self) -> bool {
46 self.points.is_empty()
47 }
48 pub fn closest_point_to_origin(&self) -> (Vec3, Vec<Real>) {
51 match self.points.len() {
52 1 => (self.points[0].point, vec![1.0]),
53 2 => closest_point_line(&self.points[0].point, &self.points[1].point),
54 3 => closest_point_triangle(
55 &self.points[0].point,
56 &self.points[1].point,
57 &self.points[2].point,
58 ),
59 _ => (Vec3::zeros(), vec![0.25; 4]),
60 }
61 }
62}
63#[derive(Debug, Clone, Copy)]
65pub struct RayCastResult {
66 pub t: f64,
68 pub point: Vec3,
70 pub normal: Vec3,
72}
73pub struct WarmStartGjk {
79 pub(super) prev_simplex: Option<Simplex>,
81 pub(super) prev_direction: Option<Vec3>,
83 pub(super) avg_iterations: f64,
85 pub(super) query_count: usize,
87}
88impl WarmStartGjk {
89 pub fn new() -> Self {
91 Self {
92 prev_simplex: None,
93 prev_direction: None,
94 avg_iterations: 0.0,
95 query_count: 0,
96 }
97 }
98 pub fn reset(&mut self) {
100 self.prev_simplex = None;
101 self.prev_direction = None;
102 }
103 pub fn avg_iterations(&self) -> f64 {
105 self.avg_iterations
106 }
107 pub fn query_count(&self) -> usize {
109 self.query_count
110 }
111 pub fn query(
116 &mut self,
117 shape_a: &dyn Shape,
118 transform_a: &Transform,
119 shape_b: &dyn Shape,
120 transform_b: &Transform,
121 ) -> GjkResult {
122 let mut direction = if let Some(d) = self.prev_direction {
123 d
124 } else {
125 let d = transform_b.position - transform_a.position;
126 if d.norm_squared() < TOLERANCE {
127 Vec3::new(1.0, 0.0, 0.0)
128 } else {
129 d
130 }
131 };
132 let mut simplex = Simplex::new();
133 if let Some(ref ps) = self.prev_simplex {
134 for sp in &ps.points {
135 if sp.point.dot(&direction) > 0.0 {
136 simplex.push(*sp);
137 }
138 }
139 }
140 if simplex.is_empty() {
141 let first = support(shape_a, transform_a, shape_b, transform_b, &direction);
142 simplex.push(first);
143 direction = -first.point;
144 } else if let Some(new_dir) = do_simplex(&mut simplex) {
145 direction = new_dir;
146 }
147 let mut iters = 0usize;
148 let result = loop {
149 iters += 1;
150 if iters > MAX_ITERATIONS {
151 break GjkResult::Intersecting(simplex.clone());
152 }
153 let new_pt = support(shape_a, transform_a, shape_b, transform_b, &direction);
154 if new_pt.point.dot(&direction) < -TOLERANCE {
155 let (closest, bary) = simplex.closest_point_to_origin();
156 let dist = closest.norm();
157 let (pa, pb) = barycentric_witnesses(&simplex, &bary);
158 break GjkResult::Separated {
159 point_a: pa,
160 point_b: pb,
161 distance: dist,
162 };
163 }
164 simplex.push(new_pt);
165 if let Some(new_dir) = do_simplex(&mut simplex) {
166 direction = new_dir;
167 } else {
168 break GjkResult::Intersecting(simplex.clone());
169 }
170 };
171 self.prev_direction = Some(direction);
172 match &result {
173 GjkResult::Intersecting(s) => {
174 self.prev_simplex = Some(s.clone());
175 }
176 GjkResult::Separated {
177 point_b, point_a, ..
178 } => {
179 let d = *point_b - *point_a;
180 if d.norm_squared() > TOLERANCE {
181 self.prev_direction = Some(d.normalize());
182 }
183 self.prev_simplex = None;
184 }
185 }
186 self.query_count += 1;
187 let n = self.query_count as f64;
188 self.avg_iterations = self.avg_iterations * (n - 1.0) / n + iters as f64 / n;
189 result
190 }
191}
192pub struct GjkSolver {
198 pub(super) cached_simplex: Option<Simplex>,
200 pub(super) last_direction: Option<Vec3>,
202}
203impl GjkSolver {
204 pub fn new() -> Self {
206 Self {
207 cached_simplex: None,
208 last_direction: None,
209 }
210 }
211 pub fn reset(&mut self) {
213 self.cached_simplex = None;
214 self.last_direction = None;
215 }
216 pub fn compute_signed_distance(
225 &mut self,
226 shape_a: &dyn Shape,
227 transform_a: &Transform,
228 shape_b: &dyn Shape,
229 transform_b: &Transform,
230 ) -> f64 {
231 match Gjk::query(shape_a, transform_a, shape_b, transform_b) {
232 GjkResult::Separated {
233 distance,
234 point_a,
235 point_b,
236 } => {
237 let dir = point_b - point_a;
238 self.last_direction = Some(if dir.norm_squared() > 1e-14 {
239 dir.normalize()
240 } else {
241 Vec3::new(1.0, 0.0, 0.0)
242 });
243 self.cached_simplex = None;
244 distance
245 }
246 GjkResult::Intersecting(simplex) => {
247 let depth = simplex
248 .points
249 .iter()
250 .map(|p| p.point.norm())
251 .fold(f64::MAX, f64::min);
252 self.cached_simplex = Some(simplex);
253 self.last_direction = None;
254 -(depth.max(0.0))
255 }
256 }
257 }
258 pub fn compute_witness_points(
266 &mut self,
267 shape_a: &dyn Shape,
268 transform_a: &Transform,
269 shape_b: &dyn Shape,
270 transform_b: &Transform,
271 ) -> Option<(Vec3, Vec3)> {
272 match Gjk::query(shape_a, transform_a, shape_b, transform_b) {
273 GjkResult::Separated {
274 point_a,
275 point_b,
276 distance,
277 } => {
278 let _ = distance;
279 self.cached_simplex = None;
280 Some((point_a, point_b))
281 }
282 GjkResult::Intersecting(simplex) => {
283 self.cached_simplex = Some(simplex);
284 None
285 }
286 }
287 }
288 pub fn warm_start(&mut self, previous_direction: Vec3) {
297 if previous_direction.norm_squared() > 1e-14 {
298 self.last_direction = Some(previous_direction.normalize());
299 }
300 }
301 pub fn warm_direction(&self) -> Option<Vec3> {
305 self.last_direction
306 }
307 pub fn has_cached_simplex(&self) -> bool {
309 self.cached_simplex.is_some()
310 }
311 pub fn take_simplex(&mut self) -> Option<Simplex> {
315 self.cached_simplex.take()
316 }
317}
318#[derive(Debug, Clone)]
324pub struct SpeculativeContact {
325 pub point_a: Vec3,
327 pub point_b: Vec3,
329 pub normal: Vec3,
331 pub gap: f64,
333}
334pub struct VoronoiSimplexSolver {
340 pub(super) points: Vec<Vec3>,
342 pub(super) bary: Vec<f64>,
344 pub(super) active: Vec<bool>,
346}
347impl Default for VoronoiSimplexSolver {
348 fn default() -> Self {
349 Self::new()
350 }
351}
352impl VoronoiSimplexSolver {
353 pub fn new() -> Self {
355 Self {
356 points: Vec::with_capacity(4),
357 bary: Vec::with_capacity(4),
358 active: Vec::with_capacity(4),
359 }
360 }
361 pub fn reset(&mut self) {
363 self.points.clear();
364 self.bary.clear();
365 self.active.clear();
366 }
367 pub fn add_point(&mut self, p: Vec3) {
369 self.points.push(p);
370 self.bary.push(0.0);
371 self.active.push(true);
372 }
373 pub fn num_points(&self) -> usize {
375 self.points.len()
376 }
377 pub fn solve(&mut self) -> (Vec3, ClosestFeature) {
385 match self.points.len() {
386 1 => {
387 self.bary = vec![1.0];
388 (self.points[0], ClosestFeature::Vertex(0))
389 }
390 2 => self.solve_line(),
391 3 => self.solve_triangle(),
392 4 => self.solve_tetrahedron(),
393 _ => (Vec3::zeros(), ClosestFeature::Vertex(0)),
394 }
395 }
396 fn solve_line(&mut self) -> (Vec3, ClosestFeature) {
397 let a = &self.points[0];
398 let b = &self.points[1];
399 let ab = b - a;
400 let ao = -a;
401 let t = ao.dot(&ab) / ab.dot(&ab);
402 if t <= 0.0 {
403 self.bary = vec![1.0, 0.0];
404 (*a, ClosestFeature::Vertex(0))
405 } else if t >= 1.0 {
406 self.bary = vec![0.0, 1.0];
407 (*b, ClosestFeature::Vertex(1))
408 } else {
409 self.bary = vec![1.0 - t, t];
410 (a + ab * t, ClosestFeature::Edge(0, 1))
411 }
412 }
413 fn solve_triangle(&mut self) -> (Vec3, ClosestFeature) {
414 let (pt, bary) = closest_point_triangle(&self.points[0], &self.points[1], &self.points[2]);
415 self.bary = bary.clone();
416 let num_nonzero = bary.iter().filter(|&&b| b.abs() > 1e-10).count();
417 let feature = match num_nonzero {
418 1 => {
419 let idx = bary.iter().position(|&b| b.abs() > 1e-10).unwrap_or(0);
420 ClosestFeature::Vertex(idx)
421 }
422 2 => {
423 let indices: Vec<usize> = bary
424 .iter()
425 .enumerate()
426 .filter(|&(_, &b)| b.abs() > 1e-10)
427 .map(|(i, _)| i)
428 .collect();
429 ClosestFeature::Edge(indices[0], indices[1])
430 }
431 _ => ClosestFeature::Face(0, 1, 2),
432 };
433 (pt, feature)
434 }
435 fn solve_tetrahedron(&mut self) -> (Vec3, ClosestFeature) {
436 let a = self.points[0];
437 let b = self.points[1];
438 let c = self.points[2];
439 let d = self.points[3];
440 let ab = b - a;
441 let ac = c - a;
442 let ad = d - a;
443 let det = ab.dot(&ac.cross(&ad));
444 if det.abs() < 1e-14 {
445 return self.solve_triangle();
446 }
447 let ao = -a;
448 let abc = ab.cross(&ac);
449 let acd = ac.cross(&ad);
450 let adb = ad.cross(&ab);
451 let inside_abc = abc.dot(&ao) * abc.dot(&(d - a)) >= 0.0;
452 let inside_acd = acd.dot(&ao) * acd.dot(&(b - a)) >= 0.0;
453 let inside_adb = adb.dot(&ao) * adb.dot(&(c - a)) >= 0.0;
454 let bc = c - b;
455 let bd = d - b;
456 let bo = -b;
457 let bcd = bc.cross(&bd);
458 let inside_bcd = bcd.dot(&bo) * bcd.dot(&(a - b)) >= 0.0;
459 if inside_abc && inside_acd && inside_adb && inside_bcd {
460 self.bary = vec![0.25; 4];
461 return (Vec3::zeros(), ClosestFeature::Interior);
462 }
463 let mut best_dist = f64::MAX;
464 let mut best_point = Vec3::zeros();
465 let mut best_feature = ClosestFeature::Vertex(0);
466 let faces: [(usize, usize, usize); 4] = [(0, 1, 2), (0, 1, 3), (0, 2, 3), (1, 2, 3)];
467 for &(i, j, k) in &faces {
468 let (pt, _bary) =
469 closest_point_triangle(&self.points[i], &self.points[j], &self.points[k]);
470 let dist = pt.norm_squared();
471 if dist < best_dist {
472 best_dist = dist;
473 best_point = pt;
474 best_feature = ClosestFeature::Face(i, j, k);
475 }
476 }
477 (best_point, best_feature)
478 }
479}
480pub struct Gjk;
482impl Gjk {
483 pub fn intersect(
485 shape_a: &dyn Shape,
486 transform_a: &Transform,
487 shape_b: &dyn Shape,
488 transform_b: &Transform,
489 ) -> bool {
490 matches!(
491 Self::query(shape_a, transform_a, shape_b, transform_b),
492 GjkResult::Intersecting(_)
493 )
494 }
495 pub fn closest_points(
497 shape_a: &dyn Shape,
498 transform_a: &Transform,
499 shape_b: &dyn Shape,
500 transform_b: &Transform,
501 ) -> Option<(Vec3, Vec3)> {
502 match Self::query(shape_a, transform_a, shape_b, transform_b) {
503 GjkResult::Separated {
504 point_a, point_b, ..
505 } => Some((point_a, point_b)),
506 GjkResult::Intersecting(_) => None,
507 }
508 }
509 pub fn query(
511 shape_a: &dyn Shape,
512 transform_a: &Transform,
513 shape_b: &dyn Shape,
514 transform_b: &Transform,
515 ) -> GjkResult {
516 let mut simplex = Simplex::new();
517 let mut direction = transform_b.position - transform_a.position;
518 if direction.norm_squared() < TOLERANCE {
519 direction = Vec3::new(1.0, 0.0, 0.0);
520 }
521 let first = support(shape_a, transform_a, shape_b, transform_b, &direction);
522 simplex.push(first);
523 direction = -first.point;
524 for _ in 0..MAX_ITERATIONS {
525 let new_point = support(shape_a, transform_a, shape_b, transform_b, &direction);
526 if new_point.point.dot(&direction) < -TOLERANCE {
527 let (closest, bary) = simplex.closest_point_to_origin();
528 let _ = bary;
529 let dist = closest.norm();
530 let point_a = transform_a.position;
531 let point_b = transform_b.position;
532 return GjkResult::Separated {
533 point_a,
534 point_b,
535 distance: dist,
536 };
537 }
538 simplex.push(new_point);
539 if let Some(new_dir) = do_simplex(&mut simplex) {
540 direction = new_dir;
541 } else {
542 return GjkResult::Intersecting(simplex);
543 }
544 }
545 GjkResult::Intersecting(simplex)
546 }
547}
548impl Gjk {
549 pub fn intersect_scaled(
553 shape_a: &dyn Shape,
554 transform_a: &Transform,
555 scale_a: f64,
556 shape_b: &dyn Shape,
557 transform_b: &Transform,
558 scale_b: f64,
559 ) -> bool {
560 let mut simplex = Simplex::new();
561 let mut direction = transform_b.position - transform_a.position;
562 if direction.norm_squared() < TOLERANCE {
563 direction = Vec3::new(1.0, 0.0, 0.0);
564 }
565 let first = scaled_support_minkowski(
566 shape_a,
567 transform_a,
568 scale_a,
569 shape_b,
570 transform_b,
571 scale_b,
572 &direction,
573 );
574 simplex.push(first);
575 direction = -first.point;
576 for _ in 0..MAX_ITERATIONS {
577 let new_pt = scaled_support_minkowski(
578 shape_a,
579 transform_a,
580 scale_a,
581 shape_b,
582 transform_b,
583 scale_b,
584 &direction,
585 );
586 if new_pt.point.dot(&direction) < -TOLERANCE {
587 return false;
588 }
589 simplex.push(new_pt);
590 if do_simplex(&mut simplex).is_none() {
591 return true;
592 }
593 }
594 true
595 }
596}
597pub enum GjkResult {
599 Intersecting(Simplex),
601 Separated {
603 point_a: Vec3,
605 point_b: Vec3,
607 distance: Real,
609 },
610}
611#[derive(Debug, Clone)]
613pub struct GjkDebugTrace {
614 pub(super) iterations: Vec<GjkIterationRecord>,
615 pub(super) terminated_intersecting: bool,
616}
617impl GjkDebugTrace {
618 pub fn collect(
620 shape_a: &dyn Shape,
621 transform_a: &Transform,
622 shape_b: &dyn Shape,
623 transform_b: &Transform,
624 ) -> Self {
625 let mut simplex = Simplex::new();
626 let mut direction = transform_b.position - transform_a.position;
627 if direction.norm_squared() < TOLERANCE {
628 direction = Vec3::new(1.0, 0.0, 0.0);
629 }
630 let mut records = Vec::new();
631 let first = support(shape_a, transform_a, shape_b, transform_b, &direction);
632 simplex.push(first);
633 direction = -first.point;
634 let mut terminated_intersecting = false;
635 for _ in 0..MAX_ITERATIONS {
636 let new_point = support(shape_a, transform_a, shape_b, transform_b, &direction);
637 let dot = new_point.point.dot(&direction);
638 records.push(GjkIterationRecord {
639 simplex_size: simplex.len(),
640 direction,
641 new_support: new_point.point,
642 dot_positive: dot >= -TOLERANCE,
643 });
644 if dot < -TOLERANCE {
645 break;
646 }
647 simplex.push(new_point);
648 if let Some(new_dir) = do_simplex(&mut simplex) {
649 direction = new_dir;
650 } else {
651 terminated_intersecting = true;
652 break;
653 }
654 }
655 Self {
656 iterations: records,
657 terminated_intersecting,
658 }
659 }
660 pub fn n_iterations(&self) -> usize {
662 self.iterations.len()
663 }
664 pub fn terminated_as_intersecting(&self) -> bool {
666 self.terminated_intersecting
667 }
668 pub fn records(&self) -> &[GjkIterationRecord] {
670 &self.iterations
671 }
672 pub fn directions(&self) -> Vec<Vec3> {
674 self.iterations.iter().map(|r| r.direction).collect()
675 }
676 pub fn support_points(&self) -> Vec<Vec3> {
678 self.iterations.iter().map(|r| r.new_support).collect()
679 }
680 pub fn summary(&self) -> String {
682 let mut s = format!(
683 "GJK trace: {} iterations, intersecting={}\n",
684 self.iterations.len(),
685 self.terminated_intersecting
686 );
687 for (i, rec) in self.iterations.iter().enumerate() {
688 s.push_str(
689 &format!(
690 " iter {i}: simplex_size={}, dir=({:.3},{:.3},{:.3}), support=({:.3},{:.3},{:.3}), dot_ok={}\n",
691 rec.simplex_size, rec.direction.x, rec.direction.y, rec.direction.z,
692 rec.new_support.x, rec.new_support.y, rec.new_support.z, rec
693 .dot_positive,
694 ),
695 );
696 }
697 s
698 }
699}
700pub struct SupportCache {
702 pub(super) directions: Vec<Vec3>,
704 pub(super) results: Vec<SupportPoint>,
706 pub(super) max_size: usize,
708}
709impl SupportCache {
710 pub fn new(max_size: usize) -> Self {
712 Self {
713 directions: Vec::with_capacity(max_size),
714 results: Vec::with_capacity(max_size),
715 max_size,
716 }
717 }
718 pub fn lookup(&self, direction: &Vec3) -> Option<&SupportPoint> {
723 let dir_norm = direction.norm();
724 if dir_norm < 1e-14 {
725 return None;
726 }
727 let unit_dir = direction * (1.0 / dir_norm);
728 for (i, cached_dir) in self.directions.iter().enumerate() {
729 if unit_dir.dot(cached_dir) > 0.999 {
730 return Some(&self.results[i]);
731 }
732 }
733 None
734 }
735 pub fn insert(&mut self, direction: Vec3, result: SupportPoint) {
737 let dir_norm = direction.norm();
738 if dir_norm < 1e-14 {
739 return;
740 }
741 let unit_dir = direction * (1.0 / dir_norm);
742 if self.directions.len() >= self.max_size {
743 self.directions.remove(0);
744 self.results.remove(0);
745 }
746 self.directions.push(unit_dir);
747 self.results.push(result);
748 }
749 pub fn clear(&mut self) {
751 self.directions.clear();
752 self.results.clear();
753 }
754 pub fn len(&self) -> usize {
756 self.directions.len()
757 }
758 pub fn is_empty(&self) -> bool {
760 self.directions.is_empty()
761 }
762}
763#[derive(Debug, Clone, Copy, PartialEq)]
765pub enum ClosestFeature {
766 Vertex(usize),
768 Edge(usize, usize),
770 Face(usize, usize, usize),
772 Interior,
774}
775#[derive(Debug, Clone)]
777pub enum MprResult {
778 Intersecting {
780 normal: Vec3,
782 depth: f64,
784 point: Vec3,
786 },
787 Separated,
789}
790#[derive(Debug, Clone, Copy)]
792pub struct SupportPoint {
793 pub point: Vec3,
795 pub support_a: Vec3,
797 pub support_b: Vec3,
799}
800#[derive(Debug, Clone)]
802pub struct CcdResult {
803 pub toi: f64,
805 pub normal: Vec3,
807 pub point: Vec3,
809}
810#[derive(Debug, Clone)]
812pub struct GjkDistanceResult {
813 pub distance: f64,
815 pub closest_a: Vec3,
817 pub closest_b: Vec3,
819 pub iterations: usize,
821}