public PSolver(PShape shape1, PShape shape2, PContact[] contacts, int num)
{
s1 = shape1;
s2 = shape2;
b1 = s1._parent;
b2 = s2._parent;
fric = (float)System.Math.Sqrt(s1._fric * s2._fric);
rest = (float)System.Math.Sqrt(s1._rest * s2._rest);
cs = contacts;
numContacts = num;
for (int i = 0; i < numContacts; i++)
{
PContact c = cs[i];
c.rel1 = c.pos.Sub(b1.pos);
c.rel2 = c.pos.Sub(b2.pos);
c.massN = PTransformer.CalcEffectiveMass(b1, b2, c.rel1, c.rel2,
c.normal);
c.massT = PTransformer.CalcEffectiveMass(b1, b2, c.rel1, c.rel2,
c.tangent);
c.relVel = PTransformer
.CalcRelativeVelocity(b1, b2, c.rel1, c.rel2);
float rvn = c.relVel.Dot(c.normal);
if (rvn < -0.5F)
{
c.targetVelocity = System.Math.Max(rest * -rvn, 0.0F);
}
else
{
c.targetVelocity = 0.0F;
}
c.tangent.Set(c.normal.y, -c.normal.x);
c.localRel1.Set(c.rel1.x, c.rel1.y);
c.localRel2.Set(c.rel2.x, c.rel2.y);
b1.mAng.Transpose().MulEqual(c.localRel1);
b2.mAng.Transpose().MulEqual(c.localRel2);
}
}
internal void SolveVelocity()
{
for (int i = 0; i < numContacts; i++)
{
PContact c = cs[i];
c.relVel = PTransformer
.CalcRelativeVelocity(b1, b2, c.rel1, c.rel2);
float rvn = c.normal.x * c.relVel.x + c.normal.y * c.relVel.y;
float subNormalI = -c.massN * (rvn - c.targetVelocity);
float newNormalI = Max(c.norI + subNormalI, 0.0F);
subNormalI = newNormalI - c.norI;
float forceX = c.normal.x * subNormalI;
float forceY = c.normal.y * subNormalI;
b1.ApplyImpulse(forceX, forceY, c.pos.x, c.pos.y);
b2.ApplyImpulse(-forceX, -forceY, c.pos.x, c.pos.y);
c.norI = newNormalI;
}
for (int i_0 = 0; i_0 < numContacts; i_0++)
{
PContact c_1 = cs[i_0];
c_1.relVel = PTransformer
.CalcRelativeVelocity(b1, b2, c_1.rel1, c_1.rel2);
float rvt = c_1.tangent.x * c_1.relVel.x + c_1.tangent.y * c_1.relVel.y;
float maxFriction = c_1.norI * fric;
float subTangentI = c_1.massT * -rvt;
float newTangentI = Clamp(c_1.tanI + subTangentI, -maxFriction,
maxFriction);
subTangentI = newTangentI - c_1.tanI;
float forceX_2 = c_1.tangent.x * subTangentI;
float forceY_3 = c_1.tangent.y * subTangentI;
b1.ApplyImpulse(forceX_2, forceY_3, c_1.pos.x, c_1.pos.y);
b2.ApplyImpulse(-forceX_2, -forceY_3, c_1.pos.x, c_1.pos.y);
c_1.tanI = newTangentI;
}
}
