bool containsOrigin(std::vector<MathVector> &simplex, MathVector &direction)
{
MathVector a = simplex.back();
MathVector b, c, ab, ac, abPerp, acPerp;
MathVector ao = a.negate();
if(simplex.size() == 3)
{
b = simplex[0];
c = simplex[1];
ab = b.subtractVectors(a);
ac = c.subtractVectors(a);
abPerp = tripleProduct(ac, ab, ab);
acPerp = tripleProduct(ab, ac, ac);
if(abPerp.dotProduct(ao) > 0)
{
simplex.erase(simplex.begin() + 1);
direction = abPerp;
} else if (acPerp.dotProduct(ao) > 0)
{
simplex.erase(simplex.begin());
direction = acPerp;
} else
{
return true;
}
} else
{
b = simplex[0];
ab = b.subtractVectors(a);
abPerp = tripleProduct(ab, ao, ab);
if(abPerp.getX() == 0 || abPerp.getY() == 0)
{
direction = ab.perpendicular();
} else
{
direction = abPerp;
}
}
return false;
}
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);
}
}
}
}
}
MathVector getSupportVertex(std::vector<MathVector> a, std::vector<MathVector> b, MathVector direction)
{
MathVector point0 = getFurthestPoint(direction,a);
MathVector point1 = getFurthestPoint(direction.negate(),b);
return point0.subtractVectors(point1);
}