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