private void SolveC3()
{
int count3 = m_count - 2;
for (int i = 0; i < count3; ++i)
{
b2Vec2 p1 = m_ps[i];
b2Vec2 p2 = m_ps[i + 1];
b2Vec2 p3 = m_ps[i + 2];
float m1 = m_ims[i];
float m2 = m_ims[i + 1];
float m3 = m_ims[i + 2];
b2Vec2 d1 = p2 - p1;
b2Vec2 d2 = p3 - p2;
float L1sqr = d1.LengthSquared;
float L2sqr = d2.LengthSquared;
if (L1sqr * L2sqr == 0.0f)
{
continue;
}
float a = b2Math.b2Cross(d1, d2);
float b = b2Math.b2Dot(d1, d2);
float angle = b2Math.b2Atan2(a, b);
b2Vec2 Jd1 = (-1.0f / L1sqr) * d1.Skew();
b2Vec2 Jd2 = (1.0f / L2sqr) * d2.Skew();
b2Vec2 J1 = -Jd1;
b2Vec2 J2 = Jd1 - Jd2;
b2Vec2 J3 = Jd2;
float mass = m1 * b2Math.b2Dot(J1, J1) + m2 * b2Math.b2Dot(J2, J2) + m3 * b2Math.b2Dot(J3, J3);
if (mass == 0.0f)
{
continue;
}
mass = 1.0f / mass;
float C = angle - m_as[i];
while (C > b2Settings.b2_pi)
{
angle -= 2f * (float)Math.PI;
C = angle - m_as[i];
}
while (C < -(float)Math.PI)
{
angle += 2.0f * (float)Math.PI;
C = angle - m_as[i];
}
float impulse = -m_k3 * mass * C;
p1 += (m1 * impulse) * J1;
p2 += (m2 * impulse) * J2;
p3 += (m3 * impulse) * J3;
m_ps[i] = p1;
m_ps[i + 1] = p2;
m_ps[i + 2] = p3;
}
}