C++ Simplex::Add方法代码示例

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