use na::{self, ComplexField, Unit};
use crate::query::gjk::{CSOPoint, ConstantOrigin, VoronoiSimplex};
use crate::shape::SupportMap;
use crate::math::{Isometry, Point, Real, Vector, DIM};
use crate::query::{self, Ray};
use num::{Bounded, Zero};
#[derive(Clone, Debug, PartialEq)]
pub enum GJKResult {
Intersection,
ClosestPoints(Point<Real>, Point<Real>, Unit<Vector<Real>>),
Proximity(Unit<Vector<Real>>),
NoIntersection(Unit<Vector<Real>>),
}
pub fn eps_tol() -> Real {
let _eps = crate::math::DEFAULT_EPSILON;
_eps * 10.0
}
pub fn project_origin<G: ?Sized>(
m: &Isometry<Real>,
g: &G,
simplex: &mut VoronoiSimplex,
) -> Option<Point<Real>>
where
G: SupportMap,
{
match closest_points(
&m.inverse(),
g,
&ConstantOrigin,
Real::max_value(),
true,
simplex,
) {
GJKResult::Intersection => None,
GJKResult::ClosestPoints(p, _, _) => Some(p),
_ => unreachable!(),
}
}
pub fn closest_points<G1: ?Sized, G2: ?Sized>(
pos12: &Isometry<Real>,
g1: &G1,
g2: &G2,
max_dist: Real,
exact_dist: bool,
simplex: &mut VoronoiSimplex,
) -> GJKResult
where
G1: SupportMap,
G2: SupportMap,
{
let _eps = crate::math::DEFAULT_EPSILON;
let _eps_tol: Real = eps_tol();
let _eps_rel: Real = ComplexField::sqrt(_eps_tol);
let mut proj = simplex.project_origin_and_reduce();
let mut old_dir;
if let Some(proj_dir) = Unit::try_new(proj.coords, 0.0) {
old_dir = -proj_dir;
} else {
return GJKResult::Intersection;
}
let mut max_bound = Real::max_value();
let mut dir;
let mut niter = 0;
loop {
let old_max_bound = max_bound;
if let Some((new_dir, dist)) = Unit::try_new_and_get(-proj.coords, _eps_tol) {
dir = new_dir;
max_bound = dist;
} else {
return GJKResult::Intersection;
}
if max_bound >= old_max_bound {
if exact_dist {
let (p1, p2) = result(simplex, true);
return GJKResult::ClosestPoints(p1, p2, old_dir);
} else {
return GJKResult::Proximity(old_dir);
}
}
let cso_point = CSOPoint::from_shapes(pos12, g1, g2, &dir);
let min_bound = -dir.dot(&cso_point.point.coords);
assert!(min_bound == min_bound);
if min_bound > max_dist {
return GJKResult::NoIntersection(dir);
} else if !exact_dist && min_bound > 0.0 && max_bound <= max_dist {
return GJKResult::Proximity(old_dir);
} else if max_bound - min_bound <= _eps_rel * max_bound {
if exact_dist {
let (p1, p2) = result(simplex, false);
return GJKResult::ClosestPoints(p1, p2, dir);
} else {
return GJKResult::Proximity(dir);
}
}
if !simplex.add_point(cso_point) {
if exact_dist {
let (p1, p2) = result(simplex, false);
return GJKResult::ClosestPoints(p1, p2, dir);
} else {
return GJKResult::Proximity(dir);
}
}
old_dir = dir;
proj = simplex.project_origin_and_reduce();
if simplex.dimension() == DIM {
if min_bound >= _eps_tol {
if exact_dist {
let (p1, p2) = result(simplex, true);
return GJKResult::ClosestPoints(p1, p2, old_dir);
} else {
return GJKResult::Proximity(old_dir);
}
} else {
return GJKResult::Intersection;
}
}
niter += 1;
if niter == 10000 {
return GJKResult::NoIntersection(Vector::x_axis());
}
}
}
pub fn cast_local_ray<G: ?Sized>(
shape: &G,
simplex: &mut VoronoiSimplex,
ray: &Ray,
max_toi: Real,
) -> Option<(Real, Vector<Real>)>
where
G: SupportMap,
{
let g2 = ConstantOrigin;
minkowski_ray_cast(&Isometry::identity(), shape, &g2, ray, max_toi, simplex)
}
pub fn directional_distance<G1: ?Sized, G2: ?Sized>(
pos12: &Isometry<Real>,
g1: &G1,
g2: &G2,
dir: &Vector<Real>,
simplex: &mut VoronoiSimplex,
) -> Option<(Real, Vector<Real>, Point<Real>, Point<Real>)>
where
G1: SupportMap,
G2: SupportMap,
{
let ray = Ray::new(Point::origin(), *dir);
minkowski_ray_cast(pos12, g1, g2, &ray, Real::max_value(), simplex).map(|(toi, normal)| {
let witnesses = if !toi.is_zero() {
result(simplex, simplex.dimension() == DIM)
} else {
(Point::origin(), Point::origin())
};
(toi, normal, witnesses.0, witnesses.1)
})
}
fn minkowski_ray_cast<G1: ?Sized, G2: ?Sized>(
pos12: &Isometry<Real>,
g1: &G1,
g2: &G2,
ray: &Ray,
max_toi: Real,
simplex: &mut VoronoiSimplex,
) -> Option<(Real, Vector<Real>)>
where
G1: SupportMap,
G2: SupportMap,
{
let _eps = crate::math::DEFAULT_EPSILON;
let _eps_tol: Real = eps_tol();
let _eps_rel: Real = ComplexField::sqrt(_eps_tol);
let ray_length = ray.dir.norm();
if relative_eq!(ray_length, 0.0) {
return None;
}
let mut ltoi = 0.0;
let mut curr_ray = Ray::new(ray.origin, ray.dir / ray_length);
let dir = -curr_ray.dir;
let mut ldir = dir;
let support_point = CSOPoint::from_shapes(pos12, g1, g2, &dir);
simplex.reset(support_point.translate(&-curr_ray.origin.coords));
let mut proj = simplex.project_origin_and_reduce();
let mut max_bound = Real::max_value();
let mut dir;
let mut niter = 0;
let mut last_chance = false;
loop {
let old_max_bound = max_bound;
if let Some((new_dir, dist)) = Unit::try_new_and_get(-proj.coords, _eps_tol) {
dir = new_dir;
max_bound = dist;
} else {
return Some((ltoi / ray_length, ldir));
}
let support_point = if max_bound >= old_max_bound {
last_chance = true;
CSOPoint::single_point(proj + curr_ray.origin.coords)
} else {
CSOPoint::from_shapes(pos12, g1, g2, &dir)
};
if last_chance && ltoi > 0.0 {
return Some((ltoi / ray_length, ldir));
}
match query::details::ray_toi_with_halfspace(&support_point.point, &dir, &curr_ray) {
Some(t) => {
if dir.dot(&curr_ray.dir) < 0.0 && t > 0.0 {
ldir = *dir;
ltoi += t;
if ltoi / ray_length > max_toi {
return None;
}
let shift = curr_ray.dir * t;
curr_ray.origin += shift;
max_bound = Real::max_value();
simplex.modify_pnts(&|pt| pt.translate_mut(&-shift));
last_chance = false;
}
}
None => {
if dir.dot(&curr_ray.dir) > _eps_tol {
return None;
}
}
}
if last_chance {
return None;
}
let min_bound = -dir.dot(&(support_point.point.coords - curr_ray.origin.coords));
assert!(min_bound == min_bound);
if max_bound - min_bound <= _eps_rel * max_bound {
if cfg!(feature = "improved_fixed_point_support") {
return Some((ltoi / ray_length, ldir));
} else {
return None;
}
}
let _ = simplex.add_point(support_point.translate(&-curr_ray.origin.coords));
proj = simplex.project_origin_and_reduce();
if simplex.dimension() == DIM {
if min_bound >= _eps_tol {
return None;
} else {
return Some((ltoi / ray_length, ldir));
}
}
niter += 1;
if niter == 10000 {
return None;
}
}
}
fn result(simplex: &VoronoiSimplex, prev: bool) -> (Point<Real>, Point<Real>) {
let mut res = (Point::origin(), Point::origin());
if prev {
for i in 0..simplex.prev_dimension() + 1 {
let coord = simplex.prev_proj_coord(i);
let point = simplex.prev_point(i);
res.0 += point.orig1.coords * coord;
res.1 += point.orig2.coords * coord;
}
res
} else {
for i in 0..simplex.dimension() + 1 {
let coord = simplex.proj_coord(i);
let point = simplex.point(i);
res.0 += point.orig1.coords * coord;
res.1 += point.orig2.coords * coord;
}
res
}
}