public void ShutDown()
{
for (int i = 0; i < Contacts.Count; ++i)
{
ContactConstraintState c = Contacts[i];
ContactConstraint cc = Island.Contacts[i];
for (int j = 0; j < c.contactCount; ++j)
{
Contact oc = cc.manifold.contacts[j];
ContactState cs = c.contacts[j];
oc.normalImpulse = cs.normalImpulse;
oc.tangentImpulse = cs.tangentImpulse;
oc.bitangentImpulse = cs.bitangentImpulse;
}
}
}
public void Clear()
{
Bodies.Clear();
Velocities.Clear();
Contacts.Clear();
foreach (var state in ContactStates)
{
for (int i = 0; i < state.contactCount; i++)
{
ContactState.Free(state.contacts[i]);
}
ContactConstraintState.Free(state);
// Array.Clear(state.contacts, 0, state.contactCount);
}
ContactStates.Clear();
}
public void Initialize()
{
for (int i = 0; i < Contacts.Count; ++i)
{
ContactConstraint cc = Contacts[i];
ContactConstraintState c = ContactStates[i];
c.centerA = cc.bodyA.WorldCenter;
c.centerB = cc.bodyB.WorldCenter;
c.iA = cc.bodyA.InvInertiaWorld;
c.iB = cc.bodyB.InvInertiaWorld;
c.mA = cc.bodyA.InvMass;
c.mB = cc.bodyB.InvMass;
c.restitution = cc.restitution;
c.friction = cc.friction;
c.indexA = cc.bodyA.IslandIndex;
c.indexB = cc.bodyB.IslandIndex;
c.normal = cc.manifold.normal;
c.tangentVectors = cc.manifold.tangentVectors;
c.bitangentVectors = cc.manifold.bitangentVectors;
c.contactCount = cc.manifold.contactCount;
for (int j = 0; j < c.contactCount; ++j)
{
ContactState s = c.contacts[j] = ContactState.Allocate();
Contact cp = cc.manifold.contacts[j];
s.ra = cp.position - c.centerA;
s.rb = cp.position - c.centerB;
s.penetration = cp.penetration;
s.normalImpulse = cp.normalImpulse;
s.tangentImpulse = cp.tangentImpulse;
s.bitangentImpulse = cp.bitangentImpulse;
}
}
}
public void Add(ContactConstraint contact)
{
Contacts.Add(contact);
ContactStates.Add(ContactConstraintState.Allocate());
}
static public void Free(ContactConstraintState instance)
{
heap.Push(instance);
}
public void Solve()
{
for (int i = 0; i < Contacts.Count; ++i)
{
ContactConstraintState cs = Contacts[i];
Vec3 vA = Velocities[cs.indexA].v;
Vec3 wA = Velocities[cs.indexA].w;
Vec3 vB = Velocities[cs.indexB].v;
Vec3 wB = Velocities[cs.indexB].w;
for (int j = 0; j < cs.contactCount; ++j)
{
ContactState c = cs.contacts[j];
// relative velocity at contact
Vec3 dv = vB + Vec3.Cross(wB, c.rb) - vA - Vec3.Cross(wA, c.ra);
// Friction
if (EnableFriction)
{
{
double lambda = -Vec3.Dot(dv, cs.tangentVectors) * c.tangentMass;
// Calculate frictional impulse
double maxLambda = cs.friction * c.normalImpulse;
// Clamp frictional impulse
double oldPT = c.tangentImpulse;
c.tangentImpulse = Clamp(-maxLambda, maxLambda, oldPT + lambda);
lambda = c.tangentImpulse - oldPT;
// Apply friction impulse
Vec3 impulse = cs.tangentVectors * lambda;
vA -= impulse * cs.mA;
wA -= cs.iA * Vec3.Cross(c.ra, impulse);
vB += impulse * cs.mB;
wB += cs.iB * Vec3.Cross(c.rb, impulse);
}
{
double lambda = -Vec3.Dot(dv, cs.bitangentVectors) * c.bitangentMass;
// Calculate frictional impulse
double maxLambda = cs.friction * c.normalImpulse;
// Clamp frictional impulse
double oldPT = c.bitangentImpulse;
c.bitangentImpulse = Clamp(-maxLambda, maxLambda, oldPT + lambda);
lambda = c.bitangentImpulse - oldPT;
// Apply friction impulse
Vec3 impulse = cs.bitangentVectors * lambda;
vA -= impulse * cs.mA;
wA -= cs.iA * Vec3.Cross(c.ra, impulse);
vB += impulse * cs.mB;
wB += cs.iB * Vec3.Cross(c.rb, impulse);
}
}
// Normal
{
dv = vB + Vec3.Cross(wB, c.rb) - vA - Vec3.Cross(wA, c.ra);
// Normal impulse
double vn = Vec3.Dot(dv, cs.normal);
// Factor in positional bias to calculate impulse scalar j
double lambda = c.normalMass * (-vn + c.bias);
// Clamp impulse
double tempPN = c.normalImpulse;
c.normalImpulse = Math.Max(tempPN + lambda, 0);
lambda = c.normalImpulse - tempPN;
// Apply impulse
Vec3 impulse = cs.normal * lambda;
vA -= impulse * cs.mA;
wA -= cs.iA * Vec3.Cross(c.ra, impulse);
vB += impulse * cs.mB;
wB += cs.iB * Vec3.Cross(c.rb, impulse);
}
}
Velocities[cs.indexA] = new VelocityState {
v = vA, w = wA
};
Velocities[cs.indexB] = new VelocityState {
v = vB, w = wB
};
}
}
public void PreSolve(double dt)
{
for (int i = 0; i < Contacts.Count; ++i)
{
ContactConstraintState cs = Contacts[i];
Vec3 vA = Velocities[cs.indexA].v;
Vec3 wA = Velocities[cs.indexA].w;
Vec3 vB = Velocities[cs.indexB].v;
Vec3 wB = Velocities[cs.indexB].w;
for (int j = 0; j < cs.contactCount; ++j)
{
ContactState c = cs.contacts[j];
// Precalculate JM^-1JT for contact and friction constraints
Vec3 raCn = Vec3.Cross(c.ra, cs.normal);
Vec3 rbCn = Vec3.Cross(c.rb, cs.normal);
double nm = cs.mA + cs.mB;
nm += Vec3.Dot(raCn, cs.iA * raCn) + Vec3.Dot(rbCn, cs.iB * rbCn);
c.normalMass = Invert(nm);
{
Vec3 raCt = Vec3.Cross(cs.tangentVectors, c.ra);
Vec3 rbCt = Vec3.Cross(cs.tangentVectors, c.rb);
var tm = nm + Vec3.Dot(raCt, cs.iA * raCt) + Vec3.Dot(rbCt, cs.iB * rbCt);
c.tangentMass = Invert(tm);
}
{
Vec3 raCt = Vec3.Cross(cs.bitangentVectors, c.ra);
Vec3 rbCt = Vec3.Cross(cs.bitangentVectors, c.rb);
var tm = nm + Vec3.Dot(raCt, cs.iA * raCt) + Vec3.Dot(rbCt, cs.iB * rbCt);
c.bitangentMass = Invert(tm);
}
// Precalculate bias factor
c.bias = -Q3_BAUMGARTE * (1 / dt) * Math.Min(0, c.penetration + Q3_PENETRATION_SLOP);
// Warm start contact
Vec3 P = cs.normal * c.normalImpulse;
if (EnableFriction)
{
P += cs.tangentVectors * c.tangentImpulse;
P += cs.bitangentVectors * c.bitangentImpulse;
}
vA -= P * cs.mA;
wA -= cs.iA * Vec3.Cross(c.ra, P);
vB += P * cs.mB;
wB += cs.iB * Vec3.Cross(c.rb, P);
// Add in restitution bias
double dv = Vec3.Dot(vB + Vec3.Cross(wB, c.rb) - vA - Vec3.Cross(wA, c.ra), cs.normal);
if (dv < -1)
{
c.bias += -(cs.restitution) * dv;
}
}
Velocities[cs.indexA] = new VelocityState {
v = vA, w = wA
};
Velocities[cs.indexB] = new VelocityState {
v = vB, w = wB
};
}
}