// 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;
}
}
