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

///////////////////////////////////////////////////////////////////
// CLOSEST IN
///////////////////////////////////////////////////////////////////
Vec3f ClosestIn(Simplex& P){
	assert(P.GetSize() > 0);
	if(P.GetSize() == 1) return P.at(0);
	else if(P.GetSize() == 2){ //We have a line.
		Vector3f AO(-P.at(1)[0], -P.at(1)[1], -P.at(1)[2]);
		Vector3f AB(P.at(0)[0] - P.at(1)[0], P.at(0)[1] - P.at(1)[1], P.at(0)[2] - P.at(1)[2]);
		Vector3f BO(-P.at(0)[0], -P.at(0)[1], -P.at(0)[2]);
		float v = (AB.dot(AO))/(AB.dot(AO) + (-AB).dot(BO));
		return (P.at(1)*(1-v) + (P.at(0))*v);
	}else{//We have a triangle
		Vector3f A(P.at(2)[0], P.at(2)[1], P.at(2)[2]);
		Vector3f B(P.at(1)[0], P.at(1)[1], P.at(1)[2]);
		Vector3f C (P.at(0)[0], P.at(0)[1], P.at(0)[2]);
		Vector3f AB = B-A;
		Vector3f AC = C-A;
		Vector3f N = AB.cross(AC);
		Vector3f Nab = N.cross(AB);
		Vector3f Nac = N.cross(AC);

		float v = (-A).dot(Nac)/((AB).dot(Nac));
		float w = (-A).dot(Nab)/((AC).dot(Nab));
		float u = 1-v-w;

		return (P.at(2)*u + P.at(1)*v + P.at(0)*w);
	}
}

	void SetToSimplex(Simplex& s){
		data.clear();
		for(unsigned int i = 0; i< s.GetSize(); i++){
			data.push_back(s[i]);
			dataA.push_back(s.atA(i));
			dataB.push_back(s.atB(i));
		}
	}

//////////////////////////////////////////////////////////////////////////////////////////
//
// RESOLVE CONTACT DATA USING BARYCENTRIC COORDINATES
//
////////////////////////////////////////////////////////////////////////////////////////////
Vec3f ResolveContact(Simplex& P){
	/////////////////////////////////////////////////
	// Barycentrics: Use them for A and B.
	///////////////////////////////////////////////
	assert(P.GetSize() > 0);
	Vec3f contactA, contactB;
	if(P.GetSize() == 1){
		contactA = P.atA(0);
		contactB = P.atB(0);
	}else if(P.GetSize() == 2){//linear interpo;
		Vec3f A = P.at(1);
		Vec3f B = P.at(0);
		Vec3f AB = B-A;
		float v = (dot(AB, -A))/(dot(AB, -A)+ dot(-AB, -B));
		contactA = P.atA(1)*(1-v) + P.atA(0)*v;
		contactB = P.atB(1)*(1-v) + P.atB(0)*v;
	}else{//triangle
		Vec3f A = (P.at(2));
		Vec3f B = (P.at(1));
		Vec3f C = (P.at(0));
		Vec3f N = cross(B-A,C-A);
		Vec3f Nab = cross(N, B-A);
		Vec3f Nac = cross(N, C-A);
		float v = (dot(-A, Nac))/(dot(B-A,Nac));
		float w = (dot(-A, Nab))/(dot(C-A, Nab));
		float u = 1 - v - w;
		contactA = P.atA(2)*u + P.atA(1)*v + P.atA(0)*w;
		contactB = P.atB(2)*u + P.atB(1)*v + P.atB(0)*w;
	}

	Vec3f norm = contactB-contactA;
	norm.normalize();

	return contactA;

}

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

