void DContact::ResolveCollision(float2 &C0, float2 &C1)
{
SimBody &a = *collidingBodies[0];
SimBody &b = *collidingBodies[1];
float m0 = a.invMass;
float m1 = b.invMass;
float i0 = a.invInertia;
float i1 = b.invInertia;
const float2 &P0 = a.position;
const float2 &P1 = b.position;
float2 &V0 = a.velocity;
float2 &V1 = b.velocity;
float& w0 = a.angularVelocity;
float& w1 = b.angularVelocity;
//float fCoR = 0.3f;
//fCoF = 0.2f;
ResolveCollisions(-DContactNormal, t, cmat.coFriction, cmat.coRestitution,
C1, P1, V1, w1, m1, i1,
C0, P0, V0, w0, m0, i0);
};
void DContact::ResolveCollisions(const float2& Ncoll, float t, float fCoF, float fCoR,
const float2& C0, const float2& P0, float2& V0, float& w0, float m0, float i0,
const float2& C1, const float2& P1, float2& V1, float& w1, float m1, float i1)
{
// pre-computations
float tcoll = t > 0.0f ? t : 0.0f;
float2 Q0 = P0 + V0 * tcoll;
float2 Q1 = P1 + V1 * tcoll;
float2 R0 = C0 - Q0;
float2 R1 = C1 - Q1;
float2 T0(-R0.y, R0.x);
float2 T1(-R1.y, R1.x);
float2 VP0 = V0 - T0 * w0;
float2 VP1 = V1 - T1 * w1;
// impact velocity
float2 Vcoll = VP0 - VP1;
float vn = Vcoll.dot(Ncoll);
float2 Vn = Ncoll * vn;
float2 Vt = Vcoll - Vn;
if(vn > 0.0f) { return; }
float vt = Vt.magnitude();
Vt = Vt.normalize();
// compute impulse (friction and restitution)
float2 J, Jt(0.0f, 0.0f), Jn(0.0f, 0.0f);
float t0 = (R0 ^ Ncoll) * (R0 ^ Ncoll) * i0;
float t1 = (R1 ^ Ncoll) * (R1 ^ Ncoll) * i1;
float m = m0 + m1;
float denom = m + t0 + t1;
float jn = vn / denom;
Jn = Ncoll * (-(1.0f + fCoR) * jn); // restitution
Jt = Vt.normalize() * (fCoF * jn); // friction
J = Jn + Jt;
// changes in momentum
float2 dV0 = J * m0;
float2 dV1 = -J * m1;
float dw0 =-(R0 ^ J) * i0;
float dw1 = (R1 ^ J) * i1;
if(m0 > 0.0f) { V0 += dV0; w0 += dw0; }; // apply changes in momentum
if(m1 > 0.0f) { V1 += dV1; w1 += dw1; }; // apply changes in momentum
// Check for static frcition
if (vn < 0.0f && fCoF > 0.0f)
{
float cone = -vt / vn;
if (cone < fCoF)
{
float2 Nfriction = -Vt.normalize();
float fCoS = cmat.coStaticFriction;
ResolveCollisions(Nfriction, 0.0f, 0.0f, fCoS,
C0, P0, V0, w0, m0, i0,
C1, P1, V1, w1, m1, i1);
}
}
};
void Grid::ResolveCollisions(Entity* ent)
{
RigidBody* rigid = ent->GetComponent<RigidBody>();
if (rigid)
{
rigid->ResolveCollisions();
BoxCollider* box = ent->GetComponent<BoxCollider>();
if (box)
{
Vector3& max = box->GetMaxCoord();
Vector3& min = box->GetMinCoord();
if (min.x < 0 || min.y < 0 || max.x > GridWidth || max.y > GridHeight)
ent->OnCollisionEnter(Collision());
}
}
for (auto &child : ent->GetChildren())
{
ResolveCollisions(child);
}
}
void PhysicsManager::CollisionUpdate(const float& dt)
{
//First off, let's begin our coarse collision detection. This tests for only intersections in each frame, no fancy stuff.
std::vector<CollisionPair> sceneCollisions = CoarseCollisionDetection(sceneCollideables);
if (!sceneCollisions.empty())
{
GenerateContacts(sceneCollisions);
ResolveCollisions(sceneCollisions, dt);
}
}
void JelloMesh::Update(double dt, const World& world, const vec3& externalForces)
{
m_externalForces = externalForces;
CheckForCollisions(m_vparticles, world);
ComputeForces(m_vparticles);
ResolveContacts(m_vparticles);
ResolveCollisions(m_vparticles);
switch (m_integrationType)
{
case EULER: EulerIntegrate(dt); break;
case MIDPOINT: MidPointIntegrate(dt); break;
case RK4: RK4Integrate(dt); break;
}
}
void NPCManager::Update()
{
Camera2D * cam = Camera2D::GetInstance();
for (NPC * npc : m_npcList)
{
for (NPC * otherNpc : m_npcList)
{
if (npc != otherNpc)
{
// push away from each other
ResolveCollisions(npc, otherNpc);
}
}
}
for (NPC * npc : m_npcList)
{
npc->ClearNPCCollisionSet();
}
}
void CPhysEnv::Simulate(float DeltaTime, BOOL running)
{
float CurrentTime = 0.0f;
float TargetTime = DeltaTime;
tParticle *tempSys;
int collisionState;
while(CurrentTime < DeltaTime)
{
if (running)
{
ComputeForces(m_CurrentSys);
// IN ORDER TO MAKE THINGS RUN FASTER, I HAVE THIS LITTLE TRICK
// IF THE SYSTEM IS DOING A BINARY SEARCH FOR THE COLLISION POINT,
// I FORCE EULER'S METHOD ON IT. OTHERWISE, LET THE USER CHOOSE.
// THIS DOESN'T SEEM TO EFFECT STABILITY EITHER WAY
if (m_CollisionRootFinding)
{
EulerIntegrate(TargetTime-CurrentTime);
}
else
{
switch (m_IntegratorType)
{
case EULER_INTEGRATOR:
EulerIntegrate(TargetTime-CurrentTime);
break;
case MIDPOINT_INTEGRATOR:
MidPointIntegrate(TargetTime-CurrentTime);
break;
case HEUN_INTEGRATOR:
HeunIntegrate(TargetTime-CurrentTime);
break;
case RK4_INTEGRATOR:
RK4Integrate(TargetTime-CurrentTime);
break;
case RK4_ADAPTIVE_INTEGRATOR:
RK4AdaptiveIntegrate(TargetTime-CurrentTime);
break;
case FEHLBERG:
FehlbergIntegrate(TargetTime-CurrentTime);
break;
}
}
}
collisionState = CheckForCollisions(m_TargetSys);
if(collisionState == PENETRATING)
{
// TELL THE SYSTEM I AM LOOKING FOR A COLLISION SO IT WILL USE EULER
m_CollisionRootFinding = TRUE;
// we simulated too far, so subdivide time and try again
TargetTime = (CurrentTime + TargetTime) / 2.0f;
// blow up if we aren't moving forward each step, which is
// probably caused by interpenetration at the frame start
assert(fabs(TargetTime - CurrentTime) > EPSILON);
}
else
{
// either colliding or clear
if(collisionState == COLLIDING)
{
int Counter = 0;
do
{
ResolveCollisions(m_TargetSys);
Counter++;
} while((CheckForCollisions(m_TargetSys) ==
COLLIDING) && (Counter < 100));
assert(Counter < 100);
m_CollisionRootFinding = FALSE; // FOUND THE COLLISION POINT
}
// we made a successful step, so swap configurations
// to "save" the data for the next step
CurrentTime = TargetTime;
TargetTime = DeltaTime;
// SWAP MY TWO PARTICLE SYSTEM BUFFERS SO I CAN DO IT AGAIN
tempSys = m_CurrentSys;
m_CurrentSys = m_TargetSys;
m_TargetSys = tempSys;
}
}
}
void PhysicsWorld::Update()
{
StepPhysics();
Collide();
ResolveCollisions();
}
void CollisionSystem::Update()
{
InitBuckets();
ResolveCollisions();
}
