// Sequential position solver for position constraints.
public bool SolveTOIPositionConstraints(int toiIndexA, int toiIndexB)
{
float minSeparation = 0.0f;
for (int i = 0; i < Count; ++i)
{
ContactPositionConstraint pc = PositionConstraints[i];
int indexA = pc.IndexA;
int indexB = pc.IndexB;
Vec2 localCenterA = pc.LocalCenterA;
Vec2 localCenterB = pc.LocalCenterB;
int pointCount = pc.PointCount;
float mA = 0.0f;
float iA = 0.0f;
if (indexA == toiIndexA || indexA == toiIndexB)
{
mA = pc.InvMassA;
iA = pc.InvIA;
}
float mB = pc.InvMassB;
float iB = pc.InvIB;
if (indexB == toiIndexA || indexB == toiIndexB)
{
mB = pc.InvMassB;
iB = pc.InvIB;
}
Vec2 cA = Positions[indexA].C;
float aA = Positions[indexA].A;
Vec2 cB = Positions[indexB].C;
float aB = 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;
float 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.
float C = MathUtils.Clamp(Settings.TOI_BAUGARTE * (separation + Settings.LINEAR_SLOP), -Settings.MAX_LINEAR_CORRECTION, 0.0f);
// Compute the effective mass.
float rnA = Vec2.Cross(rA, normal);
float rnB = Vec2.Cross(rB, normal);
float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
// Compute normal impulse
float impulse = K > 0.0f ? (-C) / K : 0.0f;
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);
}
Positions[indexA].C.Set(cA);
Positions[indexA].A = aA;
Positions[indexB].C.Set(cB);
Positions[indexB].A = aB;
}
// We can't expect minSpeparation >= -_linearSlop because we don't
// push the separation above -_linearSlop.
return(minSeparation >= (-1.5f) * Settings.LINEAR_SLOP);
}
public void InitializeVelocityConstraints()
{
//Console.WriteLine("Initializing velocity constraints for " + m_count + " contacts");
// Warm start.
for (int i = 0; i < Count; ++i)
{
ContactVelocityConstraint vc = VelocityConstraints[i];
ContactPositionConstraint pc = PositionConstraints[i];
float radiusA = pc.RadiusA;
float radiusB = pc.RadiusB;
Manifold manifold = Contacts[vc.ContactIndex].Manifold;
int indexA = vc.IndexA;
int indexB = vc.IndexB;
float mA = vc.InvMassA;
float mB = vc.InvMassB;
float iA = vc.InvIA;
float iB = vc.InvIB;
Vec2 localCenterA = pc.LocalCenterA;
Vec2 localCenterB = pc.LocalCenterB;
Vec2 cA = Positions[indexA].C;
float aA = Positions[indexA].A;
Vec2 vA = Velocities[indexA].V;
float wA = Velocities[indexA].W;
Vec2 cB = Positions[indexB].C;
float aB = Positions[indexB].A;
Vec2 vB = Velocities[indexB].V;
float wB = Velocities[indexB].W;
Debug.Assert(manifold.PointCount > 0);
xfA.Q.Set(aA);
xfB.Q.Set(aB);
Rot.MulToOutUnsafe(xfA.Q, localCenterA, temp);
xfA.P.Set(cA).SubLocal(temp);
Rot.MulToOutUnsafe(xfB.Q, localCenterB, temp);
xfB.P.Set(cB).SubLocal(temp);
worldManifold.Initialize(manifold, xfA, radiusA, xfB, radiusB);
vc.Normal.Set(worldManifold.Normal);
int pointCount = vc.PointCount;
for (int j = 0; j < pointCount; ++j)
{
ContactVelocityConstraint.VelocityConstraintPoint vcp = vc.Points[j];
vcp.RA.Set(worldManifold.Points[j]).SubLocal(cA);
vcp.RB.Set(worldManifold.Points[j]).SubLocal(cB);
float rnA = Vec2.Cross(vcp.RA, vc.Normal);
float rnB = Vec2.Cross(vcp.RB, vc.Normal);
float kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
vcp.NormalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f;
Vec2.CrossToOutUnsafe(vc.Normal, 1.0f, tangent);
float rtA = Vec2.Cross(vcp.RA, tangent);
float rtB = Vec2.Cross(vcp.RB, tangent);
float kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB;
vcp.TangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f;
// Setup a velocity bias for restitution.
vcp.VelocityBias = 0.0f;
Vec2.CrossToOutUnsafe(wB, vcp.RB, temp1);
Vec2.CrossToOutUnsafe(wA, vcp.RA, temp2);
temp.Set(vB).AddLocal(temp1).SubLocal(vA).SubLocal(temp2);
float vRel = Vec2.Dot(vc.Normal, temp);
if (vRel < -Settings.VELOCITY_THRESHOLD)
{
vcp.VelocityBias = (-vc.Restitution) * vRel;
}
}
// If we have two points, then prepare the block solver.
if (vc.PointCount == 2)
{
ContactVelocityConstraint.VelocityConstraintPoint vcp1 = vc.Points[0];
ContactVelocityConstraint.VelocityConstraintPoint vcp2 = vc.Points[1];
float rn1A = Vec2.Cross(vcp1.RA, vc.Normal);
float rn1B = Vec2.Cross(vcp1.RB, vc.Normal);
float rn2A = Vec2.Cross(vcp2.RA, vc.Normal);
float rn2B = Vec2.Cross(vcp2.RB, vc.Normal);
float k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B;
float k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B;
float k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B;
if (k11 * k11 < MAX_CONDITION_NUMBER * (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;
}
}
}
}
public void Init(ContactSolverDef def)
{
//Console.WriteLine("Initializing contact solver");
Step = def.Step;
Count = def.Count;
if (PositionConstraints.Length < Count)
{
ContactPositionConstraint[] old = PositionConstraints;
PositionConstraints = new ContactPositionConstraint[MathUtils.Max(old.Length * 2, Count)];
Array.Copy(old, 0, PositionConstraints, 0, old.Length);
for (int i = old.Length; i < PositionConstraints.Length; i++)
{
PositionConstraints[i] = new ContactPositionConstraint();
}
}
if (VelocityConstraints.Length < Count)
{
ContactVelocityConstraint[] old = VelocityConstraints;
VelocityConstraints = new ContactVelocityConstraint[MathUtils.Max(old.Length * 2, Count)];
Array.Copy(old, 0, VelocityConstraints, 0, old.Length);
for (int i = old.Length; i < VelocityConstraints.Length; i++)
{
VelocityConstraints[i] = new ContactVelocityConstraint();
}
}
Positions = def.Positions;
Velocities = def.Velocities;
Contacts = def.Contacts;
for (int i = 0; i < Count; ++i)
{
//Console.WriteLine("contacts: " + m_count);
Contact contact = Contacts[i];
Fixture fixtureA = contact.FixtureA;
Fixture fixtureB = contact.FixtureB;
Shape shapeA = fixtureA.Shape;
Shape shapeB = fixtureB.Shape;
float radiusA = shapeA.Radius;
float radiusB = shapeB.Radius;
Body bodyA = fixtureA.Body;
Body bodyB = fixtureB.Body;
Manifold manifold = contact.Manifold;
int pointCount = manifold.PointCount;
Debug.Assert(pointCount > 0);
ContactVelocityConstraint vc = VelocityConstraints[i];
vc.Friction = contact.Friction;
vc.Restitution = contact.Restitution;
vc.TangentSpeed = contact.TangentSpeed;
vc.IndexA = bodyA.IslandIndex;
vc.IndexB = bodyB.IslandIndex;
vc.InvMassA = bodyA.InvMass;
vc.InvMassB = bodyB.InvMass;
vc.InvIA = bodyA.InvI;
vc.InvIB = bodyB.InvI;
vc.ContactIndex = i;
vc.PointCount = pointCount;
vc.K.SetZero();
vc.NormalMass.SetZero();
ContactPositionConstraint pc = PositionConstraints[i];
pc.IndexA = bodyA.IslandIndex;
pc.IndexB = bodyB.IslandIndex;
pc.InvMassA = bodyA.InvMass;
pc.InvMassB = bodyB.InvMass;
pc.LocalCenterA.Set(bodyA.Sweep.LocalCenter);
pc.LocalCenterB.Set(bodyB.Sweep.LocalCenter);
pc.InvIA = bodyA.InvI;
pc.InvIB = bodyB.InvI;
pc.LocalNormal.Set(manifold.LocalNormal);
pc.LocalPoint.Set(manifold.LocalPoint);
pc.PointCount = pointCount;
pc.RadiusA = radiusA;
pc.RadiusB = radiusB;
pc.Type = manifold.Type;
//Console.WriteLine("contact point count: " + pointCount);
for (int j = 0; j < pointCount; j++)
{
ManifoldPoint cp = manifold.Points[j];
ContactVelocityConstraint.VelocityConstraintPoint vcp = vc.Points[j];
if (Step.WarmStarting)
{
//Debug.Assert(cp.normalImpulse == 0);
//Console.WriteLine("contact normal impulse: " + cp.normalImpulse);
vcp.NormalImpulse = Step.DtRatio * cp.NormalImpulse;
vcp.TangentImpulse = 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].Set(cp.LocalPoint);
}
}
}
public void Init(ContactSolverDef def)
{
//Console.WriteLine("Initializing contact solver");
Step = def.Step;
Count = def.Count;
if (PositionConstraints.Length < Count)
{
ContactPositionConstraint[] old = PositionConstraints;
PositionConstraints = new ContactPositionConstraint[MathUtils.Max(old.Length * 2, Count)];
Array.Copy(old, 0, PositionConstraints, 0, old.Length);
for (int i = old.Length; i < PositionConstraints.Length; i++)
{
PositionConstraints[i] = new ContactPositionConstraint();
}
}
if (VelocityConstraints.Length < Count)
{
ContactVelocityConstraint[] old = VelocityConstraints;
VelocityConstraints = new ContactVelocityConstraint[MathUtils.Max(old.Length * 2, Count)];
Array.Copy(old, 0, VelocityConstraints, 0, old.Length);
for (int i = old.Length; i < VelocityConstraints.Length; i++)
{
VelocityConstraints[i] = new ContactVelocityConstraint();
}
}
Positions = def.Positions;
Velocities = def.Velocities;
Contacts = def.Contacts;
for (int i = 0; i < Count; ++i)
{
//Console.WriteLine("contacts: " + m_count);
Contact contact = Contacts[i];
Fixture fixtureA = contact.FixtureA;
Fixture fixtureB = contact.FixtureB;
Shape shapeA = fixtureA.Shape;
Shape shapeB = fixtureB.Shape;
float radiusA = shapeA.Radius;
float radiusB = shapeB.Radius;
Body bodyA = fixtureA.Body;
Body bodyB = fixtureB.Body;
Manifold manifold = contact.Manifold;
int pointCount = manifold.PointCount;
Debug.Assert(pointCount > 0);
ContactVelocityConstraint vc = VelocityConstraints[i];
vc.Friction = contact.Friction;
vc.Restitution = contact.Restitution;
vc.TangentSpeed = contact.TangentSpeed;
vc.IndexA = bodyA.IslandIndex;
vc.IndexB = bodyB.IslandIndex;
vc.InvMassA = bodyA.InvMass;
vc.InvMassB = bodyB.InvMass;
vc.InvIA = bodyA.InvI;
vc.InvIB = bodyB.InvI;
vc.ContactIndex = i;
vc.PointCount = pointCount;
vc.K.SetZero();
vc.NormalMass.SetZero();
ContactPositionConstraint pc = PositionConstraints[i];
pc.IndexA = bodyA.IslandIndex;
pc.IndexB = bodyB.IslandIndex;
pc.InvMassA = bodyA.InvMass;
pc.InvMassB = bodyB.InvMass;
pc.LocalCenterA.Set(bodyA.Sweep.LocalCenter);
pc.LocalCenterB.Set(bodyB.Sweep.LocalCenter);
pc.InvIA = bodyA.InvI;
pc.InvIB = bodyB.InvI;
pc.LocalNormal.Set(manifold.LocalNormal);
pc.LocalPoint.Set(manifold.LocalPoint);
pc.PointCount = pointCount;
pc.RadiusA = radiusA;
pc.RadiusB = radiusB;
pc.Type = manifold.Type;
//Console.WriteLine("contact point count: " + pointCount);
for (int j = 0; j < pointCount; j++)
{
ManifoldPoint cp = manifold.Points[j];
ContactVelocityConstraint.VelocityConstraintPoint vcp = vc.Points[j];
if (Step.WarmStarting)
{
//Debug.Assert(cp.normalImpulse == 0);
//Console.WriteLine("contact normal impulse: " + cp.normalImpulse);
vcp.NormalImpulse = Step.DtRatio * cp.NormalImpulse;
vcp.TangentImpulse = 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].Set(cp.LocalPoint);
}
}
}
public ContactSolver()
{
PositionConstraints = new ContactPositionConstraint[INITIAL_NUM_CONSTRAINTS];
VelocityConstraints = new ContactVelocityConstraint[INITIAL_NUM_CONSTRAINTS];
for (int i = 0; i < INITIAL_NUM_CONSTRAINTS; i++)
{
PositionConstraints[i] = new ContactPositionConstraint();
VelocityConstraints[i] = new ContactVelocityConstraint();
}
}
public void Initialize(ContactPositionConstraint pc, Transform xfA, Transform xfB, int index)
{
Debug.Assert(pc.PointCount > 0);
switch (pc.Type)
{
case Manifold.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(.5f);
temp.Set(pointB).SubLocal(pointA);
Separation = Vec2.Dot(temp, Normal) - pc.RadiusA - pc.RadiusB;
break;
}
case Manifold.ManifoldType.FaceA:
{
Rot.MulToOutUnsafe(xfA.Q, 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);
break;
}
case Manifold.ManifoldType.FaceB:
{
Rot.MulToOutUnsafe(xfB.Q, pc.LocalNormal, Normal);
Transform.MulToOutUnsafe(xfB, pc.LocalPoint, planePoint);
Transform.MulToOutUnsafe(xfA, pc.LocalPoints[index], 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();
}
break;
}
}
public void init(ContactSolverDef def)
{
//Console.WriteLine("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)];
Array.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)];
Array.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)
{
//Console.WriteLine("contacts: " + m_count);
Contact contact = m_contacts[i];
Fixture fixtureA = contact.m_fixtureA;
Fixture fixtureB = contact.m_fixtureB;
Shape shapeA = fixtureA.Shape;
Shape shapeB = fixtureB.Shape;
float radiusA = shapeA.m_radius;
float radiusB = shapeB.m_radius;
Body bodyA = fixtureA.Body;
Body bodyB = fixtureB.Body;
Manifold manifold = contact.Manifold;
int pointCount = manifold.pointCount;
Debug.Assert(pointCount > 0);
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_Renamed(bodyA.m_sweep.localCenter);
pc.localCenterB.set_Renamed(bodyB.m_sweep.localCenter);
pc.invIA = bodyA.m_invI;
pc.invIB = bodyB.m_invI;
pc.localNormal.set_Renamed(manifold.localNormal);
pc.localPoint.set_Renamed(manifold.localPoint);
pc.pointCount = pointCount;
pc.radiusA = radiusA;
pc.radiusB = radiusB;
pc.type = manifold.type;
//Console.WriteLine("contact point count: " + pointCount);
for (int j = 0; j < pointCount; j++)
{
ManifoldPoint cp = manifold.points[j];
ContactVelocityConstraint.VelocityConstraintPoint vcp = vc.points[j];
if (m_step.warmStarting)
{
//Debug.Assert(cp.normalImpulse == 0);
//Console.WriteLine("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].set_Renamed(cp.localPoint);
}
}
}
public virtual void initialize(ContactPositionConstraint pc, Transform xfA, Transform xfB, int index)
{
Debug.Assert(pc.pointCount > 0);
switch (pc.type)
{
case Manifold.ManifoldType.CIRCLES:
{
Transform.mulToOutUnsafe(xfA, pc.localPoint, pointA);
Transform.mulToOutUnsafe(xfB, pc.localPoints[0], pointB);
normal.set_Renamed(pointB).subLocal(pointA);
normal.normalize();
point.set_Renamed(pointA).addLocal(pointB).mulLocal(.5f);
temp.set_Renamed(pointB).subLocal(pointA);
separation = Vec2.dot(temp, normal) - pc.radiusA - pc.radiusB;
break;
}
case Manifold.ManifoldType.FACE_A:
{
Rot.mulToOutUnsafe(xfA.q, pc.localNormal, normal);
Transform.mulToOutUnsafe(xfA, pc.localPoint, planePoint);
Transform.mulToOutUnsafe(xfB, pc.localPoints[index], clipPoint);
temp.set_Renamed(clipPoint).subLocal(planePoint);
separation = Vec2.dot(temp, normal) - pc.radiusA - pc.radiusB;
point.set_Renamed(clipPoint);
break;
}
case Manifold.ManifoldType.FACE_B:
{
Rot.mulToOutUnsafe(xfB.q, pc.localNormal, normal);
Transform.mulToOutUnsafe(xfB, pc.localPoint, planePoint);
Transform.mulToOutUnsafe(xfA, pc.localPoints[index], clipPoint);
temp.set_Renamed(clipPoint).subLocal(planePoint);
separation = Vec2.dot(temp, normal) - pc.radiusA - pc.radiusB;
point.set_Renamed(clipPoint);
// Ensure normal points from A to B
normal.negateLocal();
}
break;
}
}
