// djm pooling public void init(ContactSolverDef def) { // System.out.println("Initializing contact solver"); m_step = def.step; m_count = def.count; if (m_positionConstraints.Length < m_count) { ContactPositionConstraint[] old = m_positionConstraints; m_positionConstraints = new ContactPositionConstraint[MathUtils.max(old.Length * 2, m_count)]; ArrayHelper.Copy(old, 0, m_positionConstraints, 0, old.Length); for (int i = old.Length; i < m_positionConstraints.Length; i++) { m_positionConstraints[i] = new ContactPositionConstraint(); } } if (m_velocityConstraints.Length < m_count) { ContactVelocityConstraint[] old = m_velocityConstraints; m_velocityConstraints = new ContactVelocityConstraint[MathUtils.max(old.Length * 2, m_count)]; ArrayHelper.Copy(old, 0, m_velocityConstraints, 0, old.Length); for (int i = old.Length; i < m_velocityConstraints.Length; i++) { m_velocityConstraints[i] = new ContactVelocityConstraint(); } } m_positions = def.positions; m_velocities = def.velocities; m_contacts = def.contacts; for (int i = 0; i < m_count; ++i) { // System.out.println("contacts: " + m_count); Contact contact = m_contacts[i]; Fixture fixtureA = contact.m_fixtureA; Fixture fixtureB = contact.m_fixtureB; Shape shapeA = fixtureA.getShape(); Shape shapeB = fixtureB.getShape(); double radiusA = shapeA.m_radius; double radiusB = shapeB.m_radius; Body bodyA = fixtureA.getBody(); Body bodyB = fixtureB.getBody(); Manifold manifold = contact.getManifold(); int pointCount = manifold.pointCount; ContactVelocityConstraint vc = m_velocityConstraints[i]; vc.friction = contact.m_friction; vc.restitution = contact.m_restitution; vc.tangentSpeed = contact.m_tangentSpeed; vc.indexA = bodyA.m_islandIndex; vc.indexB = bodyB.m_islandIndex; vc.invMassA = bodyA.m_invMass; vc.invMassB = bodyB.m_invMass; vc.invIA = bodyA.m_invI; vc.invIB = bodyB.m_invI; vc.contactIndex = i; vc.pointCount = pointCount; vc.K.setZero(); vc.normalMass.setZero(); ContactPositionConstraint pc = m_positionConstraints[i]; pc.indexA = bodyA.m_islandIndex; pc.indexB = bodyB.m_islandIndex; pc.invMassA = bodyA.m_invMass; pc.invMassB = bodyB.m_invMass; pc.localCenterA.set(bodyA.m_sweep.localCenter); pc.localCenterB.set(bodyB.m_sweep.localCenter); pc.invIA = bodyA.m_invI; pc.invIB = bodyB.m_invI; pc.localNormal.set(manifold.localNormal); pc.localPoint.set(manifold.localPoint); pc.pointCount = pointCount; pc.radiusA = radiusA; pc.radiusB = radiusB; pc.type = manifold.type; // System.out.println("contact point count: " + pointCount); for (int j = 0; j < pointCount; j++) { ManifoldPoint cp = manifold.points[j]; VelocityConstraintPoint vcp = vc.points[j]; if (m_step.warmStarting) { // assert(cp.normalImpulse == 0); // System.out.println("contact normal impulse: " + cp.normalImpulse); vcp.normalImpulse = m_step.dtRatio * cp.normalImpulse; vcp.tangentImpulse = m_step.dtRatio * cp.tangentImpulse; } else { vcp.normalImpulse = 0; vcp.tangentImpulse = 0; } vcp.rA.setZero(); vcp.rB.setZero(); vcp.normalMass = 0; vcp.tangentMass = 0; vcp.velocityBias = 0; pc.localPoints[j].x = cp.localPoint.x; pc.localPoints[j].y = cp.localPoint.y; } } }
// djm pooling, and from above public void initializeVelocityConstraints() { // Warm start. for (int i = 0; i < m_count; ++i) { ContactVelocityConstraint vc = m_velocityConstraints[i]; ContactPositionConstraint pc = m_positionConstraints[i]; double radiusA = pc.radiusA; double radiusB = pc.radiusB; Manifold manifold = m_contacts[vc.contactIndex].getManifold(); int indexA = vc.indexA; int indexB = vc.indexB; double mA = vc.invMassA; double mB = vc.invMassB; double iA = vc.invIA; double iB = vc.invIB; Vec2 localCenterA = pc.localCenterA; Vec2 localCenterB = pc.localCenterB; Vec2 cA = m_positions[indexA].c; double aA = m_positions[indexA].a; Vec2 vA = m_velocities[indexA].v; double wA = m_velocities[indexA].w; Vec2 cB = m_positions[indexB].c; double aB = m_positions[indexB].a; Vec2 vB = m_velocities[indexB].v; double wB = m_velocities[indexB].w; xfA.q.set(aA); xfB.q.set(aB); xfA.p.x = cA.x - (xfA.q.c * localCenterA.x - xfA.q.s * localCenterA.y); xfA.p.y = cA.y - (xfA.q.s * localCenterA.x + xfA.q.c * localCenterA.y); xfB.p.x = cB.x - (xfB.q.c * localCenterB.x - xfB.q.s * localCenterB.y); xfB.p.y = cB.y - (xfB.q.s * localCenterB.x + xfB.q.c * localCenterB.y); worldManifold.initialize(manifold, xfA, radiusA, xfB, radiusB); vc.normal.set(worldManifold.normal); int pointCount = vc.pointCount; for (int j = 0; j < pointCount; ++j) { VelocityConstraintPoint vcp = vc.points[j]; vcp.rA.set(worldManifold.points[j]).subLocal(cA); vcp.rB.set(worldManifold.points[j]).subLocal(cB); double rnA = vcp.rA.x * vc.normal.y - vcp.rA.y * vc.normal.x; double rnB = vcp.rB.x * vc.normal.y - vcp.rB.y * vc.normal.x; double kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB; vcp.normalMass = kNormal > 0.0d ? 1.0d / kNormal : 0.0d; double tangentx = 1.0d * vc.normal.y; double tangenty = -1.0d * vc.normal.x; double rtA = vcp.rA.x * tangenty - vcp.rA.y * tangentx; double rtB = vcp.rB.x * tangenty - vcp.rB.y * tangentx; double kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB; vcp.tangentMass = kTangent > 0.0d ? 1.0d / kTangent : 0.0d; // Setup a velocity bias for restitution. vcp.velocityBias = 0.0d; double tempx = vB.x + -wB * vcp.rB.y - vA.x - (-wA * vcp.rA.y); double tempy = vB.y + wB * vcp.rB.x - vA.y - (wA * vcp.rA.x); double vRel = vc.normal.x * tempx + vc.normal.y * tempy; if (vRel < -Settings.velocityThreshold) { vcp.velocityBias = -vc.restitution * vRel; } } // If we have two points, then prepare the block solver. if (vc.pointCount == 2) { VelocityConstraintPoint vcp1 = vc.points[0]; VelocityConstraintPoint vcp2 = vc.points[1]; double rn1A = Vec2.cross(vcp1.rA, vc.normal); double rn1B = Vec2.cross(vcp1.rB, vc.normal); double rn2A = Vec2.cross(vcp2.rA, vc.normal); double rn2B = Vec2.cross(vcp2.rB, vc.normal); double k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B; double k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B; double k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B; 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.K.invertToOut(vc.normalMass); } else { // The constraints are redundant, just use one. // TODO_ERIN use deepest? vc.pointCount = 1; } } } }
// Sequential position solver for position constraints. public bool solveTOIPositionConstraints(int toiIndexA, int toiIndexB) { double minSeparation = 0.0d; for (int i = 0; i < m_count; ++i) { ContactPositionConstraint pc = m_positionConstraints[i]; int indexA = pc.indexA; int indexB = pc.indexB; Vec2 localCenterA = pc.localCenterA; Vec2 localCenterB = pc.localCenterB; int pointCount = pc.pointCount; double mA = 0.0d; double iA = 0.0d; if (indexA == toiIndexA || indexA == toiIndexB) { mA = pc.invMassA; iA = pc.invIA; } double mB = 0d; double iB = 0d; if (indexB == toiIndexA || indexB == toiIndexB) { mB = pc.invMassB; iB = pc.invIB; } Vec2 cA = m_positions[indexA].c; double aA = m_positions[indexA].a; Vec2 cB = m_positions[indexB].c; double aB = m_positions[indexB].a; // Solve normal constraints for (int j = 0; j < pointCount; ++j) { xfA.q.set(aA); xfB.q.set(aB); Rot.mulToOutUnsafe(xfA.q, localCenterA, xfA.p); xfA.p.negateLocal().addLocal(cA); Rot.mulToOutUnsafe(xfB.q, localCenterB, xfB.p); xfB.p.negateLocal().addLocal(cB); PositionSolverManifold psm = psolver; psm.initialize(pc, xfA, xfB, j); Vec2 normal = psm.normal; Vec2 point = psm.point; double separation = psm.separation; rA.set(point).subLocal(cA); rB.set(point).subLocal(cB); // Track max constraint error. minSeparation = MathUtils.min(minSeparation, separation); // Prevent large corrections and allow slop. double C = MathUtils.clamp(Settings.toiBaugarte * (separation + Settings.linearSlop), -Settings.maxLinearCorrection, 0.0d); // Compute the effective mass. double rnA = Vec2.cross(rA, normal); double rnB = Vec2.cross(rB, normal); double K = mA + mB + iA * rnA * rnA + iB * rnB * rnB; // Compute normal impulse double impulse = K > 0.0d ? -C / K : 0.0d; P.set(normal).mulLocal(impulse); cA.subLocal(temp.set(P).mulLocal(mA)); aA -= iA * Vec2.cross(rA, P); cB.addLocal(temp.set(P).mulLocal(mB)); aB += iB * Vec2.cross(rB, P); } // m_positions[indexA].c.set(cA); m_positions[indexA].a = aA; // m_positions[indexB].c.set(cB); m_positions[indexB].a = aB; } // We can't expect minSpeparation >= -_linearSlop because we don't // push the separation above -_linearSlop. return(minSeparation >= -1.5d * Settings.linearSlop); }
public void initialize(ContactPositionConstraint pc, Transform xfA, Transform xfB, int index) { Rot xfAq = xfA.q; Rot xfBq = xfB.q; Vec2 pcLocalPointsI = pc.localPoints[index]; switch (pc.type) { case ManifoldType.CIRCLES: { // Transform.mulToOutUnsafe(xfA, pc.localPoint, pointA); // Transform.mulToOutUnsafe(xfB, pc.localPoints[0], pointB); // normal.set(pointB).subLocal(pointA); // normal.normalize(); // // point.set(pointA).addLocal(pointB).mulLocal(.5d); // temp.set(pointB).subLocal(pointA); // separation = Vec2.dot(temp, normal) - pc.radiusA - pc.radiusB; Vec2 plocalPoint = pc.localPoint; Vec2 pLocalPoints0 = pc.localPoints[0]; double pointAx = (xfAq.c * plocalPoint.x - xfAq.s * plocalPoint.y) + xfA.p.x; double pointAy = (xfAq.s * plocalPoint.x + xfAq.c * plocalPoint.y) + xfA.p.y; double pointBx = (xfBq.c * pLocalPoints0.x - xfBq.s * pLocalPoints0.y) + xfB.p.x; double pointBy = (xfBq.s * pLocalPoints0.x + xfBq.c * pLocalPoints0.y) + xfB.p.y; normal.x = pointBx - pointAx; normal.y = pointBy - pointAy; normal.normalize(); point.x = (pointAx + pointBx) * .5d; point.y = (pointAy + pointBy) * .5d; double tempx = pointBx - pointAx; double tempy = pointBy - pointAy; separation = tempx * normal.x + tempy * normal.y - pc.radiusA - pc.radiusB; break; } case ManifoldType.FACE_A: { // Rot.mulToOutUnsafe(xfAq, pc.localNormal, normal); // Transform.mulToOutUnsafe(xfA, pc.localPoint, planePoint); // // Transform.mulToOutUnsafe(xfB, pc.localPoints[index], clipPoint); // temp.set(clipPoint).subLocal(planePoint); // separation = Vec2.dot(temp, normal) - pc.radiusA - pc.radiusB; // point.set(clipPoint); Vec2 pcLocalNormal = pc.localNormal; Vec2 pcLocalPoint = pc.localPoint; normal.x = xfAq.c * pcLocalNormal.x - xfAq.s * pcLocalNormal.y; normal.y = xfAq.s * pcLocalNormal.x + xfAq.c * pcLocalNormal.y; double planePointx = (xfAq.c * pcLocalPoint.x - xfAq.s * pcLocalPoint.y) + xfA.p.x; double planePointy = (xfAq.s * pcLocalPoint.x + xfAq.c * pcLocalPoint.y) + xfA.p.y; double clipPointx = (xfBq.c * pcLocalPointsI.x - xfBq.s * pcLocalPointsI.y) + xfB.p.x; double clipPointy = (xfBq.s * pcLocalPointsI.x + xfBq.c * pcLocalPointsI.y) + xfB.p.y; double tempx = clipPointx - planePointx; double tempy = clipPointy - planePointy; separation = tempx * normal.x + tempy * normal.y - pc.radiusA - pc.radiusB; point.x = clipPointx; point.y = clipPointy; break; } case ManifoldType.FACE_B: { // Rot.mulToOutUnsafe(xfBq, pc.localNormal, normal); // Transform.mulToOutUnsafe(xfB, pc.localPoint, planePoint); // // Transform.mulToOutUnsafe(xfA, pcLocalPointsI, clipPoint); // temp.set(clipPoint).subLocal(planePoint); // separation = Vec2.dot(temp, normal) - pc.radiusA - pc.radiusB; // point.set(clipPoint); // // // Ensure normal points from A to B // normal.negateLocal(); Vec2 pcLocalNormal = pc.localNormal; Vec2 pcLocalPoint = pc.localPoint; normal.x = xfBq.c * pcLocalNormal.x - xfBq.s * pcLocalNormal.y; normal.y = xfBq.s * pcLocalNormal.x + xfBq.c * pcLocalNormal.y; double planePointx = (xfBq.c * pcLocalPoint.x - xfBq.s * pcLocalPoint.y) + xfB.p.x; double planePointy = (xfBq.s * pcLocalPoint.x + xfBq.c * pcLocalPoint.y) + xfB.p.y; double clipPointx = (xfAq.c * pcLocalPointsI.x - xfAq.s * pcLocalPointsI.y) + xfA.p.x; double clipPointy = (xfAq.s * pcLocalPointsI.x + xfAq.c * pcLocalPointsI.y) + xfA.p.y; double tempx = clipPointx - planePointx; double tempy = clipPointy - planePointy; separation = tempx * normal.x + tempy * normal.y - pc.radiusA - pc.radiusB; point.x = clipPointx; point.y = clipPointy; normal.x *= -1; normal.y *= -1; } break; } }