// Collide and edge and polygon. This uses the SAT and clipping to produce up to 2 contact points.
// Edge adjacency is handle to produce locally valid contact points and normals. This is intended
// to allow the polygon to slide smoothly over an edge chain.
//
// Algorithm
// 1. Classify front-side or back-side collision with edge.
// 2. Compute separation
// 3. Process adjacent edges
// 4. Classify adjacent edge as convex, flat, null, or concave
// 5. Skip null or concave edges. Concave edges get a separate manifold.
// 6. If the edge is flat, compute contact points as normal. Discard boundary points.
// 7. If the edge is convex, compute it's separation.
// 8. Use the minimum separation of up to three edges. If the minimum separation
// is not the primary edge, return.
// 9. If the minimum separation is the primary edge, compute the contact points and return.
void b2CollideEdgeAndPolygon( b2Manifold* manifold,
const b2EdgeShape* edgeA, const b2Transform& xfA,
const b2PolygonShape* polygonB_in, const b2Transform& xfB)
{
manifold->pointCount = 0;
b2Transform xf = b2MulT(xfA, xfB);
// Create a polygon for edge shape A
b2PolygonShape polygonA;
polygonA.SetAsEdge(edgeA->m_vertex1, edgeA->m_vertex2);
// Build polygonB in frame A
b2PolygonShape polygonB;
polygonB.m_radius = polygonB_in->m_radius;
polygonB.m_vertexCount = polygonB_in->m_vertexCount;
polygonB.m_centroid = b2Mul(xf, polygonB_in->m_centroid);
for (int32 i = 0; i < polygonB.m_vertexCount; ++i)
{
polygonB.m_vertices[i] = b2Mul(xf, polygonB_in->m_vertices[i]);
polygonB.m_normals[i] = b2Mul(xf.R, polygonB_in->m_normals[i]);
}
float32 totalRadius = polygonA.m_radius + polygonB.m_radius;
// Edge geometry
b2Vec2 v1 = edgeA->m_vertex1;
b2Vec2 v2 = edgeA->m_vertex2;
b2Vec2 e = v2 - v1;
b2Vec2 edgeNormal(e.y, -e.x);
edgeNormal.Normalize();
// Determine side
bool isFrontSide = b2Dot(edgeNormal, polygonB.m_centroid - v1) >= 0.0f;
if (isFrontSide == false)
{
edgeNormal = -edgeNormal;
}
// Compute primary separating axis
b2EPAxis edgeAxis = b2EPEdgeSeparation(v1, v2, edgeNormal, &polygonB, totalRadius);
if (edgeAxis.separation > totalRadius)
{
// Shapes are separated
return;
}
// Classify adjacent edges
b2EdgeType types[2] = {b2_isolated, b2_isolated};
if (edgeA->m_hasVertex0)
{
b2Vec2 v0 = edgeA->m_vertex0;
float32 s = b2Dot(edgeNormal, v0 - v1);
if (s > 0.1f * b2_linearSlop)
{
types[0] = b2_concave;
}
else if (s >= -0.1f * b2_linearSlop)
{
types[0] = b2_flat;
}
else
{
types[0] = b2_convex;
}
}
if (edgeA->m_hasVertex3)
{
b2Vec2 v3 = edgeA->m_vertex3;
float32 s = b2Dot(edgeNormal, v3 - v2);
if (s > 0.1f * b2_linearSlop)
{
types[1] = b2_concave;
}
else if (s >= -0.1f * b2_linearSlop)
{
types[1] = b2_flat;
}
else
{
types[1] = b2_convex;
}
}
if (types[0] == b2_convex)
{
// Check separation on previous edge.
b2Vec2 v0 = edgeA->m_vertex0;
b2Vec2 e0 = v1 - v0;
b2Vec2 n0(e0.y, -e0.x);
n0.Normalize();
if (isFrontSide == false)
{
n0 = -n0;
}
b2EPAxis axis1 = b2EPEdgeSeparation(v0, v1, n0, &polygonB, totalRadius);
if (axis1.separation > edgeAxis.separation)
{
// The polygon should collide with previous edge
return;
}
}
if (types[1] == b2_convex)
{
// Check separation on next edge.
b2Vec2 v3 = edgeA->m_vertex3;
b2Vec2 e2 = v3 - v2;
b2Vec2 n2(e2.y, -e2.x);
n2.Normalize();
if (isFrontSide == false)
{
n2 = -n2;
}
b2EPAxis axis2 = b2EPEdgeSeparation(v2, v3, n2, &polygonB, totalRadius);
if (axis2.separation > edgeAxis.separation)
{
// The polygon should collide with the next edge
return;
}
}
b2EPAxis polygonAxis = b2EPPolygonSeparation(v1, v2, edgeNormal, &polygonB, totalRadius);
if (polygonAxis.separation > totalRadius)
{
return;
}
// Use hysteresis for jitter reduction.
const float32 k_relativeTol = 0.98f;
const float32 k_absoluteTol = 0.001f;
b2EPAxis primaryAxis;
if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol)
{
primaryAxis = polygonAxis;
}
else
{
primaryAxis = edgeAxis;
}
b2PolygonShape* poly1;
b2PolygonShape* poly2;
b2ClipVertex incidentEdge[2];
if (primaryAxis.type == b2EPAxis::e_edgeA)
{
poly1 = &polygonA;
poly2 = &polygonB;
if (isFrontSide == false)
{
primaryAxis.index = 1;
}
manifold->type = b2Manifold::e_faceA;
}
else
{
poly1 = &polygonB;
poly2 = &polygonA;
manifold->type = b2Manifold::e_faceB;
}
int32 edge1 = primaryAxis.index;
b2FindIncidentEdge(incidentEdge, poly1, primaryAxis.index, poly2);
int32 count1 = poly1->m_vertexCount;
const b2Vec2* vertices1 = poly1->m_vertices;
int32 iv1 = edge1;
int32 iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
b2Vec2 v11 = vertices1[iv1];
b2Vec2 v12 = vertices1[iv2];
b2Vec2 tangent = v12 - v11;
tangent.Normalize();
b2Vec2 normal = b2Cross(tangent, 1.0f);
b2Vec2 planePoint = 0.5f * (v11 + v12);
// Face offset.
float32 frontOffset = b2Dot(normal, v11);
// Side offsets, extended by polytope skin thickness.
float32 sideOffset1 = -b2Dot(tangent, v11) + totalRadius;
float32 sideOffset2 = b2Dot(tangent, v12) + totalRadius;
// Clip incident edge against extruded edge1 side edges.
b2ClipVertex clipPoints1[2];
b2ClipVertex clipPoints2[2];
int np;
// Clip to box side 1
np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, iv1);
if (np < b2_maxManifoldPoints)
{
return;
}
// Clip to negative box side 1
np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, iv2);
if (np < b2_maxManifoldPoints)
{
return;
}
// Now clipPoints2 contains the clipped points.
if (primaryAxis.type == b2EPAxis::e_edgeA)
{
manifold->localNormal = normal;
manifold->localPoint = planePoint;
}
else
{
manifold->localNormal = b2MulT(xf.R, normal);
manifold->localPoint = b2MulT(xf, planePoint);
}
int32 pointCount = 0;
for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
{
float32 separation;
separation = b2Dot(normal, clipPoints2[i].v) - frontOffset;
if (separation <= totalRadius)
{
b2ManifoldPoint* cp = manifold->points + pointCount;
if (primaryAxis.type == b2EPAxis::e_edgeA)
{
cp->localPoint = b2MulT(xf, clipPoints2[i].v);
cp->id = clipPoints2[i].id;
}
else
{
cp->localPoint = clipPoints2[i].v;
cp->id.cf.typeA = clipPoints2[i].id.cf.typeB;
cp->id.cf.typeB = clipPoints2[i].id.cf.typeA;
cp->id.cf.indexA = clipPoints2[i].id.cf.indexB;
cp->id.cf.indexB = clipPoints2[i].id.cf.indexA;
}
if (cp->id.cf.typeA == b2ContactFeature::e_vertex && types[cp->id.cf.indexA] == b2_flat)
{
continue;
}
++pointCount;
}
}
manifold->pointCount = pointCount;
}
void b2ContactSolver::SolveVelocityConstraints()
{
for (int32 i = 0; i < m_count; ++i)
{
b2ContactVelocityConstraint* vc = m_velocityConstraints + i;
int32 indexA = vc->indexA;
int32 indexB = vc->indexB;
float32 mA = vc->invMassA;
float32 iA = vc->invIA;
float32 mB = vc->invMassB;
float32 iB = vc->invIB;
int32 pointCount = vc->pointCount;
b2Vec2 vA = m_velocities[indexA].v;
float32 wA = m_velocities[indexA].w;
b2Vec2 vB = m_velocities[indexB].v;
float32 wB = m_velocities[indexB].w;
b2Vec2 normal = vc->normal;
b2Vec2 tangent = b2Cross(normal, 1.0f);
float32 friction = vc->friction;
b2Assert(pointCount == 1 || pointCount == 2);
// Solve tangent constraints first because non-penetration is more important
// than friction.
for (int32 j = 0; j < pointCount; ++j)
{
b2VelocityConstraintPoint* vcp = vc->points + j;
// Relative velocity at contact
b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA);
// Compute tangent force
float32 vt = b2Dot(dv, tangent);
float32 lambda = vcp->tangentMass * (-vt);
// b2Clamp the accumulated force
float32 maxFriction = friction * vcp->normalImpulse;
float32 newImpulse = b2Clamp(vcp->tangentImpulse + lambda, -maxFriction, maxFriction);
lambda = newImpulse - vcp->tangentImpulse;
vcp->tangentImpulse = newImpulse;
// Apply contact impulse
b2Vec2 P = lambda * tangent;
vA -= mA * P;
wA -= iA * b2Cross(vcp->rA, P);
vB += mB * P;
wB += iB * b2Cross(vcp->rB, P);
}
// Solve normal constraints
if (vc->pointCount == 1)
{
b2VelocityConstraintPoint* vcp = vc->points + 0;
// Relative velocity at contact
b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA);
// Compute normal impulse
float32 vn = b2Dot(dv, normal);
float32 lambda = -vcp->normalMass * (vn - vcp->velocityBias);
// b2Clamp the accumulated impulse
float32 newImpulse = b2Max(vcp->normalImpulse + lambda, 0.0f);
lambda = newImpulse - vcp->normalImpulse;
vcp->normalImpulse = newImpulse;
// Apply contact impulse
b2Vec2 P = lambda * normal;
vA -= mA * P;
wA -= iA * b2Cross(vcp->rA, P);
vB += mB * P;
wB += iB * b2Cross(vcp->rB, P);
}
else
{
// Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite).
// Build the mini LCP for this contact patch
//
// vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
//
// A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )
// b = vn0 - velocityBias
//
// The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i
// implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases
// vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid
// solution that satisfies the problem is chosen.
//
// In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires
// that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i).
//
// Substitute:
//
// x = a + d
//
// a := old total impulse
// x := new total impulse
// d := incremental impulse
//
// For the current iteration we extend the formula for the incremental impulse
// to compute the new total impulse:
//
// vn = A * d + b
// = A * (x - a) + b
// = A * x + b - A * a
// = A * x + b'
// b' = b - A * a;
b2VelocityConstraintPoint* cp1 = vc->points + 0;
b2VelocityConstraintPoint* cp2 = vc->points + 1;
b2Vec2 a(cp1->normalImpulse, cp2->normalImpulse);
b2Assert(a.x >= 0.0f && a.y >= 0.0f);
// Relative velocity at contact
b2Vec2 dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);
b2Vec2 dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);
// Compute normal velocity
float32 vn1 = b2Dot(dv1, normal);
float32 vn2 = b2Dot(dv2, normal);
b2Vec2 b;
b.x = vn1 - cp1->velocityBias;
b.y = vn2 - cp2->velocityBias;
// Compute b'
b -= b2Mul(vc->K, a);
const float32 k_errorTol = 1e-3f;
B2_NOT_USED(k_errorTol);
for (;;)
{
//
// Case 1: vn = 0
//
// 0 = A * x + b'
//
// Solve for x:
//
// x = - inv(A) * b'
//
b2Vec2 x = - b2Mul(vc->normalMass, b);
if (x.x >= 0.0f && x.y >= 0.0f)
{
// Get the incremental impulse
b2Vec2 d = x - a;
// Apply incremental impulse
b2Vec2 P1 = d.x * normal;
b2Vec2 P2 = d.y * normal;
vA -= mA * (P1 + P2);
wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
vB += mB * (P1 + P2);
wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
// Accumulate
cp1->normalImpulse = x.x;
cp2->normalImpulse = x.y;
#if B2_DEBUG_SOLVER == 1
// Postconditions
dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);
dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);
// Compute normal velocity
vn1 = b2Dot(dv1, normal);
vn2 = b2Dot(dv2, normal);
b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol);
b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol);
#endif
break;
}
//
// Case 2: vn1 = 0 and x2 = 0
//
// 0 = a11 * x1 + a12 * 0 + b1'
// vn2 = a21 * x1 + a22 * 0 + b2'
//
x.x = - cp1->normalMass * b.x;
x.y = 0.0f;
vn1 = 0.0f;
vn2 = vc->K.ex.y * x.x + b.y;
if (x.x >= 0.0f && vn2 >= 0.0f)
{
// Get the incremental impulse
b2Vec2 d = x - a;
// Apply incremental impulse
b2Vec2 P1 = d.x * normal;
b2Vec2 P2 = d.y * normal;
vA -= mA * (P1 + P2);
wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
vB += mB * (P1 + P2);
wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
// Accumulate
cp1->normalImpulse = x.x;
cp2->normalImpulse = x.y;
#if B2_DEBUG_SOLVER == 1
// Postconditions
dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);
// Compute normal velocity
vn1 = b2Dot(dv1, normal);
b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol);
#endif
break;
}
//
// Case 3: vn2 = 0 and x1 = 0
//
// vn1 = a11 * 0 + a12 * x2 + b1'
// 0 = a21 * 0 + a22 * x2 + b2'
//
x.x = 0.0f;
x.y = - cp2->normalMass * b.y;
vn1 = vc->K.ey.x * x.y + b.x;
vn2 = 0.0f;
if (x.y >= 0.0f && vn1 >= 0.0f)
{
// Resubstitute for the incremental impulse
b2Vec2 d = x - a;
// Apply incremental impulse
b2Vec2 P1 = d.x * normal;
b2Vec2 P2 = d.y * normal;
vA -= mA * (P1 + P2);
wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
vB += mB * (P1 + P2);
wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
// Accumulate
cp1->normalImpulse = x.x;
cp2->normalImpulse = x.y;
#if B2_DEBUG_SOLVER == 1
// Postconditions
dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);
// Compute normal velocity
vn2 = b2Dot(dv2, normal);
b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol);
#endif
break;
}
//
// Case 4: x1 = 0 and x2 = 0
//
// vn1 = b1
// vn2 = b2;
x.x = 0.0f;
x.y = 0.0f;
vn1 = b.x;
vn2 = b.y;
if (vn1 >= 0.0f && vn2 >= 0.0f )
{
// Resubstitute for the incremental impulse
b2Vec2 d = x - a;
// Apply incremental impulse
b2Vec2 P1 = d.x * normal;
b2Vec2 P2 = d.y * normal;
vA -= mA * (P1 + P2);
wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
vB += mB * (P1 + P2);
wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
// Accumulate
cp1->normalImpulse = x.x;
cp2->normalImpulse = x.y;
break;
}
// No solution, give up. This is hit sometimes, but it doesn't seem to matter.
break;
}
}
m_velocities[indexA].v = vA;
m_velocities[indexA].w = wA;
m_velocities[indexB].v = vB;
m_velocities[indexB].w = wB;
}
}
void b2FrictionJoint::InitVelocityConstraints(const b2SolverData& data)
{
m_indexA = m_bodyA->m_islandIndex;
m_indexB = m_bodyB->m_islandIndex;
m_localCenterA = m_bodyA->m_sweep.localCenter;
m_localCenterB = m_bodyB->m_sweep.localCenter;
m_invMassA = m_bodyA->m_invMass;
m_invMassB = m_bodyB->m_invMass;
m_invIA = m_bodyA->m_invI;
m_invIB = m_bodyB->m_invI;
float32 aA = data.positions[m_indexA].a;
b2Vec2 vA = data.velocities[m_indexA].v;
float32 wA = data.velocities[m_indexA].w;
float32 aB = data.positions[m_indexB].a;
b2Vec2 vB = data.velocities[m_indexB].v;
float32 wB = data.velocities[m_indexB].w;
b2Rot qA(aA), qB(aB);
// Compute the effective mass matrix.
m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
float32 mA = m_invMassA, mB = m_invMassB;
float32 iA = m_invIA, iB = m_invIB;
b2Mat22 K;
K.ex.x = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y;
K.ex.y = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y;
K.ey.x = K.ex.y;
K.ey.y = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x;
m_linearMass = K.GetInverse();
m_angularMass = iA + iB;
if (m_angularMass > 0.0f)
{
m_angularMass = 1.0f / m_angularMass;
}
if (data.step.warmStarting)
{
// Scale impulses to support a variable time step.
m_linearImpulse *= data.step.dtRatio;
m_angularImpulse *= data.step.dtRatio;
b2Vec2 P(m_linearImpulse.x, m_linearImpulse.y);
vA -= mA * P;
wA -= iA * (b2Cross(m_rA, P) + m_angularImpulse);
vB += mB * P;
wB += iB * (b2Cross(m_rB, P) + m_angularImpulse);
}
else
{
m_linearImpulse.SetZero();
m_angularImpulse = 0.0f;
}
data.velocities[m_indexA].v = vA;
data.velocities[m_indexA].w = wA;
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}
bool b2RevoluteJoint::SolvePositionConstraints(float32 baumgarte)
{
// TODO_ERIN block solve with limit.
B2_NOT_USED(baumgarte);
b2Body* b1 = m_bodyA;
b2Body* b2 = m_bodyB;
float32 angularError = 0.0f;
float32 positionError = 0.0f;
// Solve angular limit constraint.
if (m_enableLimit && m_limitState != e_inactiveLimit)
{
float32 angle = b2->m_sweep.a - b1->m_sweep.a - m_referenceAngle;
float32 limitImpulse = 0.0f;
if (m_limitState == e_equalLimits)
{
// Prevent large angular corrections
float32 C = b2Clamp(angle - m_lowerAngle, -b2_maxAngularCorrection, b2_maxAngularCorrection);
limitImpulse = -m_motorMass * C;
angularError = b2Abs(C);
}
else if (m_limitState == e_atLowerLimit)
{
float32 C = angle - m_lowerAngle;
angularError = -C;
// Prevent large angular corrections and allow some slop.
C = b2Clamp(C + b2_angularSlop, -b2_maxAngularCorrection, 0.0f);
limitImpulse = -m_motorMass * C;
}
else if (m_limitState == e_atUpperLimit)
{
float32 C = angle - m_upperAngle;
angularError = C;
// Prevent large angular corrections and allow some slop.
C = b2Clamp(C - b2_angularSlop, 0.0f, b2_maxAngularCorrection);
limitImpulse = -m_motorMass * C;
}
b1->m_sweep.a -= b1->m_invI * limitImpulse;
b2->m_sweep.a += b2->m_invI * limitImpulse;
b1->SynchronizeTransform();
b2->SynchronizeTransform();
}
// Solve point-to-point constraint.
{
b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
b2Vec2 C = b2->m_sweep.c + r2 - b1->m_sweep.c - r1;
positionError = C.Length();
float32 invMass1 = b1->m_invMass, invMass2 = b2->m_invMass;
float32 invI1 = b1->m_invI, invI2 = b2->m_invI;
// Handle large detachment.
const float32 k_allowedStretch = 10.0f * b2_linearSlop;
if (C.LengthSquared() > k_allowedStretch * k_allowedStretch)
{
// Use a particle solution (no rotation).
b2Vec2 u = C; u.Normalize();
float32 m = invMass1 + invMass2;
if (m > 0.0f)
{
m = 1.0f / m;
}
b2Vec2 impulse = m * (-C);
const float32 k_beta = 0.5f;
b1->m_sweep.c -= k_beta * invMass1 * impulse;
b2->m_sweep.c += k_beta * invMass2 * impulse;
C = b2->m_sweep.c + r2 - b1->m_sweep.c - r1;
}
b2Mat22 K1;
K1.col1.x = invMass1 + invMass2; K1.col2.x = 0.0f;
K1.col1.y = 0.0f; K1.col2.y = invMass1 + invMass2;
b2Mat22 K2;
K2.col1.x = invI1 * r1.y * r1.y; K2.col2.x = -invI1 * r1.x * r1.y;
K2.col1.y = -invI1 * r1.x * r1.y; K2.col2.y = invI1 * r1.x * r1.x;
b2Mat22 K3;
K3.col1.x = invI2 * r2.y * r2.y; K3.col2.x = -invI2 * r2.x * r2.y;
K3.col1.y = -invI2 * r2.x * r2.y; K3.col2.y = invI2 * r2.x * r2.x;
b2Mat22 K = K1 + K2 + K3;
b2Vec2 impulse = K.Solve(-C);
b1->m_sweep.c -= b1->m_invMass * impulse;
b1->m_sweep.a -= b1->m_invI * b2Cross(r1, impulse);
b2->m_sweep.c += b2->m_invMass * impulse;
b2->m_sweep.a += b2->m_invI * b2Cross(r2, impulse);
b1->SynchronizeTransform();
b2->SynchronizeTransform();
}
return positionError <= b2_linearSlop && angularError <= b2_angularSlop;
}
void b2Body::ResetMassData()
{
// Compute mass data from shapes. Each shape has its own density.
m_mass = 0.0f;
m_invMass = 0.0f;
m_I = 0.0f;
m_invI = 0.0f;
m_sweep.localCenter.SetZero();
// Static and kinematic bodies have zero mass.
if (m_type == b2_staticBody || m_type == b2_kinematicBody)
{
m_sweep.c0 = m_xf.p;
m_sweep.c = m_xf.p;
m_sweep.a0 = m_sweep.a;
return;
}
b2Assert(m_type == b2_dynamicBody);
// Accumulate mass over all fixtures.
b2Vec2 localCenter = b2Vec2_zero;
for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
{
if (f->m_density == 0.0f)
{
continue;
}
b2MassData massData;
f->GetMassData(&massData);
m_mass += massData.mass;
localCenter += massData.mass * massData.center;
m_I += massData.I;
}
// Compute center of mass.
if (m_mass > 0.0f)
{
m_invMass = 1.0f / m_mass;
localCenter *= m_invMass;
}
else
{
// Force all dynamic bodies to have a positive mass.
m_mass = 1.0f;
m_invMass = 1.0f;
}
if (m_I > 0.0f && (m_flags & e_fixedRotationFlag) == 0)
{
// Center the inertia about the center of mass.
m_I -= m_mass * b2Dot(localCenter, localCenter);
b2Assert(m_I > 0.0f);
m_invI = 1.0f / m_I;
}
else
{
m_I = 0.0f;
m_invI = 0.0f;
}
// Move center of mass.
b2Vec2 oldCenter = m_sweep.c;
m_sweep.localCenter = localCenter;
m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
// Update center of mass velocity.
m_linearVelocity += b2Cross(m_angularVelocity, m_sweep.c - oldCenter);
}
bool b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
const b2Transform& xf, int32 childIndex) const
{
B2_NOT_USED(childIndex);
// Put the ray into the polygon's frame of reference.
b2Vec2 p1 = b2MulT(xf.R, input.p1 - xf.position);
b2Vec2 p2 = b2MulT(xf.R, input.p2 - xf.position);
b2Vec2 d = p2 - p1;
float32 lower = 0.0f, upper = input.maxFraction;
int32 index = -1;
for (int32 i = 0; i < m_vertexCount; ++i)
{
// p = p1 + a * d
// dot(normal, p - v) = 0
// dot(normal, p1 - v) + a * dot(normal, d) = 0
float32 numerator = b2Dot(m_normals[i], m_vertices[i] - p1);
float32 denominator = b2Dot(m_normals[i], d);
if (denominator == 0.0f)
{
if (numerator < 0.0f)
{
return false;
}
}
else
{
// Note: we want this predicate without division:
// lower < numerator / denominator, where denominator < 0
// Since denominator < 0, we have to flip the inequality:
// lower < numerator / denominator <==> denominator * lower > numerator.
if (denominator < 0.0f && numerator < lower * denominator)
{
// Increase lower.
// The segment enters this half-space.
lower = numerator / denominator;
index = i;
}
else if (denominator > 0.0f && numerator < upper * denominator)
{
// Decrease upper.
// The segment exits this half-space.
upper = numerator / denominator;
}
}
// The use of epsilon here causes the assert on lower to trip
// in some cases. Apparently the use of epsilon was to make edge
// shapes work, but now those are handled separately.
//if (upper < lower - b2_epsilon)
if (upper < lower)
{
return false;
}
}
b2Assert(0.0f <= lower && lower <= input.maxFraction);
if (index >= 0)
{
output->fraction = lower;
output->normal = b2Mul(xf.R, m_normals[index]);
return true;
}
return false;
}
void b2FrictionJoint::InitVelocityConstraints(const b2TimeStep& step)
{
b2Body* bA = m_bodyA;
b2Body* bB = m_bodyB;
// Compute the effective mass matrix.
b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter());
b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter());
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
float32 mA = bA->m_invMass, mB = bB->m_invMass;
float32 iA = bA->m_invI, iB = bB->m_invI;
b2Mat22 K1;
K1.col1.x = mA + mB; K1.col2.x = 0.0f;
K1.col1.y = 0.0f; K1.col2.y = mA + mB;
b2Mat22 K2;
K2.col1.x = iA * rA.y * rA.y; K2.col2.x = -iA * rA.x * rA.y;
K2.col1.y = -iA * rA.x * rA.y; K2.col2.y = iA * rA.x * rA.x;
b2Mat22 K3;
K3.col1.x = iB * rB.y * rB.y; K3.col2.x = -iB * rB.x * rB.y;
K3.col1.y = -iB * rB.x * rB.y; K3.col2.y = iB * rB.x * rB.x;
b2Mat22 K = K1 + K2 + K3;
m_linearMass = K.GetInverse();
m_angularMass = iA + iB;
if (m_angularMass > 0.0f)
{
m_angularMass = 1.0f / m_angularMass;
}
if (step.warmStarting)
{
// Scale impulses to support a variable time step.
m_linearImpulse *= step.dtRatio;
m_angularImpulse *= step.dtRatio;
b2Vec2 P(m_linearImpulse.x, m_linearImpulse.y);
bA->m_linearVelocity -= mA * P;
bA->m_angularVelocity -= iA * (b2Cross(rA, P) + m_angularImpulse);
bB->m_linearVelocity += mB * P;
bB->m_angularVelocity += iB * (b2Cross(rB, P) + m_angularImpulse);
}
else
{
m_linearImpulse.SetZero();
m_angularImpulse = 0.0f;
}
}
b2Body::b2Body(const b2BodyDef* bd, b2World* world)
{
b2Assert(bd->position.IsValid());
b2Assert(bd->linearVelocity.IsValid());
b2Assert(b2IsValid(bd->angle));
b2Assert(b2IsValid(bd->angularVelocity));
b2Assert(b2IsValid(bd->inertiaScale) && bd->inertiaScale >= 0.0f);
b2Assert(b2IsValid(bd->angularDamping) && bd->angularDamping >= 0.0f);
b2Assert(b2IsValid(bd->linearDamping) && bd->linearDamping >= 0.0f);
m_flags = 0;
if (bd->bullet)
{
m_flags |= e_bulletFlag;
}
if (bd->fixedRotation)
{
m_flags |= e_fixedRotationFlag;
}
if (bd->allowSleep)
{
m_flags |= e_autoSleepFlag;
}
if (bd->awake)
{
m_flags |= e_awakeFlag;
}
if (bd->active)
{
m_flags |= e_activeFlag;
}
m_world = world;
m_xf.position = bd->position;
m_xf.R.Set(bd->angle);
m_sweep.localCenter.SetZero();
m_sweep.a0 = m_sweep.a = bd->angle;
m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
m_jointList = NULL;
m_contactList = NULL;
m_prev = NULL;
m_next = NULL;
m_linearVelocity = bd->linearVelocity;
m_angularVelocity = bd->angularVelocity;
m_linearDamping = bd->linearDamping;
m_angularDamping = bd->angularDamping;
m_force.SetZero();
m_torque = 0.0f;
m_sleepTime = 0.0f;
m_type = bd->type;
if (m_type == b2_dynamicBody)
{
m_mass = 1.0f;
m_invMass = 1.0f;
}
else
{
m_mass = 0.0f;
m_invMass = 0.0f;
}
m_I = 0.0f;
m_invI = 0.0f;
m_userData = bd->userData;
m_fixtureList = NULL;
m_fixtureCount = 0;
}
void b2CollidePolygonAndCircle(
b2Manifold* manifold,
const b2PolygonShape* polygonA, const b2Transform& xfA,
const b2CircleShape* circleB, const b2Transform& xfB)
{
manifold->pointCount = 0;
// Compute circle position in the frame of the polygon.
b2Vec2 c = b2Mul(xfB, circleB->m_p);
b2Vec2 cLocal = b2MulT(xfA, c);
// Find the min separating edge.
int normalIndex = 0;
float separation = -b2_maxFloat;
float radius = polygonA->m_radius + circleB->m_radius;
int vertexCount = polygonA->m_count;
const b2Vec2* vertices = polygonA->m_vertices;
const b2Vec2* normals = polygonA->m_normals;
for (int i = 0; i < vertexCount; ++i)
{
float s = b2Dot(normals[i], cLocal - vertices[i]);
if (s > radius)
{
// Early out.
return;
}
if (s > separation)
{
separation = s;
normalIndex = i;
}
}
// Vertices that subtend the incident face.
int vertIndex1 = normalIndex;
int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
b2Vec2 v1 = vertices[vertIndex1];
b2Vec2 v2 = vertices[vertIndex2];
// If the center is inside the polygon ...
if (separation < b2_epsilon)
{
manifold->pointCount = 1;
manifold->type = b2Manifold::e_faceA;
manifold->localNormal = normals[normalIndex];
manifold->localPoint = 0.5f * (v1 + v2);
manifold->points[0].localPoint = circleB->m_p;
manifold->points[0].id.key = 0;
return;
}
// Compute barycentric coordinates
float u1 = b2Dot(cLocal - v1, v2 - v1);
float u2 = b2Dot(cLocal - v2, v1 - v2);
if (u1 <= 0.0f)
{
if (b2DistanceSquared(cLocal, v1) > radius * radius)
{
return;
}
manifold->pointCount = 1;
manifold->type = b2Manifold::e_faceA;
manifold->localNormal = cLocal - v1;
manifold->localNormal.Normalize();
manifold->localPoint = v1;
manifold->points[0].localPoint = circleB->m_p;
manifold->points[0].id.key = 0;
}
else if (u2 <= 0.0f)
{
if (b2DistanceSquared(cLocal, v2) > radius * radius)
{
return;
}
manifold->pointCount = 1;
manifold->type = b2Manifold::e_faceA;
manifold->localNormal = cLocal - v2;
manifold->localNormal.Normalize();
manifold->localPoint = v2;
manifold->points[0].localPoint = circleB->m_p;
manifold->points[0].id.key = 0;
}
else
{
b2Vec2 faceCenter = 0.5f * (v1 + v2);
float separation = b2Dot(cLocal - faceCenter, normals[vertIndex1]);
if (separation > radius)
{
return;
}
manifold->pointCount = 1;
manifold->type = b2Manifold::e_faceA;
manifold->localNormal = normals[vertIndex1];
manifold->localPoint = faceCenter;
manifold->points[0].localPoint = circleB->m_p;
manifold->points[0].id.key = 0;
}
}
bool b2PulleyJoint::SolvePositionConstraints()
{
b2Body* b1 = m_body1;
b2Body* b2 = m_body2;
b2Vec2 s1 = m_ground->GetXForm().position + m_groundAnchor1;
b2Vec2 s2 = m_ground->GetXForm().position + m_groundAnchor2;
float32 linearError = 0.0f;
if (m_state == e_atUpperLimit)
{
b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
b2Vec2 p1 = b1->m_sweep.c + r1;
b2Vec2 p2 = b2->m_sweep.c + r2;
// Get the pulley axes.
m_u1 = p1 - s1;
m_u2 = p2 - s2;
float32 length1 = m_u1.Length();
float32 length2 = m_u2.Length();
if (length1 > b2_linearSlop)
{
m_u1 *= 1.0f / length1;
}
else
{
m_u1.SetZero();
}
if (length2 > b2_linearSlop)
{
m_u2 *= 1.0f / length2;
}
else
{
m_u2.SetZero();
}
float32 C = m_constant - length1 - m_ratio * length2;
linearError = b2Max(linearError, -C);
C = b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
float32 impulse = -m_pulleyMass * C;
float32 oldImpulse = m_positionImpulse;
m_positionImpulse = b2Max(0.0f, m_positionImpulse + impulse);
impulse = m_positionImpulse - oldImpulse;
b2Vec2 P1 = -impulse * m_u1;
b2Vec2 P2 = -m_ratio * impulse * m_u2;
b1->m_sweep.c += b1->m_invMass * P1;
b1->m_sweep.a += b1->m_invI * b2Cross(r1, P1);
b2->m_sweep.c += b2->m_invMass * P2;
b2->m_sweep.a += b2->m_invI * b2Cross(r2, P2);
b1->SynchronizeTransform();
b2->SynchronizeTransform();
}
if (m_limitState1 == e_atUpperLimit)
{
b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
b2Vec2 p1 = b1->m_sweep.c + r1;
m_u1 = p1 - s1;
float32 length1 = m_u1.Length();
if (length1 > b2_linearSlop)
{
m_u1 *= 1.0f / length1;
}
else
{
m_u1.SetZero();
}
float32 C = m_maxLength1 - length1;
linearError = b2Max(linearError, -C);
C = b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
float32 impulse = -m_limitMass1 * C;
float32 oldLimitPositionImpulse = m_limitPositionImpulse1;
m_limitPositionImpulse1 = b2Max(0.0f, m_limitPositionImpulse1 + impulse);
impulse = m_limitPositionImpulse1 - oldLimitPositionImpulse;
b2Vec2 P1 = -impulse * m_u1;
b1->m_sweep.c += b1->m_invMass * P1;
b1->m_sweep.a += b1->m_invI * b2Cross(r1, P1);
b1->SynchronizeTransform();
}
if (m_limitState2 == e_atUpperLimit)
{
b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
b2Vec2 p2 = b2->m_sweep.c + r2;
m_u2 = p2 - s2;
float32 length2 = m_u2.Length();
if (length2 > b2_linearSlop)
{
m_u2 *= 1.0f / length2;
}
else
{
m_u2.SetZero();
}
float32 C = m_maxLength2 - length2;
linearError = b2Max(linearError, -C);
C = b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
float32 impulse = -m_limitMass2 * C;
float32 oldLimitPositionImpulse = m_limitPositionImpulse2;
m_limitPositionImpulse2 = b2Max(0.0f, m_limitPositionImpulse2 + impulse);
impulse = m_limitPositionImpulse2 - oldLimitPositionImpulse;
b2Vec2 P2 = -impulse * m_u2;
b2->m_sweep.c += b2->m_invMass * P2;
b2->m_sweep.a += b2->m_invI * b2Cross(r2, P2);
b2->SynchronizeTransform();
}
return linearError < b2_linearSlop;
}
void b2PulleyJoint::InitVelocityConstraints(const b2TimeStep& step)
{
b2Body* b1 = m_body1;
b2Body* b2 = m_body2;
b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
b2Vec2 p1 = b1->m_sweep.c + r1;
b2Vec2 p2 = b2->m_sweep.c + r2;
b2Vec2 s1 = m_ground->GetXForm().position + m_groundAnchor1;
b2Vec2 s2 = m_ground->GetXForm().position + m_groundAnchor2;
// Get the pulley axes.
m_u1 = p1 - s1;
m_u2 = p2 - s2;
float32 length1 = m_u1.Length();
float32 length2 = m_u2.Length();
if (length1 > b2_linearSlop)
{
m_u1 *= 1.0f / length1;
}
else
{
m_u1.SetZero();
}
if (length2 > b2_linearSlop)
{
m_u2 *= 1.0f / length2;
}
else
{
m_u2.SetZero();
}
float32 C = m_constant - length1 - m_ratio * length2;
if (C > 0.0f)
{
m_state = e_inactiveLimit;
m_force = 0.0f;
}
else
{
m_state = e_atUpperLimit;
m_positionImpulse = 0.0f;
}
if (length1 < m_maxLength1)
{
m_limitState1 = e_inactiveLimit;
m_limitForce1 = 0.0f;
}
else
{
m_limitState1 = e_atUpperLimit;
m_limitPositionImpulse1 = 0.0f;
}
if (length2 < m_maxLength2)
{
m_limitState2 = e_inactiveLimit;
m_limitForce2 = 0.0f;
}
else
{
m_limitState2 = e_atUpperLimit;
m_limitPositionImpulse2 = 0.0f;
}
// Compute effective mass.
float32 cr1u1 = b2Cross(r1, m_u1);
float32 cr2u2 = b2Cross(r2, m_u2);
m_limitMass1 = b1->m_invMass + b1->m_invI * cr1u1 * cr1u1;
m_limitMass2 = b2->m_invMass + b2->m_invI * cr2u2 * cr2u2;
m_pulleyMass = m_limitMass1 + m_ratio * m_ratio * m_limitMass2;
b2Assert(m_limitMass1 > B2_FLT_EPSILON);
b2Assert(m_limitMass2 > B2_FLT_EPSILON);
b2Assert(m_pulleyMass > B2_FLT_EPSILON);
m_limitMass1 = 1.0f / m_limitMass1;
m_limitMass2 = 1.0f / m_limitMass2;
m_pulleyMass = 1.0f / m_pulleyMass;
if (step.warmStarting)
{
// Warm starting.
b2Vec2 P1 = B2FORCE_SCALE(step.dt) * (-m_force - m_limitForce1) * m_u1;
b2Vec2 P2 = B2FORCE_SCALE(step.dt) * (-m_ratio * m_force - m_limitForce2) * m_u2;
b1->m_linearVelocity += b1->m_invMass * P1;
b1->m_angularVelocity += b1->m_invI * b2Cross(r1, P1);
b2->m_linearVelocity += b2->m_invMass * P2;
b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P2);
}
else
{
m_force = 0.0f;
m_limitForce1 = 0.0f;
m_limitForce2 = 0.0f;
}
}
b2GearJoint::b2GearJoint(const b2GearJointDef* def)
: b2Joint(def)
{
m_joint1 = def->joint1;
m_joint2 = def->joint2;
m_typeA = m_joint1->GetType();
m_typeB = m_joint2->GetType();
b2Assert(m_typeA == e_revoluteJoint || m_typeA == e_prismaticJoint);
b2Assert(m_typeB == e_revoluteJoint || m_typeB == e_prismaticJoint);
float32 coordinateA, coordinateB;
// TODO_ERIN there might be some problem with the joint edges in b2Joint.
m_bodyC = m_joint1->GetBodyA();
m_bodyA = m_joint1->GetBodyB();
// Get geometry of joint1
b2Transform xfA = m_bodyA->m_xf;
float32 aA = m_bodyA->m_sweep.a;
b2Transform xfC = m_bodyC->m_xf;
float32 aC = m_bodyC->m_sweep.a;
if (m_typeA == e_revoluteJoint)
{
b2RevoluteJoint* revolute = (b2RevoluteJoint*)def->joint1;
m_localAnchorC = revolute->m_localAnchorA;
m_localAnchorA = revolute->m_localAnchorB;
m_referenceAngleA = revolute->m_referenceAngle;
m_localAxisC.SetZero();
coordinateA = aA - aC - m_referenceAngleA;
}
else
{
b2PrismaticJoint* prismatic = (b2PrismaticJoint*)def->joint1;
m_localAnchorC = prismatic->m_localAnchorA;
m_localAnchorA = prismatic->m_localAnchorB;
m_referenceAngleA = prismatic->m_referenceAngle;
m_localAxisC = prismatic->m_localXAxisA;
b2Vec2 pC = m_localAnchorC;
b2Vec2 pA = b2MulT(xfC.q, b2Mul(xfA.q, m_localAnchorA) + (xfA.p - xfC.p));
coordinateA = b2Dot(pA - pC, m_localAxisC);
}
m_bodyD = m_joint2->GetBodyA();
m_bodyB = m_joint2->GetBodyB();
// Get geometry of joint2
b2Transform xfB = m_bodyB->m_xf;
float32 aB = m_bodyB->m_sweep.a;
b2Transform xfD = m_bodyD->m_xf;
float32 aD = m_bodyD->m_sweep.a;
if (m_typeB == e_revoluteJoint)
{
b2RevoluteJoint* revolute = (b2RevoluteJoint*)def->joint2;
m_localAnchorD = revolute->m_localAnchorA;
m_localAnchorB = revolute->m_localAnchorB;
m_referenceAngleB = revolute->m_referenceAngle;
m_localAxisD.SetZero();
coordinateB = aB - aD - m_referenceAngleB;
}
else
{
b2PrismaticJoint* prismatic = (b2PrismaticJoint*)def->joint2;
m_localAnchorD = prismatic->m_localAnchorA;
m_localAnchorB = prismatic->m_localAnchorB;
m_referenceAngleB = prismatic->m_referenceAngle;
m_localAxisD = prismatic->m_localXAxisA;
b2Vec2 pD = m_localAnchorD;
b2Vec2 pB = b2MulT(xfD.q, b2Mul(xfB.q, m_localAnchorB) + (xfB.p - xfD.p));
coordinateB = b2Dot(pB - pD, m_localAxisD);
}
m_ratio = def->ratio;
m_constant = coordinateA + m_ratio * coordinateB;
m_impulse = 0.0f;
}
bool b2GearJoint::SolvePositionConstraints(const b2SolverData& data)
{
b2Vec2 cA = data.positions[m_indexA].c;
float32 aA = data.positions[m_indexA].a;
b2Vec2 cB = data.positions[m_indexB].c;
float32 aB = data.positions[m_indexB].a;
b2Vec2 cC = data.positions[m_indexC].c;
float32 aC = data.positions[m_indexC].a;
b2Vec2 cD = data.positions[m_indexD].c;
float32 aD = data.positions[m_indexD].a;
b2Rot qA(aA), qB(aB), qC(aC), qD(aD);
float32 linearError = 0.0f;
float32 coordinateA, coordinateB;
b2Vec2 JvAC, JvBD;
float32 JwA, JwB, JwC, JwD;
float32 mass = 0.0f;
if (m_typeA == e_revoluteJoint)
{
JvAC.SetZero();
JwA = 1.0f;
JwC = 1.0f;
mass += m_iA + m_iC;
coordinateA = aA - aC - m_referenceAngleA;
}
else
{
b2Vec2 u = b2Mul(qC, m_localAxisC);
b2Vec2 rC = b2Mul(qC, m_localAnchorC - m_lcC);
b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_lcA);
JvAC = u;
JwC = b2Cross(rC, u);
JwA = b2Cross(rA, u);
mass += m_mC + m_mA + m_iC * JwC * JwC + m_iA * JwA * JwA;
b2Vec2 pC = m_localAnchorC - m_lcC;
b2Vec2 pA = b2MulT(qC, rA + (cA - cC));
coordinateA = b2Dot(pA - pC, m_localAxisC);
}
if (m_typeB == e_revoluteJoint)
{
JvBD.SetZero();
JwB = m_ratio;
JwD = m_ratio;
mass += m_ratio * m_ratio * (m_iB + m_iD);
coordinateB = aB - aD - m_referenceAngleB;
}
else
{
b2Vec2 u = b2Mul(qD, m_localAxisD);
b2Vec2 rD = b2Mul(qD, m_localAnchorD - m_lcD);
b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_lcB);
JvBD = m_ratio * u;
JwD = m_ratio * b2Cross(rD, u);
JwB = m_ratio * b2Cross(rB, u);
mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * JwD * JwD + m_iB * JwB * JwB;
b2Vec2 pD = m_localAnchorD - m_lcD;
b2Vec2 pB = b2MulT(qD, rB + (cB - cD));
coordinateB = b2Dot(pB - pD, m_localAxisD);
}
float32 C = (coordinateA + m_ratio * coordinateB) - m_constant;
float32 impulse = 0.0f;
if (mass > 0.0f)
{
impulse = -C / mass;
}
cA += m_mA * impulse * JvAC;
aA += m_iA * impulse * JwA;
cB += m_mB * impulse * JvBD;
aB += m_iB * impulse * JwB;
cC -= m_mC * impulse * JvAC;
aC -= m_iC * impulse * JwC;
cD -= m_mD * impulse * JvBD;
aD -= m_iD * impulse * JwD;
data.positions[m_indexA].c = cA;
data.positions[m_indexA].a = aA;
data.positions[m_indexB].c = cB;
data.positions[m_indexB].a = aB;
data.positions[m_indexC].c = cC;
data.positions[m_indexC].a = aC;
data.positions[m_indexD].c = cD;
data.positions[m_indexD].a = aD;
// TODO_ERIN not implemented
return linearError < b2_linearSlop;
}
void b2GearJoint::InitVelocityConstraints(const b2SolverData& data)
{
m_indexA = m_bodyA->m_islandIndex;
m_indexB = m_bodyB->m_islandIndex;
m_indexC = m_bodyC->m_islandIndex;
m_indexD = m_bodyD->m_islandIndex;
m_lcA = m_bodyA->m_sweep.localCenter;
m_lcB = m_bodyB->m_sweep.localCenter;
m_lcC = m_bodyC->m_sweep.localCenter;
m_lcD = m_bodyD->m_sweep.localCenter;
m_mA = m_bodyA->m_invMass;
m_mB = m_bodyB->m_invMass;
m_mC = m_bodyC->m_invMass;
m_mD = m_bodyD->m_invMass;
m_iA = m_bodyA->m_invI;
m_iB = m_bodyB->m_invI;
m_iC = m_bodyC->m_invI;
m_iD = m_bodyD->m_invI;
float32 aA = data.positions[m_indexA].a;
b2Vec2 vA = data.velocities[m_indexA].v;
float32 wA = data.velocities[m_indexA].w;
float32 aB = data.positions[m_indexB].a;
b2Vec2 vB = data.velocities[m_indexB].v;
float32 wB = data.velocities[m_indexB].w;
float32 aC = data.positions[m_indexC].a;
b2Vec2 vC = data.velocities[m_indexC].v;
float32 wC = data.velocities[m_indexC].w;
float32 aD = data.positions[m_indexD].a;
b2Vec2 vD = data.velocities[m_indexD].v;
float32 wD = data.velocities[m_indexD].w;
b2Rot qA(aA), qB(aB), qC(aC), qD(aD);
m_mass = 0.0f;
if (m_typeA == e_revoluteJoint)
{
m_JvAC.SetZero();
m_JwA = 1.0f;
m_JwC = 1.0f;
m_mass += m_iA + m_iC;
}
else
{
b2Vec2 u = b2Mul(qC, m_localAxisC);
b2Vec2 rC = b2Mul(qC, m_localAnchorC - m_lcC);
b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_lcA);
m_JvAC = u;
m_JwC = b2Cross(rC, u);
m_JwA = b2Cross(rA, u);
m_mass += m_mC + m_mA + m_iC * m_JwC * m_JwC + m_iA * m_JwA * m_JwA;
}
if (m_typeB == e_revoluteJoint)
{
m_JvBD.SetZero();
m_JwB = m_ratio;
m_JwD = m_ratio;
m_mass += m_ratio * m_ratio * (m_iB + m_iD);
}
else
{
b2Vec2 u = b2Mul(qD, m_localAxisD);
b2Vec2 rD = b2Mul(qD, m_localAnchorD - m_lcD);
b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_lcB);
m_JvBD = m_ratio * u;
m_JwD = m_ratio * b2Cross(rD, u);
m_JwB = m_ratio * b2Cross(rB, u);
m_mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * m_JwD * m_JwD + m_iB * m_JwB * m_JwB;
}
// Compute effective mass.
m_mass = m_mass > 0.0f ? 1.0f / m_mass : 0.0f;
if (data.step.warmStarting)
{
vA += (m_mA * m_impulse) * m_JvAC;
wA += m_iA * m_impulse * m_JwA;
vB += (m_mB * m_impulse) * m_JvBD;
wB += m_iB * m_impulse * m_JwB;
vC -= (m_mC * m_impulse) * m_JvAC;
wC -= m_iC * m_impulse * m_JwC;
vD -= (m_mD * m_impulse) * m_JvBD;
wD -= m_iD * m_impulse * m_JwD;
}
else
{
m_impulse = 0.0f;
}
data.velocities[m_indexA].v = vA;
data.velocities[m_indexA].w = wA;
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
data.velocities[m_indexC].v = vC;
data.velocities[m_indexC].w = wC;
data.velocities[m_indexD].v = vD;
data.velocities[m_indexD].w = wD;
}
bool b2WeldJoint::SolvePositionConstraints(const b2SolverData& data)
{
b2Vec2 cA = data.positions[m_indexA].c;
float32 aA = data.positions[m_indexA].a;
b2Vec2 cB = data.positions[m_indexB].c;
float32 aB = data.positions[m_indexB].a;
b2Rot qA(aA), qB(aB);
float32 mA = m_invMassA, mB = m_invMassB;
float32 iA = m_invIA, iB = m_invIB;
b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
float32 positionError, angularError;
b2Mat33 K;
K.ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
K.ey.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
K.ez.x = -rA.y * iA - rB.y * iB;
K.ex.y = K.ey.x;
K.ey.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
K.ez.y = rA.x * iA + rB.x * iB;
K.ex.z = K.ez.x;
K.ey.z = K.ez.y;
K.ez.z = iA + iB;
if (m_frequencyHz > 0.0f)
{
b2Vec2 C1 = cB + rB - cA - rA;
positionError = C1.Length();
angularError = 0.0f;
b2Vec2 P = -K.Solve22(C1);
cA -= mA * P;
aA -= iA * b2Cross(rA, P);
cB += mB * P;
aB += iB * b2Cross(rB, P);
}
else
{
b2Vec2 C1 = cB + rB - cA - rA;
float32 C2 = aB - aA - m_referenceAngle;
positionError = C1.Length();
angularError = b2Abs(C2);
b2Vec3 C(C1.x, C1.y, C2);
b2Vec3 impulse;
if (K.ez.z > 0.0f)
{
impulse = -K.Solve33(C);
}
else
{
b2Vec2 impulse2 = -K.Solve22(C1);
impulse.Set(impulse2.x, impulse2.y, 0.0f);
}
b2Vec2 P(impulse.x, impulse.y);
cA -= mA * P;
aA -= iA * (b2Cross(rA, P) + impulse.z);
cB += mB * P;
aB += iB * (b2Cross(rB, P) + impulse.z);
}
data.positions[m_indexA].c = cA;
data.positions[m_indexA].a = aA;
data.positions[m_indexB].c = cB;
data.positions[m_indexB].a = aB;
return positionError <= b2_linearSlop && angularError <= b2_angularSlop;
}
// Compute contact points for edge versus circle.
// This accounts for edge connectivity.
void b2CollideEdgeAndCircle(b2Manifold* manifold,
const b2EdgeShape* edgeA, const b2Transform& xfA,
const b2CircleShape* circleB, const b2Transform& xfB)
{
manifold->pointCount = 0;
// Compute circle in frame of edge
b2Vec2 Q = b2MulT(xfA, b2Mul(xfB, circleB->m_p));
b2Vec2 A = edgeA->m_vertex1, B = edgeA->m_vertex2;
b2Vec2 e = B - A;
// Barycentric coordinates
float32 u = b2Dot(e, B - Q);
float32 v = b2Dot(e, Q - A);
float32 radius = edgeA->m_radius + circleB->m_radius;
b2ContactFeature cf;
cf.indexB = 0;
cf.typeB = b2ContactFeature::e_vertex;
// Region A
if (v <= 0.0f)
{
b2Vec2 P = A;
b2Vec2 d = Q - P;
float32 dd = b2Dot(d, d);
if (dd > radius * radius)
{
return;
}
// Is there an edge connected to A?
if (edgeA->m_hasVertex0)
{
b2Vec2 A1 = edgeA->m_vertex0;
b2Vec2 B1 = A;
b2Vec2 e1 = B1 - A1;
float32 u1 = b2Dot(e1, B1 - Q);
// Is the circle in Region AB of the previous edge?
if (u1 > 0.0f)
{
return;
}
}
cf.indexA = 0;
cf.typeA = b2ContactFeature::e_vertex;
manifold->pointCount = 1;
manifold->type = b2Manifold::e_circles;
manifold->localNormal.SetZero();
manifold->localPoint = P;
manifold->points[0].id.key = 0;
manifold->points[0].id.cf = cf;
manifold->points[0].localPoint = circleB->m_p;
return;
}
// Region B
if (u <= 0.0f)
{
b2Vec2 P = B;
b2Vec2 d = Q - P;
float32 dd = b2Dot(d, d);
if (dd > radius * radius)
{
return;
}
// Is there an edge connected to B?
if (edgeA->m_hasVertex3)
{
b2Vec2 B2 = edgeA->m_vertex3;
b2Vec2 A2 = B;
b2Vec2 e2 = B2 - A2;
float32 v2 = b2Dot(e2, Q - A2);
// Is the circle in Region AB of the next edge?
if (v2 > 0.0f)
{
return;
}
}
cf.indexA = 1;
cf.typeA = b2ContactFeature::e_vertex;
manifold->pointCount = 1;
manifold->type = b2Manifold::e_circles;
manifold->localNormal.SetZero();
manifold->localPoint = P;
manifold->points[0].id.key = 0;
manifold->points[0].id.cf = cf;
manifold->points[0].localPoint = circleB->m_p;
return;
}
// Region AB
float32 den = b2Dot(e, e);
b2Assert(den > 0.0f);
b2Vec2 P = (1.0f / den) * (u * A + v * B);
b2Vec2 d = Q - P;
float32 dd = b2Dot(d, d);
if (dd > radius * radius)
{
return;
}
b2Vec2 n(-e.y, e.x);
if (b2Dot(n, Q - A) < 0.0f)
{
n.Set(-n.x, -n.y);
}
n.Normalize();
cf.indexA = 0;
cf.typeA = b2ContactFeature::e_face;
manifold->pointCount = 1;
manifold->type = b2Manifold::e_faceA;
manifold->localNormal = n;
manifold->localPoint = A;
manifold->points[0].id.key = 0;
manifold->points[0].id.cf = cf;
manifold->points[0].localPoint = circleB->m_p;
}
void b2WeldJoint::InitVelocityConstraints(const b2SolverData& data)
{
m_indexA = m_bodyA->m_islandIndex;
m_indexB = m_bodyB->m_islandIndex;
m_localCenterA = m_bodyA->m_sweep.localCenter;
m_localCenterB = m_bodyB->m_sweep.localCenter;
m_invMassA = m_bodyA->m_invMass;
m_invMassB = m_bodyB->m_invMass;
m_invIA = m_bodyA->m_invI;
m_invIB = m_bodyB->m_invI;
float32 aA = data.positions[m_indexA].a;
b2Vec2 vA = data.velocities[m_indexA].v;
float32 wA = data.velocities[m_indexA].w;
float32 aB = data.positions[m_indexB].a;
b2Vec2 vB = data.velocities[m_indexB].v;
float32 wB = data.velocities[m_indexB].w;
b2Rot qA(aA), qB(aB);
m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
float32 mA = m_invMassA, mB = m_invMassB;
float32 iA = m_invIA, iB = m_invIB;
b2Mat33 K;
K.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB;
K.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB;
K.ez.x = -m_rA.y * iA - m_rB.y * iB;
K.ex.y = K.ey.x;
K.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB;
K.ez.y = m_rA.x * iA + m_rB.x * iB;
K.ex.z = K.ez.x;
K.ey.z = K.ez.y;
K.ez.z = iA + iB;
if (m_frequencyHz > 0.0f)
{
K.GetInverse22(&m_mass);
float32 invM = iA + iB;
float32 m = invM > 0.0f ? 1.0f / invM : 0.0f;
float32 C = aB - aA - m_referenceAngle;
// Frequency
float32 omega = 2.0f * b2_pi * m_frequencyHz;
// Damping coefficient
float32 d = 2.0f * m * m_dampingRatio * omega;
// Spring stiffness
float32 k = m * omega * omega;
// magic formulas
float32 h = data.step.dt;
m_gamma = h * (d + h * k);
m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f;
m_bias = C * h * k * m_gamma;
invM += m_gamma;
m_mass.ez.z = invM != 0.0f ? 1.0f / invM : 0.0f;
}
else if (K.ez.z == 0.0f)
{
K.GetInverse22(&m_mass);
m_gamma = 0.0f;
m_bias = 0.0f;
}
else
{
K.GetSymInverse33(&m_mass);
m_gamma = 0.0f;
m_bias = 0.0f;
}
if (data.step.warmStarting)
{
// Scale impulses to support a variable time step.
m_impulse *= data.step.dtRatio;
b2Vec2 P(m_impulse.x, m_impulse.y);
vA -= mA * P;
wA -= iA * (b2Cross(m_rA, P) + m_impulse.z);
vB += mB * P;
wB += iB * (b2Cross(m_rB, P) + m_impulse.z);
}
else
{
m_impulse.SetZero();
}
data.velocities[m_indexA].v = vA;
data.velocities[m_indexA].w = wA;
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}
// The normal points from 1 to 2
void b2CollidePolygons(b2Manifold* manifold,
const b2PolygonShape* polyA, const b2Transform& xfA,
const b2PolygonShape* polyB, const b2Transform& xfB)
{
manifold->pointCount = 0;
float32 totalRadius = polyA->m_radius + polyB->m_radius;
int32 edgeA = 0;
float32 separationA = b2FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB);
if (separationA > totalRadius)
return;
int32 edgeB = 0;
float32 separationB = b2FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA);
if (separationB > totalRadius)
return;
const b2PolygonShape* poly1; // reference polygon
const b2PolygonShape* poly2; // incident polygon
b2Transform xf1, xf2;
int32 edge1; // reference edge
uint8 flip;
const float32 k_relativeTol = 0.98f;
const float32 k_absoluteTol = 0.001f;
if (separationB > k_relativeTol * separationA + k_absoluteTol)
{
poly1 = polyB;
poly2 = polyA;
xf1 = xfB;
xf2 = xfA;
edge1 = edgeB;
manifold->type = b2Manifold::e_faceB;
flip = 1;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
manifold->type = b2Manifold::e_faceA;
flip = 0;
}
b2ClipVertex incidentEdge[2];
b2FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
int32 count1 = poly1->m_vertexCount;
const b2Vec2* vertices1 = poly1->m_vertices;
int32 iv1 = edge1;
int32 iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
b2Vec2 v11 = vertices1[iv1];
b2Vec2 v12 = vertices1[iv2];
b2Vec2 localTangent = v12 - v11;
localTangent.Normalize();
b2Vec2 localNormal = b2Cross(localTangent, 1.0f);
b2Vec2 planePoint = 0.5f * (v11 + v12);
b2Vec2 tangent = b2Mul(xf1.q, localTangent);
b2Vec2 normal = b2Cross(tangent, 1.0f);
v11 = b2Mul(xf1, v11);
v12 = b2Mul(xf1, v12);
// Face offset.
float32 frontOffset = b2Dot(normal, v11);
// Side offsets, extended by polytope skin thickness.
float32 sideOffset1 = -b2Dot(tangent, v11) + totalRadius;
float32 sideOffset2 = b2Dot(tangent, v12) + totalRadius;
// Clip incident edge against extruded edge1 side edges.
b2ClipVertex clipPoints1[2];
b2ClipVertex clipPoints2[2];
int np;
// Clip to box side 1
np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, iv1);
if (np < 2)
return;
// Clip to negative box side 1
np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, iv2);
if (np < 2)
{
return;
}
// Now clipPoints2 contains the clipped points.
manifold->localNormal = localNormal;
manifold->localPoint = planePoint;
int32 pointCount = 0;
for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
{
float32 separation = b2Dot(normal, clipPoints2[i].v) - frontOffset;
if (separation <= totalRadius)
{
b2ManifoldPoint* cp = manifold->points + pointCount;
cp->localPoint = b2MulT(xf2, clipPoints2[i].v);
cp->id = clipPoints2[i].id;
if (flip)
{
// Swap features
b2ContactFeature cf = cp->id.cf;
cp->id.cf.indexA = cf.indexB;
cp->id.cf.indexB = cf.indexA;
cp->id.cf.typeA = cf.typeB;
cp->id.cf.typeB = cf.typeA;
}
++pointCount;
}
}
manifold->pointCount = pointCount;
}
void b2FrictionJoint::SolveVelocityConstraints(const b2TimeStep& step)
{
B2_NOT_USED(step);
b2Body* bA = m_bodyA;
b2Body* bB = m_bodyB;
b2Vec2 vA = bA->m_linearVelocity;
float32 wA = bA->m_angularVelocity;
b2Vec2 vB = bB->m_linearVelocity;
float32 wB = bB->m_angularVelocity;
float32 mA = bA->m_invMass, mB = bB->m_invMass;
float32 iA = bA->m_invI, iB = bB->m_invI;
b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter());
b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter());
// Solve angular friction
{
float32 Cdot = wB - wA;
float32 impulse = -m_angularMass * Cdot;
float32 oldImpulse = m_angularImpulse;
float32 maxImpulse = step.dt * m_maxTorque;
m_angularImpulse = b2Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse);
impulse = m_angularImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
// Solve linear friction
{
b2Vec2 Cdot = vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA);
b2Vec2 impulse = -b2Mul(m_linearMass, Cdot);
b2Vec2 oldImpulse = m_linearImpulse;
m_linearImpulse += impulse;
float32 maxImpulse = step.dt * m_maxForce;
if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse)
{
m_linearImpulse.Normalize();
m_linearImpulse *= maxImpulse;
}
impulse = m_linearImpulse - oldImpulse;
vA -= mA * impulse;
wA -= iA * b2Cross(rA, impulse);
vB += mB * impulse;
wB += iB * b2Cross(rB, impulse);
}
bA->m_linearVelocity = vA;
bA->m_angularVelocity = wA;
bB->m_linearVelocity = vB;
bB->m_angularVelocity = wB;
}
// Find the max separation between poly1 and poly2 using edge normals from poly1.
static float32 b2FindMaxSeparation(int32* edgeIndex,
const b2PolygonShape* poly1, const b2Transform& xf1,
const b2PolygonShape* poly2, const b2Transform& xf2)
{
int32 count1 = poly1->m_vertexCount;
const b2Vec2* normals1 = poly1->m_normals;
// Vector pointing from the centroid of poly1 to the centroid of poly2.
b2Vec2 d = b2Mul(xf2, poly2->m_centroid) - b2Mul(xf1, poly1->m_centroid);
b2Vec2 dLocal1 = b2MulT(xf1.q, d);
// Find edge normal on poly1 that has the largest projection onto d.
int32 edge = 0;
float32 maxDot = -b2_maxFloat;
for (int32 i = 0; i < count1; ++i)
{
float32 dot = b2Dot(normals1[i], dLocal1);
if (dot > maxDot)
{
maxDot = dot;
edge = i;
}
}
// Get the separation for the edge normal.
float32 s = b2EdgeSeparation(poly1, xf1, edge, poly2, xf2);
// Check the separation for the previous edge normal.
int32 prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
float32 sPrev = b2EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2);
// Check the separation for the next edge normal.
int32 nextEdge = edge + 1 < count1 ? edge + 1 : 0;
float32 sNext = b2EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2);
// Find the best edge and the search direction.
int32 bestEdge;
float32 bestSeparation;
int32 increment;
if (sPrev > s && sPrev > sNext)
{
increment = -1;
bestEdge = prevEdge;
bestSeparation = sPrev;
}
else if (sNext > s)
{
increment = 1;
bestEdge = nextEdge;
bestSeparation = sNext;
}
else
{
*edgeIndex = edge;
return s;
}
// Perform a local search for the best edge normal.
for ( ; ; )
{
if (increment == -1)
edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
else
edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
s = b2EdgeSeparation(poly1, xf1, edge, poly2, xf2);
if (s > bestSeparation)
{
bestEdge = edge;
bestSeparation = s;
}
else
{
break;
}
}
*edgeIndex = bestEdge;
return bestSeparation;
}
void b2RevoluteJoint::SolveVelocityConstraints(const b2TimeStep& step)
{
b2Body* b1 = m_bodyA;
b2Body* b2 = m_bodyB;
b2Vec2 v1 = b1->m_linearVelocity;
float32 w1 = b1->m_angularVelocity;
b2Vec2 v2 = b2->m_linearVelocity;
float32 w2 = b2->m_angularVelocity;
float32 m1 = b1->m_invMass, m2 = b2->m_invMass;
float32 i1 = b1->m_invI, i2 = b2->m_invI;
// Solve motor constraint.
if (m_enableMotor && m_limitState != e_equalLimits)
{
float32 Cdot = w2 - w1 - m_motorSpeed;
float32 impulse = m_motorMass * (-Cdot);
float32 oldImpulse = m_motorImpulse;
float32 maxImpulse = step.dt * m_maxMotorTorque;
m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = m_motorImpulse - oldImpulse;
w1 -= i1 * impulse;
w2 += i2 * impulse;
}
// Solve limit constraint.
if (m_enableLimit && m_limitState != e_inactiveLimit)
{
b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
// Solve point-to-point constraint
b2Vec2 Cdot1 = v2 + b2Cross(w2, r2) - v1 - b2Cross(w1, r1);
float32 Cdot2 = w2 - w1;
b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);
b2Vec3 impulse = m_mass.Solve33(-Cdot);
if (m_limitState == e_equalLimits)
{
m_impulse += impulse;
}
else if (m_limitState == e_atLowerLimit)
{
float32 newImpulse = m_impulse.z + impulse.z;
if (newImpulse < 0.0f)
{
b2Vec2 reduced = m_mass.Solve22(-Cdot1);
impulse.x = reduced.x;
impulse.y = reduced.y;
impulse.z = -m_impulse.z;
m_impulse.x += reduced.x;
m_impulse.y += reduced.y;
m_impulse.z = 0.0f;
}
}
else if (m_limitState == e_atUpperLimit)
{
float32 newImpulse = m_impulse.z + impulse.z;
if (newImpulse > 0.0f)
{
b2Vec2 reduced = m_mass.Solve22(-Cdot1);
impulse.x = reduced.x;
impulse.y = reduced.y;
impulse.z = -m_impulse.z;
m_impulse.x += reduced.x;
m_impulse.y += reduced.y;
m_impulse.z = 0.0f;
}
}
b2Vec2 P(impulse.x, impulse.y);
v1 -= m1 * P;
w1 -= i1 * (b2Cross(r1, P) + impulse.z);
v2 += m2 * P;
w2 += i2 * (b2Cross(r2, P) + impulse.z);
}
else
{
b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
// Solve point-to-point constraint
b2Vec2 Cdot = v2 + b2Cross(w2, r2) - v1 - b2Cross(w1, r1);
b2Vec2 impulse = m_mass.Solve22(-Cdot);
m_impulse.x += impulse.x;
m_impulse.y += impulse.y;
v1 -= m1 * impulse;
w1 -= i1 * b2Cross(r1, impulse);
v2 += m2 * impulse;
w2 += i2 * b2Cross(r2, impulse);
}
b1->m_linearVelocity = v1;
b1->m_angularVelocity = w1;
b2->m_linearVelocity = v2;
b2->m_angularVelocity = w2;
}
void b2World::DrawShape(b2Fixture* fixture, const b2Transform& xf, const b2Color& color)
{
switch (fixture->GetType())
{
case b2Shape::e_circle:
{
b2CircleShape* circle = (b2CircleShape*)fixture->GetShape();
b2Vec2 center = b2Mul(xf, circle->m_p);
float32 radius = circle->m_radius;
b2Vec2 axis = b2Mul(xf.q, b2Vec2(1.0f, 0.0f));
m_debugDraw->DrawSolidCircle(center, radius, axis, color);
}
break;
case b2Shape::e_edge:
{
b2EdgeShape* edge = (b2EdgeShape*)fixture->GetShape();
b2Vec2 v1 = b2Mul(xf, edge->m_vertex1);
b2Vec2 v2 = b2Mul(xf, edge->m_vertex2);
m_debugDraw->DrawSegment(v1, v2, color);
}
break;
case b2Shape::e_chain:
{
b2ChainShape* chain = (b2ChainShape*)fixture->GetShape();
int32 count = chain->m_count;
const b2Vec2* vertices = chain->m_vertices;
b2Vec2 v1 = b2Mul(xf, vertices[0]);
for (int32 i = 1; i < count; ++i)
{
b2Vec2 v2 = b2Mul(xf, vertices[i]);
m_debugDraw->DrawSegment(v1, v2, color);
m_debugDraw->DrawCircle(v1, 0.05f, color);
v1 = v2;
}
}
break;
case b2Shape::e_polygon:
{
b2PolygonShape* poly = (b2PolygonShape*)fixture->GetShape();
int32 vertexCount = poly->m_vertexCount;
b2Assert(vertexCount <= b2_maxPolygonVertices);
b2Vec2 vertices[b2_maxPolygonVertices];
for (int32 i = 0; i < vertexCount; ++i)
{
vertices[i] = b2Mul(xf, poly->m_vertices[i]);
}
m_debugDraw->DrawSolidPolygon(vertices, vertexCount, color);
}
break;
default:
break;
}
}
void b2RevoluteJoint::InitVelocityConstraints(const b2TimeStep& step)
{
b2Body* b1 = m_bodyA;
b2Body* b2 = m_bodyB;
if (m_enableMotor || m_enableLimit)
{
// You cannot create a rotation limit between bodies that
// both have fixed rotation.
b2Assert(b1->m_invI > 0.0f || b2->m_invI > 0.0f);
}
// Compute the effective mass matrix.
b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ m1+r1y^2*i1+m2+r2y^2*i2, -r1y*i1*r1x-r2y*i2*r2x, -r1y*i1-r2y*i2]
// [ -r1y*i1*r1x-r2y*i2*r2x, m1+r1x^2*i1+m2+r2x^2*i2, r1x*i1+r2x*i2]
// [ -r1y*i1-r2y*i2, r1x*i1+r2x*i2, i1+i2]
float32 m1 = b1->m_invMass, m2 = b2->m_invMass;
float32 i1 = b1->m_invI, i2 = b2->m_invI;
m_mass.col1.x = m1 + m2 + r1.y * r1.y * i1 + r2.y * r2.y * i2;
m_mass.col2.x = -r1.y * r1.x * i1 - r2.y * r2.x * i2;
m_mass.col3.x = -r1.y * i1 - r2.y * i2;
m_mass.col1.y = m_mass.col2.x;
m_mass.col2.y = m1 + m2 + r1.x * r1.x * i1 + r2.x * r2.x * i2;
m_mass.col3.y = r1.x * i1 + r2.x * i2;
m_mass.col1.z = m_mass.col3.x;
m_mass.col2.z = m_mass.col3.y;
m_mass.col3.z = i1 + i2;
m_motorMass = i1 + i2;
if (m_motorMass > 0.0f)
{
m_motorMass = 1.0f / m_motorMass;
}
if (m_enableMotor == false)
{
m_motorImpulse = 0.0f;
}
if (m_enableLimit)
{
float32 jointAngle = b2->m_sweep.a - b1->m_sweep.a - m_referenceAngle;
if (b2Abs(m_upperAngle - m_lowerAngle) < 2.0f * b2_angularSlop)
{
m_limitState = e_equalLimits;
}
else if (jointAngle <= m_lowerAngle)
{
if (m_limitState != e_atLowerLimit)
{
m_impulse.z = 0.0f;
}
m_limitState = e_atLowerLimit;
}
else if (jointAngle >= m_upperAngle)
{
if (m_limitState != e_atUpperLimit)
{
m_impulse.z = 0.0f;
}
m_limitState = e_atUpperLimit;
}
else
{
m_limitState = e_inactiveLimit;
m_impulse.z = 0.0f;
}
}
else
{
m_limitState = e_inactiveLimit;
}
if (step.warmStarting)
{
// Scale impulses to support a variable time step.
m_impulse *= step.dtRatio;
m_motorImpulse *= step.dtRatio;
b2Vec2 P(m_impulse.x, m_impulse.y);
b1->m_linearVelocity -= m1 * P;
b1->m_angularVelocity -= i1 * (b2Cross(r1, P) + m_motorImpulse + m_impulse.z);
b2->m_linearVelocity += m2 * P;
b2->m_angularVelocity += i2 * (b2Cross(r2, P) + m_motorImpulse + m_impulse.z);
}
else
{
m_impulse.SetZero();
m_motorImpulse = 0.0f;
}
}
void b2ContactSolver::SolveVelocityConstraints()
{
for (int32 i = 0; i < m_constraintCount; ++i)
{
b2ContactConstraint* c = m_constraints + i;
b2Body* bodyA = c->bodyA;
b2Body* bodyB = c->bodyB;
float32 wA = bodyA->m_angularVelocity;
float32 wB = bodyB->m_angularVelocity;
b2Vec2 vA = bodyA->m_linearVelocity;
b2Vec2 vB = bodyB->m_linearVelocity;
float32 invMassA = bodyA->m_invMass;
float32 invIA = bodyA->m_invI;
float32 invMassB = bodyB->m_invMass;
float32 invIB = bodyB->m_invI;
b2Vec2 normal = c->normal;
b2Vec2 tangent = b2Cross(normal, 1.0f);
float32 friction = c->friction;
b2Assert(c->pointCount == 1 || c->pointCount == 2);
// Solve tangent constraints
for (int32 j = 0; j < c->pointCount; ++j)
{
b2ContactConstraintPoint* ccp = c->points + j;
// Relative velocity at contact
b2Vec2 dv = vB + b2Cross(wB, ccp->rB) - vA - b2Cross(wA, ccp->rA);
// Compute tangent force
float32 vt = b2Dot(dv, tangent);
float32 lambda = ccp->tangentMass * (-vt);
// b2Clamp the accumulated force
float32 maxFriction = friction * ccp->normalImpulse;
float32 newImpulse = b2Clamp(ccp->tangentImpulse + lambda, -maxFriction, maxFriction);
lambda = newImpulse - ccp->tangentImpulse;
// Apply contact impulse
b2Vec2 P = lambda * tangent;
vA -= invMassA * P;
wA -= invIA * b2Cross(ccp->rA, P);
vB += invMassB * P;
wB += invIB * b2Cross(ccp->rB, P);
ccp->tangentImpulse = newImpulse;
}
// Solve normal constraints
if (c->pointCount == 1)
{
b2ContactConstraintPoint* ccp = c->points + 0;
// Relative velocity at contact
b2Vec2 dv = vB + b2Cross(wB, ccp->rB) - vA - b2Cross(wA, ccp->rA);
// Compute normal impulse
float32 vn = b2Dot(dv, normal);
float32 lambda = -ccp->normalMass * (vn - ccp->velocityBias);
// b2Clamp the accumulated impulse
float32 newImpulse = b2Max(ccp->normalImpulse + lambda, 0.0f);
lambda = newImpulse - ccp->normalImpulse;
// Apply contact impulse
b2Vec2 P = lambda * normal;
vA -= invMassA * P;
wA -= invIA * b2Cross(ccp->rA, P);
vB += invMassB * P;
wB += invIB * b2Cross(ccp->rB, P);
ccp->normalImpulse = newImpulse;
}
else
{
// Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite).
// Build the mini LCP for this contact patch
//
// vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
//
// A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )
// b = vn_0 - velocityBias
//
// The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i
// implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases
// vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid
// solution that satisfies the problem is chosen.
//
// In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires
// that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i).
//
// Substitute:
//
// x = x' - a
//
// Plug into above equation:
//
// vn = A * x + b
// = A * (x' - a) + b
// = A * x' + b - A * a
// = A * x' + b'
// b' = b - A * a;
b2ContactConstraintPoint* cp1 = c->points + 0;
b2ContactConstraintPoint* cp2 = c->points + 1;
b2Vec2 a(cp1->normalImpulse, cp2->normalImpulse);
b2Assert(a.x >= 0.0f && a.y >= 0.0f);
// Relative velocity at contact
b2Vec2 dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);
b2Vec2 dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);
// Compute normal velocity
float32 vn1 = b2Dot(dv1, normal);
float32 vn2 = b2Dot(dv2, normal);
b2Vec2 b;
b.x = vn1 - cp1->velocityBias;
b.y = vn2 - cp2->velocityBias;
b -= b2Mul(c->K, a);
const float32 k_errorTol = 1e-3f;
B2_NOT_USED(k_errorTol);
for (;;)
{
//
// Case 1: vn = 0
//
// 0 = A * x' + b'
//
// Solve for x':
//
// x' = - inv(A) * b'
//
b2Vec2 x = - b2Mul(c->normalMass, b);
if (x.x >= 0.0f && x.y >= 0.0f)
{
// Resubstitute for the incremental impulse
b2Vec2 d = x - a;
// Apply incremental impulse
b2Vec2 P1 = d.x * normal;
b2Vec2 P2 = d.y * normal;
vA -= invMassA * (P1 + P2);
wA -= invIA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
vB += invMassB * (P1 + P2);
wB += invIB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
// Accumulate
cp1->normalImpulse = x.x;
cp2->normalImpulse = x.y;
#if B2_DEBUG_SOLVER == 1
// Postconditions
dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);
dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);
// Compute normal velocity
vn1 = b2Dot(dv1, normal);
vn2 = b2Dot(dv2, normal);
b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol);
b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol);
#endif
break;
}
//
// Case 2: vn1 = 0 and x2 = 0
//
// 0 = a11 * x1' + a12 * 0 + b1'
// vn2 = a21 * x1' + a22 * 0 + b2'
//
x.x = - cp1->normalMass * b.x;
x.y = 0.0f;
vn1 = 0.0f;
vn2 = c->K.col1.y * x.x + b.y;
if (x.x >= 0.0f && vn2 >= 0.0f)
{
// Resubstitute for the incremental impulse
b2Vec2 d = x - a;
// Apply incremental impulse
b2Vec2 P1 = d.x * normal;
b2Vec2 P2 = d.y * normal;
vA -= invMassA * (P1 + P2);
wA -= invIA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
vB += invMassB * (P1 + P2);
wB += invIB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
// Accumulate
cp1->normalImpulse = x.x;
cp2->normalImpulse = x.y;
#if B2_DEBUG_SOLVER == 1
// Postconditions
dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);
// Compute normal velocity
vn1 = b2Dot(dv1, normal);
b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol);
#endif
break;
}
//
// Case 3: vn2 = 0 and x1 = 0
//
// vn1 = a11 * 0 + a12 * x2' + b1'
// 0 = a21 * 0 + a22 * x2' + b2'
//
x.x = 0.0f;
x.y = - cp2->normalMass * b.y;
vn1 = c->K.col2.x * x.y + b.x;
vn2 = 0.0f;
if (x.y >= 0.0f && vn1 >= 0.0f)
{
// Resubstitute for the incremental impulse
b2Vec2 d = x - a;
// Apply incremental impulse
b2Vec2 P1 = d.x * normal;
b2Vec2 P2 = d.y * normal;
vA -= invMassA * (P1 + P2);
wA -= invIA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
vB += invMassB * (P1 + P2);
wB += invIB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
// Accumulate
cp1->normalImpulse = x.x;
cp2->normalImpulse = x.y;
#if B2_DEBUG_SOLVER == 1
// Postconditions
dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);
// Compute normal velocity
vn2 = b2Dot(dv2, normal);
b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol);
#endif
break;
}
//
// Case 4: x1 = 0 and x2 = 0
//
// vn1 = b1
// vn2 = b2;
x.x = 0.0f;
x.y = 0.0f;
vn1 = b.x;
vn2 = b.y;
if (vn1 >= 0.0f && vn2 >= 0.0f )
{
// Resubstitute for the incremental impulse
b2Vec2 d = x - a;
// Apply incremental impulse
b2Vec2 P1 = d.x * normal;
b2Vec2 P2 = d.y * normal;
vA -= invMassA * (P1 + P2);
wA -= invIA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
vB += invMassB * (P1 + P2);
wB += invIB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
// Accumulate
cp1->normalImpulse = x.x;
cp2->normalImpulse = x.y;
break;
}
// No solution, give up. This is hit sometimes, but it doesn't seem to matter.
break;
}
}
bodyA->m_linearVelocity = vA;
bodyA->m_angularVelocity = wA;
bodyB->m_linearVelocity = vB;
bodyB->m_angularVelocity = wB;
}
}
// Initialize position dependent portions of the velocity constraints.
void b2ContactSolver::InitializeVelocityConstraints()
{
for (int32 i = 0; i < m_count; ++i)
{
b2ContactVelocityConstraint* vc = m_velocityConstraints + i;
b2ContactPositionConstraint* pc = m_positionConstraints + i;
float32 radiusA = pc->radiusA;
float32 radiusB = pc->radiusB;
b2Manifold* manifold = m_contacts[vc->contactIndex]->GetManifold();
int32 indexA = vc->indexA;
int32 indexB = vc->indexB;
float32 mA = vc->invMassA;
float32 mB = vc->invMassB;
float32 iA = vc->invIA;
float32 iB = vc->invIB;
b2Vec2 localCenterA = pc->localCenterA;
b2Vec2 localCenterB = pc->localCenterB;
b2Vec2 cA = m_positions[indexA].c;
float32 aA = m_positions[indexA].a;
b2Vec2 vA = m_velocities[indexA].v;
float32 wA = m_velocities[indexA].w;
b2Vec2 cB = m_positions[indexB].c;
float32 aB = m_positions[indexB].a;
b2Vec2 vB = m_velocities[indexB].v;
float32 wB = m_velocities[indexB].w;
b2Assert(manifold->pointCount > 0);
b2Transform xfA, xfB;
xfA.q.Set(aA);
xfB.q.Set(aB);
xfA.p = cA - b2Mul(xfA.q, localCenterA);
xfB.p = cB - b2Mul(xfB.q, localCenterB);
b2WorldManifold worldManifold;
worldManifold.Initialize(manifold, xfA, radiusA, xfB, radiusB);
vc->normal = worldManifold.normal;
int32 pointCount = vc->pointCount;
for (int32 j = 0; j < pointCount; ++j)
{
b2VelocityConstraintPoint* vcp = vc->points + j;
vcp->rA = worldManifold.points[j] - cA;
vcp->rB = worldManifold.points[j] - cB;
float32 rnA = b2Cross(vcp->rA, vc->normal);
float32 rnB = b2Cross(vcp->rB, vc->normal);
float32 kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
vcp->normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f;
b2Vec2 tangent = b2Cross(vc->normal, 1.0f);
float32 rtA = b2Cross(vcp->rA, tangent);
float32 rtB = b2Cross(vcp->rB, tangent);
float32 kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB;
vcp->tangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f;
// Setup a velocity bias for restitution.
vcp->velocityBias = 0.0f;
float32 vRel = b2Dot(vc->normal, vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA));
if (vRel < -b2_velocityThreshold)
{
vcp->velocityBias = -vc->restitution * vRel;
}
}
// If we have two points, then prepare the block solver.
if (vc->pointCount == 2)
{
b2VelocityConstraintPoint* vcp1 = vc->points + 0;
b2VelocityConstraintPoint* vcp2 = vc->points + 1;
float32 rn1A = b2Cross(vcp1->rA, vc->normal);
float32 rn1B = b2Cross(vcp1->rB, vc->normal);
float32 rn2A = b2Cross(vcp2->rA, vc->normal);
float32 rn2B = b2Cross(vcp2->rB, vc->normal);
float32 k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B;
float32 k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B;
float32 k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B;
// Ensure a reasonable condition number.
const float32 k_maxConditionNumber = 1000.0f;
if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
{
// K is safe to invert.
vc->K.ex.Set(k11, k12);
vc->K.ey.Set(k12, k22);
vc->normalMass = vc->K.GetInverse();
}
else
{
// The constraints are redundant, just use one.
// TODO_ERIN use deepest?
vc->pointCount = 1;
}
}
}
}
void b2Distance(b2DistanceOutput* output,
b2SimplexCache* cache,
const b2DistanceInput* input)
{
++b2_gjkCalls;
const b2DistanceProxy* proxyA = &input->proxyA;
const b2DistanceProxy* proxyB = &input->proxyB;
b2Transform transformA = input->transformA;
b2Transform transformB = input->transformB;
// Initialize the simplex.
b2Simplex simplex;
simplex.ReadCache(cache, proxyA, transformA, proxyB, transformB);
// Get simplex vertices as an array.
b2SimplexVertex* vertices = &simplex.m_v1;
const int32 k_maxIters = 20;
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
int32 saveA[3], saveB[3];
int32 saveCount = 0;
b2Vec2 closestPoint = simplex.GetClosestPoint();
float32 distanceSqr1 = closestPoint.LengthSquared();
float32 distanceSqr2 = distanceSqr1;
// Main iteration loop.
int32 iter = 0;
while (iter < k_maxIters)
{
// Copy simplex so we can identify duplicates.
saveCount = simplex.m_count;
for (int32 i = 0; i < saveCount; ++i)
{
saveA[i] = vertices[i].indexA;
saveB[i] = vertices[i].indexB;
}
switch (simplex.m_count)
{
case 1:
break;
case 2:
simplex.Solve2();
break;
case 3:
simplex.Solve3();
break;
default:
b2Assert(false);
}
// If we have 3 points, then the origin is in the corresponding triangle.
if (simplex.m_count == 3)
{
break;
}
// Compute closest point.
b2Vec2 p = simplex.GetClosestPoint();
distanceSqr2 = p.LengthSquared();
// Ensure progress
if (distanceSqr2 >= distanceSqr1)
{
//break;
}
distanceSqr1 = distanceSqr2;
// Get search direction.
b2Vec2 d = simplex.GetSearchDirection();
// Ensure the search direction is numerically fit.
if (d.LengthSquared() < b2_epsilon * b2_epsilon)
{
// The origin is probably contained by a line segment
// or triangle. Thus the shapes are overlapped.
// We can't return zero here even though there may be overlap.
// In case the simplex is a point, segment, or triangle it is difficult
// to determine if the origin is contained in the CSO or very close to it.
break;
}
// Compute a tentative new simplex vertex using support points.
b2SimplexVertex* vertex = vertices + simplex.m_count;
vertex->indexA = proxyA->GetSupport(b2MulT(transformA.q, -d));
vertex->wA = b2Mul(transformA, proxyA->GetVertex(vertex->indexA));
b2Vec2 wBLocal;
vertex->indexB = proxyB->GetSupport(b2MulT(transformB.q, d));
vertex->wB = b2Mul(transformB, proxyB->GetVertex(vertex->indexB));
vertex->w = vertex->wB - vertex->wA;
// Iteration count is equated to the number of support point calls.
++iter;
++b2_gjkIters;
// Check for duplicate support points. This is the main termination criteria.
bool duplicate = false;
for (int32 i = 0; i < saveCount; ++i)
{
if (vertex->indexA == saveA[i] && vertex->indexB == saveB[i])
{
duplicate = true;
break;
}
}
// If we found a duplicate support point we must exit to avoid cycling.
if (duplicate)
{
break;
}
// New vertex is ok and needed.
++simplex.m_count;
}
b2_gjkMaxIters = b2Max(b2_gjkMaxIters, iter);
// Prepare output.
simplex.GetWitnessPoints(&output->pointA, &output->pointB);
output->distance = b2Distance(output->pointA, output->pointB);
output->iterations = iter;
// Cache the simplex.
simplex.WriteCache(cache);
// Apply radii if requested.
if (input->useRadii)
{
float32 rA = proxyA->m_radius;
float32 rB = proxyB->m_radius;
if (output->distance > rA + rB && output->distance > b2_epsilon)
{
// Shapes are still no overlapped.
// Move the witness points to the outer surface.
output->distance -= rA + rB;
b2Vec2 normal = output->pointB - output->pointA;
normal.Normalize();
output->pointA += rA * normal;
output->pointB -= rB * normal;
}
else
{
// Shapes are overlapped when radii are considered.
// Move the witness points to the middle.
b2Vec2 p = 0.5f * (output->pointA + output->pointB);
output->pointA = p;
output->pointB = p;
output->distance = 0.0f;
}
}
}
// Sequential position solver for position constraints.
bool b2ContactSolver::SolveTOIPositionConstraints(int32 toiIndexA, int32 toiIndexB)
{
float32 minSeparation = 0.0f;
for (int32 i = 0; i < m_count; ++i)
{
b2ContactPositionConstraint* pc = m_positionConstraints + i;
int32 indexA = pc->indexA;
int32 indexB = pc->indexB;
b2Vec2 localCenterA = pc->localCenterA;
b2Vec2 localCenterB = pc->localCenterB;
int32 pointCount = pc->pointCount;
float32 mA = 0.0f;
float32 iA = 0.0f;
if (indexA == toiIndexA || indexA == toiIndexB)
{
mA = pc->invMassA;
iA = pc->invIA;
}
float32 mB = pc->invMassB;
float32 iB = pc->invIB;
if (indexB == toiIndexA || indexB == toiIndexB)
{
mB = pc->invMassB;
iB = pc->invIB;
}
b2Vec2 cA = m_positions[indexA].c;
float32 aA = m_positions[indexA].a;
b2Vec2 cB = m_positions[indexB].c;
float32 aB = m_positions[indexB].a;
// Solve normal constraints
for (int32 j = 0; j < pointCount; ++j)
{
b2Transform xfA, xfB;
xfA.q.Set(aA);
xfB.q.Set(aB);
xfA.p = cA - b2Mul(xfA.q, localCenterA);
xfB.p = cB - b2Mul(xfB.q, localCenterB);
b2PositionSolverManifold psm;
psm.Initialize(pc, xfA, xfB, j);
b2Vec2 normal = psm.normal;
b2Vec2 point = psm.point;
float32 separation = psm.separation;
b2Vec2 rA = point - cA;
b2Vec2 rB = point - cB;
// Track max constraint error.
minSeparation = b2Min(minSeparation, separation);
// Prevent large corrections and allow slop.
float32 C = b2Clamp(b2_toiBaugarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f);
// Compute the effective mass.
float32 rnA = b2Cross(rA, normal);
float32 rnB = b2Cross(rB, normal);
float32 K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
// Compute normal impulse
float32 impulse = K > 0.0f ? - C / K : 0.0f;
b2Vec2 P = impulse * normal;
cA -= mA * P;
aA -= iA * b2Cross(rA, P);
cB += mB * P;
aB += iB * b2Cross(rB, P);
}
m_positions[indexA].c = cA;
m_positions[indexA].a = aA;
m_positions[indexB].c = cB;
m_positions[indexB].a = aB;
}
// We can't expect minSpeparation >= -b2_linearSlop because we don't
// push the separation above -b2_linearSlop.
return minSeparation >= -1.5f * b2_linearSlop;
}
void b2WorldManifold::Initialize(const b2Manifold* manifold,
const b2Transform& xfA, float32 radiusA,
const b2Transform& xfB, float32 radiusB)
{
if (manifold->pointCount == 0)
{
return;
}
switch (manifold->type)
{
case b2Manifold::e_circles:
{
normal.Set(1.0f, 0.0f);
b2Vec2 pointA = b2Mul(xfA, manifold->localPoint);
b2Vec2 pointB = b2Mul(xfB, manifold->points[0].localPoint);
if (b2DistanceSquared(pointA, pointB) > b2_epsilon * b2_epsilon)
{
normal = pointB - pointA;
normal.Normalize();
}
b2Vec2 cA = pointA + radiusA * normal;
b2Vec2 cB = pointB - radiusB * normal;
points[0] = 0.5f * (cA + cB);
separations[0] = b2Dot(cB - cA, normal);
}
break;
case b2Manifold::e_faceA:
{
normal = b2Mul(xfA.q, manifold->localNormal);
b2Vec2 planePoint = b2Mul(xfA, manifold->localPoint);
for (int32 i = 0; i < manifold->pointCount; ++i)
{
b2Vec2 clipPoint = b2Mul(xfB, manifold->points[i].localPoint);
b2Vec2 cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint, normal)) * normal;
b2Vec2 cB = clipPoint - radiusB * normal;
points[i] = 0.5f * (cA + cB);
separations[i] = b2Dot(cB - cA, normal);
}
}
break;
case b2Manifold::e_faceB:
{
normal = b2Mul(xfB.q, manifold->localNormal);
b2Vec2 planePoint = b2Mul(xfB, manifold->localPoint);
for (int32 i = 0; i < manifold->pointCount; ++i)
{
b2Vec2 clipPoint = b2Mul(xfA, manifold->points[i].localPoint);
b2Vec2 cB = clipPoint + (radiusB - b2Dot(clipPoint - planePoint, normal)) * normal;
b2Vec2 cA = clipPoint - radiusA * normal;
points[i] = 0.5f * (cA + cB);
separations[i] = b2Dot(cA - cB, normal);
}
// Ensure normal points from A to B.
normal = -normal;
}
break;
}
}
void b2PrismaticJoint::InitVelocityConstraints(const b2SolverData& data)
{
m_indexA = m_bodyA->m_islandIndex;
m_indexB = m_bodyB->m_islandIndex;
m_localCenterA = m_bodyA->m_sweep.localCenter;
m_localCenterB = m_bodyB->m_sweep.localCenter;
m_invMassA = m_bodyA->m_invMass;
m_invMassB = m_bodyB->m_invMass;
m_invIA = m_bodyA->m_invI;
m_invIB = m_bodyB->m_invI;
b2Vec2 cA = data.positions[m_indexA].c;
float32 aA = data.positions[m_indexA].a;
b2Vec2 vA = data.velocities[m_indexA].v;
float32 wA = data.velocities[m_indexA].w;
b2Vec2 cB = data.positions[m_indexB].c;
float32 aB = data.positions[m_indexB].a;
b2Vec2 vB = data.velocities[m_indexB].v;
float32 wB = data.velocities[m_indexB].w;
b2Rot qA(aA), qB(aB);
// Compute the effective masses.
b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
b2Vec2 d = (cB - cA) + rB - rA;
float32 mA = m_invMassA, mB = m_invMassB;
float32 iA = m_invIA, iB = m_invIB;
// Compute motor Jacobian and effective mass.
{
m_axis = b2Mul(qA, m_localXAxisA);
m_a1 = b2Cross(d + rA, m_axis);
m_a2 = b2Cross(rB, m_axis);
m_motorMass = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
if (m_motorMass > 0.0f)
{
m_motorMass = 1.0f / m_motorMass;
}
}
// Prismatic constraint.
{
m_perp = b2Mul(qA, m_localYAxisA);
m_s1 = b2Cross(d + rA, m_perp);
m_s2 = b2Cross(rB, m_perp);
float32 k11 = mA + mB + iA * m_s1 * m_s1 + iB * m_s2 * m_s2;
float32 k12 = iA * m_s1 + iB * m_s2;
float32 k13 = iA * m_s1 * m_a1 + iB * m_s2 * m_a2;
float32 k22 = iA + iB;
if (k22 == 0.0f)
{
// For bodies with fixed rotation.
k22 = 1.0f;
}
float32 k23 = iA * m_a1 + iB * m_a2;
float32 k33 = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
m_K.ex.Set(k11, k12, k13);
m_K.ey.Set(k12, k22, k23);
m_K.ez.Set(k13, k23, k33);
}
// Compute motor and limit terms.
if (m_enableLimit)
{
float32 jointTranslation = b2Dot(m_axis, d);
if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop)
{
m_limitState = e_equalLimits;
}
else if (jointTranslation <= m_lowerTranslation)
{
if (m_limitState != e_atLowerLimit)
{
m_limitState = e_atLowerLimit;
m_impulse.z = 0.0f;
}
}
else if (jointTranslation >= m_upperTranslation)
{
if (m_limitState != e_atUpperLimit)
{
m_limitState = e_atUpperLimit;
m_impulse.z = 0.0f;
}
}
else
{
m_limitState = e_inactiveLimit;
m_impulse.z = 0.0f;
}
}
else
{
m_limitState = e_inactiveLimit;
m_impulse.z = 0.0f;
}
if (m_enableMotor == false)
{
m_motorImpulse = 0.0f;
}
if (data.step.warmStarting)
{
// Account for variable time step.
m_impulse *= data.step.dtRatio;
m_motorImpulse *= data.step.dtRatio;
b2Vec2 P = m_impulse.x * m_perp + (m_motorImpulse + m_impulse.z) * m_axis;
float32 LA = m_impulse.x * m_s1 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a1;
float32 LB = m_impulse.x * m_s2 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a2;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
else
{
m_impulse.SetZero();
m_motorImpulse = 0.0f;
}
data.velocities[m_indexA].v = vA;
data.velocities[m_indexA].w = wA;
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}
b2Body::b2Body(const b2BodyDef* bd, b2World* world)
{
m_flags = 0;
if (bd->isBullet)
{
m_flags |= e_bulletFlag;
}
if (bd->fixedRotation)
{
m_flags |= e_fixedRotationFlag;
}
if (bd->allowSleep)
{
m_flags |= e_allowSleepFlag;
}
if (bd->isSleeping)
{
m_flags |= e_sleepFlag;
}
m_world = world;
m_xf.position = bd->position;
m_xf.R.Set(bd->angle);
m_sweep.localCenter = bd->massData.center;
m_sweep.t0 = 1.0f;
m_sweep.a0 = m_sweep.a = bd->angle;
m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
m_jointList = NULL;
m_contactList = NULL;
m_controllerList = NULL;
m_prev = NULL;
m_next = NULL;
m_linearVelocity = bd->linearVelocity;
m_angularVelocity = bd->angularVelocity;
m_linearDamping = bd->linearDamping;
m_angularDamping = bd->angularDamping;
m_force.Set(0.0f, 0.0f);
m_torque = 0.0f;
m_linearVelocity.SetZero();
m_angularVelocity = 0.0f;
m_sleepTime = 0.0f;
m_invMass = 0.0f;
m_I = 0.0f;
m_invI = 0.0f;
m_mass = bd->massData.mass;
if (m_mass > 0.0f)
{
m_invMass = 1.0f / m_mass;
}
m_I = bd->massData.I;
if (m_I > 0.0f && (m_flags & b2Body::e_fixedRotationFlag) == 0)
{
m_invI = 1.0f / m_I;
}
if (m_invMass == 0.0f && m_invI == 0.0f)
{
m_type = e_staticType;
}
else
{
m_type = e_dynamicType;
}
m_userData = bd->userData;
m_fixtureList = NULL;
m_fixtureCount = 0;
}
