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; }
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; } } } }
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(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); }
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(); } }