//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; } }