public void FinalizeVelocityConstraints() { for (int i = 0; i < m_constraintCount; ++i) { b2ContactConstraint c = m_constraints[i]; b2Manifold m = c.manifold; for (int j = 0; j < c.pointCount; ++j) { b2ManifoldPoint point1 = m.m_points[j]; b2ContactConstraintPoint point2 = c.points[j]; point1.m_normalImpulse = point2.normalImpulse; point1.m_tangentImpulse = point2.tangentImpulse; } } }
public void Initialize(b2ContactConstraint cc) { b2Settings.b2Assert(cc.pointCount > 0); int i; float clipPointX; float clipPointY; b2Mat22 tMat; b2Vec2 tVec; float planePointX; float planePointY; if (cc.type == b2Manifold.e_circles) { //var pointA:b2Vec2 = cc.bodyA.GetWorldPoint(cc.localPoint); tMat = cc.bodyA.m_xf.R; tVec = cc.localPoint; float pointAX = cc.bodyA.m_xf.position.x + (tMat.col1.x * tVec.x + tMat.col2.x * tVec.y); float pointAY = cc.bodyA.m_xf.position.y + (tMat.col1.y * tVec.x + tMat.col2.y * tVec.y); //var pointB:b2Vec2 = cc.bodyB.GetWorldPoint(cc.points[0].localPoint); tMat = cc.bodyB.m_xf.R; tVec = cc.points[0].localPoint; float pointBX = cc.bodyB.m_xf.position.x + (tMat.col1.x * tVec.x + tMat.col2.x * tVec.y); float pointBY = cc.bodyB.m_xf.position.y + (tMat.col1.y * tVec.x + tMat.col2.y * tVec.y); float dX = pointBX - pointAX; float dY = pointBY - pointAY; float d2 = dX * dX + dY * dY; if (d2 > 0.0f /*float.MinValue*float.MinValue*/) { float d = Mathf.Sqrt(d2); m_normal.x = dX / d; m_normal.y = dY / d; } else { m_normal.x = 1.0f; m_normal.y = 0.0f; } m_points[0].x = 0.5f * (pointAX + pointBX); m_points[0].y = 0.5f * (pointAY + pointBY); m_separations[0] = dX * m_normal.x + dY * m_normal.y - cc.radius; } else if (cc.type == b2Manifold.e_faceA) { //m_normal = cc.bodyA.GetWorldVector(cc.localPlaneNormal); tMat = cc.bodyA.m_xf.R; tVec = cc.localPlaneNormal; m_normal.x = tMat.col1.x * tVec.x + tMat.col2.x * tVec.y; m_normal.y = tMat.col1.y * tVec.x + tMat.col2.y * tVec.y; //planePoint = cc.bodyA.GetWorldPoint(cc.localPoint); tMat = cc.bodyA.m_xf.R; tVec = cc.localPoint; planePointX = cc.bodyA.m_xf.position.x + (tMat.col1.x * tVec.x + tMat.col2.x * tVec.y); planePointY = cc.bodyA.m_xf.position.y + (tMat.col1.y * tVec.x + tMat.col2.y * tVec.y); tMat = cc.bodyB.m_xf.R; for (i = 0; i < cc.pointCount; ++i) { //clipPoint = cc.bodyB.GetWorldPoint(cc.points[i].localPoint); tVec = cc.points[i].localPoint; clipPointX = cc.bodyB.m_xf.position.x + (tMat.col1.x * tVec.x + tMat.col2.x * tVec.y); clipPointY = cc.bodyB.m_xf.position.y + (tMat.col1.y * tVec.x + tMat.col2.y * tVec.y); m_separations[i] = (clipPointX - planePointX) * m_normal.x + (clipPointY - planePointY) * m_normal.y - cc.radius; m_points[i].x = clipPointX; m_points[i].y = clipPointY; } } else if (cc.type == b2Manifold.e_faceB) { //m_normal = cc.bodyB.GetWorldVector(cc.localPlaneNormal); tMat = cc.bodyB.m_xf.R; tVec = cc.localPlaneNormal; m_normal.x = tMat.col1.x * tVec.x + tMat.col2.x * tVec.y; m_normal.y = tMat.col1.y * tVec.x + tMat.col2.y * tVec.y; //planePoint = cc.bodyB.GetWorldPoint(cc.localPoint); tMat = cc.bodyB.m_xf.R; tVec = cc.localPoint; planePointX = cc.bodyB.m_xf.position.x + (tMat.col1.x * tVec.x + tMat.col2.x * tVec.y); planePointY = cc.bodyB.m_xf.position.y + (tMat.col1.y * tVec.x + tMat.col2.y * tVec.y); tMat = cc.bodyA.m_xf.R; for (i = 0; i < cc.pointCount; ++i) { //clipPoint = cc.bodyA.GetWorldPoint(cc.points[i].localPoint); tVec = cc.points[i].localPoint; clipPointX = cc.bodyA.m_xf.position.x + (tMat.col1.x * tVec.x + tMat.col2.x * tVec.y); clipPointY = cc.bodyA.m_xf.position.y + (tMat.col1.y * tVec.x + tMat.col2.y * tVec.y); m_separations[i] = (clipPointX - planePointX) * m_normal.x + (clipPointY - planePointY) * m_normal.y - cc.radius; m_points[i].Set(clipPointX, clipPointY); } // Ensure normal points from A to B m_normal.x *= -1.0f; m_normal.y *= -1.0f; } }
public bool SolvePositionConstraints(float baumgarte) { float minSeparation = 0.0f; for (int i = 0; i < m_constraintCount; i++) { b2ContactConstraint c = m_constraints[i]; b2Body bodyA = c.bodyA; b2Body bodyB = c.bodyB; float invMassA = bodyA.m_mass * bodyA.m_invMass; float invIA = bodyA.m_mass * bodyA.m_invI; float invMassB = bodyB.m_mass * bodyB.m_invMass; float invIB = bodyB.m_mass * bodyB.m_invI; s_psm.Initialize(c); b2Vec2 normal = s_psm.m_normal; // Solve normal constraints for (int j = 0; j < c.pointCount; j++) { b2ContactConstraintPoint ccp = c.points[j]; b2Vec2 point = s_psm.m_points[j]; float separation = s_psm.m_separations[j]; float rAX = point.x - bodyA.m_sweep.c.x; float rAY = point.y - bodyA.m_sweep.c.y; float rBX = point.x - bodyB.m_sweep.c.x; float rBY = point.y - bodyB.m_sweep.c.y; // Track max constraint error. minSeparation = minSeparation < separation?minSeparation:separation; // Prevent large corrections and allow slop. float C = b2Math.Clamp(baumgarte * (separation + b2Settings.b2_linearSlop), -b2Settings.b2_maxLinearCorrection, 0.0f); // Compute normal impulse float impulse = -ccp.equalizedMass * C; float PX = impulse * normal.x; float PY = impulse * normal.y; //bodyA.m_sweep.c -= invMassA * P; if (bodyA.m_allowMovement) { bodyA.m_sweep.c.x -= invMassA * PX; bodyA.m_sweep.c.y -= invMassA * PY; } //bodyA.m_sweep.a -= invIA * b2Cross(rA, P); bodyA.m_sweep.a -= invIA * (rAX * PY - rAY * PX); bodyA.SynchronizeTransform(); //bodyB.m_sweep.c += invMassB * P; //--------------------修改start kingBook------------ if (bodyB.m_allowMovement) { bodyB.m_sweep.c.x += invMassB * PX; bodyB.m_sweep.c.y += invMassB * PY; } //--------------------修改end kingBook------------ //bodyB.m_sweep.a += invIB * b2Cross(rB, P); bodyB.m_sweep.a += invIB * (rBX * PY - rBY * PX); bodyB.SynchronizeTransform(); } } // We can't expect minSpeparation >= -b2_linearSlop because we don't // push the separation above -b2_linearSlop. return(minSeparation > -1.5f * b2Settings.b2_linearSlop); }
public void SolveVelocityConstraints() { int j; b2ContactConstraintPoint ccp; float rAX; float rAY; float rBX; float rBY; float dvX; float dvY; float vn; float vt; float lambda; float maxFriction; float newImpulse; float PX; float PY; float dX; float dY; float P1X; float P1Y; float P2X; float P2Y; b2Mat22 tMat; b2Vec2 tVec; for (int i = 0; i < m_constraintCount; ++i) { b2ContactConstraint c = m_constraints[i]; b2Body bodyA = c.bodyA; b2Body bodyB = c.bodyB; float wA = bodyA.m_angularVelocity; float wB = bodyB.m_angularVelocity; b2Vec2 vA = bodyA.m_linearVelocity; b2Vec2 vB = bodyB.m_linearVelocity; float invMassA = bodyA.m_invMass; float invIA = bodyA.m_invI; float invMassB = bodyB.m_invMass; float invIB = bodyB.m_invI; //var normal:b2Vec2 = new b2Vec2(c.normal.x, c.normal.y); float normalX = c.normal.x; float normalY = c.normal.y; //var tangent:b2Vec2 = b2Math.b2CrossVF(normal, 1.0); float tangentX = normalY; float tangentY = -normalX; float friction = c.friction; float tX; //b2Settings.b2Assert(c.pointCount == 1 || c.pointCount == 2); // Solve the tangent constraints for (j = 0; j < c.pointCount; j++) { ccp = c.points[j]; // Relative velocity at contact //b2Vec2 dv = vB + b2Cross(wB, ccp->rB) - vA - b2Cross(wA, ccp->rA); dvX = vB.x - wB * ccp.rB.y - vA.x + wA * ccp.rA.y; dvY = vB.y + wB * ccp.rB.x - vA.y - wA * ccp.rA.x; // Compute tangent force vt = dvX * tangentX + dvY * tangentY; lambda = ccp.tangentMass * -vt; // b2Clamp the accumulated force maxFriction = friction * ccp.normalImpulse; newImpulse = b2Math.Clamp(ccp.tangentImpulse + lambda, -maxFriction, maxFriction); lambda = newImpulse - ccp.tangentImpulse; // Apply contact impulse PX = lambda * tangentX; PY = lambda * tangentY; vA.x -= invMassA * PX; vA.y -= invMassA * PY; wA -= invIA * (ccp.rA.x * PY - ccp.rA.y * PX); vB.x += invMassB * PX; vB.y += invMassB * PY; wB += invIB * (ccp.rB.x * PY - ccp.rB.y * PX); ccp.tangentImpulse = newImpulse; } // Solve the normal constraints int tCount = c.pointCount; if (c.pointCount == 1) { ccp = c.points[0]; // Relative velocity at contact //b2Vec2 dv = vB + b2Cross(wB, ccp->rB) - vA - b2Cross(wA, ccp->rA); dvX = vB.x + (-wB * ccp.rB.y) - vA.x - (-wA * ccp.rA.y); dvY = vB.y + (wB * ccp.rB.x) - vA.y - (wA * ccp.rA.x); // Compute normal impulse //var vn:Number = b2Math.b2Dot(dv, normal); vn = dvX * normalX + dvY * normalY; lambda = -ccp.normalMass * (vn - ccp.velocityBias); // b2Clamp the accumulated impulse //newImpulse = b2Math.b2Max(ccp.normalImpulse + lambda, 0.0); newImpulse = ccp.normalImpulse + lambda; newImpulse = newImpulse > 0 ? newImpulse : 0.0f; lambda = newImpulse - ccp.normalImpulse; // Apply contact impulse //b2Vec2 P = lambda * normal; PX = lambda * normalX; PY = lambda * normalY; //vA.Subtract( b2Math.MulFV( invMassA, P ) ); vA.x -= invMassA * PX; vA.y -= invMassA * PY; wA -= invIA * (ccp.rA.x * PY - ccp.rA.y * PX); //invIA * b2Math.b2CrossVV(ccp.rA, P); //vB.Add( b2Math.MulFV( invMass2, P ) ); vB.x += invMassB * PX; vB.y += invMassB * PY; wB += invIB * (ccp.rB.x * PY - ccp.rB.y * PX); //invIB * b2Math.b2CrossVV(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]; float aX = cp1.normalImpulse; float aY = cp2.normalImpulse; //b2Settings.b2Assert( aX >= 0.0f && aY >= 0.0f ); // Relative velocity at contact //var dv1:b2Vec2 = vB + b2Cross(wB, cp1.rB) - vA - b2Cross(wA, cp1.rA); float dv1X = vB.x - wB * cp1.rB.y - vA.x + wA * cp1.rA.y; float dv1Y = vB.y + wB * cp1.rB.x - vA.y - wA * cp1.rA.x; //var dv2:b2Vec2 = vB + b2Cross(wB, cpB.r2) - vA - b2Cross(wA, cp2.rA); float dv2X = vB.x - wB * cp2.rB.y - vA.x + wA * cp2.rA.y; float dv2Y = vB.y + wB * cp2.rB.x - vA.y - wA * cp2.rA.x; // Compute normal velocity //var vn1:Number = b2Dot(dv1, normal); float vn1 = dv1X * normalX + dv1Y * normalY; //var vn2:Number = b2Dot(dv2, normal); float vn2 = dv2X * normalX + dv2Y * normalY; float bX = vn1 - cp1.velocityBias; float bY = vn2 - cp2.velocityBias; //b -= b2Mul(c.K,a); tMat = c.K; bX -= tMat.col1.x * aX + tMat.col2.x * aY; bY -= tMat.col1.y * aX + tMat.col2.y * aY; float k_errorTol = 0.001f; while (true) { // // Case 1: vn = 0 // // 0 = A * x' + b' // // Solve for x': // // x' = -inv(A) * b' // //var x:b2Vec2 = - b2Mul(c->normalMass, b); tMat = c.normalMass; float xX = -(tMat.col1.x * bX + tMat.col2.x * bY); float xY = -(tMat.col1.y * bX + tMat.col2.y * bY); if (xX >= 0.0f && xY >= 0.0f) { // Resubstitute for the incremental impulse //d = x - a; dX = xX - aX; dY = xY - aY; //Aply incremental impulse //P1 = d.x * normal; P1X = dX * normalX; P1Y = dX * normalY; //P2 = d.y * normal; P2X = dY * normalX; P2Y = dY * normalY; //vA -= invMass1 * (P1 + P2) vA.x -= invMassA * (P1X + P2X); vA.y -= invMassA * (P1Y + P2Y); //wA -= invIA * (b2Cross(cp1.rA, P1) + b2Cross(cp2.rA, P2)); wA -= invIA * (cp1.rA.x * P1Y - cp1.rA.y * P1X + cp2.rA.x * P2Y - cp2.rA.y * P2X); //vB += invMassB * (P1 + P2) vB.x += invMassB * (P1X + P2X); vB.y += invMassB * (P1Y + P2Y); //wB += invIB * (b2Cross(cp1.rB, P1) + b2Cross(cp2.rB, P2)); wB += invIB * (cp1.rB.x * P1Y - cp1.rB.y * P1X + cp2.rB.x * P2Y - cp2.rB.y * P2X); // Accumulate cp1.normalImpulse = xX; cp2.normalImpulse = xY; //#if B2_DEBUG_SOLVER == 1 // // Post conditions // //dv1 = vB + b2Cross(wB, cp1.rB) - vA - b2Cross(wA, cp1.rA); // dv1X = vB.x - wB * cp1.rB.y - vA.x + wA * cp1.rA.y; // dv1Y = vB.y + wB * cp1.rB.x - vA.y - wA * cp1.rA.x; // //dv2 = vB + b2Cross(wB, cp2.rB) - vA - b2Cross(wA, cp2.rA); // dv1X = vB.x - wB * cp2.rB.y - vA.x + wA * cp2.rA.y; // dv1Y = vB.y + wB * cp2.rB.x - vA.y - wA * cp2.rA.x; // // Compute normal velocity // //vn1 = b2Dot(dv1, normal); // vn1 = dv1X * normalX + dv1Y * normalY; // //vn2 = b2Dot(dv2, normal); // vn2 = dv2X * normalX + dv2Y * normalY; // // //b2Settings.b2Assert(b2Abs(vn1 - cp1.velocityBias) < k_errorTol); // //b2Settings.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' // xX = -cp1.normalMass * bX; xY = 0.0f; vn1 = 0.0f; vn2 = c.K.col1.y * xX + bY; if (xX >= 0.0f && vn2 >= 0.0f) { // Resubstitute for the incremental impulse //d = x - a; dX = xX - aX; dY = xY - aY; //Aply incremental impulse //P1 = d.x * normal; P1X = dX * normalX; P1Y = dX * normalY; //P2 = d.y * normal; P2X = dY * normalX; P2Y = dY * normalY; //vA -= invMassA * (P1 + P2) vA.x -= invMassA * (P1X + P2X); vA.y -= invMassA * (P1Y + P2Y); //wA -= invIA * (b2Cross(cp1.rA, P1) + b2Cross(cp2.rA, P2)); wA -= invIA * (cp1.rA.x * P1Y - cp1.rA.y * P1X + cp2.rA.x * P2Y - cp2.rA.y * P2X); //vB += invMassB * (P1 + P2) vB.x += invMassB * (P1X + P2X); vB.y += invMassB * (P1Y + P2Y); //wB += invIB * (b2Cross(cp1.rB, P1) + b2Cross(cp2.rB, P2)); wB += invIB * (cp1.rB.x * P1Y - cp1.rB.y * P1X + cp2.rB.x * P2Y - cp2.rB.y * P2X); // Accumulate cp1.normalImpulse = xX; cp2.normalImpulse = xY; //#if B2_DEBUG_SOLVER == 1 // // Post conditions // //dv1 = vB + b2Cross(wB, cp1.rB) - vA - b2Cross(wA, cp1.rA); // dv1X = vB.x - wB * cp1.rB.y - vA.x + wA * cp1.rA.y; // dv1Y = vB.y + wB * cp1.rB.x - vA.y - wA * cp1.rA.x; // //dv2 = vB + b2Cross(wB, cp2.rB) - vA - b2Cross(wA, cp2.rA); // dv1X = vB.x - wB * cp2.rB.y - vA.x + wA * cp2.rA.y; // dv1Y = vB.y + wB * cp2.rB.x - vA.y - wA * cp2.rA.x; // // Compute normal velocity // //vn1 = b2Dot(dv1, normal); // vn1 = dv1X * normalX + dv1Y * normalY; // //vn2 = b2Dot(dv2, normal); // vn2 = dv2X * normalX + dv2Y * normalY; // // //b2Settings.b2Assert(b2Abs(vn1 - cp1.velocityBias) < k_errorTol); // //b2Settings.b2Assert(b2Abs(vn2 - cp2.velocityBias) < k_errorTol); //#endif break; } // // Case 3: wB = 0 and x1 = 0 // // vn1 = a11 * 0 + a12 * x2' + b1' // 0 = a21 * 0 + a22 * x2' + b2' // xX = 0.0f; xY = -cp2.normalMass * bY; vn1 = c.K.col2.x * xY + bX; vn2 = 0.0f; if (xY >= 0.0f && vn1 >= 0.0f) { // Resubstitute for the incremental impulse //d = x - a; dX = xX - aX; dY = xY - aY; //Aply incremental impulse //P1 = d.x * normal; P1X = dX * normalX; P1Y = dX * normalY; //P2 = d.y * normal; P2X = dY * normalX; P2Y = dY * normalY; //vA -= invMassA * (P1 + P2) vA.x -= invMassA * (P1X + P2X); vA.y -= invMassA * (P1Y + P2Y); //wA -= invIA * (b2Cross(cp1.rA, P1) + b2Cross(cp2.rA, P2)); wA -= invIA * (cp1.rA.x * P1Y - cp1.rA.y * P1X + cp2.rA.x * P2Y - cp2.rA.y * P2X); //vB += invMassB * (P1 + P2) vB.x += invMassB * (P1X + P2X); vB.y += invMassB * (P1Y + P2Y); //wB += invIB * (b2Cross(cp1.rB, P1) + b2Cross(cp2.rB, P2)); wB += invIB * (cp1.rB.x * P1Y - cp1.rB.y * P1X + cp2.rB.x * P2Y - cp2.rB.y * P2X); // Accumulate cp1.normalImpulse = xX; cp2.normalImpulse = xY; //#if B2_DEBUG_SOLVER == 1 // // Post conditions // //dv1 = vB + b2Cross(wB, cp1.rB) - vA - b2Cross(wA, cp1.rA); // dv1X = vB.x - wB * cp1.rB.y - vA.x + wA * cp1.rA.y; // dv1Y = vB.y + wB * cp1.rB.x - vA.y - wA * cp1.rA.x; // //dv2 = vB + b2Cross(wB, cp2.rB) - vA - b2Cross(wA, cp2.rA); // dv1X = vB.x - wB * cp2.rB.y - vA.x + wA * cp2.rA.y; // dv1Y = vB.y + wB * cp2.rB.x - vA.y - wA * cp2.rA.x; // // Compute normal velocity // //vn1 = b2Dot(dv1, normal); // vn1 = dv1X * normalX + dv1Y * normalY; // //vn2 = b2Dot(dv2, normal); // vn2 = dv2X * normalX + dv2Y * normalY; // // //b2Settings.b2Assert(b2Abs(vn1 - cp1.velocityBias) < k_errorTol); // //b2Settings.b2Assert(b2Abs(vn2 - cp2.velocityBias) < k_errorTol); //#endif break; } // // Case 4: x1 = 0 and x2 = 0 // // vn1 = b1 // vn2 = b2 xX = 0.0f; xY = 0.0f; vn1 = bX; vn2 = bY; if (vn1 >= 0.0f && vn2 >= 0.0f) { // Resubstitute for the incremental impulse //d = x - a; dX = xX - aX; dY = xY - aY; //Aply incremental impulse //P1 = d.x * normal; P1X = dX * normalX; P1Y = dX * normalY; //P2 = d.y * normal; P2X = dY * normalX; P2Y = dY * normalY; //vA -= invMassA * (P1 + P2) vA.x -= invMassA * (P1X + P2X); vA.y -= invMassA * (P1Y + P2Y); //wA -= invIA * (b2Cross(cp1.rA, P1) + b2Cross(cp2.rA, P2)); wA -= invIA * (cp1.rA.x * P1Y - cp1.rA.y * P1X + cp2.rA.x * P2Y - cp2.rA.y * P2X); //vB += invMassB * (P1 + P2) vB.x += invMassB * (P1X + P2X); vB.y += invMassB * (P1Y + P2Y); //wB += invIB * (b2Cross(cp1.rB, P1) + b2Cross(cp2.rB, P2)); wB += invIB * (cp1.rB.x * P1Y - cp1.rB.y * P1X + cp2.rB.x * P2Y - cp2.rB.y * P2X); // Accumulate cp1.normalImpulse = xX; cp2.normalImpulse = xY; //#if B2_DEBUG_SOLVER == 1 // // Post conditions // //dv1 = vB + b2Cross(wB, cp1.rB) - vA - b2Cross(wA, cp1.rA); // dv1X = vB.x - wB * cp1.rB.y - vA.x + wA * cp1.rA.y; // dv1Y = vB.y + wB * cp1.rB.x - vA.y - wA * cp1.rA.x; // //dv2 = vB + b2Cross(wB, cp2.rB) - vA - b2Cross(wA, cp2.rA); // dv1X = vB.x - wB * cp2.rB.y - vA.x + wA * cp2.rA.y; // dv1Y = vB.y + wB * cp2.rB.x - vA.y - wA * cp2.rA.x; // // Compute normal velocity // //vn1 = b2Dot(dv1, normal); // vn1 = dv1X * normalX + dv1Y * normalY; // //vn2 = b2Dot(dv2, normal); // vn2 = dv2X * normalX + dv2Y * normalY; // // //b2Settings.b2Assert(b2Abs(vn1 - cp1.velocityBias) < k_errorTol); // //b2Settings.b2Assert(b2Abs(vn2 - cp2.velocityBias) < k_errorTol); //#endif break; } // No solution, give up. This is hit sometimes, but it doesn't seem to matter. break; } } // b2Vec2s in AS3 are copied by reference. The originals are // references to the same things here and there is no need to // copy them back, unlike in C++ land where b2Vec2s are // copied by value. /*bodyA->m_linearVelocity = vA; * bodyB->m_linearVelocity = vB;*/ bodyA.m_angularVelocity = wA; bodyB.m_angularVelocity = wB; //--------------------修改start kingBook-------------------- if (!bodyA.m_allowMovement) { bodyA.m_linearVelocity.x = 0.0f; bodyA.m_linearVelocity.y = 0.0f; } if (!bodyB.m_allowMovement) { bodyB.m_linearVelocity.x = 0.0f; bodyB.m_linearVelocity.y = 0.0f; } //--------------------修改end-------------------- } }
public void Initialize(b2TimeStep step, List <b2Contact> contacts, int contactCount, object allocator) { b2Contact contact; m_step.Set(step); m_allocator = allocator; int i; b2Vec2 tVec; b2Mat22 tMat; m_constraintCount = contactCount; // fill vector to hold enough constraints while (m_constraints.Count < m_constraintCount) { m_constraints.Add(new b2ContactConstraint()); } for (i = 0; i < contactCount; ++i) { contact = contacts[i]; b2Fixture fixtureA = contact.m_fixtureA; b2Fixture fixtureB = contact.m_fixtureB; b2Shape shapeA = fixtureA.m_shape; b2Shape shapeB = fixtureB.m_shape; float radiusA = shapeA.m_radius; float radiusB = shapeB.m_radius; b2Body bodyA = fixtureA.m_body; b2Body bodyB = fixtureB.m_body; b2Manifold manifold = contact.GetManifold(); float friction = b2Settings.b2MixFriction(fixtureA.GetFriction(), fixtureB.GetFriction());; float restitution = b2Settings.b2MixRestitution(fixtureA.GetRestitution(), fixtureB.GetRestitution()); //var vA:b2Vec2 = bodyA.m_linearVelocity.Copy(); float vAX = bodyA.m_linearVelocity.x; float vAY = bodyA.m_linearVelocity.y; //var vB:b2Vec2 = bodyB.m_linearVelocity.Copy(); float vBX = bodyB.m_linearVelocity.x; float vBY = bodyB.m_linearVelocity.y; float wA = bodyA.m_angularVelocity; float wB = bodyB.m_angularVelocity; b2Settings.b2Assert(manifold.m_pointCount > 0); s_worldManifold.Initialize(manifold, bodyA.m_xf, radiusA, bodyB.m_xf, radiusB); float normalX = s_worldManifold.m_normal.x; float normalY = s_worldManifold.m_normal.y; b2ContactConstraint cc = m_constraints[i]; cc.bodyA = bodyA; //p cc.bodyB = bodyB; //p cc.manifold = manifold; //p //c.normal = normal; cc.normal.x = normalX; cc.normal.y = normalY; cc.pointCount = manifold.m_pointCount; cc.friction = friction; //-----------------------------修改 2015/12/10 13:07 by kingBook------------------ float bevel = 10.0f; float planeAngle; int vx; if (!bodyA.m_allowBevelSlither || bodyA.m_uphillZeroFriction) { planeAngle = Mathf.Atan2(normalY, normalX) * 57.3f + 90.0f; vx = (int)(bodyA.m_linearVelocity.x); if (planeAngle < 0.0f) { planeAngle += 360.0f; } if ((planeAngle > bevel && planeAngle < 90.0f - bevel) || (planeAngle > 180.0f + bevel && planeAngle < 270.0f - bevel))//斜面 左上角-右下角 { if (!bodyA.m_allowBevelSlither) { if (vx >= 0.0f) { cc.friction = 1.0f; } } if (bodyA.m_uphillZeroFriction) { if (vx < 0.0f) { cc.friction = 0.0f; } } } else if ((planeAngle > 90.0f && planeAngle < 180.0f - bevel) || (planeAngle > 270.0f + bevel && planeAngle < 360.0f - bevel))//斜面 左下角-右上角 { if (!bodyA.m_allowBevelSlither) { if (vx <= 0.0f) { cc.friction = 1.0f; } } if (bodyA.m_uphillZeroFriction) { if (vx > 0.0f) { cc.friction = 0.0f; } } } } else if (!bodyB.m_allowBevelSlither || bodyB.m_uphillZeroFriction) { planeAngle = Mathf.Atan2(-normalY, -normalX) * 57.3f + 90.0f; vx = (int)(bodyB.m_linearVelocity.x); if (planeAngle < 0.0f) { planeAngle += 360.0f; } if ((planeAngle > bevel && planeAngle < 90.0f - bevel) || (planeAngle > 180.0f + bevel && planeAngle < 270.0f - bevel))//斜面 左上角-右下角 { if (!bodyB.m_allowBevelSlither) { if (vx >= 0.0f) { cc.friction = 1.0f; } } if (bodyB.m_uphillZeroFriction) { if (vx < 0.0f) { cc.friction = 0.0f; } } } else if ((planeAngle > 90.0f && planeAngle < 180.0f - bevel) || (planeAngle > 270.0f + bevel && planeAngle < 360.0f - bevel))//斜面 左下角-右上角 { if (!bodyB.m_allowBevelSlither) { if (vx <= 0.0f) { cc.friction = 1.0f; } } if (bodyB.m_uphillZeroFriction) { if (vx > 0) { cc.friction = 0.0f; } } } } if (bodyA.m_isIgnoreFrictionX || bodyB.m_isIgnoreFrictionX) { if (Mathf.Abs(normalY) > 0.9f) { cc.friction = 0.0f; } } else if (bodyA.m_isIgnoreFrictionY || bodyB.m_isIgnoreFrictionY) { if (Mathf.Abs(normalX) > 0.9f) { cc.friction = 0.0f; } } //-----------------------------修改结束------------------ cc.restitution = restitution; cc.localPlaneNormal.x = manifold.m_localPlaneNormal.x; cc.localPlaneNormal.y = manifold.m_localPlaneNormal.y; cc.localPoint.x = manifold.m_localPoint.x; cc.localPoint.y = manifold.m_localPoint.y; cc.radius = radiusA + radiusB; cc.type = manifold.m_type; for (int k = 0; k < cc.pointCount; ++k) { b2ManifoldPoint cp = manifold.m_points[k]; b2ContactConstraintPoint ccp = cc.points[k]; ccp.normalImpulse = cp.m_normalImpulse; ccp.tangentImpulse = cp.m_tangentImpulse; ccp.localPoint.SetV(cp.m_localPoint); float rAX = ccp.rA.x = s_worldManifold.m_points[k].x - bodyA.m_sweep.c.x; float rAY = ccp.rA.y = s_worldManifold.m_points[k].y - bodyA.m_sweep.c.y; float rBX = ccp.rB.x = s_worldManifold.m_points[k].x - bodyB.m_sweep.c.x; float rBY = ccp.rB.y = s_worldManifold.m_points[k].y - bodyB.m_sweep.c.y; float rnA = rAX * normalY - rAY * normalX; //b2Math.b2Cross(r1, normal); float rnB = rBX * normalY - rBY * normalX; //b2Math.b2Cross(r2, normal); rnA *= rnA; rnB *= rnB; float kNormal = bodyA.m_invMass + bodyB.m_invMass + bodyA.m_invI * rnA + bodyB.m_invI * rnB; //b2Settings.b2Assert(kNormal > Number.MIN_VALUE); ccp.normalMass = 1.0f / kNormal; float kEqualized = bodyA.m_mass * bodyA.m_invMass + bodyB.m_mass * bodyB.m_invMass; kEqualized += bodyA.m_mass * bodyA.m_invI * rnA + bodyB.m_mass * bodyB.m_invI * rnB; //b2Assert(kEqualized > Number.MIN_VALUE); ccp.equalizedMass = 1.0f / kEqualized; //var tangent:b2Vec2 = b2Math.b2CrossVF(normal, 1.0); float tangentX = normalY; float tangentY = -normalX; //var rtA:Number = b2Math.b2Cross(rA, tangent); float rtA = rAX * tangentY - rAY * tangentX; //var rtB:Number = b2Math.b2Cross(rB, tangent); float rtB = rBX * tangentY - rBY * tangentX; rtA *= rtA; rtB *= rtB; float kTangent = bodyA.m_invMass + bodyB.m_invMass + bodyA.m_invI * rtA + bodyB.m_invI * rtB; //b2Settings.b2Assert(kTangent > Number.MIN_VALUE); ccp.tangentMass = 1.0f / kTangent; // Setup a velocity bias for restitution. ccp.velocityBias = 0.0f; //b2Dot(c.normal, vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA)); float tX = vBX + (-wB * rBY) - vAX - (-wA * rAY); float tY = vBY + (wB * rBX) - vAY - (wA * rAX); //var vRel:Number = b2Dot(cc.normal, t); float vRel = cc.normal.x * tX + cc.normal.y * tY; if (vRel < -b2Settings.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]; float invMassA = bodyA.m_invMass; float invIA = bodyA.m_invI; float invMassB = bodyB.m_invMass; float invIB = bodyB.m_invI; //var rn1A:Number = b2Cross(ccp1.rA, normal); //var rn1B:Number = b2Cross(ccp1.rB, normal); //var rn2A:Number = b2Cross(ccp2.rA, normal); //var rn2B:Number = b2Cross(ccp2.rB, normal); float rn1A = ccp1.rA.x * normalY - ccp1.rA.y * normalX; float rn1B = ccp1.rB.x * normalY - ccp1.rB.y * normalX; float rn2A = ccp2.rA.x * normalY - ccp2.rA.y * normalX; float rn2B = ccp2.rB.x * normalY - ccp2.rB.y * normalX; float k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B; float k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B; float k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B; // Ensure a reasonable condition number. float 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.K.GetInverse(cc.normalMass); } else { // The constraints are redundant, just use one. // TODO_ERIN use deepest? cc.pointCount = 1; } } } //b2Settings.b2Assert(count == m_constraintCount); }
//~b2ContactSolver(); public void InitVelocityConstraints(b2TimeStep step) { b2Vec2 tVec; b2Vec2 tVec2; b2Mat22 tMat; // Warm start. for (int i = 0; i < m_constraintCount; ++i) { b2ContactConstraint c = m_constraints[i]; b2Body bodyA = c.bodyA; b2Body bodyB = c.bodyB; float invMassA = bodyA.m_invMass; float invIA = bodyA.m_invI; float invMassB = bodyB.m_invMass; float invIB = bodyB.m_invI; //var normal:b2Vec2 = new b2Vec2(c.normal.x, c.normal.y); float normalX = c.normal.x; float normalY = c.normal.y; //var tangent:b2Vec2 = b2Math.b2CrossVF(normal, 1.0); float tangentX = normalY; float tangentY = -normalX; float tX; int j; int tCount; if (step.warmStarting) { tCount = c.pointCount; for (j = 0; j < tCount; ++j) { b2ContactConstraintPoint ccp = c.points[j]; ccp.normalImpulse *= step.dtRatio; ccp.tangentImpulse *= step.dtRatio; //b2Vec2 P = ccp->normalImpulse * normal + ccp->tangentImpulse * tangent; float PX = ccp.normalImpulse * normalX + ccp.tangentImpulse * tangentX; float PY = ccp.normalImpulse * normalY + ccp.tangentImpulse * tangentY; //bodyA.m_angularVelocity -= invIA * b2Math.b2CrossVV(rA, P); bodyA.m_angularVelocity -= invIA * (ccp.rA.x * PY - ccp.rA.y * PX); //bodyA.m_linearVelocity.Subtract( b2Math.MulFV(invMassA, P) ); bodyA.m_linearVelocity.x -= invMassA * PX; bodyA.m_linearVelocity.y -= invMassA * PY; //bodyB.m_angularVelocity += invIB * b2Math.b2CrossVV(rB, P); bodyB.m_angularVelocity += invIB * (ccp.rB.x * PY - ccp.rB.y * PX); //bodyB.m_linearVelocity.Add( b2Math.MulFV(invMassB, P) ); bodyB.m_linearVelocity.x += invMassB * PX; bodyB.m_linearVelocity.y += invMassB * PY; //--------------------修改start kingBook-------------------- if (!bodyA.m_allowMovement) { bodyA.m_linearVelocity.x = 0.0f; bodyA.m_linearVelocity.y = 0.0f; } if (!bodyB.m_allowMovement) { bodyB.m_linearVelocity.x = 0.0f; bodyB.m_linearVelocity.y = 0.0f; } //--------------------修改end-------------------- } } else { tCount = c.pointCount; for (j = 0; j < tCount; ++j) { b2ContactConstraintPoint ccp2 = c.points[j]; ccp2.normalImpulse = 0.0f; ccp2.tangentImpulse = 0.0f; } } } }