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); }