// main
void PhysicsWorld::Step(float dt, unsigned char velocityIterations, unsigned char positionIterations)
{
const TimeStep timeStep(dt, velocityIterations, positionIterations, mParams, mStats);
Stopwatch timer;
mStats.Clear();
// update rigid bodies
const float linearDampFactor = 1.0f - Clamp(mParams.linearDamping * timeStep.dt, 0.0f, 1.0f);
const float angularDampFactor = 1.0f - Clamp(mParams.angularDamping * timeStep.dt, 0.0f, 1.0f);
timer.Start();
for (auto &body : mRigidBodies)
{
// TODOKai mass
if (!body.CollidersValid())
body.ValidateColliders();
// update
body.UpdateMassAndLocalCentroid();
body.UpdateOrientation();
body.UpdatePositionFromGlobalCentroid();
body.UpdateGlobalInverseInertiaTensor();
body.UpdateProxies();
timeStep.stats.colliders += body.mColliders.size();
}
timeStep.stats.rigidBodies = mRigidBodies.size();
timeStep.stats.integration += timer.Stop();
// broadphase
timer.Start();
auto &pairList = mBroadphase->ComputePairs();
timeStep.stats.broadphasePairs = pairList.size();
timeStep.stats.broadphase += timer.Stop();
// narrowphase
timer.Start();
mContactManager.PreNarrowphase();
for (ColliderPair &pair : pairList)
{
Collider *colliderA = pair.mCollider1;
Collider *colliderB = pair.mCollider2;
RigidBody &bodyA = *colliderA->mParent;
RigidBody &bodyB = *colliderB->mParent;
CPhysics* cphyA = bodyA.mParent->cphy;
CPhysics* cphyB = bodyB.mParent->cphy;
if (!bodyA.CanCollide(bodyB) || !colliderA->CanBeCollide(*colliderB))
continue;
// collision table check
if (!cphyA->gameObject->GetState()->GetCollisionTable()->GetDoesIDsCollide(cphyA->GetCOllisionID(), cphyB->GetCOllisionID()))
continue;
// make sure colliderA is always less than colliderB in memory address for consistency
if (colliderA > colliderB)
{
std::swap(colliderA, colliderB);
auto temp = cphyA;
cphyA = cphyB;
cphyB = temp;
}
ContactManifold *manifold = new (mManifoldAllocator.Allocate()) ContactManifold();
manifold->colliderA = colliderA;
manifold->colliderB = colliderB;
//TODO
manifold->isColliding =
Collide(*manifold, *colliderA->mGeometry, *colliderB->mGeometry, mContactAllocator);
if (!manifold->isColliding
|| !mContactManager.Add(*manifold, mParams.contactPersistenceThreshold * mParams.contactPersistenceThreshold))
{
// manifold not colliding OR persistent manifold already exists, delete
manifold->~ContactManifold();
mManifoldAllocator.Free(manifold);
}
else
{
//these will be deleted by the script subsystem
CollisionData* dataA = new CollisionData();
CollisionData* dataB = new CollisionData();
// send manifold to OnCollide function
{
dataA->isA = true;
dataA->collidedObj = cphyB->gameObject;
dataA->selfCPhy = cphyA;
dataA->collidedObjCPhy = cphyB;
dataA->numContacts = manifold->numContacts;
for (int i = 0; i < dataA->numContacts; ++i)
{
dataA->normals.push_back(manifold->contacts[i]->normal);
}
ScriptSubsystem::getInstance()->QueueEvent<CollisionData>(cphyA->gameObject, "OnCollisionEnter", dataA);
}
{
dataB->isA = false;
dataB->collidedObj = cphyA->gameObject;
dataB->selfCPhy = cphyB;
dataB->collidedObjCPhy = cphyA;
dataB->numContacts = manifold->numContacts;
for (int i = 0; i < dataB->numContacts; ++i)
{
dataB->normals.push_back(manifold->contacts[i]->normal);
}
ScriptSubsystem::getInstance()->QueueEvent<CollisionData>(cphyB->gameObject, "OnCollisionEnter", dataB);
}
if (cphyA->mIsTriggered || cphyB->mIsTriggered)
{
manifold->~ContactManifold();
mManifoldAllocator.Free(manifold);
}
}
}
mContactManager.PostNarrowphase();
timeStep.stats.narrowphase += timer.Stop();
SimulateIslands(timeStep);
// update broadphase after position integration
mBroadphase->Update(timeStep);
mContactManager.Draw();
}
