Vec3f GJKDistance(vector<Vec3f>&A, vector<Vec3f>&B, Simplex& P){
P.clearSimplex();
Vec3f v= Support(A, B, A[0] - B[0],P);
P.Add(v);
v = ClosestIn(P);
float lastDist = FLT_MAX;
Simplex lastP;
lastP.SetToSimplex(P);
Vec3f lastV = v;
float epsilon = 0.1;
while(true){
float dist = v.norm();
Vec3f w = Support(A, B, -v, P);
Vector3f vE(v[0], v[1], v[2]);
Vector3f wE(w[0], w[1], w[2]);
float f = dist - (dist - w.norm());
if(f<= tolerance*dist || dist<tolerance){
return v;
}if(lastDist-dist<= epsilon*lastDist){
P.SetToSimplex(lastP);
return lastV;
}else{
lastP.SetToSimplex(P);
lastV = v;
}
if(P.alreadyIn(w))
return v;
if(vE.dot(wE) > 0)return v;
P.Add(w);
v = ClosestIn(P);
P.DeleteNonClosestIn();
if(P.GetSize() > 3) return v; //Should never reach here.
}
return v;
}
bool IsColliding(const Collider &collider_a, const Collider &collider_b, Contacts &contacts)
{
// quick check with the individual collider AABBs for a quick out
if (!collider_a.aabb.Overlap(collider_b.aabb))
return false;
// our simplex for this collision test
Simplex simplex;
// Set initial search direction to the difference of centers
Vector2 d = Vector2(1, -1);//Vector2(collider_b.root_trans.PositionWC() - collider_a.root_trans.PositionWC());
// get the first minkowski difference point
simplex.Add(Support(collider_a, collider_b, d));
// negate the support point, giving us a vector in the direction of the origin
d = -simplex.A().vert;
int count = 0;
// start looping
while (count < 100)
{
// add a new point to the simplex because we haven't terminated yet
simplex.Add(Support(collider_a, collider_b, d));
// see if the simplex is on the correct side of the origin
if (Vector2::OppositeDirection(simplex.A().vert, d))
{
// if the point added last was not past the origin in the direction of d
// then the Minkowski Sum cannot possibly contain the origin since
// the last point added is on the edge of the Minkowski Difference
return false;
}
else
{
// oterwise we need to determine if the origin is in the current simplex
// this function will set the next search direction for us if it fails.
if (simplex.ContainsOrigin(d))
{
// if it does then we know there is a collision
// handle the collision with the EPA algorithm
EPAHandle(collider_a, collider_b, simplex, contacts);
// find the incident edge.
if (contacts.size())
{
auto &it = contacts.back();
it.info.e.edge_a = collider_a.FindIndex(it.normal);
it.info.e.edge_b = collider_b.FindIndex(-it.normal);
}
return true;
}
}
++count;
}
return false;
}
