public void Reset(Contact[] contacts, int contactCount, float impulseRatio)
{
_contacts = contacts;
_constraintCount = contactCount;
// grow the array
if (Constraints == null || Constraints.Length < _constraintCount)
{
Constraints = new ContactConstraint[_constraintCount * 2];
for (int i = 0; i < _constraintCount * 2; i++)
{
Constraints[i] = new ContactConstraint();
}
}
for (int i = 0; i < _constraintCount; ++i)
{
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.GetManifold(out manifold);
float friction = Settings.MixFriction(fixtureA.Friction, fixtureB.Friction);
float restitution = Settings.MixRestitution(fixtureA.Restitution, fixtureB.Restitution);
Vector2 vA = bodyA.LinearVelocityInternal;
Vector2 vB = bodyB.LinearVelocityInternal;
float wA = bodyA.AngularVelocityInternal;
float wB = bodyB.AngularVelocityInternal;
Debug.Assert(manifold.PointCount > 0);
WorldManifold worldManifold = new WorldManifold(ref manifold, ref bodyA.Xf, radiusA, ref bodyB.Xf,
radiusB);
ContactConstraint cc = Constraints[i];
cc.BodyA = bodyA;
cc.BodyB = bodyB;
cc.Manifold = manifold;
cc.Normal = worldManifold.Normal;
cc.PointCount = manifold.PointCount;
cc.Friction = friction;
cc.LocalNormal = manifold.LocalNormal;
cc.LocalPoint = manifold.LocalPoint;
cc.Radius = radiusA + radiusB;
cc.Type = manifold.Type;
for (int j = 0; j < cc.PointCount; ++j)
{
ManifoldPoint cp = manifold.Points[j];
ContactConstraintPoint ccp = cc.Points[j];
ccp.NormalImpulse = impulseRatio * cp.NormalImpulse;
ccp.TangentImpulse = impulseRatio * cp.TangentImpulse;
ccp.LocalPoint = cp.LocalPoint;
ccp.rA = worldManifold.Points[j] - bodyA.Sweep.c;
ccp.rB = worldManifold.Points[j] - bodyB.Sweep.c;
#if MATH_OVERLOADS
float rnA = MathUtils.Cross(ccp.rA, cc.Normal);
float rnB = MathUtils.Cross(ccp.rB, cc.Normal);
#else
float rnA = ccp.rA.X * cc.Normal.Y - ccp.rA.Y * cc.Normal.X;
float rnB = ccp.rB.X * cc.Normal.Y - ccp.rB.Y * cc.Normal.X;
#endif
rnA *= rnA;
rnB *= rnB;
float kNormal = bodyA.InvMass + bodyB.InvMass + bodyA.InvI * rnA + bodyB.InvI * rnB;
Debug.Assert(kNormal > Settings.Epsilon);
ccp.NormalMass = 1.0f / kNormal;
#if MATH_OVERLOADS
Vector2 tangent = MathUtils.Cross(cc.Normal, 1.0f);
float rtA = MathUtils.Cross(ccp.rA, tangent);
float rtB = MathUtils.Cross(ccp.rB, tangent);
#else
Vector2 tangent = new Vector2(cc.Normal.Y, -cc.Normal.X);
float rtA = ccp.rA.X * tangent.Y - ccp.rA.Y * tangent.X;
float rtB = ccp.rB.X * tangent.Y - ccp.rB.Y * tangent.X;
#endif
rtA *= rtA;
rtB *= rtB;
float kTangent = bodyA.InvMass + bodyB.InvMass + bodyA.InvI * rtA + bodyB.InvI * rtB;
Debug.Assert(kTangent > Settings.Epsilon);
ccp.TangentMass = 1.0f / kTangent;
// Setup a velocity bias for restitution.
ccp.VelocityBias = 0.0f;
float vRel = Vector2.Dot(cc.Normal,
vB + MathUtils.Cross(wB, ccp.rB) - vA - MathUtils.Cross(wA, ccp.rA));
if (vRel < -Settings.VelocityThreshold)
{
ccp.VelocityBias = -restitution * vRel;
}
}
// If we have two points, then prepare the block solver.
if (cc.PointCount == 2)
{
ContactConstraintPoint ccp1 = cc.Points[0];
ContactConstraintPoint ccp2 = cc.Points[1];
float invMassA = bodyA.InvMass;
float invIA = bodyA.InvI;
float invMassB = bodyB.InvMass;
float invIB = bodyB.InvI;
float rn1A = MathUtils.Cross(ccp1.rA, cc.Normal);
float rn1B = MathUtils.Cross(ccp1.rB, cc.Normal);
float rn2A = MathUtils.Cross(ccp2.rA, cc.Normal);
float rn2B = MathUtils.Cross(ccp2.rB, cc.Normal);
float k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B;
float k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B;
float k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B;
// Ensure a reasonable condition number.
const float k_maxConditionNumber = 100.0f;
if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
{
// K is safe to invert.
cc.K = new Mat22(new Vector2(k11, k12), new Vector2(k12, k22));
cc.NormalMass = cc.K.Inverse;
}
else
{
// The constraints are redundant, just use one.
// TODO_ERIN use deepest?
cc.PointCount = 1;
}
}
if (fixtureA.PostSolve != null)
fixtureA.PostSolve(cc);
if (fixtureB.PostSolve != null)
fixtureB.PostSolve(cc);
}
}
/// <summary>
/// Gets the world manifold.
/// </summary>
/// <param name="worldManifold">The world manifold.</param>
public void GetWorldManifold(out WorldManifold worldManifold)
{
Body bodyA = FixtureA.Body;
Body bodyB = FixtureB.Body;
Shape shapeA = FixtureA.Shape;
Shape shapeB = FixtureB.Shape;
Transform xfA, xfB;
bodyA.GetTransform(out xfA);
bodyB.GetTransform(out xfB);
worldManifold = new WorldManifold(ref Manifold, ref xfA, shapeA.Radius, ref xfB, shapeB.Radius);
}
protected bool isGround(WorldManifold manifold, Fixture maybeGround)
{
if (maybeGround.UserData is PhysicsObject) //if its a physics object, its not the ground
return false;
if (manifold.Normal.Y > GROUND_NORMAL_Y_LIMIT) //which is -0.2
return false;
return manifold.Points[0].Y < (getFeetPosition().Y);// ||
//manifold.Points[1].Y < (PhysicsBody.Body.Position.Y + PhysicsBody.Shape.Radius);
}
