use oxiphysics_core::Transform;
use oxiphysics_core::math::{Real, Vec3};
use oxiphysics_geometry::Shape;
use super::functions::*;
#[derive(Debug, Clone)]
pub struct GjkIterationRecord {
pub simplex_size: usize,
pub direction: Vec3,
pub new_support: Vec3,
pub dot_positive: bool,
}
#[derive(Debug, Clone)]
pub struct Simplex {
pub points: Vec<SupportPoint>,
}
impl Simplex {
pub fn new() -> Self {
Self {
points: Vec::with_capacity(4),
}
}
pub fn push(&mut self, point: SupportPoint) {
self.points.push(point);
}
pub fn len(&self) -> usize {
self.points.len()
}
pub fn is_empty(&self) -> bool {
self.points.is_empty()
}
pub fn closest_point_to_origin(&self) -> (Vec3, Vec<Real>) {
match self.points.len() {
1 => (self.points[0].point, vec![1.0]),
2 => closest_point_line(&self.points[0].point, &self.points[1].point),
3 => closest_point_triangle(
&self.points[0].point,
&self.points[1].point,
&self.points[2].point,
),
_ => (Vec3::zeros(), vec![0.25; 4]),
}
}
}
#[derive(Debug, Clone, Copy)]
pub struct RayCastResult {
pub t: f64,
pub point: Vec3,
pub normal: Vec3,
}
pub struct WarmStartGjk {
pub(super) prev_simplex: Option<Simplex>,
pub(super) prev_direction: Option<Vec3>,
pub(super) avg_iterations: f64,
pub(super) query_count: usize,
}
impl WarmStartGjk {
pub fn new() -> Self {
Self {
prev_simplex: None,
prev_direction: None,
avg_iterations: 0.0,
query_count: 0,
}
}
pub fn reset(&mut self) {
self.prev_simplex = None;
self.prev_direction = None;
}
pub fn avg_iterations(&self) -> f64 {
self.avg_iterations
}
pub fn query_count(&self) -> usize {
self.query_count
}
pub fn query(
&mut self,
shape_a: &dyn Shape,
transform_a: &Transform,
shape_b: &dyn Shape,
transform_b: &Transform,
) -> GjkResult {
let mut direction = if let Some(d) = self.prev_direction {
d
} else {
let d = transform_b.position - transform_a.position;
if d.norm_squared() < TOLERANCE {
Vec3::new(1.0, 0.0, 0.0)
} else {
d
}
};
let mut simplex = Simplex::new();
if let Some(ref ps) = self.prev_simplex {
for sp in &ps.points {
if sp.point.dot(&direction) > 0.0 {
simplex.push(*sp);
}
}
}
if simplex.is_empty() {
let first = support(shape_a, transform_a, shape_b, transform_b, &direction);
simplex.push(first);
direction = -first.point;
} else if let Some(new_dir) = do_simplex(&mut simplex) {
direction = new_dir;
}
let mut iters = 0usize;
let result = loop {
iters += 1;
if iters > MAX_ITERATIONS {
break GjkResult::Intersecting(simplex.clone());
}
let new_pt = support(shape_a, transform_a, shape_b, transform_b, &direction);
if new_pt.point.dot(&direction) < -TOLERANCE {
let (closest, bary) = simplex.closest_point_to_origin();
let dist = closest.norm();
let (pa, pb) = barycentric_witnesses(&simplex, &bary);
break GjkResult::Separated {
point_a: pa,
point_b: pb,
distance: dist,
};
}
simplex.push(new_pt);
if let Some(new_dir) = do_simplex(&mut simplex) {
direction = new_dir;
} else {
break GjkResult::Intersecting(simplex.clone());
}
};
self.prev_direction = Some(direction);
match &result {
GjkResult::Intersecting(s) => {
self.prev_simplex = Some(s.clone());
}
GjkResult::Separated {
point_b, point_a, ..
} => {
let d = *point_b - *point_a;
if d.norm_squared() > TOLERANCE {
self.prev_direction = Some(d.normalize());
}
self.prev_simplex = None;
}
}
self.query_count += 1;
let n = self.query_count as f64;
self.avg_iterations = self.avg_iterations * (n - 1.0) / n + iters as f64 / n;
result
}
}
pub struct GjkSolver {
pub(super) cached_simplex: Option<Simplex>,
pub(super) last_direction: Option<Vec3>,
}
impl GjkSolver {
pub fn new() -> Self {
Self {
cached_simplex: None,
last_direction: None,
}
}
pub fn reset(&mut self) {
self.cached_simplex = None;
self.last_direction = None;
}
pub fn compute_signed_distance(
&mut self,
shape_a: &dyn Shape,
transform_a: &Transform,
shape_b: &dyn Shape,
transform_b: &Transform,
) -> f64 {
match Gjk::query(shape_a, transform_a, shape_b, transform_b) {
GjkResult::Separated {
distance,
point_a,
point_b,
} => {
let dir = point_b - point_a;
self.last_direction = Some(if dir.norm_squared() > 1e-14 {
dir.normalize()
} else {
Vec3::new(1.0, 0.0, 0.0)
});
self.cached_simplex = None;
distance
}
GjkResult::Intersecting(simplex) => {
let depth = simplex
.points
.iter()
.map(|p| p.point.norm())
.fold(f64::MAX, f64::min);
self.cached_simplex = Some(simplex);
self.last_direction = None;
-(depth.max(0.0))
}
}
}
pub fn compute_witness_points(
&mut self,
shape_a: &dyn Shape,
transform_a: &Transform,
shape_b: &dyn Shape,
transform_b: &Transform,
) -> Option<(Vec3, Vec3)> {
match Gjk::query(shape_a, transform_a, shape_b, transform_b) {
GjkResult::Separated {
point_a,
point_b,
distance,
} => {
let _ = distance;
self.cached_simplex = None;
Some((point_a, point_b))
}
GjkResult::Intersecting(simplex) => {
self.cached_simplex = Some(simplex);
None
}
}
}
pub fn warm_start(&mut self, previous_direction: Vec3) {
if previous_direction.norm_squared() > 1e-14 {
self.last_direction = Some(previous_direction.normalize());
}
}
pub fn warm_direction(&self) -> Option<Vec3> {
self.last_direction
}
pub fn has_cached_simplex(&self) -> bool {
self.cached_simplex.is_some()
}
pub fn take_simplex(&mut self) -> Option<Simplex> {
self.cached_simplex.take()
}
}
#[derive(Debug, Clone)]
pub struct SpeculativeContact {
pub point_a: Vec3,
pub point_b: Vec3,
pub normal: Vec3,
pub gap: f64,
}
pub struct VoronoiSimplexSolver {
pub(super) points: Vec<Vec3>,
pub(super) bary: Vec<f64>,
pub(super) active: Vec<bool>,
}
impl Default for VoronoiSimplexSolver {
fn default() -> Self {
Self::new()
}
}
impl VoronoiSimplexSolver {
pub fn new() -> Self {
Self {
points: Vec::with_capacity(4),
bary: Vec::with_capacity(4),
active: Vec::with_capacity(4),
}
}
pub fn reset(&mut self) {
self.points.clear();
self.bary.clear();
self.active.clear();
}
pub fn add_point(&mut self, p: Vec3) {
self.points.push(p);
self.bary.push(0.0);
self.active.push(true);
}
pub fn num_points(&self) -> usize {
self.points.len()
}
pub fn solve(&mut self) -> (Vec3, ClosestFeature) {
match self.points.len() {
1 => {
self.bary = vec![1.0];
(self.points[0], ClosestFeature::Vertex(0))
}
2 => self.solve_line(),
3 => self.solve_triangle(),
4 => self.solve_tetrahedron(),
_ => (Vec3::zeros(), ClosestFeature::Vertex(0)),
}
}
fn solve_line(&mut self) -> (Vec3, ClosestFeature) {
let a = &self.points[0];
let b = &self.points[1];
let ab = b - a;
let ao = -a;
let t = ao.dot(&ab) / ab.dot(&ab);
if t <= 0.0 {
self.bary = vec![1.0, 0.0];
(*a, ClosestFeature::Vertex(0))
} else if t >= 1.0 {
self.bary = vec![0.0, 1.0];
(*b, ClosestFeature::Vertex(1))
} else {
self.bary = vec![1.0 - t, t];
(a + ab * t, ClosestFeature::Edge(0, 1))
}
}
fn solve_triangle(&mut self) -> (Vec3, ClosestFeature) {
let (pt, bary) = closest_point_triangle(&self.points[0], &self.points[1], &self.points[2]);
self.bary = bary.clone();
let num_nonzero = bary.iter().filter(|&&b| b.abs() > 1e-10).count();
let feature = match num_nonzero {
1 => {
let idx = bary.iter().position(|&b| b.abs() > 1e-10).unwrap_or(0);
ClosestFeature::Vertex(idx)
}
2 => {
let indices: Vec<usize> = bary
.iter()
.enumerate()
.filter(|&(_, &b)| b.abs() > 1e-10)
.map(|(i, _)| i)
.collect();
ClosestFeature::Edge(indices[0], indices[1])
}
_ => ClosestFeature::Face(0, 1, 2),
};
(pt, feature)
}
fn solve_tetrahedron(&mut self) -> (Vec3, ClosestFeature) {
let a = self.points[0];
let b = self.points[1];
let c = self.points[2];
let d = self.points[3];
let ab = b - a;
let ac = c - a;
let ad = d - a;
let det = ab.dot(&ac.cross(&ad));
if det.abs() < 1e-14 {
return self.solve_triangle();
}
let ao = -a;
let abc = ab.cross(&ac);
let acd = ac.cross(&ad);
let adb = ad.cross(&ab);
let inside_abc = abc.dot(&ao) * abc.dot(&(d - a)) >= 0.0;
let inside_acd = acd.dot(&ao) * acd.dot(&(b - a)) >= 0.0;
let inside_adb = adb.dot(&ao) * adb.dot(&(c - a)) >= 0.0;
let bc = c - b;
let bd = d - b;
let bo = -b;
let bcd = bc.cross(&bd);
let inside_bcd = bcd.dot(&bo) * bcd.dot(&(a - b)) >= 0.0;
if inside_abc && inside_acd && inside_adb && inside_bcd {
self.bary = vec![0.25; 4];
return (Vec3::zeros(), ClosestFeature::Interior);
}
let mut best_dist = f64::MAX;
let mut best_point = Vec3::zeros();
let mut best_feature = ClosestFeature::Vertex(0);
let faces: [(usize, usize, usize); 4] = [(0, 1, 2), (0, 1, 3), (0, 2, 3), (1, 2, 3)];
for &(i, j, k) in &faces {
let (pt, _bary) =
closest_point_triangle(&self.points[i], &self.points[j], &self.points[k]);
let dist = pt.norm_squared();
if dist < best_dist {
best_dist = dist;
best_point = pt;
best_feature = ClosestFeature::Face(i, j, k);
}
}
(best_point, best_feature)
}
}
pub struct Gjk;
impl Gjk {
pub fn intersect(
shape_a: &dyn Shape,
transform_a: &Transform,
shape_b: &dyn Shape,
transform_b: &Transform,
) -> bool {
matches!(
Self::query(shape_a, transform_a, shape_b, transform_b),
GjkResult::Intersecting(_)
)
}
pub fn closest_points(
shape_a: &dyn Shape,
transform_a: &Transform,
shape_b: &dyn Shape,
transform_b: &Transform,
) -> Option<(Vec3, Vec3)> {
match Self::query(shape_a, transform_a, shape_b, transform_b) {
GjkResult::Separated {
point_a, point_b, ..
} => Some((point_a, point_b)),
GjkResult::Intersecting(_) => None,
}
}
pub fn query(
shape_a: &dyn Shape,
transform_a: &Transform,
shape_b: &dyn Shape,
transform_b: &Transform,
) -> GjkResult {
let mut simplex = Simplex::new();
let mut direction = transform_b.position - transform_a.position;
if direction.norm_squared() < TOLERANCE {
direction = Vec3::new(1.0, 0.0, 0.0);
}
let first = support(shape_a, transform_a, shape_b, transform_b, &direction);
simplex.push(first);
direction = -first.point;
for _ in 0..MAX_ITERATIONS {
let new_point = support(shape_a, transform_a, shape_b, transform_b, &direction);
if new_point.point.dot(&direction) < -TOLERANCE {
let (closest, bary) = simplex.closest_point_to_origin();
let _ = bary;
let dist = closest.norm();
let point_a = transform_a.position;
let point_b = transform_b.position;
return GjkResult::Separated {
point_a,
point_b,
distance: dist,
};
}
simplex.push(new_point);
if let Some(new_dir) = do_simplex(&mut simplex) {
direction = new_dir;
} else {
return GjkResult::Intersecting(simplex);
}
}
GjkResult::Intersecting(simplex)
}
}
impl Gjk {
pub fn intersect_scaled(
shape_a: &dyn Shape,
transform_a: &Transform,
scale_a: f64,
shape_b: &dyn Shape,
transform_b: &Transform,
scale_b: f64,
) -> bool {
let mut simplex = Simplex::new();
let mut direction = transform_b.position - transform_a.position;
if direction.norm_squared() < TOLERANCE {
direction = Vec3::new(1.0, 0.0, 0.0);
}
let first = scaled_support_minkowski(
shape_a,
transform_a,
scale_a,
shape_b,
transform_b,
scale_b,
&direction,
);
simplex.push(first);
direction = -first.point;
for _ in 0..MAX_ITERATIONS {
let new_pt = scaled_support_minkowski(
shape_a,
transform_a,
scale_a,
shape_b,
transform_b,
scale_b,
&direction,
);
if new_pt.point.dot(&direction) < -TOLERANCE {
return false;
}
simplex.push(new_pt);
if do_simplex(&mut simplex).is_none() {
return true;
}
}
true
}
}
pub enum GjkResult {
Intersecting(Simplex),
Separated {
point_a: Vec3,
point_b: Vec3,
distance: Real,
},
}
#[derive(Debug, Clone)]
pub struct GjkDebugTrace {
pub(super) iterations: Vec<GjkIterationRecord>,
pub(super) terminated_intersecting: bool,
}
impl GjkDebugTrace {
pub fn collect(
shape_a: &dyn Shape,
transform_a: &Transform,
shape_b: &dyn Shape,
transform_b: &Transform,
) -> Self {
let mut simplex = Simplex::new();
let mut direction = transform_b.position - transform_a.position;
if direction.norm_squared() < TOLERANCE {
direction = Vec3::new(1.0, 0.0, 0.0);
}
let mut records = Vec::new();
let first = support(shape_a, transform_a, shape_b, transform_b, &direction);
simplex.push(first);
direction = -first.point;
let mut terminated_intersecting = false;
for _ in 0..MAX_ITERATIONS {
let new_point = support(shape_a, transform_a, shape_b, transform_b, &direction);
let dot = new_point.point.dot(&direction);
records.push(GjkIterationRecord {
simplex_size: simplex.len(),
direction,
new_support: new_point.point,
dot_positive: dot >= -TOLERANCE,
});
if dot < -TOLERANCE {
break;
}
simplex.push(new_point);
if let Some(new_dir) = do_simplex(&mut simplex) {
direction = new_dir;
} else {
terminated_intersecting = true;
break;
}
}
Self {
iterations: records,
terminated_intersecting,
}
}
pub fn n_iterations(&self) -> usize {
self.iterations.len()
}
pub fn terminated_as_intersecting(&self) -> bool {
self.terminated_intersecting
}
pub fn records(&self) -> &[GjkIterationRecord] {
&self.iterations
}
pub fn directions(&self) -> Vec<Vec3> {
self.iterations.iter().map(|r| r.direction).collect()
}
pub fn support_points(&self) -> Vec<Vec3> {
self.iterations.iter().map(|r| r.new_support).collect()
}
pub fn summary(&self) -> String {
let mut s = format!(
"GJK trace: {} iterations, intersecting={}\n",
self.iterations.len(),
self.terminated_intersecting
);
for (i, rec) in self.iterations.iter().enumerate() {
s.push_str(
&format!(
" iter {i}: simplex_size={}, dir=({:.3},{:.3},{:.3}), support=({:.3},{:.3},{:.3}), dot_ok={}\n",
rec.simplex_size, rec.direction.x, rec.direction.y, rec.direction.z,
rec.new_support.x, rec.new_support.y, rec.new_support.z, rec
.dot_positive,
),
);
}
s
}
}
pub struct SupportCache {
pub(super) directions: Vec<Vec3>,
pub(super) results: Vec<SupportPoint>,
pub(super) max_size: usize,
}
impl SupportCache {
pub fn new(max_size: usize) -> Self {
Self {
directions: Vec::with_capacity(max_size),
results: Vec::with_capacity(max_size),
max_size,
}
}
pub fn lookup(&self, direction: &Vec3) -> Option<&SupportPoint> {
let dir_norm = direction.norm();
if dir_norm < 1e-14 {
return None;
}
let unit_dir = direction * (1.0 / dir_norm);
for (i, cached_dir) in self.directions.iter().enumerate() {
if unit_dir.dot(cached_dir) > 0.999 {
return Some(&self.results[i]);
}
}
None
}
pub fn insert(&mut self, direction: Vec3, result: SupportPoint) {
let dir_norm = direction.norm();
if dir_norm < 1e-14 {
return;
}
let unit_dir = direction * (1.0 / dir_norm);
if self.directions.len() >= self.max_size {
self.directions.remove(0);
self.results.remove(0);
}
self.directions.push(unit_dir);
self.results.push(result);
}
pub fn clear(&mut self) {
self.directions.clear();
self.results.clear();
}
pub fn len(&self) -> usize {
self.directions.len()
}
pub fn is_empty(&self) -> bool {
self.directions.is_empty()
}
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum ClosestFeature {
Vertex(usize),
Edge(usize, usize),
Face(usize, usize, usize),
Interior,
}
#[derive(Debug, Clone)]
pub enum MprResult {
Intersecting {
normal: Vec3,
depth: f64,
point: Vec3,
},
Separated,
}
#[derive(Debug, Clone, Copy)]
pub struct SupportPoint {
pub point: Vec3,
pub support_a: Vec3,
pub support_b: Vec3,
}
#[derive(Debug, Clone)]
pub struct CcdResult {
pub toi: f64,
pub normal: Vec3,
pub point: Vec3,
}
#[derive(Debug, Clone)]
pub struct GjkDistanceResult {
pub distance: f64,
pub closest_a: Vec3,
pub closest_b: Vec3,
pub iterations: usize,
}