public void Reset(SolverData data, int contactCount, Contact[] contacts)
{
_linearVelocities = data.LinearVelocities;
_angularVelocities = data.AngularVelocities;
_positions = data.Positions;
_positions = data.Positions;
_angles = data.Angles;
_contactCount = contactCount;
_contacts = contacts;
// If we need more constraints then grow the cached arrays
if (_velocityConstraints.Length < contactCount)
{
var oldLength = _velocityConstraints.Length;
Array.Resize(ref _velocityConstraints, contactCount * 2);
Array.Resize(ref _positionConstraints, contactCount * 2);
for (var i = oldLength; i < _velocityConstraints.Length; i++)
{
_velocityConstraints[i] = new ContactVelocityConstraint();
_positionConstraints[i] = new ContactPositionConstraint();
}
}
// Build constraints
// For now these are going to be bare but will change
for (var i = 0; i < _contactCount; i++)
{
var contact = contacts[i];
Fixture fixtureA = contact.FixtureA !;
Fixture fixtureB = contact.FixtureB !;
var shapeA = fixtureA.Shape;
var shapeB = fixtureB.Shape;
float radiusA = shapeA.Radius;
float radiusB = shapeB.Radius;
var bodyA = fixtureA.Body;
var bodyB = fixtureB.Body;
var manifold = contact.Manifold;
int pointCount = manifold.PointCount;
DebugTools.Assert(pointCount > 0);
var velocityConstraint = _velocityConstraints[i];
velocityConstraint.Friction = contact.Friction;
velocityConstraint.Restitution = contact.Restitution;
velocityConstraint.TangentSpeed = contact.TangentSpeed;
velocityConstraint.IndexA = bodyA.IslandIndex;
velocityConstraint.IndexB = bodyB.IslandIndex;
(velocityConstraint.InvMassA, velocityConstraint.InvMassB) = GetInvMass(bodyA, bodyB);
velocityConstraint.InvIA = bodyA.InvI;
velocityConstraint.InvIB = bodyB.InvI;
velocityConstraint.ContactIndex = i;
velocityConstraint.PointCount = pointCount;
for (var x = 0; x < 2; x++)
{
velocityConstraint.K[x] = Vector2.Zero;
velocityConstraint.NormalMass[x] = Vector2.Zero;
}
var positionConstraint = _positionConstraints[i];
positionConstraint.IndexA = bodyA.IslandIndex;
positionConstraint.IndexB = bodyB.IslandIndex;
(positionConstraint.InvMassA, positionConstraint.InvMassB) = GetInvMass(bodyA, bodyB);
// TODO: Dis
// positionConstraint.LocalCenterA = bodyA._sweep.LocalCenter;
// positionConstraint.LocalCenterB = bodyB._sweep.LocalCenter;
positionConstraint.LocalCenterA = bodyA.LocalCenter;
positionConstraint.LocalCenterB = bodyB.LocalCenter;
positionConstraint.InvIA = bodyA.InvI;
positionConstraint.InvIB = bodyB.InvI;
positionConstraint.LocalNormal = manifold.LocalNormal;
positionConstraint.LocalPoint = manifold.LocalPoint;
positionConstraint.PointCount = pointCount;
positionConstraint.RadiusA = radiusA;
positionConstraint.RadiusB = radiusB;
positionConstraint.Type = manifold.Type;
for (int j = 0; j < pointCount; ++j)
{
var contactPoint = manifold.Points[j];
var constraintPoint = velocityConstraint.Points[j];
if (_warmStarting)
{
constraintPoint.NormalImpulse = data.DtRatio * contactPoint.NormalImpulse;
constraintPoint.TangentImpulse = data.DtRatio * contactPoint.TangentImpulse;
}
else
{
constraintPoint.NormalImpulse = 0.0f;
constraintPoint.TangentImpulse = 0.0f;
}
constraintPoint.RelativeVelocityA = Vector2.Zero;
constraintPoint.RelativeVelocityB = Vector2.Zero;
constraintPoint.NormalMass = 0.0f;
constraintPoint.TangentMass = 0.0f;
constraintPoint.VelocityBias = 0.0f;
positionConstraint.LocalPoints[j] = contactPoint.LocalPoint;
}
}
}
/// <summary>
/// Tries to solve positions for all contacts specified.
/// </summary>
/// <returns>true if all positions solved</returns>
public bool SolvePositionConstraints()
{
float minSeparation = 0.0f;
for (int i = 0; i < _contactCount; ++i)
{
ContactPositionConstraint pc = _positionConstraints[i];
int indexA = pc.IndexA;
int indexB = pc.IndexB;
Vector2 localCenterA = pc.LocalCenterA;
float mA = pc.InvMassA;
float iA = pc.InvIA;
Vector2 localCenterB = pc.LocalCenterB;
float mB = pc.InvMassB;
float iB = pc.InvIB;
int pointCount = pc.PointCount;
Vector2 centerA = _positions[indexA];
float angleA = _angles[indexA];
Vector2 centerB = _positions[indexB];
float angleB = _angles[indexB];
// Solve normal constraints
for (int j = 0; j < pointCount; ++j)
{
Transform xfA = new Transform(angleA);
Transform xfB = new Transform(angleB);
xfA.Position = centerA - Transform.Mul(xfA.Quaternion2D, localCenterA);
xfB.Position = centerB - Transform.Mul(xfB.Quaternion2D, localCenterB);
Vector2 normal;
Vector2 point;
float separation;
PositionSolverManifoldInitialize(pc, j, xfA, xfB, out normal, out point, out separation);
Vector2 rA = point - centerA;
Vector2 rB = point - centerB;
// Track max constraint error.
minSeparation = Math.Min(minSeparation, separation);
// Prevent large corrections and allow slop.
float C = Math.Clamp(_baumgarte * (separation + _linearSlop), -_maxLinearCorrection, 0.0f);
// Compute the effective mass.
float rnA = Vector2.Cross(rA, normal);
float rnB = Vector2.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;
Vector2 P = normal * impulse;
centerA -= P * mA;
angleA -= iA * Vector2.Cross(rA, P);
centerB += P * mB;
angleB += iB * Vector2.Cross(rB, P);
}
_positions[indexA] = centerA;
_angles[indexA] = angleA;
_positions[indexB] = centerB;
_angles[indexB] = angleB;
}
// We can't expect minSpeparation >= -b2_linearSlop because we don't
// push the separation above -b2_linearSlop.
return(minSeparation >= -3.0f * _linearSlop);
}