/////////////////////////////////////////////////////////////////// // 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(); o1->tier0CollisionData[o2] = o1D; o2->tier0CollisionData[o1] = o2D; } } } } //////////////////////////////////////////////// // Tier 1 collision /////////////////////////////////////////////// map<GameObject*, vector<GameObject*> >::iterator it = room->collisionTier0List.begin(); while(it!=room->collisionTier0List.end()){ GameObject* o1 = it->first; if(o1->CollisionTierNum<2) continue; Vector3f pos = o1->GetPosition(); Vec3f position(pos.x(), pos.y(), pos.z()); Vec4f rotation = o1->GetRotation(); Vec3f vel = o1->velocity; Vec4f wvel = o1->angularVelocity; vector<Vec3f> aBox = o1->boundingBox; UpdateCoords(aBox,position, vel, rotation, wvel ,0, 1.0,true, 1.f, xScale, yScale, zNear, zFar); // vector<Vec3f> oldABox = o1->boundingBox; // UpdateCoords(aBox,position, vel, rotation, wvel ,0, 1.0,true, 1.f, xScale, yScale, zNear, zFar); // for(unsigned int i = 0; i <oldABox.size(); i++){ // aBox.push_back(oldABox[i]); // } vector<GameObject*> Bs = it->second; it++; for(unsigned int b = 0; b<Bs.size(); b++){ GameObject* o2 = Bs[b]; if(o2->CollisionTierNum<2) continue; vector<Vec3f> bBox = o2->boundingBox; //pos = o2->GetPosition(); position = Vec3f(pos.x(), pos.y(), pos.z()); vel = o2->velocity; wvel = o2->angularVelocity; rotation = o2->GetRotation(); UpdateCoords(bBox, position, vel, rotation, wvel ,0,1.0, true, 1.f, xScale, yScale, zNear, zFar); vector<Vec3f> oldBBox = o2->boundingBox; //UpdateCoords(oldBBox,position, vel, rotation, wvel ,0, 1.0, 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, 1, room)) { if(o1->objType == ACTIVE_OBJECT_TYPE && o2->objType == ACTIVE_OBJECT_TYPE){ o1->drawCollision = true; o2->drawCollision = true; } Vec3f contact = ResolveContact(P); room->collisionTier1List[o1].push_back(o2); room->collisionTier1List[o2].push_back(o1); CollisionData o1D, o2D; o1D.pointOfContact = contact; o2D.pointOfContact = contact; o1D.contactNormal = V.normalized(); o2D.contactNormal = -V.normalized(); o1->tier1CollisionData[o2] = o1D; o2->tier1CollisionData[o1] = o2D; } } } } ////////////////////////////////////////////////////////////////////////////// // // Mesh Level Detection // ///////////////////////////////////////////////////////////////////////////// }