public void Report(ContactConstraint[] constraints)
{
if (this._listener != null)
{
for (int i = 0; i < this._contactCount; i++)
{
Contact contact = this._contacts[i];
ContactConstraint contactConstraint = constraints[i];
ContactResult contactResult = new ContactResult();
contactResult.Shape1 = contact.GetShape1();
contactResult.Shape2 = contact.GetShape2();
Body body = contactResult.Shape1.GetBody();
int manifoldCount = contact.GetManifoldCount();
Manifold[] manifolds = contact.GetManifolds();
for (int j = 0; j < manifoldCount; j++)
{
Manifold manifold = manifolds[j];
contactResult.Normal = manifold.Normal;
for (int k = 0; k < manifold.PointCount; k++)
{
ManifoldPoint manifoldPoint = manifold.Points[k];
ContactConstraintPoint contactConstraintPoint = contactConstraint.Points[k];
contactResult.Position = body.GetWorldPoint(manifoldPoint.LocalPoint1);
contactResult.NormalImpulse = contactConstraintPoint.NormalImpulse;
contactResult.TangentImpulse = contactConstraintPoint.TangentImpulse;
contactResult.ID = manifoldPoint.ID;
this._listener.Result(contactResult);
}
}
}
}
}
public static void Destroy(Contact contact)
{
Box2DXDebug.Assert(Contact.s_initialized);
if (contact.GetManifoldCount() > 0)
{
contact.GetShape1().GetBody().WakeUp();
contact.GetShape2().GetBody().WakeUp();
}
ShapeType type = contact.GetShape1().GetType();
ShapeType type2 = contact.GetShape2().GetType();
Box2DXDebug.Assert(ShapeType.UnknownShape < type && type < ShapeType.ShapeTypeCount);
Box2DXDebug.Assert(ShapeType.UnknownShape < type2 && type2 < ShapeType.ShapeTypeCount);
ContactDestroyFcn destroyFcn = Contact.s_registers[(int)type][(int)type2].DestroyFcn;
destroyFcn(contact);
}
public static Contact Create(Shape shape1, Shape shape2)
{
if (!Contact.s_initialized)
{
Contact.InitializeRegisters();
Contact.s_initialized = true;
}
ShapeType type = shape1.GetType();
ShapeType type2 = shape2.GetType();
Box2DXDebug.Assert(ShapeType.UnknownShape < type && type < ShapeType.ShapeTypeCount);
Box2DXDebug.Assert(ShapeType.UnknownShape < type2 && type2 < ShapeType.ShapeTypeCount);
ContactCreateFcn createFcn = Contact.s_registers[(int)type][(int)type2].CreateFcn;
Contact result;
if (createFcn != null)
{
if (Contact.s_registers[(int)type][(int)type2].Primary)
{
result = createFcn(shape1, shape2);
}
else
{
Contact contact = createFcn(shape2, shape1);
for (int i = 0; i < contact.GetManifoldCount(); i++)
{
Manifold manifold = contact.GetManifolds()[i];
manifold.Normal = -manifold.Normal;
}
result = contact;
}
}
else
{
result = null;
}
return(result);
}
public static Contact Create(Shape shape1, Shape shape2)
{
if (s_initialized == false)
{
InitializeRegisters();
s_initialized = true;
}
ShapeType type1 = shape1.GetType();
ShapeType type2 = shape2.GetType();
Box2DXDebug.Assert(ShapeType.UnknownShape < type1 && type1 < ShapeType.ShapeTypeCount);
Box2DXDebug.Assert(ShapeType.UnknownShape < type2 && type2 < ShapeType.ShapeTypeCount);
ContactCreateFcn createFcn = s_registers[(int)type1][(int)type2].CreateFcn;
if (createFcn != null)
{
if (s_registers[(int)type1][(int)type2].Primary)
{
return(createFcn(shape1, shape2));
}
else
{
Contact c = createFcn(shape2, shape1);
for (int i = 0; i < c.GetManifoldCount(); ++i)
{
Manifold m = c.GetManifolds()[i];
m.Normal = -m.Normal;
}
return(c);
}
}
else
{
return(null);
}
}
public void Destroy(Contact c)
{
Shape shape1 = c.GetShape1();
Shape shape2 = c.GetShape2();
Body body1 = shape1.GetBody();
Body body2 = shape2.GetBody();
ContactPoint cp = new ContactPoint();
cp.Shape1 = shape1;
cp.Shape2 = shape2;
cp.Friction = Settings.MixFriction(shape1.Friction, shape2.Friction);
cp.Restitution = Settings.MixRestitution(shape1.Restitution, shape2.Restitution);
// Inform the user that this contact is ending.
int manifoldCount = c.GetManifoldCount();
if (manifoldCount > 0 && _world._contactListener != null)
{
Manifold[] manifolds = c.GetManifolds();
for (int i = 0; i < manifoldCount; ++i)
{
Manifold manifold = manifolds[i];
cp.Normal = manifold.Normal;
for (int j = 0; j < manifold.PointCount; ++j)
{
ManifoldPoint mp = manifold.Points[j];
cp.Position = body1.GetWorldPoint(mp.LocalPoint1);
Vec2 v1 = body1.GetLinearVelocityFromLocalPoint(mp.LocalPoint1);
Vec2 v2 = body2.GetLinearVelocityFromLocalPoint(mp.LocalPoint2);
cp.Velocity = v2 - v1;
cp.Separation = mp.Separation;
cp.ID = mp.ID;
_world._contactListener.Remove(cp);
}
}
}
// Remove from the world.
if (c._prev != null)
{
c._prev._next = c._next;
}
if (c._next != null)
{
c._next._prev = c._prev;
}
if (c == _world._contactList)
{
_world._contactList = c._next;
}
// Remove from body 1
if (c._node1.Prev != null)
{
c._node1.Prev.Next = c._node1.Next;
}
if (c._node1.Next != null)
{
c._node1.Next.Prev = c._node1.Prev;
}
if (c._node1 == body1._contactList)
{
body1._contactList = c._node1.Next;
}
// Remove from body 2
if (c._node2.Prev != null)
{
c._node2.Prev.Next = c._node2.Next;
}
if (c._node2.Next != null)
{
c._node2.Next.Prev = c._node2.Prev;
}
if (c._node2 == body2._contactList)
{
body2._contactList = c._node2.Next;
}
// Call the factory.
Contact.Destroy(c);
--_world._contactCount;
}
public void Destroy(Contact c)
{
Shape shape1 = c.GetShape1();
Shape shape2 = c.GetShape2();
Body body1 = shape1.GetBody();
Body body2 = shape2.GetBody();
ContactPoint cp = new ContactPoint();
cp.Shape1 = shape1;
cp.Shape2 = shape2;
cp.Friction = Settings.MixFriction(shape1.Friction, shape2.Friction);
cp.Restitution = Settings.MixRestitution(shape1.Restitution, shape2.Restitution);
// Inform the user that this contact is ending.
int manifoldCount = c.GetManifoldCount();
if (manifoldCount > 0 && _world._contactListener!=null)
{
Manifold[] manifolds = c.GetManifolds();
for (int i = 0; i < manifoldCount; ++i)
{
Manifold manifold = manifolds[i];
cp.Normal = manifold.Normal;
for (int j = 0; j < manifold.PointCount; ++j)
{
ManifoldPoint mp = manifold.Points[j];
cp.Position = body1.GetWorldPoint(mp.LocalPoint1);
Vec2 v1 = body1.GetLinearVelocityFromLocalPoint(mp.LocalPoint1);
Vec2 v2 = body2.GetLinearVelocityFromLocalPoint(mp.LocalPoint2);
cp.Velocity = v2 - v1;
cp.Separation = mp.Separation;
cp.ID = mp.ID;
_world._contactListener.Remove(cp);
}
}
}
// Remove from the world.
if (c._prev != null)
{
c._prev._next = c._next;
}
if (c._next != null)
{
c._next._prev = c._prev;
}
if (c == _world._contactList)
{
_world._contactList = c._next;
}
// Remove from body 1
if (c._node1.Prev != null)
{
c._node1.Prev.Next = c._node1.Next;
}
if (c._node1.Next != null)
{
c._node1.Next.Prev = c._node1.Prev;
}
if (c._node1 == body1._contactList)
{
body1._contactList = c._node1.Next;
}
// Remove from body 2
if (c._node2.Prev != null)
{
c._node2.Prev.Next = c._node2.Next;
}
if (c._node2.Next != null)
{
c._node2.Next.Prev = c._node2.Prev;
}
if (c._node2 == body2._contactList)
{
body2._contactList = c._node2.Next;
}
// Call the factory.
Contact.Destroy(c);
--_world._contactCount;
}
public ContactSolver(TimeStep step, Contact[] contacts, int contactCount)
{
_step = step;
_constraintCount = 0;
for (int i = 0; i < contactCount; ++i)
{
Box2DXDebug.Assert(contacts[i].IsSolid());
_constraintCount += contacts[i].GetManifoldCount();
}
_constraints = new ContactConstraint[_constraintCount];
for (int i = 0; i < _constraintCount; i++)
{
_constraints[i] = new ContactConstraint();
}
int count = 0;
for (int i = 0; i < contactCount; ++i)
{
Contact contact = contacts[i];
Shape shape1 = contact._shape1;
Shape shape2 = contact._shape2;
Body b1 = shape1.GetBody();
Body b2 = shape2.GetBody();
int manifoldCount = contact.GetManifoldCount();
Manifold[] manifolds = contact.GetManifolds();
float friction = Settings.MixFriction(shape1.Friction, shape2.Friction);
float restitution = Settings.MixRestitution(shape1.Restitution, shape2.Restitution);
Vec2 v1 = b1._linearVelocity;
Vec2 v2 = b2._linearVelocity;
float w1 = b1._angularVelocity;
float w2 = b2._angularVelocity;
for (int j = 0; j < manifoldCount; ++j)
{
Manifold manifold = manifolds[j];
Box2DXDebug.Assert(manifold.PointCount > 0);
Vec2 normal = manifold.Normal;
Box2DXDebug.Assert(count < _constraintCount);
ContactConstraint cc = _constraints[count];
cc.Body1 = b1;
cc.Body2 = b2;
cc.Manifold = manifold;
cc.Normal = normal;
cc.PointCount = manifold.PointCount;
cc.Friction = friction;
cc.Restitution = restitution;
for (int k = 0; k < cc.PointCount; ++k)
{
ManifoldPoint cp = manifold.Points[k];
ContactConstraintPoint ccp = cc.Points[k];
ccp.NormalImpulse = cp.NormalImpulse;
ccp.TangentImpulse = cp.TangentImpulse;
ccp.Separation = cp.Separation;
ccp.LocalAnchor1 = cp.LocalPoint1;
ccp.LocalAnchor2 = cp.LocalPoint2;
ccp.R1 = Common.Math.Mul(b1.GetXForm().R, cp.LocalPoint1 - b1.GetLocalCenter());
ccp.R2 = Common.Math.Mul(b2.GetXForm().R, cp.LocalPoint2 - b2.GetLocalCenter());
float rn1 = Vec2.Cross(ccp.R1, normal);
float rn2 = Vec2.Cross(ccp.R2, normal);
rn1 *= rn1;
rn2 *= rn2;
float kNormal = b1._invMass + b2._invMass + b1._invI * rn1 + b2._invI * rn2;
Box2DXDebug.Assert(kNormal > Common.Settings.FLT_EPSILON);
ccp.NormalMass = 1.0f / kNormal;
float kEqualized = b1._mass * b1._invMass + b2._mass * b2._invMass;
kEqualized += b1._mass * b1._invI * rn1 + b2._mass * b2._invI * rn2;
Box2DXDebug.Assert(kEqualized > Common.Settings.FLT_EPSILON);
ccp.EqualizedMass = 1.0f / kEqualized;
Vec2 tangent = Vec2.Cross(normal, 1.0f);
float rt1 = Vec2.Cross(ccp.R1, tangent);
float rt2 = Vec2.Cross(ccp.R2, tangent);
rt1 *= rt1;
rt2 *= rt2;
float kTangent = b1._invMass + b2._invMass + b1._invI * rt1 + b2._invI * rt2;
Box2DXDebug.Assert(kTangent > Common.Settings.FLT_EPSILON);
ccp.TangentMass = 1.0f / kTangent;
// Setup a velocity bias for restitution.
ccp.VelocityBias = 0.0f;
if (ccp.Separation > 0.0f)
{
ccp.VelocityBias = -step.Inv_Dt * ccp.Separation; // TODO_ERIN b2TimeStep
}
else
{
float vRel = Vec2.Dot(cc.Normal, v2 + Vec2.Cross(w2, ccp.R2) - v1 - Vec2.Cross(w1, ccp.R1));
if (vRel < -Settings.VelocityThreshold)
{
ccp.VelocityBias = -cc.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 invMass1 = b1._invMass;
float invI1 = b1._invI;
float invMass2 = b2._invMass;
float invI2 = b2._invI;
float rn11 = Vec2.Cross(ccp1.R1, normal);
float rn12 = Vec2.Cross(ccp1.R2, normal);
float rn21 = Vec2.Cross(ccp2.R1, normal);
float rn22 = Vec2.Cross(ccp2.R2, normal);
float k11 = invMass1 + invMass2 + invI1 * rn11 * rn11 + invI2 * rn12 * rn12;
float k22 = invMass1 + invMass2 + invI1 * rn21 * rn21 + invI2 * rn22 * rn22;
float k12 = invMass1 + invMass2 + invI1 * rn11 * rn21 + invI2 * rn12 * rn22;
// 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.Col1.Set(k11, k12);
cc.K.Col2.Set(k12, k22);
cc.NormalMass = cc.K.Invert();
}
else
{
// The constraints are redundant, just use one.
// TODO_ERIN use deepest?
cc.PointCount = 1;
}
}
++count;
}
}
Box2DXDebug.Assert(count == _constraintCount);
}
public static void Destroy(Contact contact)
{
//Box2DXDebug.Assert(s_initialized == true); // COMMENT IS HACK HACK HACK
if (contact.GetManifoldCount() > 0)
{
contact.GetShape1().GetBody().WakeUp();
contact.GetShape2().GetBody().WakeUp();
}
ShapeType type1 = contact.GetShape1().GetType();
ShapeType type2 = contact.GetShape2().GetType();
Box2DXDebug.Assert(ShapeType.UnknownShape < type1 && type1 < ShapeType.ShapeTypeCount);
Box2DXDebug.Assert(ShapeType.UnknownShape < type2 && type2 < ShapeType.ShapeTypeCount);
try
{
ContactDestroyFcn destroyFcn = s_registers[(int)type1][(int)type2].DestroyFcn;
destroyFcn(contact);
}
catch (NullReferenceException e)
{
Console.WriteLine("NullReferenceException caught in Box2DX.Dynamics.Contact#Destroy");
}
}
public void Destroy(Contact c)
{
Shape shape = c.GetShape1();
Shape shape2 = c.GetShape2();
Body body = shape.GetBody();
Body body2 = shape2.GetBody();
ContactPoint contactPoint = new ContactPoint();
contactPoint.Shape1 = shape;
contactPoint.Shape2 = shape2;
contactPoint.Friction = Settings.MixFriction(shape.Friction, shape2.Friction);
contactPoint.Restitution = Settings.MixRestitution(shape.Restitution, shape2.Restitution);
int manifoldCount = c.GetManifoldCount();
if (manifoldCount > 0 && this._world._contactListener != null)
{
Manifold[] manifolds = c.GetManifolds();
for (int i = 0; i < manifoldCount; i++)
{
Manifold manifold = manifolds[i];
contactPoint.Normal = manifold.Normal;
for (int j = 0; j < manifold.PointCount; j++)
{
ManifoldPoint manifoldPoint = manifold.Points[j];
contactPoint.Position = body.GetWorldPoint(manifoldPoint.LocalPoint1);
Vec2 linearVelocityFromLocalPoint = body.GetLinearVelocityFromLocalPoint(manifoldPoint.LocalPoint1);
Vec2 linearVelocityFromLocalPoint2 = body2.GetLinearVelocityFromLocalPoint(manifoldPoint.LocalPoint2);
contactPoint.Velocity = linearVelocityFromLocalPoint2 - linearVelocityFromLocalPoint;
contactPoint.Separation = manifoldPoint.Separation;
contactPoint.ID = manifoldPoint.ID;
this._world._contactListener.Remove(contactPoint);
}
}
}
if (c._prev != null)
{
c._prev._next = c._next;
}
if (c._next != null)
{
c._next._prev = c._prev;
}
if (c == this._world._contactList)
{
this._world._contactList = c._next;
}
if (c._node1.Prev != null)
{
c._node1.Prev.Next = c._node1.Next;
}
if (c._node1.Next != null)
{
c._node1.Next.Prev = c._node1.Prev;
}
if (c._node1 == body._contactList)
{
body._contactList = c._node1.Next;
}
if (c._node2.Prev != null)
{
c._node2.Prev.Next = c._node2.Next;
}
if (c._node2.Next != null)
{
c._node2.Next.Prev = c._node2.Prev;
}
if (c._node2 == body2._contactList)
{
body2._contactList = c._node2.Next;
}
Contact.Destroy(c);
this._world._contactCount--;
}
public ContactSolver(TimeStep step, Contact[] contacts, int contactCount)
{
this._step = step;
this._constraintCount = 0;
for (int i = 0; i < contactCount; i++)
{
Box2DXDebug.Assert(contacts[i].IsSolid());
this._constraintCount += contacts[i].GetManifoldCount();
}
this._constraints = new ContactConstraint[this._constraintCount];
for (int i = 0; i < this._constraintCount; i++)
{
this._constraints[i] = new ContactConstraint();
}
int num = 0;
for (int i = 0; i < contactCount; i++)
{
Contact contact = contacts[i];
Shape shape = contact._shape1;
Shape shape2 = contact._shape2;
Body body = shape.GetBody();
Body body2 = shape2.GetBody();
int manifoldCount = contact.GetManifoldCount();
Manifold[] manifolds = contact.GetManifolds();
float friction = Settings.MixFriction(shape.Friction, shape2.Friction);
float restitution = Settings.MixRestitution(shape.Restitution, shape2.Restitution);
Vec2 linearVelocity = body._linearVelocity;
Vec2 linearVelocity2 = body2._linearVelocity;
float angularVelocity = body._angularVelocity;
float angularVelocity2 = body2._angularVelocity;
for (int j = 0; j < manifoldCount; j++)
{
Manifold manifold = manifolds[j];
Box2DXDebug.Assert(manifold.PointCount > 0);
Vec2 normal = manifold.Normal;
Box2DXDebug.Assert(num < this._constraintCount);
ContactConstraint contactConstraint = this._constraints[num];
contactConstraint.Body1 = body;
contactConstraint.Body2 = body2;
contactConstraint.Manifold = manifold;
contactConstraint.Normal = normal;
contactConstraint.PointCount = manifold.PointCount;
contactConstraint.Friction = friction;
contactConstraint.Restitution = restitution;
for (int k = 0; k < contactConstraint.PointCount; k++)
{
ManifoldPoint manifoldPoint = manifold.Points[k];
ContactConstraintPoint contactConstraintPoint = contactConstraint.Points[k];
contactConstraintPoint.NormalImpulse = manifoldPoint.NormalImpulse;
contactConstraintPoint.TangentImpulse = manifoldPoint.TangentImpulse;
contactConstraintPoint.Separation = manifoldPoint.Separation;
contactConstraintPoint.LocalAnchor1 = manifoldPoint.LocalPoint1;
contactConstraintPoint.LocalAnchor2 = manifoldPoint.LocalPoint2;
contactConstraintPoint.R1 = Box2DX.Common.Math.Mul(body.GetXForm().R, manifoldPoint.LocalPoint1 - body.GetLocalCenter());
contactConstraintPoint.R2 = Box2DX.Common.Math.Mul(body2.GetXForm().R, manifoldPoint.LocalPoint2 - body2.GetLocalCenter());
float num2 = Vec2.Cross(contactConstraintPoint.R1, normal);
float num3 = Vec2.Cross(contactConstraintPoint.R2, normal);
num2 *= num2;
num3 *= num3;
float num4 = body._invMass + body2._invMass + body._invI * num2 + body2._invI * num3;
Box2DXDebug.Assert(num4 > Settings.FLT_EPSILON);
contactConstraintPoint.NormalMass = 1f / num4;
float num5 = body._mass * body._invMass + body2._mass * body2._invMass;
num5 += body._mass * body._invI * num2 + body2._mass * body2._invI * num3;
Box2DXDebug.Assert(num5 > Settings.FLT_EPSILON);
contactConstraintPoint.EqualizedMass = 1f / num5;
Vec2 b = Vec2.Cross(normal, 1f);
float num6 = Vec2.Cross(contactConstraintPoint.R1, b);
float num7 = Vec2.Cross(contactConstraintPoint.R2, b);
num6 *= num6;
num7 *= num7;
float num8 = body._invMass + body2._invMass + body._invI * num6 + body2._invI * num7;
Box2DXDebug.Assert(num8 > Settings.FLT_EPSILON);
contactConstraintPoint.TangentMass = 1f / num8;
contactConstraintPoint.VelocityBias = 0f;
if (contactConstraintPoint.Separation > 0f)
{
contactConstraintPoint.VelocityBias = -step.Inv_Dt * contactConstraintPoint.Separation;
}
else
{
float num9 = Vec2.Dot(contactConstraint.Normal, linearVelocity2 + Vec2.Cross(angularVelocity2, contactConstraintPoint.R2) - linearVelocity - Vec2.Cross(angularVelocity, contactConstraintPoint.R1));
if (num9 < -Settings.VelocityThreshold)
{
contactConstraintPoint.VelocityBias = -contactConstraint.Restitution * num9;
}
}
}
if (contactConstraint.PointCount == 2)
{
ContactConstraintPoint contactConstraintPoint2 = contactConstraint.Points[0];
ContactConstraintPoint contactConstraintPoint3 = contactConstraint.Points[1];
float invMass = body._invMass;
float invI = body._invI;
float invMass2 = body2._invMass;
float invI2 = body2._invI;
float num10 = Vec2.Cross(contactConstraintPoint2.R1, normal);
float num11 = Vec2.Cross(contactConstraintPoint2.R2, normal);
float num12 = Vec2.Cross(contactConstraintPoint3.R1, normal);
float num13 = Vec2.Cross(contactConstraintPoint3.R2, normal);
float num14 = invMass + invMass2 + invI * num10 * num10 + invI2 * num11 * num11;
float num15 = invMass + invMass2 + invI * num12 * num12 + invI2 * num13 * num13;
float num16 = invMass + invMass2 + invI * num10 * num12 + invI2 * num11 * num13;
if (num14 * num14 < 100f * (num14 * num15 - num16 * num16))
{
contactConstraint.K.Col1.Set(num14, num16);
contactConstraint.K.Col2.Set(num16, num15);
contactConstraint.NormalMass = contactConstraint.K.Invert();
}
else
{
contactConstraint.PointCount = 1;
}
}
num++;
}
}
Box2DXDebug.Assert(num == this._constraintCount);
}
public static void Destroy(Contact contact)
{
Box2DXDebug.Assert(s_initialized == true);
if (contact.GetManifoldCount() > 0)
{
contact.GetShape1().GetBody().WakeUp();
contact.GetShape2().GetBody().WakeUp();
}
ShapeType type1 = contact.GetShape1().GetType();
ShapeType type2 = contact.GetShape2().GetType();
Box2DXDebug.Assert(ShapeType.UnknownShape < type1 && type1 < ShapeType.ShapeTypeCount);
Box2DXDebug.Assert(ShapeType.UnknownShape < type2 && type2 < ShapeType.ShapeTypeCount);
ContactDestroyFcn destroyFcn = s_registers[(int)type1][(int)type2].DestroyFcn;
destroyFcn(contact);
}
public void Destroy(Contact c)
{
Shape shape = c.GetShape1();
Shape shape2 = c.GetShape2();
Body body = shape.GetBody();
Body body2 = shape2.GetBody();
ContactPoint contactPoint = new ContactPoint();
contactPoint.Shape1 = shape;
contactPoint.Shape2 = shape2;
contactPoint.Friction = Settings.MixFriction(shape.Friction, shape2.Friction);
contactPoint.Restitution = Settings.MixRestitution(shape.Restitution, shape2.Restitution);
int manifoldCount = c.GetManifoldCount();
if (manifoldCount > 0 && this._world._contactListener != null)
{
Manifold[] manifolds = c.GetManifolds();
for (int i = 0; i < manifoldCount; i++)
{
Manifold manifold = manifolds[i];
contactPoint.Normal = manifold.Normal;
for (int j = 0; j < manifold.PointCount; j++)
{
ManifoldPoint manifoldPoint = manifold.Points[j];
contactPoint.Position = body.GetWorldPoint(manifoldPoint.LocalPoint1);
Vec2 linearVelocityFromLocalPoint = body.GetLinearVelocityFromLocalPoint(manifoldPoint.LocalPoint1);
Vec2 linearVelocityFromLocalPoint2 = body2.GetLinearVelocityFromLocalPoint(manifoldPoint.LocalPoint2);
contactPoint.Velocity = linearVelocityFromLocalPoint2 - linearVelocityFromLocalPoint;
contactPoint.Separation = manifoldPoint.Separation;
contactPoint.ID = manifoldPoint.ID;
this._world._contactListener.Remove(contactPoint);
}
}
}
if (c._prev != null)
{
c._prev._next = c._next;
}
if (c._next != null)
{
c._next._prev = c._prev;
}
if (c == this._world._contactList)
{
this._world._contactList = c._next;
}
if (c._node1.Prev != null)
{
c._node1.Prev.Next = c._node1.Next;
}
if (c._node1.Next != null)
{
c._node1.Next.Prev = c._node1.Prev;
}
if (c._node1 == body._contactList)
{
body._contactList = c._node1.Next;
}
if (c._node2.Prev != null)
{
c._node2.Prev.Next = c._node2.Next;
}
if (c._node2.Next != null)
{
c._node2.Next.Prev = c._node2.Prev;
}
if (c._node2 == body2._contactList)
{
body2._contactList = c._node2.Next;
}
Contact.Destroy(c);
this._world._contactCount--;
}
