inline void Contact::GetWorldManifold(WorldManifold* worldManifold) const
{
const Body* bodyA = m_fixtureA->GetBody();
const Body* bodyB = m_fixtureB->GetBody();
const Shape* shapeA = m_fixtureA->GetShape();
const Shape* shapeB = m_fixtureB->GetShape();
worldManifold->Initialize(&m_manifold, bodyA->GetTransform(), shapeA->m_radius, bodyB->GetTransform(), shapeB->m_radius);
}
void PhysicsWorld::FindAllCollisions() {
contactManager.numOldContacts = contactManager.numContacts;
assert(contactManager.numOldContacts < MaxNumContacts );
memcpy(contactManager.oldContacts, contactManager.contacts, contactManager.numOldContacts * sizeof(Contact));
contactManager.numContacts = 0;
/*
O(n^2) collsion detection ( or worse LOL )
Check all fixtures vs all fixtures for collisions,
We dont collide fixtures that are from the same body
*/
for ( size_t i = 0; i < numBodies; i++ ) {
for ( size_t j = i + 1; j < numBodies; j++ ) {
if ( (bodies[i]->type != eDynamic && bodies[j]->type != eDynamic) ) {
continue;
}
if ( contactManager.contactFilter && !contactManager.contactFilter->ShouldCollide(bodies[i], bodies[j]) ) {
continue;
}
Fixture *fi = bodies[i]->GetFixtureList();
while ( fi ) {
Fixture *fj = bodies[j]->GetFixtureList();
while ( fj ) {
if ( fi->IsSensor( ) || fj->IsSensor( ) ) {
if ( CheckOverlap(fi->GetShape(), bodies[i]->GetPosition(), fj->GetShape(), bodies[j]->GetPosition()) ) {
contactManager.contactListener->OnSensor(fi, fj);
}
} else {
// check collision and add to the contact list if they intersect
contactManager.AddPair(fi, fj);
}
fj = fj->GetNext();
}
fi = fi->GetNext();
}
}
}
contactManager.MergeContacts();
}
Fixture * World::QueryPoint(const Vector2 &point) const
{
for(int i = 0; i < entities.size(); i++)
{
for(Fixture *f = entities[i]->body.GetFixtureList(); f != 0; f = f->GetNext())
{
if(f->GetShape()->TestPoint(point, entities[i]->body.GetPosition()))
{
return f;
}
}
}
return 0;
}
ContactSolver::ContactSolver(ContactSolverDef* def)
{
m_step = def->step;
m_allocator = def->allocator;
m_count = def->count;
m_positionConstraints = (ContactPositionConstraint*)m_allocator->Allocate(m_count * sizeof(ContactPositionConstraint));
m_velocityConstraints = (ContactVelocityConstraint*)m_allocator->Allocate(m_count * sizeof(ContactVelocityConstraint));
m_positions = def->positions;
m_velocities = def->velocities;
m_contacts = def->contacts;
// Initialize position independent portions of the constraints.
for (int32 i = 0; i < m_count; ++i)
{
Contact* contact = m_contacts[i];
Fixture* fixtureA = contact->m_fixtureA;
Fixture* fixtureB = contact->m_fixtureB;
Shape* shapeA = fixtureA->GetShape();
Shape* shapeB = fixtureB->GetShape();
float32 radiusA = shapeA->m_radius;
float32 radiusB = shapeB->m_radius;
Body* bodyA = fixtureA->GetBody();
Body* bodyB = fixtureB->GetBody();
Manifold* manifold = contact->GetManifold();
int32 pointCount = manifold->pointCount;
assert(pointCount > 0);
ContactVelocityConstraint* vc = m_velocityConstraints + i;
vc->friction = contact->m_friction;
vc->restitution = contact->m_restitution;
vc->tangentSpeed = contact->m_tangentSpeed;
vc->indexA = bodyA->m_islandIndex;
vc->indexB = bodyB->m_islandIndex;
vc->invMassA = bodyA->m_invMass;
vc->invMassB = bodyB->m_invMass;
vc->invIA = bodyA->m_invI;
vc->invIB = bodyB->m_invI;
vc->contactIndex = i;
vc->pointCount = pointCount;
vc->K.SetZero();
vc->normalMass.SetZero();
ContactPositionConstraint* pc = m_positionConstraints + i;
pc->indexA = bodyA->m_islandIndex;
pc->indexB = bodyB->m_islandIndex;
pc->invMassA = bodyA->m_invMass;
pc->invMassB = bodyB->m_invMass;
pc->localCenterA = bodyA->m_sweep.localCenter;
pc->localCenterB = bodyB->m_sweep.localCenter;
pc->invIA = bodyA->m_invI;
pc->invIB = bodyB->m_invI;
pc->localNormal = manifold->localNormal;
pc->localPoint = manifold->localPoint;
pc->pointCount = pointCount;
pc->radiusA = radiusA;
pc->radiusB = radiusB;
pc->type = manifold->type;
for (int32 j = 0; j < pointCount; ++j)
{
ManifoldPoint* cp = manifold->points + j;
VelocityConstraintPoint* vcp = vc->points + j;
if (m_step.warmStarting)
{
vcp->normalImpulse = m_step.dtRatio * cp->normalImpulse;
vcp->tangentImpulse = m_step.dtRatio * cp->tangentImpulse;
}
else
{
vcp->normalImpulse = 0.0f;
vcp->tangentImpulse = 0.0f;
}
vcp->rA.SetZero();
vcp->rB.SetZero();
vcp->normalMass = 0.0f;
vcp->tangentMass = 0.0f;
vcp->velocityBias = 0.0f;
pc->localPoints[j] = cp->localPoint;
}
}
}
void Island::SolveTOI(const PTimeStep& subStep, s32 toiIndexA, s32 toiIndexB)
{
assert(toiIndexA < m_bodyCount);
assert(toiIndexB < m_bodyCount);
// Initialize the body state.
for (s32 i = 0; i < m_bodyCount; ++i)
{
Body* b = m_bodies[i];
m_positions[i].c = b->m_sweep.c;
m_positions[i].a = b->m_sweep.a;
m_velocities[i].v = b->m_linearVelocity;
m_velocities[i].w = b->m_angularVelocity;
}
ContactSolverDef contactSolverDef;
contactSolverDef.contacts = m_contacts;
contactSolverDef.count = m_contactCount;
contactSolverDef.allocator = m_allocator;
contactSolverDef.step = subStep;
contactSolverDef.positions = m_positions;
contactSolverDef.velocities = m_velocities;
ContactSolver contactSolver(&contactSolverDef);
// Solve position constraints.
for (s32 i = 0; i < subStep.positionIterations; ++i)
{
bool contactsOkay = contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB);
if (contactsOkay)
{
break;
}
}
#if 0
// Is the new position really safe?
for (s32 i = 0; i < m_contactCount; ++i)
{
Contact* c = m_contacts[i];
Fixture* fA = c->GetFixtureA();
Fixture* fB = c->GetFixtureB();
Body* bA = fA->GetBody();
Body* bB = fB->GetBody();
s32 indexA = c->GetChildIndexA();
s32 indexB = c->GetChildIndexB();
DistanceInput input;
input.proxyA.Set(fA->GetShape(), indexA);
input.proxyB.Set(fB->GetShape(), indexB);
input.Transform2DA = bA->GetTransform2D();
input.Transform2DB = bB->GetTransform2D();
input.useRadii = false;
DistanceOutput output;
SimplexCache cache;
cache.count = 0;
Distance(&output, &cache, &input);
if (output.distance == 0 || cache.count == 3)
{
cache.count += 0;
}
}
#endif
// Leap of faith to new safe state.
m_bodies[toiIndexA]->m_sweep.c0 = m_positions[toiIndexA].c;
m_bodies[toiIndexA]->m_sweep.a0 = m_positions[toiIndexA].a;
m_bodies[toiIndexB]->m_sweep.c0 = m_positions[toiIndexB].c;
m_bodies[toiIndexB]->m_sweep.a0 = m_positions[toiIndexB].a;
// No warm starting is needed for TOI events because warm
// starting impulses were applied in the discrete solver.
contactSolver.InitializeVelocityConstraints();
// Solve velocity constraints.
for (s32 i = 0; i < subStep.velocityIterations; ++i)
{
contactSolver.SolveVelocityConstraints();
}
// Don't store the TOI contact forces for warm starting
// because they can be quite large.
real32 h = subStep.delta;
// Integrate positions
for (s32 i = 0; i < m_bodyCount; ++i)
{
glm::vec2 c = m_positions[i].c;
real32 a = m_positions[i].a;
glm::vec2 v = m_velocities[i].v;
real32 w = m_velocities[i].w;
// Check for large velocities
glm::vec2 translation = h * v;
if (glm::dot(translation, translation) > maxTranslationSquared)
{
real32 ratio = maxTranslation / translation.length();
v *= ratio;
}
real32 rotation = h * w;
if (rotation * rotation > maxRotationSquared)
{
real32 ratio = maxRotation / glm::abs(rotation);
w *= ratio;
}
// Integrate
c += h * v;
a += h * w;
m_positions[i].c = c;
m_positions[i].a = a;
m_velocities[i].v = v;
m_velocities[i].w = w;
// Sync bodies
Body* body = m_bodies[i];
body->m_sweep.c = c;
body->m_sweep.a = a;
body->m_linearVelocity = v;
body->m_angularVelocity = w;
body->SynchronizeTransform2D();
}
Report(contactSolver.m_velocityConstraints);
}
void Graphics::Draw(Renderer *renderer)
{
Fixture *fixture = owner->body.GetFixtureList();
while ( fixture )
{
if(visible)
{
if ( fixture->GetType() == eCircle )
{
renderer->DrawTexturedQuad(
texture,
owner->body.GetPosition() + static_cast<Circle*>(fixture->GetShape())->localPos,
static_cast<Circle*>(fixture->GetShape())->radius * 2.0f,
static_cast<Circle*>(fixture->GetShape())->radius * 2.0f,
angle,
Vector2(0.0f, 0.0f), //owner->body.GetPosition() + static_cast<Circle*>(fixture->GetShape())->localPos,
flipX,
flipY,
color
);
}
else if ( fixture->GetType() == eAABox )
{
Vector2 minExt = static_cast<AABox*>(fixture->GetShape())->minExt;
Vector2 maxExt = static_cast<AABox*>(fixture->GetShape())->maxExt;
Vector2 boxSize = maxExt - minExt;
renderer->DrawTexturedQuad(
texture,
owner->body.GetPosition() + boxSize/2.0f,
boxSize.x,
boxSize.y,
0.0f,
owner->body.GetPosition() + boxSize/2.0f,
flipX,
flipY,
color
);
}
else if ( fixture->GetType() == eCapsule )
{
Vector2 pos0 = static_cast<Capsule*>(fixture->GetShape())->localPos0 + owner->body.GetPosition();
Vector2 pos1 = static_cast<Capsule*>(fixture->GetShape())->localPos1 + owner->body.GetPosition();
float radius = static_cast<Capsule*>(fixture->GetShape())->radius;
Vector2 normal = normalize(pos1-pos0);
Vector2 tangent = Vector2(-normal.y, normal.x);
Vector2 verts[4];
verts[0] = pos0 - normal * radius - tangent * radius;
verts[1] = pos1 + normal * radius - tangent * radius;
verts[2] = pos1 + normal * radius + tangent * radius;
verts[3] = pos0 - normal * radius + tangent * radius;
Vector2 texCoords[4];
if ( !flipX )
{
texCoords[0] = Vector2(0.0f, 0.0f);
texCoords[1] = Vector2(1.0f, 0.0f);
texCoords[2] = Vector2(1.0f, 1.0f);
texCoords[3] = Vector2(0.0f, 1.0f);
}
else
{
texCoords[2] = Vector2(0.0f, 0.0f);
texCoords[0] = Vector2(1.0f, 0.0f);
texCoords[1] = Vector2(1.0f, 1.0f);
texCoords[3] = Vector2(0.0f, 1.0f);
}
glColor4f(color.r, color.g, color.b, color.a);
renderer->DrawTexturedQuad(texture, verts, texCoords);
}
}
if ( drawShape )
{
fixture->GetShape()->Draw(*renderer, owner->body.GetPosition());
}
fixture = fixture->GetNext();
}
}
void BoxScreen::drawBody (const BodyP &b) {
for (Fixture *f = b->GetFixtureList(); f; f = f->GetNext()) {
drawShape (b,f->GetShape());
}
}
