bool CollisionDetector::SphereSphereCollision(PhysicsNode& p0, PhysicsNode& p1, CollisionData* data) {
return false;
//get the collision data
CollisionSphere& s0 = *(CollisionSphere*)p0.GetCollisionVolume();
CollisionSphere& s1 = *(CollisionSphere*)p1.GetCollisionVolume();
//get the normal
Vector3 normal = p0.GetPosition() - p1.GetPosition();
//get the distance (squared)
const float distSq = LengthSq(normal);
//get the max distance before collision
const float sumRadius = s0.GetRadius() + s1.GetRadius();
//if the distance is less than the max distance
if (distSq < sumRadius*sumRadius) {
//if there is collision data storage
if (data) {
//set the penetration depth
data->m_penetration = sumRadius - sqrtf(distSq);
//get the normal of the collision
normal.Normalise();
data->m_normal = normal;
//get the point of collision
data->m_point = p0.GetPosition() - normal*(s0.GetRadius() - data->m_penetration*0.5f);
}
return true;
}
return false;
}
bool CollisionDetector::SpherePlaneCollision(PhysicsNode& p0, PhysicsNode& p1, CollisionData* data) {
//get collision data
CollisionSphere& sphere = *(CollisionSphere*)p0.GetCollisionVolume();
CollisionPlane& plane = *(CollisionPlane*)p1.GetCollisionVolume();
//get separation
float separation = Vector3::Dot(p0.GetPosition(), plane.GetNormal()) - plane.GetDistance();
// if the separation is greater than the radius it cant of collided
if (separation > sphere.GetRadius()) {
return false;
}
//if there is collision data storage
if (data) {
//set the penetration depth
data->m_penetration = sphere.GetRadius() - separation;
//get the normal of the collision
data->m_normal = plane.GetNormal();
//get the point of collision
data->m_point = p0.GetPosition() - plane.GetNormal()*separation;
}
return true;
}
bool CollisionDetector::CylinderSphereCollision(PhysicsNode& p0, PhysicsNode& p1, CollisionData* data) {
CollisionCylinder& cylinder = *(CollisionCylinder*)p0.GetCollisionVolume();
CollisionSphere& sphere = *(CollisionSphere*)p1.GetCollisionVolume();
Vector3 cylCenterVector = cylinder.GetEnd() - cylinder.GetStart();
Vector3 pos1 = p1.GetPosition() - cylinder.GetStart();
float distanceFactorFromEP1 = Vector3::Dot(p1.GetPosition() - cylinder.GetStart(), cylCenterVector) / Vector3::Dot(cylCenterVector, cylCenterVector);
if(distanceFactorFromEP1 < 0) distanceFactorFromEP1 = 0;// clamp to endpoints if neccesary
if(distanceFactorFromEP1 > 1) distanceFactorFromEP1 = 1;
Vector3 closestPoint = cylinder.GetStart() + (cylCenterVector * distanceFactorFromEP1);
Vector3 collisionVector = p1.GetPosition() - closestPoint;
float distance = collisionVector.Length();
Vector3 collisionNormal = collisionVector / distance;
if(distance < sphere.GetRadius() + cylinder.GetRadius())
{
//collision occurred. use collisionNormal to reflect sphere off cyl
float factor = Vector3::Dot(p1.GetLinearVelocity(), collisionNormal);
p1.SetLinearVelocity(p1.GetLinearVelocity() - (collisionNormal * factor * 0.8f));
const float distSq = LengthSq(collisionNormal);
//get the max distance before collision
const float sumRadius = sphere.GetRadius() + cylinder.GetRadius();
p1.SetPosition(p1.GetPosition() + Vector3(collisionNormal * (sumRadius - sqrtf(distSq))));
return true;
}
return false;
}
PhysicsNode* PhysicsNode::create() {
PhysicsNode* ret = new PhysicsNode();
if (ret->initWithPhysics()) {
ret->autorelease();
return ret;
}
return NULL;
}
GameEntity* MyGame::BuildSpaceShip(Vector3 pos) {
GameEntity*g = new GameEntity(new KPAXNode(), new KPAXPhysicsNode(Quaternion::AxisAngleToQuaterion(Vector3(1,0,0), 90.0f), pos));
PhysicsNode* p = &g->GetPhysicsNode();
SceneNode* s= &g->GetRenderNode();
s->isTextured = true;
s->type = 1;
s->SetBoundingRadius(100);
p->SetDimension(s->GetModelScale());
p->AddTorque(Vector3(1, 1, 1));
p->SetInverseMass(500);
p->isShip = true;
g->ConnectToSystems();
return g;
}
GameEntity* MyGame::ShotProjectile(float msec){
GameEntity* g = new MoveSphere(new Cube(), new MoveSpherePhy(Quaternion::AxisAngleToQuaterion(Vector3(0, 1, 0), 0.0f),Vector3(100,0,0)));
SceneNode* s = &g->GetRenderNode();
PhysicsNode* p = &g->GetPhysicsNode();
Vector3 CamDir = gameCamera->GetCamDir();
s->SetColour(Vector4(0, 0, 1, 1));
s->type = 1;
s->SetBoundingRadius(50);
s->SetTransform(Matrix4::Translation(Vector3(1, 1, 1)) * Matrix4::Rotation( 270.0f, Vector3(1, 0, 0)));
s->SetModelScale(Vector3(10,10,20));
p->SetDimension(s->GetModelScale());
m_speed = m_speed * 1000;
p->SetInverseMass(9.0f);
p->SetSphereRadius(50);
p->AddForce(CamDir * 7000);
p->SetPosition(gameCamera->GetPosition());
p->SetOrientation(Quaternion::EulerAnglesToQuaternion(gameCamera->GetPitch(), gameCamera->GetYaw(), 0.0));
p->isMissile = true;
g->ConnectToSystems();
return g;
}
GameEntity* MyGame::BackFire(Vector3 pos) {
GameEntity* g = new MoveSphere(new Cube(), new MoveSpherePhy(Quaternion::AxisAngleToQuaterion(Vector3(0, 1, 0), 90.0f), pos + Vector3(0, 0, 150)));
SceneNode* s = &g->GetRenderNode();
PhysicsNode* p = &g->GetPhysicsNode();
Vector3 CamDir = gameCamera->GetCamDir();
s->SetTransform(Matrix4::Translation(Vector3(1, 1, 1)) * Matrix4::Rotation( 90.0f, Vector3(1, 0, 0)));
s->SetColour(Vector4(0, 0, 1, 1));
s->type = 1; // For debug boxes. If 1 = debug boxes ON.
s->SetBoundingRadius(50);
s->SetModelScale(Vector3(10,10,20));
p->SetDimension(s->GetModelScale());
p->SetInverseMass(12.0f);
p->SetSphereRadius(50);
p->AddForce(-CamDir * 9000);
p->SetOrientation(Quaternion::EulerAnglesToQuaternion(gameCamera->GetPitch(), gameCamera->GetYaw(), 0.0));
p->isMissile = true;
p->isBackFire = true;
g->ConnectToSystems();
return g;
}
void TerrainClass::BuildFloorEntity() {
for (int i = 0; i < ChunckManager::NUM_VOXEL_CHUNCKS; i++) {
SceneNode* sNode = new SceneNode();
sNode->SetMesh(terrain->getMeshAt(i));
sNode->SetModelScale(Vector3(SCALE_TERRAIN, SCALE_TERRAIN, SCALE_TERRAIN));
PhysicsNode* pNode = new PhysicsNode();
pNode->SetPosition(START_TERRAIN_POSITION);
pNode->SetLinearVelocity(Vector3(0, 0, linearVelocityZ));
pNode->setLD(false);
pNode->setSkyBox(true);
terrainPhysicsNodes.push_back(pNode);
countTerrainEntities++;
GameEntity* geFirst = new GameEntity(sNode, pNode);
geFirst->ConnectToSystems();
terrainEntities.push_back(geFirst);
//add the id of the chunck just created
//terrainChuncksId.push_back(geFirst->getId());
//reset the chunck id to be the same as the entity added; for consistency purposes
//terrain->getChunckAt(i)->setId(i);
}
}
bool CollisionDetector::AABBCollision(PhysicsNode& p0, PhysicsNode& p1, CollisionData* data) {
CollisionAABB& aabb0 = *(CollisionAABB*)p0.GetCollisionVolume();
CollisionAABB& aabb1 = *(CollisionAABB*)p1.GetCollisionVolume();
float dist = abs(p0.GetPosition().x - p1.GetPosition().x);
float sum = aabb0.getHalfDimensions().x + aabb1.getHalfDimensions().x;
if(dist <= sum) {
dist = abs(p0.GetPosition().y - p1.GetPosition().y);
sum = aabb0.getHalfDimensions().y + aabb1.getHalfDimensions().y;
if(dist <= sum) {
dist = abs(p0.GetPosition().z - p1.GetPosition().z);
sum = aabb0.getHalfDimensions().z + aabb1.getHalfDimensions().z;
if(dist <= sum) {
return true;
}
}
}
return false;
}
bool CollisionDetector::AABBSphereCollision(PhysicsNode& p0, PhysicsNode& p1, CollisionData* data) {
CollisionAABB& aabb0 = *(CollisionAABB*)p0.GetCollisionVolume();
CollisionSphere& sphere = *(CollisionSphere*)p1.GetCollisionVolume();
float dist = abs(p0.GetPosition().x - p1.GetPosition().x);
float sum = aabb0.getHalfDimensions().x + sphere.GetRadius();
if(dist <= sum) {
dist = abs(p0.GetPosition().y - p1.GetPosition().y);
sum = aabb0.getHalfDimensions().y + sphere.GetRadius();
if(dist <= sum) {
dist = abs(p0.GetPosition().z - p1.GetPosition().z);
sum = aabb0.getHalfDimensions().z + sphere.GetRadius();
if(dist <= sum) {
//if there is collision data storage
return true;
}
}
}
return false;
}
void CollisionDetector::AddCollisionImpulse(PhysicsNode &p0, PhysicsNode &p1, CollisionData &data) {
// if neither objects have mass
if(p0.GetInverseMass() + p1.GetInverseMass() == 0.0f) {
return;
}
Vector3 r0 = data.m_point - p0.GetPosition();
Vector3 r1 = data.m_point - p1.GetPosition();
//get velocities
Vector3 v0 = p0.GetLinearVelocity() + Vector3::Cross(p0.GetAngularVelocity(), r0);
Vector3 v1 = p1.GetLinearVelocity() + Vector3::Cross(p1.GetAngularVelocity(), r1);
Vector3 dv = v0 - v1;
float relMov = -Vector3::Dot(dv, data.m_normal);
//if(relMov < -0.01f) return;
{
float e = 0.0f;
float normDiv = (p0.GetInverseMass() + p1.GetInverseMass()) + Vector3::Dot(data.m_normal, Vector3::Cross(p0.GetInverseInertia()
* Vector3::Cross(r0, data.m_normal), r0) + Vector3::Cross(p1.GetInverseInertia() * Vector3::Cross(r1, data.m_normal), r1));
float jn = -1 * (1+e) * Vector3::Dot(dv, data.m_normal) / normDiv;
jn = jn + (data.m_penetration * 0.1f);
//set p0's velocity
Vector3 l0 = p0.GetLinearVelocity() + data.m_normal * (jn * p0.GetInverseMass());
p0.SetLinearVelocity(l0);
//set p0's rotation velocity
Vector3 a0 = p0.GetAngularVelocity() + p0.GetInverseInertia() * Vector3::Cross(r0, data.m_normal * jn);
p0.SetAngularVelocity(a0);
//set p1's velocity
Vector3 l1 = p1.GetLinearVelocity() - data.m_normal * ( jn * p1.GetInverseMass());
p1.SetLinearVelocity(l1);
//set p1's rotation velocity
Vector3 a1 = p1 . GetAngularVelocity () - p1.GetInverseInertia() * Vector3::Cross(r1, data.m_normal * jn);
p1.SetAngularVelocity(a1);
}
//if (false)
{
Vector3 tangent = dv - data.m_normal * Vector3::Dot(dv, data.m_normal);
tangent.Normalise();
float tangDiv = (p0.GetInverseMass() + p1.GetInverseMass()) + Vector3::Dot(tangent, Vector3::Cross(p0.GetInverseInertia() * Vector3::Cross(r0,
tangent),r0) + Vector3::Cross(p1.GetInverseInertia() * Vector3::Cross(r1, tangent), r1));
float jt = -1 * Vector3::Dot(dv, tangent) / tangDiv;
Vector3 l0 = p0.GetLinearVelocity() + tangent * (jt * p0.GetInverseMass());
//p0.SetLinearVelocity(l0);
Vector3 a0 = p0.GetAngularVelocity() + p0.GetInverseInertia() * Vector3::Cross(r0, tangent * jt);
p0.SetAngularVelocity(a0);
Vector3 l1 = p1.GetLinearVelocity() - tangent * (jt * p1.GetInverseMass());
//p1.SetLinearVelocity(l1);
Vector3 a1 = p1.GetAngularVelocity() - p1.GetInverseInertia() * Vector3::Cross(r1, tangent * jt);
p1.SetAngularVelocity(a1);
}
}
void PhysicsSystem::AddCollisionImpulse(PhysicsNode &s0,
PhysicsNode &s1,
CollisionData cd){
float invMass0 = (s0.getInvMass()<0.0001f) ? 0.0f : (s0.getInvMass());
float invMass1 = (s1.getInvMass()<0.0001f) ? 0.0f : (s1.getInvMass());
/*
invMass0 = (s0.sleep) ? 0.0f : invMass0;
invMass1 = (s1.sleep) ? 0.0f : invMass1;
/*
if(s0.sleep || s1.sleep) {
//cout <<"WHAT";
}
*/
const Matrix4 worldInvInertia0 = s0.getInvInertia();
const Matrix4 worldInvInertia1 = s1.getInvInertia();
if( (invMass0+invMass1) == 0.0f) return;
Vector3 r0 = cd.m_point- s0.GetPosition();
Vector3 r1 = cd.m_point- s1.GetPosition();
Vector3 v0 = s0.GetLinearVelocity() + Vector3::Cross(s0.GetAngularVelocity(), r0);
Vector3 v1 = s1.GetLinearVelocity() + Vector3::Cross(s1.GetAngularVelocity(), r1);
//Relative velocity
Vector3 dv = v0 - v1;
float relativeMovement = -Vector3::Dot(dv,cd.m_normal);
if(relativeMovement < -0.01f) {
return;
}
//Normal impulse
{
//Co efficient of restitution. Energy retained!
float e = 0.5f;
float normDiv = Vector3::Dot(cd.m_normal, cd.m_normal) *
( (invMass0 + invMass1)
+ Vector3::Dot(cd.m_normal, Vector3::Cross(worldInvInertia0 * Vector3::Cross(r0,cd.m_normal),r0)
+ Vector3::Cross (worldInvInertia1 * Vector3::Cross(r1, cd.m_normal), r1 ) ) );
float jn;
if(normDiv==0.0f) jn = 0.0f; else {
jn = -1 * ( 1+e) * Vector3::Dot(dv, cd.m_normal) /normDiv;
}
//Stop sinking! Hack fix
jn = jn + ( cd.m_penetration * 1.5f);
if(s0.dynamic){
s0.AddLinearVelocity(cd.m_normal * invMass0 * jn);
s0.AddAngVelocity(worldInvInertia0 * Vector3::Cross(r0,cd.m_normal * jn));
}
if(s1.dynamic) {
s1.AddLinearVelocity(Vector3(cd.m_normal * jn * invMass1 * -1));
s1.AddAngVelocity(Vector3(worldInvInertia1 * Vector3::Cross(r1,cd.m_normal * jn) * -1));
}
}
//Tangent impulse
{
/*
//Work out tangent vector, perpendicular to collision normal
Vector3 tangent = Vector3(0,0,0);
tangent = dv - (cd.m_normal * Vector3::Dot(dv,cd.m_normal));
tangent.Normalise();
float tangDiv = invMass0 + invMass1
+ Vector3::Dot(tangent, Vector3::Cross(s0.getInvInertia()
* Vector3::Cross(r0, tangent), r0)
+ Vector3::Cross(s1.getInvInertia() * Vector3::Cross(r1,tangent), r1));
float jt;
if(tangDiv==0.0f) jt =0.0f; else
jt = -1 * Vector3::Dot(dv, tangent) /tangDiv;
if(s0.dynamic) {
//s0.AddLinearVelocity(tangent * jt * invMass0);
s0.AddAngVelocity(worldInvInertia0* Vector3::Cross(r0, tangent * jt));
}
if(s1.dynamic) {
//s1.AddLinearVelocity(tangent * jt * invMass1 * -1);
s1.AddAngVelocity(worldInvInertia1* Vector3::Cross(r1, tangent * jt));
//s1.AddAngVelocity(worldInvInertia1* Vector3::Cross(r1, tangent * jt)* -1);
}
*/
}
}
