public void ApplyImpulse(Vec2 impulse, Vec2 contactVector)
{
// velocity += im * impulse;
// angularVelocity += iI * Cross( contactVector, impulse );
velocity.Addsi(impulse, invMass);
angularVelocity += invInertia * Vec2.Cross(contactVector, impulse);
}
public override void ComputeMass(float density)
{
// Calculate centroid and moment of inertia
Vec2 c = new Vec2(0.0f, 0.0f); // centroid
float area = 0.0f;
float I = 0.0f;
const float k_inv3 = 1.0f / 3.0f;
for (int i = 0; i < vertexCount; ++i)
{
// Triangle vertices, third vertex implied as (0, 0)
Vec2 p1 = vertices[i];
Vec2 p2 = vertices[(i + 1) % vertexCount];
float D = Vec2.Cross(p1, p2);
float triangleArea = 0.5f * D;
area += triangleArea;
// Use area to weight the centroid average, not just vertex position
float weight = triangleArea * k_inv3;
c.Addsi(p1, weight);
c.Addsi(p2, weight);
float intx2 = p1.x * p1.x + p2.x * p1.x + p2.x * p2.x;
float inty2 = p1.y * p1.y + p2.y * p1.y + p2.y * p2.y;
I += (0.25f * k_inv3 * D) * (intx2 + inty2);
}
c.Muli(1.0f / area);
// Translate vertices to centroid (make the centroid (0, 0)
// for the polygon in model space)
// Not really necessary, but I like doing this anyway
for (int i = 0; i < vertexCount; ++i)
{
vertices[i].Subi(c);
}
body.mass = density * area;
body.invMass = (body.mass != 0.0f) ? 1.0f / body.mass : 0.0f;
body.inertia = I * density;
body.invInertia = (body.inertia != 0.0f) ? 1.0f / body.inertia : 0.0f;
}
public void ApplyImpulse()
{
// Early out and positional correct if both objects have infinite mass
// if(Equal( A->im + B->im, 0 ))
if (ImpulseMath.Equal(A.invMass + B.invMass, 0))
{
InfiniteMassCorrection();
return;
}
for (int i = 0; i < contactCount; ++i)
{
// Calculate radii from COM to contact
// Vec2 ra = contacts[i] - A->position;
// Vec2 rb = contacts[i] - B->position;
Vec2 ra = contacts[i].Sub(A.position);
Vec2 rb = contacts[i].Sub(B.position);
// Relative velocity
// Vec2 rv = B->velocity + Cross( B->angularVelocity, rb ) -
// A->velocity - Cross( A->angularVelocity, ra );
Vec2 rv = B.velocity.Add(Vec2.Cross(B.angularVelocity, rb, new Vec2())).Subi(A.velocity).Subi(Vec2.Cross(A.angularVelocity, ra, new Vec2()));
// Relative velocity along the normal
// real contactVel = Dot( rv, normal );
float contactVel = Vec2.Dot(rv, normal);
// Do not resolve if velocities are separating
if (contactVel > 0)
{
return;
}
// real raCrossN = Cross( ra, normal );
// real rbCrossN = Cross( rb, normal );
// real invMassSum = A->im + B->im + Sqr( raCrossN ) * A->iI + Sqr(
// rbCrossN ) * B->iI;
float raCrossN = Vec2.Cross(ra, normal);
float rbCrossN = Vec2.Cross(rb, normal);
float invMassSum = A.invMass + B.invMass + (raCrossN * raCrossN) * A.invInertia + (rbCrossN * rbCrossN) * B.invInertia;
// Calculate impulse scalar
float j = -(1.0f + e) * contactVel;
j /= invMassSum;
j /= contactCount;
// Apply impulse
Vec2 impulse = normal.Mul(j);
A.ApplyImpulse(impulse.Neg(), ra);
B.ApplyImpulse(impulse, rb);
// Friction impulse
// rv = B->velocity + Cross( B->angularVelocity, rb ) -
// A->velocity - Cross( A->angularVelocity, ra );
rv = B.velocity.Add(Vec2.Cross(B.angularVelocity, rb, new Vec2())).Subi(A.velocity).Subi(Vec2.Cross(A.angularVelocity, ra, new Vec2()));
// Vec2 t = rv - (normal * Dot( rv, normal ));
// t.Normalize( );
Vec2 t = new Vec2(rv);
t.Addsi(normal, -Vec2.Dot(rv, normal));
t.Normalize();
// j tangent magnitude
float jt = -Vec2.Dot(rv, t);
jt /= invMassSum;
jt /= contactCount;
// Don't apply tiny friction impulses
if (ImpulseMath.Equal(jt, 0.0f))
{
return;
}
// Coulumb's law
Vec2 tangentImpulse;
// if(std::abs( jt ) < j * sf)
if (Mathf.Abs(jt) < j * sf)
{
// tangentImpulse = t * jt;
tangentImpulse = t.Mul(jt);
}
else
{
// tangentImpulse = t * -j * df;
tangentImpulse = t.Mul(j).Muli(-df);
}
// Apply friction impulse
// A->ApplyImpulse( -tangentImpulse, ra );
// B->ApplyImpulse( tangentImpulse, rb );
A.ApplyImpulse(tangentImpulse.Neg(), ra);
B.ApplyImpulse(tangentImpulse, rb);
}
}