//////////////////////////////////////////////////////////////////////////////////////////
// Detects all collisions of various types with each object.
void PerformCollisionDetection(GameRoom* room, GamePlayer* player, double dt,  float xScale, float yScale, float zNear, float zFar){
	dt/=1000;
	vector<GameObject*> Objects = room->GetGameObjects();
	PSystems* ps;// = Render::gameState->GetParticleSystems();
	list<Projectile*>* projs = ps->GetBullets();
	/////////////////////////////////////////////////////////////////////////////
	// PARTICLE COLLISION DETECTION
	/////////////////////////////////////////////////////////////////////////////
	for(unsigned int i = 0; i<Objects.size(); i++){
		GameObject* o = Objects[i];
		if(o->CollisionTierNum<1) continue;
		vector<Vec3f> aBox = o->boundingBox;
		Vector3f pos = o->GetPosition();
		Vec3f p(pos.x(), pos.y(), pos.z());
		Vec4f r = o->GetRotation();
		Vec4f wv = o->angularVelocity;
		Vec3f v = o->velocity;
		UpdateCoords(aBox, p, v, r, wv, 0, 1.f, true, 1.0, xScale, yScale, zNear, zFar);
		for(list<Projectile*>::iterator it = projs->begin(); it != projs->end(); ++it){
			vector<Vec3f> bBox = (*(*it)).boundingBox;
			Vector3f pos2 = (*(*it)).getPosition();
			Vec3f p2(pos.x(), pos.y(), pos.z());
			Vec4f r2(0,0,1,0);
			Vec4f wv2(1,0,0,0);
			Vector3f vel2 = (*(*it)).getVelocity();
			Vec3f v2(vel2.x(), vel2.y(), vel2.z());
			UpdateCoords(bBox, p2, v2, r2, wv2, 0, 10.f, true, 1.0,  xScale, yScale, zNear, zFar);
			Simplex P;
			Vec3f V;
			V = GJKDistance(aBox, bBox, P);
			if(P.GetSize() > 3 || V.norm() < tolerance){ //We have a collision
				Vec3f contact = ResolveContact(P); 
				o->collidedProjectiles.push_back((*it));
				CollisionData o1D, o2D;
				o1D.pointOfContact = contact;
				o1D.contactNormal = V.normalized();
				o->projectileCollisionData[(*it)]=o1D;
				(*it)->drawCollision = true;
			}
		}

	}

	////////////////////////////////////////////////////////////////////////////
	// SPECIAL DATA FOR MAIN CHARACTER ONLY
	////////////////////////////////////////////////////////////////////////////



	//////////////////////////////////////////////////
	//	Tier 0 collision
	//////////////////////////////////////////////////

	for(unsigned int a = 0; a<Objects.size()-1; a++){
		GameObject* o1 = Objects[a];
		if(o1->CollisionTierNum < 1 )continue;
		vector<Vec3f> aBox = o1->boundingBox;
		Vector3f pos = o1->GetPosition();
		Vec3f position = Vec3f(pos.x(), pos.y(), pos.z()); 
		Vec4f rotation = o1->GetRotation(); 
		Vec3f vel = o1->velocity;
		Vec4f wvel = o1->angularVelocity;
		UpdateCoords(aBox, position, vel, rotation, wvel ,dt, o1->outSideCollisionScale, true, 1.f, xScale, yScale, zNear, zFar);
		vector<Vec3f> oldABox = o1->boundingBox;
		UpdateCoords(oldABox, position, vel, rotation, wvel,0, o1->outSideCollisionScale, true, 1.f, xScale, yScale, zNear, zFar);
		for(unsigned int i = 0; i <oldABox.size(); i++){
			aBox.push_back(oldABox[i]);
		}
		for(unsigned int b = a+1; b<Objects.size(); b++){
			GameObject* o2 = Objects[b];
			if(o1->objType == WORLD_OBJECT_TYPE && o2->objType == WORLD_OBJECT_TYPE || o2->CollisionTierNum < 1){
				continue;
			}
			vector<Vec3f> bBox = o2->boundingBox;
			pos= o2->GetPosition();
			position = Vec3f(pos.x(), pos.y(), pos.z());
			vel = o2->velocity;
			rotation = o2->GetRotation();
			wvel = o2->angularVelocity;
			UpdateCoords(bBox, position, vel, rotation, wvel,dt, o2->outSideCollisionScale, true, 1.f, xScale, yScale, zNear, zFar);
			vector<Vec3f> oldBBox = o2->boundingBox;
			UpdateCoords(oldBBox, position, vel, rotation, wvel,0, o2->outSideCollisionScale,true, 1.f, xScale, yScale, zNear, zFar);
			for(unsigned int i = 0; i <oldBBox.size(); i++){
				bBox.push_back(oldBBox[i]);
			}
			Simplex P;
			Vec3f V;
			V = GJKDistance(aBox, bBox, P);
			if(V.norm() < tolerance){ //We have a collision
				if(!AlreadyIn(o1, o2, 0, room)){
					Vec3f contact = ResolveContact(P); 
					room->collisionTier0List[o1].push_back(o2);
					room->collisionTier0List[o2].push_back(o1);
					CollisionData o1D, o2D;
					o1D.pointOfContact = contact;
					o2D.pointOfContact = contact;
					o1D.contactNormal = V.normalized();
					o2D.contactNormal = -V.normalized();
//.........这里部分代码省略.........