C++ (Cpp) Simplex::DeleteNonClosestIn Examples

Example #1

File: CollisionDetection.cpp Project: scottc52/SpaceGame

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