//create the box2dBody
void Bullet::createBox2dBody(float x, float y)
{
bodyDef.type = b2_dynamicBody;
bodyDef.position.Set(x / 30, y / 30);
if(bulletForPlayer1 == true)
bodyDef.userData = "Bullet";
else bodyDef.userData = "Bullet";
body = m_world->CreateBody(&bodyDef);
dynamicBox.SetAsBox((7 / 2.0f) / 30, (7 / 2.0f) / 30);
fixtureDef.shape = &dynamicBox;
fixtureDef.density = 1.0f;
fixtureDef.friction = 0.3f;
fixtureDef.userData = this;
fixtureDef.restitution = b2MixRestitution(0, 0);
if (bulletForPlayer1) {
fixtureDef.filter.categoryBits = BULLET;
fixtureDef.filter.maskBits = PLAYER | PLATFORM;
}
else
{
fixtureDef.filter.categoryBits = BULLET;
fixtureDef.filter.maskBits = PLAYER | PLATFORM;
}
body->CreateFixture(&fixtureDef);
body->SetFixedRotation(true);
body->SetGravityScale(0);
}
b2Contact::b2Contact(b2Fixture* fA, int32 indexA, b2Fixture* fB, int32 indexB)
{
m_flags = e_enabledFlag;
m_fixtureA = fA;
m_fixtureB = fB;
m_indexA = indexA;
m_indexB = indexB;
m_manifold.pointCount = 0;
m_prev = NULL;
m_next = NULL;
m_nodeA.contact = NULL;
m_nodeA.prev = NULL;
m_nodeA.next = NULL;
m_nodeA.other = NULL;
m_nodeB.contact = NULL;
m_nodeB.prev = NULL;
m_nodeB.next = NULL;
m_nodeB.other = NULL;
m_toiCount = 0;
m_friction = b2MixFriction(m_fixtureA->m_friction, m_fixtureB->m_friction);
m_restitution = b2MixRestitution(m_fixtureA->m_restitution, m_fixtureB->m_restitution);
m_tangentSpeed = 0.0f;
}
void Tree::createBox2dBody()
{
b2BodyDef bodyDef;
bodyDef.type = b2_staticBody;
bodyDef.position.Set((m_pos.x) / SCALE, (m_pos.y) / SCALE);
bodyDef.userData = this;
bodyDef.gravityScale = 1;
body = world->CreateBody(&bodyDef);
b2PolygonShape staticBox;
staticBox.SetAsBox((40 / 2.0f) / SCALE, (40 / 2.0f) / SCALE);
fixtureDef.shape = &staticBox;
fixtureDef.density = 1;
fixtureDef.friction = 0.3f;
fixtureDef.userData = "Filler";
fixtureDef.restitution = b2MixRestitution(0, 0);
fixtureDef.filter.categoryBits = FILLER;
fixtureDef.filter.maskBits = PLAYER | MELEE | PUNCH;
body->CreateFixture(&fixtureDef);
body->SetTransform(body->GetPosition(), rotation * DEGTORAD);
ltbl::LightShape* lightBlocker;
m_pos.x -= 15;
m_pos.y -= 15;
lightBlocker = ltbl::LightSystem::GetInstance()->allocateShape();
/*lightBlocker->_shape.setPointCount(4u);
lightBlocker->_shape.setPoint(0u, { 0.f, 0.f });
lightBlocker->_shape.setPoint(1u, { 0.f, 30 });
lightBlocker->_shape.setPoint(2u, { 30, 30 });
lightBlocker->_shape.setPoint(3u, { 30, 0.f });*/
lightBlocker->_shape.setPointCount(8u);
lightBlocker->_shape.setPoint(0u, { 15.f, 0.f });
lightBlocker->_shape.setPoint(1u, { 4.f, 5 });
lightBlocker->_shape.setPoint(2u, { 0, 15 });
lightBlocker->_shape.setPoint(3u, { 4, 25.f });
lightBlocker->_shape.setPoint(4u, { 15.f, 30.f });
lightBlocker->_shape.setPoint(5u, { 25.f, 26 });
lightBlocker->_shape.setPoint(6u, { 29, 15 });
lightBlocker->_shape.setPoint(7u, { 25, 5.f });
lightBlocker->_shape.setPosition(Vector2f(m_pos.x, m_pos.y));
ltbl::LightSystem::GetInstance()->addShape(lightBlocker);
}
void Door::createBox2dBody()
{
m_bodyDef.type = b2_kinematicBody;
m_bodyDef.position.Set((position.x + size.x / 2.f) / SCALE, (position.y + size.y / 2.f) / SCALE);
m_bodyDef.userData = this;
//m_bodyDef.angle = 0;
//m_bodyDef.fixedRotation = true;
m_body = m_world->CreateBody(&m_bodyDef);
dynamicBox.SetAsBox((size.x/2) / SCALE, (size.y/2) / SCALE, b2Vec2(size.x/2/SCALE,size.y/2/SCALE),0);
fixtureDef.shape = &dynamicBox;
fixtureDef.restitution = b2MixRestitution(0, 0);
fixtureDef.density = 1.f;
fixtureDef.userData = "Door";
fixtureDef.filter.categoryBits = DOOR;
fixtureDef.filter.maskBits = PLAYER | CRATE ;
m_body->CreateFixture(&fixtureDef);
}
void Enemy::createBox2dBody()
{
b2BodyDef bodyDef;
bodyDef.type = b2_dynamicBody;
bodyDef.position.Set(m_pos.x / SCALE, m_pos.y / SCALE);
bodyDef.userData = this;
bodyDef.gravityScale = 1;
body = world->CreateBody(&bodyDef);
b2CircleShape circle;
circle.m_radius = 7 / SCALE;
fixtureDef.shape = &circle;
fixtureDef.density = 1;
fixtureDef.friction = 0.3f;
fixtureDef.userData = "Enemy";
fixtureDef.restitution = b2MixRestitution(0, 0);
fixtureDef.filter.categoryBits = ENEMY;
fixtureDef.filter.maskBits = ENEMY | PLAYER | ITEM | CONTAINER | WALL | DOOR | FILLER;
body->CreateFixture(&fixtureDef);
body->SetFixedRotation(false);
}
b2ContactSolver::b2ContactSolver(b2Contact** contacts, int32 contactCount,
b2StackAllocator* allocator, float32 impulseRatio)
{
m_allocator = allocator;
m_constraintCount = contactCount;
m_constraints = (b2ContactConstraint*)m_allocator->Allocate(m_constraintCount * sizeof(b2ContactConstraint));
for (int32 i = 0; i < m_constraintCount; ++i)
{
b2Contact* contact = contacts[i];
b2Fixture* fixtureA = contact->m_fixtureA;
b2Fixture* fixtureB = contact->m_fixtureB;
b2Shape* shapeA = fixtureA->GetShape();
b2Shape* shapeB = fixtureB->GetShape();
float32 radiusA = shapeA->m_radius;
float32 radiusB = shapeB->m_radius;
b2Body* bodyA = fixtureA->GetBody();
b2Body* bodyB = fixtureB->GetBody();
b2Manifold* manifold = contact->GetManifold();
float32 friction = b2MixFriction(fixtureA->GetFriction(), fixtureB->GetFriction());
float32 restitution = b2MixRestitution(fixtureA->GetRestitution(), fixtureB->GetRestitution());
b2Vec2 vA = bodyA->m_linearVelocity;
b2Vec2 vB = bodyB->m_linearVelocity;
float32 wA = bodyA->m_angularVelocity;
float32 wB = bodyB->m_angularVelocity;
b2Assert(manifold->pointCount > 0);
b2WorldManifold worldManifold;
worldManifold.Initialize(manifold, bodyA->m_xf, radiusA, bodyB->m_xf, radiusB);
b2ContactConstraint* cc = m_constraints + i;
cc->bodyA = bodyA;
cc->bodyB = bodyB;
cc->manifold = manifold;
cc->normal = worldManifold.normal;
cc->pointCount = manifold->pointCount;
cc->friction = friction;
cc->localNormal = manifold->localNormal;
cc->localPoint = manifold->localPoint;
cc->radius = radiusA + radiusB;
cc->type = manifold->type;
//Conveyor
cc->fixtureA = fixtureA;
cc->fixtureB = fixtureB;
//End Conveyor
for (int32 j = 0; j < cc->pointCount; ++j)
{
b2ManifoldPoint* cp = manifold->points + j;
b2ContactConstraintPoint* ccp = cc->points + j;
ccp->normalImpulse = impulseRatio * cp->normalImpulse;
ccp->tangentImpulse = impulseRatio * cp->tangentImpulse;
ccp->localPoint = cp->localPoint;
ccp->rA = worldManifold.points[j] - bodyA->m_sweep.c;
ccp->rB = worldManifold.points[j] - bodyB->m_sweep.c;
float32 rnA = b2Cross(ccp->rA, cc->normal);
float32 rnB = b2Cross(ccp->rB, cc->normal);
rnA *= rnA;
rnB *= rnB;
float32 kNormal = bodyA->m_invMass + bodyB->m_invMass + bodyA->m_invI * rnA + bodyB->m_invI * rnB;
b2Assert(kNormal > b2_epsilon);
ccp->normalMass = 1.0f / kNormal;
b2Vec2 tangent = b2Cross(cc->normal, 1.0f);
float32 rtA = b2Cross(ccp->rA, tangent);
float32 rtB = b2Cross(ccp->rB, tangent);
rtA *= rtA;
rtB *= rtB;
float32 kTangent = bodyA->m_invMass + bodyB->m_invMass + bodyA->m_invI * rtA + bodyB->m_invI * rtB;
b2Assert(kTangent > b2_epsilon);
ccp->tangentMass = 1.0f / kTangent;
// Setup a velocity bias for restitution.
ccp->velocityBias = 0.0f;
float32 vRel = b2Dot(cc->normal, vB + b2Cross(wB, ccp->rB) - vA - b2Cross(wA, ccp->rA));
if (vRel < -b2_velocityThreshold)
{
ccp->velocityBias = -restitution * vRel;
}
}
// If we have two points, then prepare the block solver.
if (cc->pointCount == 2)
{
b2ContactConstraintPoint* ccp1 = cc->points + 0;
b2ContactConstraintPoint* ccp2 = cc->points + 1;
float32 invMassA = bodyA->m_invMass;
float32 invIA = bodyA->m_invI;
float32 invMassB = bodyB->m_invMass;
float32 invIB = bodyB->m_invI;
float32 rn1A = b2Cross(ccp1->rA, cc->normal);
float32 rn1B = b2Cross(ccp1->rB, cc->normal);
float32 rn2A = b2Cross(ccp2->rA, cc->normal);
float32 rn2B = b2Cross(ccp2->rB, cc->normal);
float32 k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B;
float32 k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B;
float32 k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B;
// Ensure a reasonable condition number.
const float32 k_maxConditionNumber = 100.0f;
if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
{
// K is safe to invert.
cc->K.col1.Set(k11, k12);
cc->K.col2.Set(k12, k22);
cc->normalMass = cc->K.GetInverse();
}
else
{
// The constraints are redundant, just use one.
// TODO_ERIN use deepest?
cc->pointCount = 1;
}
}
}
}
b2ContactSolver::b2ContactSolver(const b2TimeStep& step, b2Contact** contacts, int32 contactCount, b2StackAllocator* allocator)
{
m_step = step;
m_allocator = allocator;
m_constraintCount = 0;
for (int32 i = 0; i < contactCount; ++i)
{
b2Assert(contacts[i]->IsSolid());
m_constraintCount += contacts[i]->GetManifoldCount();
}
m_constraints = (b2ContactConstraint*)m_allocator->Allocate(m_constraintCount * sizeof(b2ContactConstraint));
int32 count = 0;
for (int32 i = 0; i < contactCount; ++i)
{
b2Contact* contact = contacts[i];
b2Shape* shape1 = contact->m_shape1;
b2Shape* shape2 = contact->m_shape2;
b2Body* b1 = shape1->GetBody();
b2Body* b2 = shape2->GetBody();
int32 manifoldCount = contact->GetManifoldCount();
b2Manifold* manifolds = contact->GetManifolds();
float32 friction = b2MixFriction(shape1->GetFriction(), shape2->GetFriction());
float32 restitution = b2MixRestitution(shape1->GetRestitution(), shape2->GetRestitution());
b2Vec2 v1 = b1->m_linearVelocity;
b2Vec2 v2 = b2->m_linearVelocity;
float32 w1 = b1->m_angularVelocity;
float32 w2 = b2->m_angularVelocity;
for (int32 j = 0; j < manifoldCount; ++j)
{
b2Manifold* manifold = manifolds + j;
b2Assert(manifold->pointCount > 0);
const b2Vec2 normal = manifold->normal;
b2Assert(count < m_constraintCount);
b2ContactConstraint* cc = m_constraints + count;
cc->body1 = b1;
cc->body2 = b2;
cc->manifold = manifold;
cc->normal = normal;
cc->pointCount = manifold->pointCount;
cc->friction = friction;
cc->restitution = restitution;
for (int32 k = 0; k < cc->pointCount; ++k)
{
b2ManifoldPoint* cp = manifold->points + k;
b2ContactConstraintPoint* ccp = cc->points + k;
ccp->normalImpulse = cp->normalImpulse;
ccp->tangentImpulse = cp->tangentImpulse;
ccp->separation = cp->separation;
ccp->localAnchor1 = cp->localPoint1;
ccp->localAnchor2 = cp->localPoint2;
ccp->r1 = b2Mul(b1->GetXForm().R, cp->localPoint1 - b1->GetLocalCenter());
ccp->r2 = b2Mul(b2->GetXForm().R, cp->localPoint2 - b2->GetLocalCenter());
float32 rn1 = b2Cross(ccp->r1, normal);
float32 rn2 = b2Cross(ccp->r2, normal);
rn1 *= rn1;
rn2 *= rn2;
float32 kNormal = b1->m_invMass + b2->m_invMass + b1->m_invI * rn1 + b2->m_invI * rn2;
b2Assert(kNormal > B2_FLT_EPSILON);
ccp->normalMass = 1.0f / kNormal;
float32 kEqualized = b1->m_mass * b1->m_invMass + b2->m_mass * b2->m_invMass;
kEqualized += b1->m_mass * b1->m_invI * rn1 + b2->m_mass * b2->m_invI * rn2;
b2Assert(kEqualized > B2_FLT_EPSILON);
ccp->equalizedMass = 1.0f / kEqualized;
b2Vec2 tangent = b2Cross(normal, 1.0f);
float32 rt1 = b2Cross(ccp->r1, tangent);
float32 rt2 = b2Cross(ccp->r2, tangent);
rt1 *= rt1;
rt2 *= rt2;
float32 kTangent = b1->m_invMass + b2->m_invMass + b1->m_invI * rt1 + b2->m_invI * rt2;
b2Assert(kTangent > B2_FLT_EPSILON);
ccp->tangentMass = 1.0f / kTangent;
// Setup a velocity bias for restitution.
ccp->velocityBias = 0.0f;
if (ccp->separation > 0.0f)
{
ccp->velocityBias = -step.inv_dt * ccp->separation; // TODO_ERIN b2TimeStep
}
else
{
float32 vRel = b2Dot(cc->normal, v2 + b2Cross(w2, ccp->r2) - v1 - b2Cross(w1, ccp->r1));
if (vRel < -b2_velocityThreshold)
{
ccp->velocityBias = -cc->restitution * vRel;
}
}
}
// If we have two points, then prepare the block solver.
if (cc->pointCount == 2)
{
b2ContactConstraintPoint* ccp1 = cc->points + 0;
b2ContactConstraintPoint* ccp2 = cc->points + 1;
float32 invMass1 = b1->m_invMass;
float32 invI1 = b1->m_invI;
float32 invMass2 = b2->m_invMass;
float32 invI2 = b2->m_invI;
float32 rn11 = b2Cross(ccp1->r1, normal);
float32 rn12 = b2Cross(ccp1->r2, normal);
float32 rn21 = b2Cross(ccp2->r1, normal);
float32 rn22 = b2Cross(ccp2->r2, normal);
float32 k11 = invMass1 + invMass2 + invI1 * rn11 * rn11 + invI2 * rn12 * rn12;
float32 k22 = invMass1 + invMass2 + invI1 * rn21 * rn21 + invI2 * rn22 * rn22;
float32 k12 = invMass1 + invMass2 + invI1 * rn11 * rn21 + invI2 * rn12 * rn22;
// Ensure a reasonable condition number.
const float32 k_maxConditionNumber = 100.0f;
if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
{
// K is safe to invert.
cc->K.col1.Set(k11, k12);
cc->K.col2.Set(k12, k22);
cc->normalMass = cc->K.GetInverse();
}
else
{
// The constraints are redundant, just use one.
// TODO_ERIN use deepest?
cc->pointCount = 1;
}
}
++count;
}
}
b2Assert(count == m_constraintCount);
}
void b2PolygonContact::Evaluate(b2ContactListener* listener)
{
b2Body* b1 = m_shape1->GetBody();
b2Body* b2 = m_shape2->GetBody();
b2Manifold m0;
memcpy(&m0, &m_manifold, sizeof(b2Manifold));
b2CollidePolygons(&m_manifold, (b2PolygonShape*)m_shape1, b1->GetXForm(), (b2PolygonShape*)m_shape2, b2->GetXForm());
bool persisted[b2_maxManifoldPoints] = {false, false};
b2ContactPoint cp;
cp.shape1 = m_shape1;
cp.shape2 = m_shape2;
cp.friction = b2MixFriction(m_shape1->GetFriction(), m_shape2->GetFriction());
cp.restitution = b2MixRestitution(m_shape1->GetRestitution(), m_shape2->GetRestitution());
// Match contact ids to facilitate warm starting.
if (m_manifold.pointCount > 0)
{
// Match old contact ids to new contact ids and copy the
// stored impulses to warm start the solver.
for (int32 i = 0; i < m_manifold.pointCount; ++i)
{
b2ManifoldPoint* mp = m_manifold.points + i;
mp->normalImpulse = 0.0f;
mp->tangentImpulse = 0.0f;
bool found = false;
b2ContactID id = mp->id;
for (int32 j = 0; j < m0.pointCount; ++j)
{
if (persisted[j] == true)
{
continue;
}
b2ManifoldPoint* mp0 = m0.points + j;
if (mp0->id.key == id.key)
{
persisted[j] = true;
mp->normalImpulse = mp0->normalImpulse;
mp->tangentImpulse = mp0->tangentImpulse;
// A persistent point.
found = true;
// Report persistent point.
if (listener != NULL)
{
cp.position = b1->GetWorldPoint(mp->localPoint1);
b2Vec2 v1 = b1->GetLinearVelocityFromLocalPoint(mp->localPoint1);
b2Vec2 v2 = b2->GetLinearVelocityFromLocalPoint(mp->localPoint2);
cp.velocity = v2 - v1;
cp.normal = m_manifold.normal;
cp.separation = mp->separation;
cp.id = id;
listener->Persist(&cp);
}
break;
}
}
// Report added point.
if (found == false && listener != NULL)
{
cp.position = b1->GetWorldPoint(mp->localPoint1);
b2Vec2 v1 = b1->GetLinearVelocityFromLocalPoint(mp->localPoint1);
b2Vec2 v2 = b2->GetLinearVelocityFromLocalPoint(mp->localPoint2);
cp.velocity = v2 - v1;
cp.normal = m_manifold.normal;
cp.separation = mp->separation;
cp.id = id;
listener->Add(&cp);
}
}
m_manifoldCount = 1;
}
else
{
m_manifoldCount = 0;
}
if (listener == NULL)
{
return;
}
// Report removed points.
for (int32 i = 0; i < m0.pointCount; ++i)
{
if (persisted[i])
{
continue;
}
b2ManifoldPoint* mp0 = m0.points + i;
cp.position = b1->GetWorldPoint(mp0->localPoint1);
b2Vec2 v1 = b1->GetLinearVelocityFromLocalPoint(mp0->localPoint1);
b2Vec2 v2 = b2->GetLinearVelocityFromLocalPoint(mp0->localPoint2);
cp.velocity = v2 - v1;
cp.normal = m0.normal;
cp.separation = mp0->separation;
cp.id = mp0->id;
listener->Remove(&cp);
}
}
b2ContactSolver::b2ContactSolver(b2ContactSolverDef* def)
{
m_allocator = def->allocator;
m_count = def->count;
m_constraints = (b2ContactConstraint*)m_allocator->Allocate(m_count * sizeof(b2ContactConstraint));
// Initialize position independent portions of the constraints.
for (int32 i = 0; i < m_count; ++i)
{
b2Contact* contact = def->contacts[i];
b2Fixture* fixtureA = contact->m_fixtureA;
b2Fixture* fixtureB = contact->m_fixtureB;
b2Shape* shapeA = fixtureA->GetShape();
b2Shape* shapeB = fixtureB->GetShape();
qreal radiusA = shapeA->m_radius;
qreal radiusB = shapeB->m_radius;
b2Body* bodyA = fixtureA->GetBody();
b2Body* bodyB = fixtureB->GetBody();
b2Manifold* manifold = contact->GetManifold();
b2Assert(manifold->pointCount > 0);
b2ContactConstraint* cc = m_constraints + i;
cc->friction = b2MixFriction(fixtureA->GetFriction(), fixtureB->GetFriction());
cc->restitution = b2MixRestitution(fixtureA->GetRestitution(), fixtureB->GetRestitution());
cc->bodyA = bodyA;
cc->bodyB = bodyB;
cc->manifold = manifold;
cc->normal.SetZero();
cc->pointCount = manifold->pointCount;
cc->localNormal = manifold->localNormal;
cc->localPoint = manifold->localPoint;
cc->radiusA = radiusA;
cc->radiusB = radiusB;
cc->type = manifold->type;
for (int32 j = 0; j < cc->pointCount; ++j)
{
b2ManifoldPoint* cp = manifold->points + j;
b2ContactConstraintPoint* ccp = cc->points + j;
if (def->warmStarting)
{
ccp->normalImpulse = def->impulseRatio * cp->normalImpulse;
ccp->tangentImpulse = def->impulseRatio * cp->tangentImpulse;
}
else
{
ccp->normalImpulse = 0.0f;
ccp->tangentImpulse = 0.0f;
}
ccp->localPoint = cp->localPoint;
ccp->rA.SetZero();
ccp->rB.SetZero();
ccp->normalMass = 0.0f;
ccp->tangentMass = 0.0f;
ccp->velocityBias = 0.0f;
}
cc->K.SetZero();
cc->normalMass.SetZero();
}
}
