public void StoreImpulses()
{
for (int i = 0; i < this._count; i++)
{
ContactVelocityConstraint contactVelocityConstraint = this._velocityConstraints[i];
Manifold manifold = this._contacts[contactVelocityConstraint.contactIndex].Manifold;
for (int j = 0; j < contactVelocityConstraint.pointCount; j++)
{
ManifoldPoint value = manifold.Points[j];
value.NormalImpulse = contactVelocityConstraint.points[j].normalImpulse;
value.TangentImpulse = contactVelocityConstraint.points[j].tangentImpulse;
manifold.Points[j] = value;
}
this._contacts[contactVelocityConstraint.contactIndex].Manifold = manifold;
}
}
public void StoreImpulses()
{
for (int i = 0; i < _count; ++i)
{
ContactVelocityConstraint vc = _velocityConstraints[i];
Manifold manifold = _contacts[vc.contactIndex].Manifold;
for (int j = 0; j < vc.pointCount; ++j)
{
ManifoldPoint point = manifold.Points[j];
point.NormalImpulse = vc.points[j].normalImpulse;
point.TangentImpulse = vc.points[j].tangentImpulse;
manifold.Points[j] = point;
}
_contacts[vc.contactIndex].Manifold = manifold;
}
}
public void Reset(TimeStep step, int count, Contact[] contacts, Position[] positions, Velocity[] velocities, bool warmstarting = true)
{
this._step = step;
this._count = count;
this._positions = positions;
this._velocities = velocities;
this._contacts = contacts;
bool flag = this._velocityConstraints == null || this._velocityConstraints.Length < count;
if (flag)
{
this._velocityConstraints = new ContactVelocityConstraint[count * 2];
this._positionConstraints = new ContactPositionConstraint[count * 2];
for (int i = 0; i < this._velocityConstraints.Length; i++)
{
this._velocityConstraints[i] = new ContactVelocityConstraint();
}
for (int j = 0; j < this._positionConstraints.Length; j++)
{
this._positionConstraints[j] = new ContactPositionConstraint();
}
}
for (int k = 0; k < this._count; k++)
{
Contact contact = contacts[k];
Fixture fixtureA = contact.FixtureA;
Fixture fixtureB = contact.FixtureB;
Shape shape = fixtureA.Shape;
Shape shape2 = fixtureB.Shape;
FP radius = shape.Radius;
FP radius2 = shape2.Radius;
Body body = fixtureA.Body;
Body body2 = fixtureB.Body;
Manifold manifold = contact.Manifold;
int pointCount = manifold.PointCount;
Debug.Assert(pointCount > 0);
ContactVelocityConstraint contactVelocityConstraint = this._velocityConstraints[k];
contactVelocityConstraint.friction = contact.Friction;
contactVelocityConstraint.restitution = contact.Restitution;
contactVelocityConstraint.tangentSpeed = contact.TangentSpeed;
contactVelocityConstraint.indexA = body.IslandIndex;
contactVelocityConstraint.indexB = body2.IslandIndex;
contactVelocityConstraint.invMassA = body._invMass;
contactVelocityConstraint.invMassB = body2._invMass;
contactVelocityConstraint.invIA = body._invI;
contactVelocityConstraint.invIB = body2._invI;
contactVelocityConstraint.contactIndex = k;
contactVelocityConstraint.pointCount = pointCount;
contactVelocityConstraint.K.SetZero();
contactVelocityConstraint.normalMass.SetZero();
ContactPositionConstraint contactPositionConstraint = this._positionConstraints[k];
contactPositionConstraint.indexA = body.IslandIndex;
contactPositionConstraint.indexB = body2.IslandIndex;
contactPositionConstraint.invMassA = body._invMass;
contactPositionConstraint.invMassB = body2._invMass;
contactPositionConstraint.localCenterA = body._sweep.LocalCenter;
contactPositionConstraint.localCenterB = body2._sweep.LocalCenter;
contactPositionConstraint.invIA = body._invI;
contactPositionConstraint.invIB = body2._invI;
contactPositionConstraint.localNormal = manifold.LocalNormal;
contactPositionConstraint.localPoint = manifold.LocalPoint;
contactPositionConstraint.pointCount = pointCount;
contactPositionConstraint.radiusA = radius;
contactPositionConstraint.radiusB = radius2;
contactPositionConstraint.type = manifold.Type;
for (int l = 0; l < pointCount; l++)
{
ManifoldPoint manifoldPoint = manifold.Points[l];
VelocityConstraintPoint velocityConstraintPoint = contactVelocityConstraint.points[l];
velocityConstraintPoint.normalImpulse = this._step.dtRatio * manifoldPoint.NormalImpulse;
velocityConstraintPoint.tangentImpulse = this._step.dtRatio * manifoldPoint.TangentImpulse;
velocityConstraintPoint.rA = TSVector2.zero;
velocityConstraintPoint.rB = TSVector2.zero;
velocityConstraintPoint.normalMass = 0f;
velocityConstraintPoint.tangentMass = 0f;
velocityConstraintPoint.velocityBias = 0f;
contactPositionConstraint.localPoints[l] = manifoldPoint.LocalPoint;
}
}
}
/// <summary>
/// Update the contact manifold and touching status.
/// Note: do not assume the fixture AABBs are overlapping or are valid.
/// </summary>
/// <param name="contactManager">The contact manager.</param>
internal void Update(ContactManager contactManager)
{
Body bodyA = FixtureA.Body;
Body bodyB = FixtureB.Body;
if (FixtureA == null || FixtureB == null)
{
return;
}
if (!ContactManager.CheckCollisionConditions(FixtureA, FixtureB))
{
Enabled = false;
return;
}
Manifold oldManifold = Manifold;
// Re-enable this contact.
Enabled = true;
bool touching;
bool wasTouching = IsTouching;
bool sensor = FixtureA.IsSensor || FixtureB.IsSensor;
// Is this contact a sensor?
if (sensor)
{
Shape shapeA = FixtureA.Shape;
Shape shapeB = FixtureB.Shape;
touching = Collision.TestOverlap(shapeA, ChildIndexA, shapeB, ChildIndexB, ref bodyA._xf, ref bodyB._xf);
// Sensors don't generate manifolds.
Manifold.PointCount = 0;
}
else
{
Evaluate(ref Manifold, ref bodyA._xf, ref bodyB._xf);
touching = Manifold.PointCount > 0;
// Match old contact ids to new contact ids and copy the
// stored impulses to warm start the solver.
for (int i = 0; i < Manifold.PointCount; ++i)
{
ManifoldPoint mp2 = Manifold.Points[i];
mp2.NormalImpulse = 0.0f;
mp2.TangentImpulse = 0.0f;
ContactID id2 = mp2.Id;
for (int j = 0; j < oldManifold.PointCount; ++j)
{
ManifoldPoint mp1 = oldManifold.Points[j];
if (mp1.Id.Key == id2.Key)
{
mp2.NormalImpulse = mp1.NormalImpulse;
mp2.TangentImpulse = mp1.TangentImpulse;
break;
}
}
Manifold.Points[i] = mp2;
}
if (touching != wasTouching)
{
bodyA.Awake = true;
bodyB.Awake = true;
}
}
IsTouching = touching;
if (wasTouching == false)
{
if (touching)
{
if (Settings.AllCollisionCallbacksAgree)
{
bool enabledA = true, enabledB = true;
// Report the collision to both participants. Track which ones returned true so we can
// later call OnSeparation if the contact is disabled for a different reason.
if (FixtureA.OnCollision != null)
{
foreach (OnCollisionEventHandler handler in FixtureA.OnCollision.GetInvocationList())
{
enabledA = handler(FixtureA, FixtureB, this) && enabledA;
}
}
// Reverse the order of the reported fixtures. The first fixture is always the one that the
// user subscribed to.
if (FixtureB.OnCollision != null)
{
foreach (OnCollisionEventHandler handler in FixtureB.OnCollision.GetInvocationList())
{
enabledB = handler(FixtureB, FixtureA, this) && enabledB;
}
}
Enabled = enabledA && enabledB;
// BeginContact can also return false and disable the contact
if (enabledA && enabledB && contactManager.BeginContact != null)
{
Enabled = contactManager.BeginContact(this);
}
}
else
{
//Report the collision to both participants:
if (FixtureA.OnCollision != null)
{
foreach (OnCollisionEventHandler handler in FixtureA.OnCollision.GetInvocationList())
{
Enabled = handler(FixtureA, FixtureB, this);
}
}
//Reverse the order of the reported fixtures. The first fixture is always the one that the
//user subscribed to.
if (FixtureB.OnCollision != null)
{
foreach (OnCollisionEventHandler handler in FixtureB.OnCollision.GetInvocationList())
{
Enabled = handler(FixtureB, FixtureA, this);
}
}
//BeginContact can also return false and disable the contact
if (contactManager.BeginContact != null)
{
Enabled = contactManager.BeginContact(this);
}
}
// If the user disabled the contact (needed to exclude it in TOI solver) at any point by
// any of the callbacks, we need to mark it as not touching and call any separation
// callbacks for fixtures that didn't explicitly disable the collision.
if (!Enabled)
{
IsTouching = false;
}
}
}
else
{
if (touching == false)
{
//Report the separation to both participants:
if (FixtureA != null && FixtureA.OnSeparation != null)
{
FixtureA.OnSeparation(FixtureA, FixtureB);
}
//Reverse the order of the reported fixtures. The first fixture is always the one that the
//user subscribed to.
if (FixtureB != null && FixtureB.OnSeparation != null)
{
FixtureB.OnSeparation(FixtureB, FixtureA);
}
if (contactManager.EndContact != null)
{
contactManager.EndContact(this);
}
}
else
{
if (contactManager.StayContact != null)
{
contactManager.StayContact(this);
}
}
}
if (sensor)
{
return;
}
if (contactManager.PreSolve != null)
{
contactManager.PreSolve(this, ref oldManifold);
}
}
public void Reset(TimeStep step, int count, Contact[] contacts, Position[] positions, Velocity[] velocities, bool warmstarting = Settings.EnableWarmstarting)
{
_step = step;
_count = count;
_positions = positions;
_velocities = velocities;
_contacts = contacts;
// grow the array
if (_velocityConstraints == null || _velocityConstraints.Length < count)
{
_velocityConstraints = new ContactVelocityConstraint[count * 2];
_positionConstraints = new ContactPositionConstraint[count * 2];
for (int i = 0; i < _velocityConstraints.Length; i++)
{
_velocityConstraints[i] = new ContactVelocityConstraint();
}
for (int i = 0; i < _positionConstraints.Length; i++)
{
_positionConstraints[i] = new ContactPositionConstraint();
}
}
// Initialize position independent portions of the constraints.
for (int i = 0; i < _count; ++i)
{
Contact contact = contacts[i];
Fixture fixtureA = contact.FixtureA;
Fixture fixtureB = contact.FixtureB;
Shape shapeA = fixtureA.Shape;
Shape shapeB = fixtureB.Shape;
FP radiusA = shapeA.Radius;
FP 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 = bodyA._sweep.LocalCenter;
pc.localCenterB = bodyB._sweep.LocalCenter;
pc.invIA = bodyA._invI;
pc.invIB = bodyB._invI;
pc.localNormal = manifold.LocalNormal;
pc.localPoint = manifold.LocalPoint;
pc.pointCount = pointCount;
pc.radiusA = radiusA;
pc.radiusB = radiusB;
pc.type = manifold.Type;
for (int j = 0; j < pointCount; ++j)
{
ManifoldPoint cp = manifold.Points[j];
VelocityConstraintPoint vcp = vc.points[j];
if (Settings.EnableWarmstarting)
{
vcp.normalImpulse = _step.dtRatio * cp.NormalImpulse;
vcp.tangentImpulse = _step.dtRatio * cp.TangentImpulse;
}
else
{
vcp.normalImpulse = 0.0f;
vcp.tangentImpulse = 0.0f;
}
vcp.rA = TSVector2.zero;
vcp.rB = TSVector2.zero;
vcp.normalMass = 0.0f;
vcp.tangentMass = 0.0f;
vcp.velocityBias = 0.0f;
pc.localPoints[j] = cp.LocalPoint;
}
}
}
internal void Update(ContactManager contactManager)
{
Body body = this.FixtureA.Body;
Body body2 = this.FixtureB.Body;
bool flag = this.FixtureA == null || this.FixtureB == null;
if (!flag)
{
bool flag2 = !ContactManager.CheckCollisionConditions(this.FixtureA, this.FixtureB);
if (flag2)
{
this.Enabled = false;
}
else
{
Manifold manifold = this.Manifold;
this.Enabled = true;
bool isTouching = this.IsTouching;
bool flag3 = this.FixtureA.IsSensor || this.FixtureB.IsSensor;
bool flag4 = flag3;
bool flag5;
if (flag4)
{
Shape shape = this.FixtureA.Shape;
Shape shape2 = this.FixtureB.Shape;
flag5 = Collision.TestOverlap(shape, this.ChildIndexA, shape2, this.ChildIndexB, ref body._xf, ref body2._xf);
this.Manifold.PointCount = 0;
}
else
{
this.Evaluate(ref this.Manifold, ref body._xf, ref body2._xf);
flag5 = (this.Manifold.PointCount > 0);
for (int i = 0; i < this.Manifold.PointCount; i++)
{
ManifoldPoint manifoldPoint = this.Manifold.Points[i];
manifoldPoint.NormalImpulse = 0f;
manifoldPoint.TangentImpulse = 0f;
ContactID id = manifoldPoint.Id;
for (int j = 0; j < manifold.PointCount; j++)
{
ManifoldPoint manifoldPoint2 = manifold.Points[j];
bool flag6 = manifoldPoint2.Id.Key == id.Key;
if (flag6)
{
manifoldPoint.NormalImpulse = manifoldPoint2.NormalImpulse;
manifoldPoint.TangentImpulse = manifoldPoint2.TangentImpulse;
break;
}
}
this.Manifold.Points[i] = manifoldPoint;
}
bool flag7 = flag5 != isTouching;
if (flag7)
{
body.Awake = true;
body2.Awake = true;
}
}
this.IsTouching = flag5;
bool flag8 = !isTouching;
if (flag8)
{
bool flag9 = flag5;
if (flag9)
{
bool flag10 = true;
bool flag11 = true;
bool flag12 = this.FixtureA.OnCollision != null;
if (flag12)
{
Delegate[] invocationList = this.FixtureA.OnCollision.GetInvocationList();
for (int k = 0; k < invocationList.Length; k++)
{
OnCollisionEventHandler onCollisionEventHandler = (OnCollisionEventHandler)invocationList[k];
flag10 = (onCollisionEventHandler(this.FixtureA, this.FixtureB, this) & flag10);
}
}
bool flag13 = this.FixtureB.OnCollision != null;
if (flag13)
{
Delegate[] invocationList2 = this.FixtureB.OnCollision.GetInvocationList();
for (int l = 0; l < invocationList2.Length; l++)
{
OnCollisionEventHandler onCollisionEventHandler2 = (OnCollisionEventHandler)invocationList2[l];
flag11 = (onCollisionEventHandler2(this.FixtureB, this.FixtureA, this) & flag11);
}
}
this.Enabled = (flag10 & flag11);
bool flag14 = (flag10 & flag11) && contactManager.BeginContact != null;
if (flag14)
{
this.Enabled = contactManager.BeginContact(this);
}
bool flag15 = !this.Enabled;
if (flag15)
{
this.IsTouching = false;
}
}
}
else
{
bool flag16 = !flag5;
if (flag16)
{
bool flag17 = this.FixtureA != null && this.FixtureA.OnSeparation != null;
if (flag17)
{
this.FixtureA.OnSeparation(this.FixtureA, this.FixtureB);
}
bool flag18 = this.FixtureB != null && this.FixtureB.OnSeparation != null;
if (flag18)
{
this.FixtureB.OnSeparation(this.FixtureB, this.FixtureA);
}
bool flag19 = contactManager.EndContact != null;
if (flag19)
{
contactManager.EndContact(this);
}
}
else
{
bool flag20 = contactManager.StayContact != null;
if (flag20)
{
contactManager.StayContact(this);
}
}
}
bool flag21 = flag3;
if (!flag21)
{
bool flag22 = contactManager.PreSolve != null;
if (flag22)
{
contactManager.PreSolve(this, ref manifold);
}
}
}
}
}
