//Look at the GJK_EPA.h header file for documentation and instructions
bool SteerLib::GJK_EPA::intersect(float& return_penetration_depth, Util::Vector& return_penetration_vector, const std::vector<Util::Vector>& _shapeA, const std::vector<Util::Vector>& _shapeB)
{
if (CONCAVE_POLYGONS)
return Triangulate(_shapeA, _shapeB);
std::vector<Util::Vector> _simplex;
bool colliding;
colliding = Triangulate(_shapeA, _shapeB);
if (colliding)
{
EPA(return_penetration_depth, return_penetration_vector, _simplex, _shapeA, _shapeB);
return true;
}
else
{
return_penetration_depth = 0;
return_penetration_vector.zero();
return false;
}
// To make compiler happy
return false;
}
//Look at the GJK_EPA.h header file for documentation and instructions
bool SteerLib::GJK_EPA::intersect(float& return_penetration_depth, Util::Vector& return_penetration_vector, const std::vector<Util::Vector>& _shapeA, const std::vector<Util::Vector>& _shapeB)
{
std::vector<Util::Vector> masterSimplex;
// run decomposition
/*
std::vector<std::vector<Util::Vector>> triangleListA = decompose(_shapeA);
std::vector<std::vector<Util::Vector>> triangleListB = decompose(_shapeB);
*/
bool collision = false;
/*
// check GJK for every triangle in B with every triangle in A
for (int i = 0; i < triangleListA.size(); i++) {
for (int j = 0; j < triangleListB.size(); j++) {
if (GJK(masterSimplex, triangleListA.at(i), triangleListB.at(j))) {
collision = true;
}
}
}
*/
collision = GJK(masterSimplex, _shapeA, _shapeB);
// return result of EPA over total decomposed convex sets
if (collision) {
return_penetration_depth = EPA(masterSimplex, return_penetration_vector, _shapeA, _shapeB);
}
else {
return_penetration_depth = 0.0;
}
// return result of GJK over decomposed convex sets
return collision;
}
// Recursive implementatino of the GJK loop.
static inline struct ClosestPoints
GJKRecurse(const struct SupportContext *ctx, const struct MinkowskiPoint v0, const struct MinkowskiPoint v1, const int iteration)
{
if(iteration > MAX_GJK_ITERATIONS) {
cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration);
return ClosestPointsNew(v0, v1);
}
cpVect delta = cpvsub(v1.ab, v0.ab);
// TODO: should this be an area2x check?
if(cpvcross(delta, cpvadd(v0.ab, v1.ab)) > 0.0f) {
// Origin is behind axis. Flip and try again.
return GJKRecurse(ctx, v1, v0, iteration);
} else {
cpFloat t = ClosestT(v0.ab, v1.ab);
cpVect n = (-1.0f < t && t < 1.0f ? cpvperp(delta) : cpvneg(LerpT(v0.ab, v1.ab, t)));
struct MinkowskiPoint p = Support(ctx, n);
#if DRAW_GJK
ChipmunkDebugDrawSegment(v0.ab, v1.ab, RGBAColor(1, 1, 1, 1));
cpVect c = cpvlerp(v0.ab, v1.ab, 0.5);
ChipmunkDebugDrawSegment(c, cpvadd(c, cpvmult(cpvnormalize(n), 5.0)), RGBAColor(1, 0, 0, 1));
ChipmunkDebugDrawDot(5.0, p.ab, LAColor(1, 1));
#endif
if(
cpvcross(cpvsub(v1.ab, p.ab), cpvadd(v1.ab, p.ab)) > 0.0f &&
cpvcross(cpvsub(v0.ab, p.ab), cpvadd(v0.ab, p.ab)) < 0.0f
) {
// The triangle v0, p, v1 contains the origin. Use EPA to find the MSA.
cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK->EPA iterations: %d", iteration);
return EPA(ctx, v0, p, v1);
} else {
if(cpvdot(p.ab, n) <= cpfmax(cpvdot(v0.ab, n), cpvdot(v1.ab, n))) {
// The edge v0, v1 that we already have is the closest to (0, 0) since p was not closer.
cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration);
return ClosestPointsNew(v0, v1);
} else {
// p was closer to the origin than our existing edge.
// Need to figure out which existing point to drop.
if(ClosestDist(v0.ab, p.ab) < ClosestDist(p.ab, v1.ab)) {
return GJKRecurse(ctx, v0, p, iteration + 1);
} else {
return GJKRecurse(ctx, p, v1, iteration + 1);
}
}
}
}
}
static inline struct ClosestPoints
GJKRecurse(const struct SupportContext *ctx, const struct MinkowskiPoint v0, const struct MinkowskiPoint v1, const int iteration)
{
if(iteration > MAX_GJK_ITERATIONS){
cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration);
return ClosestPointsNew(v0, v1);
}
cpVect delta = cpvsub(v1.ab, v0.ab);
if(cpvcross(delta, cpvadd(v0.ab, v1.ab)) > 0.0f){
// Origin is behind axis. Flip and try again.
return GJKRecurse(ctx, v1, v0, iteration + 1);
} else {
cpFloat t = ClosestT(v0.ab, v1.ab);
cpVect n = (-1.0f < t && t < 1.0f ? cpvperp(delta) : cpvneg(LerpT(v0.ab, v1.ab, t)));
struct MinkowskiPoint p = Support(ctx, n);
#if DRAW_GJK
ChipmunkDebugDrawSegment(v0.ab, v1.ab, RGBAColor(1, 1, 1, 1));
cpVect c = cpvlerp(v0.ab, v1.ab, 0.5);
ChipmunkDebugDrawSegment(c, cpvadd(c, cpvmult(cpvnormalize(n), 5.0)), RGBAColor(1, 0, 0, 1));
ChipmunkDebugDrawDot(5.0, p.ab, LAColor(1, 1));
#endif
if(
cpvcross(cpvsub(v1.ab, p.ab), cpvadd(v1.ab, p.ab)) > 0.0f &&
cpvcross(cpvsub(v0.ab, p.ab), cpvadd(v0.ab, p.ab)) < 0.0f
){
cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK->EPA iterations: %d", iteration);
// The triangle v0, p, v1 contains the origin. Use EPA to find the MSA.
return EPA(ctx, v0, p, v1);
} else {
// The new point must be farther along the normal than the existing points.
if(cpvdot(p.ab, n) <= cpfmax(cpvdot(v0.ab, n), cpvdot(v1.ab, n))){
cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration);
return ClosestPointsNew(v0, v1);
} else {
if(ClosestDist(v0.ab, p.ab) < ClosestDist(p.ab, v1.ab)){
return GJKRecurse(ctx, v0, p, iteration + 1);
} else {
return GJKRecurse(ctx, p, v1, iteration + 1);
}
}
}
}
}
//Look at the GJK_EPA.h header file for documentation and instructions
bool SteerLib::GJK_EPA::intersect(float& return_penetration_depth, Util::Vector& return_penetration_vector, const std::vector<Util::Vector>& _shapeA, const std::vector<Util::Vector>& _shapeB)
{
std::vector<Util::Vector> _simplex;
bool colliding;
colliding = GJK(_simplex, _shapeA, _shapeB);
if (colliding)
{
EPA(return_penetration_depth, return_penetration_vector, _simplex, _shapeA, _shapeB);
return true;
}
else
{
return_penetration_depth = 0;
return_penetration_vector.zero();
return false;
}
}
