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