use std::f32;
use std::fmt;
use std::mem;
use std::collections::HashMap;
use cgmath::prelude::*;
use cgmath::{EuclideanSpace, InnerSpace, Point3, Vector3};
use crate::geom::*;
use crate::collision::*;
use crate::pool::*;
pub struct Simplex<Point = Point3<f32>>
where
Point: Into<Point3<f32>> + From<Point3<f32>> + Copy + Clone + 'static
{
points: [Point; 4],
state: &'static SimplexState<Point>,
}
impl<Point> fmt::Debug for Simplex<Point>
where
Point: Into<Point3<f32>> + From<Point3<f32>> + Copy + Clone + 'static + fmt::Debug
{
fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
#[derive(Debug)]
enum StateName {
Vertex,
Edge,
Face,
Volume
};
let (state, points) = if self.state as *const _ == &VERTEX_DATAPTRLOC as *const _ {
(StateName::Vertex, &self.points[..1])
} else if self.state as *const _ == &EDGE_DATAPTRLOC as * const _ {
(StateName::Edge, &self.points[..2])
} else if self.state as * const _ == &FACE_DATAPTRLOC as * const _ {
(StateName::Face, &self.points[..3])
} else {
(StateName::Volume, &self.points[..4])
};
write!(f, "Simplex {{ state: {:?}, points: {:?} }}", state, points)
}
}
impl<Point> From<[Point; 1]> for Simplex<Point>
where
Point: Into<Point3<f32>> + From<Point3<f32>> + Copy + Clone + 'static
{
fn from(p: [Point; 1]) -> Self {
Self::from(p[0])
}
}
impl<Point> From<Point> for Simplex<Point>
where
Point: Into<Point3<f32>> + From<Point3<f32>> + Copy + Clone + 'static
{
fn from(p: Point) -> Self {
Simplex {
points: [
p,
Point::from(Point3::new(0.0, 0.0, 0.0)),
Point::from(Point3::new(0.0, 0.0, 0.0)),
Point::from(Point3::new(0.0, 0.0, 0.0)),
],
state: &VERTEX_DATAPTRLOC,
}
}
}
impl<Point> From<[Point; 2]> for Simplex<Point>
where
Point: Into<Point3<f32>> + From<Point3<f32>> + Copy + Clone + 'static
{
fn from(p: [Point; 2]) -> Self {
Self::from((p[0], p[1]))
}
}
impl<Point> From<(Point, Point)> for Simplex<Point>
where
Point: Into<Point3<f32>> + From<Point3<f32>> + Copy + Clone + 'static
{
fn from(p: (Point, Point)) -> Self {
Simplex {
points: [
p.0,
p.1,
Point::from(Point3::new(0.0, 0.0, 0.0)),
Point::from(Point3::new(0.0, 0.0, 0.0)),
],
state: &EDGE_DATAPTRLOC,
}
}
}
impl<Point> From<[Point; 3]> for Simplex<Point>
where
Point: Into<Point3<f32>> + From<Point3<f32>> + Copy + Clone + 'static
{
fn from(p: [Point; 3]) -> Self {
Self::from((p[0], p[1], p[2]))
}
}
impl<Point> From<(Point, Point, Point)> for Simplex<Point>
where
Point: Into<Point3<f32>> + From<Point3<f32>> + Copy + Clone + 'static
{
fn from(p: (Point, Point, Point)) -> Self {
Simplex {
points: [
p.0,
p.1,
p.2,
Point::from(Point3::new(0.0, 0.0, 0.0)),
],
state: &FACE_DATAPTRLOC,
}
}
}
impl<Point> From<[Point; 4]> for Simplex<Point>
where
Point: Into<Point3<f32>> + From<Point3<f32>> + Copy + Clone + 'static
{
fn from(p: [Point; 4]) -> Self {
Self::from((p[0], p[1], p[2], p[3]))
}
}
impl<Point> From<(Point, Point, Point, Point)> for Simplex<Point>
where
Point: Into<Point3<f32>> + From<Point3<f32>> + Copy + Clone + 'static
{
fn from(p: (Point, Point, Point, Point)) -> Self {
Simplex {
points: [
p.0,
p.1,
p.2,
p.3,
],
state: &VOLUME_DATAPTRLOC,
}
}
}
impl<Point> Simplex<Point>
where
Point: Into<Point3<f32>> + From<Point3<f32>> + Copy + Clone + 'static + fmt::Debug
{
pub fn closest_point_to_origin<S>(&mut self, shape: &S) -> Point3<f32>
where
S: Convex<Point>
{
let mut prev_norm = Vector3::zero();
loop {
let (min_norm, next_state) = self.state.min_norm(&mut self.points);
if min_norm.magnitude2() < COLLISION_EPSILON {
for i in self.state.len()..4 {
let min_norm = -Vector3::new(prev_norm.z, prev_norm.x, prev_norm.y);
let support = shape.support(-min_norm.normalize());
prev_norm = -min_norm.normalize();
self.points[i] = support;
}
self.state = &VOLUME_DATAPTRLOC;
return Point3::new(0.0, 0.0, 0.0);
}
let support = shape.support(-min_norm.normalize());
let support_v = support.into().to_vec();
prev_norm = min_norm;
if min_norm.magnitude2() >= support_v.magnitude2() {
return Point3::from_vec(min_norm);
}
self.state = next_state;
self.state.add_point(&mut self.points, support);
}
}
}
trait SimplexState<Point>
where
Point: Into<Point3<f32>> + Copy + Clone + 'static
{
fn min_norm(&self, _: &mut [Point; 4]) -> (Vector3<f32>, &'static SimplexState<Point>);
fn add_point(&self, _: &mut [Point; 4], _: Point);
fn len(&self) -> usize;
}
struct VertexSimplex{}
struct EdgeSimplex{}
struct FaceSimplex{}
struct VolumeSimplex{}
static VERTEX_DATAPTRLOC: VertexSimplex = VertexSimplex{};
static EDGE_DATAPTRLOC: EdgeSimplex = EdgeSimplex{};
static FACE_DATAPTRLOC: FaceSimplex = FaceSimplex{};
static VOLUME_DATAPTRLOC: VolumeSimplex = VolumeSimplex{};
impl<Point> SimplexState<Point> for VertexSimplex
where
Point: Into<Point3<f32>> + Copy + Clone + 'static
{
fn min_norm(&self, simp: &mut [Point; 4]) -> (Vector3<f32>, &'static SimplexState<Point>) {
(simp[0].into().to_vec(), &EDGE_DATAPTRLOC)
}
fn add_point(&self, simp: &mut [Point; 4], p: Point) {
simp[0] = p
}
fn len(&self) -> usize { 1 }
}
impl<Point> SimplexState<Point> for EdgeSimplex
where
Point: Into<Point3<f32>> + Copy + Clone + 'static
{
fn min_norm(&self, simp: &mut [Point; 4]) -> (Vector3<f32>, &'static SimplexState<Point>) {
let ab = simp[1].into() - simp[0].into();
let t = ab.dot(-simp[0].into().to_vec());
if t <= 0.0 {
(simp[0].into().to_vec(), &EDGE_DATAPTRLOC)
} else {
let denom = ab.dot(ab);
if t >= denom {
simp[0] = simp[1];
(simp[1].into().to_vec(), &EDGE_DATAPTRLOC)
} else {
(simp[0].into().to_vec() + ab * (t / denom), &FACE_DATAPTRLOC)
}
}
}
fn add_point(&self, simp: &mut [Point; 4], p: Point) {
simp[1] = p
}
fn len(&self) -> usize { 2 }
}
impl<Point> SimplexState<Point> for FaceSimplex
where
Point: Into<Point3<f32>> + Copy + Clone + 'static
{
fn min_norm(&self, simp: &mut [Point; 4]) -> (Vector3<f32>, &'static SimplexState<Point>) {
let (a, b, c): (Point3<f32>, Point3<f32>, Point3<f32>) = (
simp[0].into(), simp[1].into(), simp[2].into()
);
let ab = b - a;
let ac = c - a;
let ap = -a.to_vec();
let d1 = ab.dot(ap);
let d2 = ac.dot(ap);
if d1 <= 0.0 && d2 <= 0.0 {
return (simp[0].into().to_vec(), &EDGE_DATAPTRLOC);
}
let bp = -b.to_vec();
let d3 = ab.dot(bp);
let d4 = ac.dot(bp);
if d3 >= 0.0 && d4 <= d3 {
simp[0] = simp[1];
return (simp[1].into().to_vec(), &EDGE_DATAPTRLOC);
}
let vc = d1 * d4 - d3 * d2;
if vc <= 0.0 && d1 >= 0.0 && d3 <= 0.0 {
let v = d1 / (d1 - d3);
return ((simp[0].into().to_vec() + ab * v), &FACE_DATAPTRLOC);
}
let cp = -c.to_vec();
let d5 = ab.dot(cp);
let d6 = ac.dot(cp);
if d6 >= 0.0 && d5 <= d6 {
simp[0] = simp[2];
return (simp[2].into().to_vec(), &EDGE_DATAPTRLOC);
}
let vb = d5 * d2 - d1 * d6;
if vb <= 0.0 && d2 >= 0.0 && d6 <= 0.0 {
let w = d2 / (d2 - d6);
simp[1] = simp[2];
return ((simp[0].into().to_vec() + ac * w), &FACE_DATAPTRLOC);
}
let va = d3 * d6 - d5 * d4;
if va <= 0.0 && (d4 - d3) >= 0.0 && (d5 - d6) >= 0.0 {
let w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
simp[0] = simp[2];
return ((simp[1].into().to_vec() + (simp[2].into() - simp[1].into()) * w), &FACE_DATAPTRLOC);
}
let denom = 1.0 / (va + vb + vc);
let v = vb * denom;
let w = vc * denom;
((simp[0].into().to_vec() + ab * v + ac * w), &VOLUME_DATAPTRLOC)
}
fn add_point(&self, simp: &mut [Point; 4], p: Point) {
simp[2] = p;
}
fn len(&self) -> usize { 3 }
}
fn origin_outside_plane(
a: Vector3<f32>, b: Vector3<f32>, c: Vector3<f32>, d: Vector3<f32>
) -> bool {
let ab_x_ac = (b - a).cross(c - a);
let sign_p = (-a).dot(ab_x_ac);
let sign_d = (d - a).dot(ab_x_ac);
sign_p * sign_d < 0.0
}
impl<Point> SimplexState<Point> for VolumeSimplex
where
Point: Into<Point3<f32>> + Copy + Clone + 'static
{
fn min_norm(&self, simp: &mut [Point; 4]) -> (Vector3<f32>, &'static SimplexState<Point>) {
let mut closest_pt: Vector3<f32> = Vector3::zero();
let mut best_dist: f32 = f32::INFINITY;
let mut next_state: &'static SimplexState<Point> = &VERTEX_DATAPTRLOC;
let (a, b, c, d) = (simp[0], simp[1], simp[2], simp[3]);
let (av, bv, cv, dv) = (a.into().to_vec(), b.into().to_vec(), c.into().to_vec(), d.into().to_vec());
if origin_outside_plane(av, bv, cv, dv) {
let mut new_simp = [ a, b, c, d ];
let (p, new_state) = FACE_DATAPTRLOC.min_norm(&mut new_simp);
let new_dist = p.magnitude2();
if new_dist < best_dist {
closest_pt = p;
best_dist = new_dist;
next_state = new_state;
*simp = new_simp;
}
}
if origin_outside_plane(av, cv, dv, bv) {
let mut new_simp = [ a, c, d, b ];
let (p, new_state) = FACE_DATAPTRLOC.min_norm(&mut new_simp);
let new_dist = p.magnitude2();
if new_dist < best_dist {
closest_pt = p;
best_dist = new_dist;
next_state = new_state;
*simp = new_simp;
}
}
if origin_outside_plane(av, dv, bv, cv) {
let mut new_simp = [ a, d, b, c ];
let (p, new_state) = FACE_DATAPTRLOC.min_norm(&mut new_simp);
let new_dist = p.magnitude2();
if new_dist < best_dist {
closest_pt = p;
best_dist = new_dist;
next_state = new_state;
*simp = new_simp;
}
}
if origin_outside_plane(bv, dv, cv, av) {
let mut new_simp = [ b, d, c, a ];
let (p, new_state) = FACE_DATAPTRLOC.min_norm(&mut new_simp);
let new_dist = p.magnitude2();
if new_dist < best_dist {
closest_pt = p;
next_state = new_state;
*simp = new_simp;
}
}
(closest_pt, next_state)
}
fn add_point(&self, simp: &mut [Point; 4], p: Point) {
simp[3] = p
}
fn len(&self) -> usize { 4 }
}
#[derive(Default)]
struct EdgeMap {
map: HashMap<[ (u32, u32, u32); 2 ], [ (Point3<f32>, Point3<f32>); 2 ]>,
}
impl EdgeMap {
fn add_edge(&mut self, a: SupportPoint, b: SupportPoint) {
let la = (a.a, a.b);
let lb = (b.a, b.b);
let a = unsafe {
(
mem::transmute::<f32, u32>(a.p.x),
mem::transmute::<f32, u32>(a.p.y),
mem::transmute::<f32, u32>(a.p.z),
)
};
let b = unsafe {
(
mem::transmute::<f32, u32>(b.p.x),
mem::transmute::<f32, u32>(b.p.y),
mem::transmute::<f32, u32>(b.p.z),
)
};
let ba = [ b, a ];
if self.map.contains_key(&ba) {
self.map.remove(&ba);
return;
}
self.map.insert(
[ a, b ],
[ la, lb ]
);
}
}
impl Simplex<SupportPoint> {
pub fn compute_contact<S1, S2>(&self, s1: &S1, s2: &S2) -> Contact
where
S1: Convex,
S2: Convex
{
if self.state.len() != 4 {
panic!("simplex is too small");
}
let diff = MinkowskiDiff{ s1, s2 };
let [ a, b, c, d ] = self.points;
let mut tris = Pool::from(
vec![
(a, b, c),
(a, c, d),
(a, d, b),
(b, d, c)
]
);
let mut edges = EdgeMap::default();
const MAX_ITERATIONS: usize = 100;
for iter in 0..=MAX_ITERATIONS {
let (closest_dist, closest_i, closest_n) = {
let mut closest_dist = f32::INFINITY;
let mut closest_i = 0usize;
let mut closest_n = Vector3::zero();
for (i, (a, b, c)) in tris.iter() {
let tri = Triangle::from((a.p, b.p, c.p));
let n = tri.normal();
let dist = n.dot(a.p.to_vec()).abs();
if closest_dist > dist {
closest_dist = dist;
closest_i = i;
closest_n = n;
}
}
(closest_dist, closest_i, closest_n)
};
let closest_tri = {
let (a, b, c) = tris[closest_i];
( Triangle::from((a.p, b.p, c.p)), Triangle::from((a.a, b.a, c.a)) )
};
let support: SupportPoint = diff.support(closest_n);
let v = closest_n.dot(support.p.to_vec()) - closest_dist;
if v < COLLISION_EPSILON || iter == MAX_ITERATIONS {
let (u, v, w ) = closest_tri.0.barycentric(Point3::from_vec(closest_dist * closest_n));
let a = u * closest_tri.1.a + v * closest_tri.1.b + w * closest_tri.1.c;
return Contact {
a: Point3::from_vec(a),
b: Point3::from_vec(a - closest_dist*closest_n),
n: closest_n,
t: 0.0
};
}
let mut to_remove = Vec::new();
for (i, (a, b, c)) in tris.iter() {
let n = Triangle::from((a.p, b.p, c.p)).normal();
if n.dot(support.p - a.p) > 0.0 {
edges.add_edge(*a, *b);
edges.add_edge(*b, *c);
edges.add_edge(*c, *a);
to_remove.push(i);
}
}
for i in to_remove {
tris.remove(i);
}
for ([( apx, apy, apz ), ( bpx, bpy, bpz ) ], [ la, lb ]) in edges.map.iter() {
let a = unsafe {
(
mem::transmute::<u32, f32>(*apx),
mem::transmute::<u32, f32>(*apy),
mem::transmute::<u32, f32>(*apz)
)
};
let b = unsafe {
(
mem::transmute::<u32, f32>(*bpx),
mem::transmute::<u32, f32>(*bpy),
mem::transmute::<u32, f32>(*bpz)
)
};
let a = SupportPoint {
p: Point3::new(a.0, a.1, a.2),
a: la.0,
b: la.1
};
let b = SupportPoint {
p: Point3::new(b.0, b.1, b.2),
a: lb.0,
b: lb.1
};
tris.push((support, a, b));
}
edges.map.clear();
}
unreachable!()
}
}