bool SteerLib::GJK_EPA::GJK(const std::vector<Util::Vector>& shapeA, const std::vector<Util::Vector>& shapeB, std::vector<Util::Vector>& simplex) {
Util::Vector centerShapeA(0,0,0);
Util::Vector centerShapeB(0,0,0);
Util::Vector d(0,0,0);
centerShapeA = central_origin_of_shape(shapeA);
centerShapeB = central_origin_of_shape(shapeB);
// direction
d = centerShapeB - centerShapeA;
simplex.push_back(support_function(shapeA, shapeB, d));
// negate the direction
d = -d;
while (true) {
simplex.push_back(support_function(shapeA, shapeB, d));
if (dot(simplex.back(), d) <= 0){
return false;
}
else if (containsOrigin(simplex, d)) {
if (simplex.size() < 3) {
simplex.push_back(support_function(shapeA, shapeB, d));
}
return true;
}
}
}
minkowskiDifference_t buildMinkowskiDifference(std::vector<MathVector> a, std::vector<MathVector> b)
{
MathVector direction = MathVector(1,1);
std::vector<MathVector> simplex;
simplex.push_back(getSupportVertex(a, b, direction));
minkowskiDifference_t difference;
direction = direction.negate();
while(true)
{
simplex.push_back(getSupportVertex(a, b, direction));
if(simplex.back().dotProduct(direction) <= 0)
{
difference.colliding = false;
difference.collisionNormal = MathVector(0,0);
difference.collisionDepth = 0;
return difference;
} else if(containsOrigin(simplex, direction) && simplex.size() == 3)
{
while(true)
{
//Perform EPA to get collision normal and penetration distance
Edge_t e = findClosestEdge(simplex);
MathVector p = getSupportVertex(a, b, direction);
double d = p.dotProduct(e.normal);
// std::cout << d - e.distance << std::endl;
// std::cout << "Simplex size: " << simplex.size() << std::endl;
if(d - e.distance < TOLERANCE)
{
difference.collisionNormal = e.normal;
difference.collisionDepth = d;
difference.colliding = true;
return difference;
} else
{
// std::cout << "Closest edge not found in this iteration, adding point to simplex and continuing." << std::endl;
simplex.insert((simplex.begin()+e.index),p);
}
}
}
}
}
bool SteerLib::GJK_EPA::GJK(std::vector<Util::Vector>& simplex, const std::vector<Util::Vector>& _shapeA, const std::vector<Util::Vector>& _shapeB)
{
// choose any initial d vector
d = Util::Vector(1, 0, 0);
// get the first Minkowski Difference point
Util::Vector minkowskiDiffPoint = support(_shapeA, d) - support(_shapeB, -d);
//std::vector<Util::Vector> simplex;
// add the point to the simplex
simplex.push_back(minkowskiDiffPoint);
// negate d for next point
d = -d;
// keep looping, exit conditions are inside loop
// for convex objects and no rounding errors, this loop should always terminate
while (true) {
// add a new point to the simplex because we haven't terminated yet
minkowskiDiffPoint = support(_shapeA, d) - support(_shapeB, -d);
simplex.push_back(minkowskiDiffPoint);
// make sure that the last point added is past the origin
if (dotProduct3d(minkowskiDiffPoint, d) < 0) {
// if the point added last is not past the origin in the direction of d, then the Minkowski Sum can't contain the origin
// since the last point added is on the edge of the Minkowski Difference
return false; // There is no collision
}
else {
// since the last point is past the origin, check if the origin is in the current simplex
if (containsOrigin(simplex)) {
// if it does then there is a collision
return true;
}
// otherwise containsOrigin updates the simplex and d vector
// the simplex is updated to be the line segment closest to the origin
// the d vector is set to be the normal of that line segment in the direction of the origin
}
}
}
