public override void Evaluate(ContactListener listener) { Body body = this._shape1.GetBody(); Body body2 = this._shape2.GetBody(); Manifold manifold = this._manifold.Clone(); Collision.Collision.CollideCircles(ref this._manifold, (CircleShape)this._shape1, body.GetXForm(), (CircleShape)this._shape2, body2.GetXForm()); ContactPoint contactPoint = new ContactPoint(); contactPoint.Shape1 = this._shape1; contactPoint.Shape2 = this._shape2; contactPoint.Friction = Settings.MixFriction(this._shape1.Friction, this._shape2.Friction); contactPoint.Restitution = Settings.MixRestitution(this._shape1.Restitution, this._shape2.Restitution); if (this._manifold.PointCount > 0) { this._manifoldCount = 1; ManifoldPoint manifoldPoint = this._manifold.Points[0]; if (manifold.PointCount == 0) { manifoldPoint.NormalImpulse = 0f; manifoldPoint.TangentImpulse = 0f; if (listener != null) { contactPoint.Position = body.GetWorldPoint(manifoldPoint.LocalPoint1); Vec2 linearVelocityFromLocalPoint = body.GetLinearVelocityFromLocalPoint(manifoldPoint.LocalPoint1); Vec2 linearVelocityFromLocalPoint2 = body2.GetLinearVelocityFromLocalPoint(manifoldPoint.LocalPoint2); contactPoint.Velocity = linearVelocityFromLocalPoint2 - linearVelocityFromLocalPoint; contactPoint.Normal = this._manifold.Normal; contactPoint.Separation = manifoldPoint.Separation; contactPoint.ID = manifoldPoint.ID; listener.Add(contactPoint); } } else { ManifoldPoint manifoldPoint2 = manifold.Points[0]; manifoldPoint.NormalImpulse = manifoldPoint2.NormalImpulse; manifoldPoint.TangentImpulse = manifoldPoint2.TangentImpulse; if (listener != null) { contactPoint.Position = body.GetWorldPoint(manifoldPoint.LocalPoint1); Vec2 linearVelocityFromLocalPoint = body.GetLinearVelocityFromLocalPoint(manifoldPoint.LocalPoint1); Vec2 linearVelocityFromLocalPoint2 = body2.GetLinearVelocityFromLocalPoint(manifoldPoint.LocalPoint2); contactPoint.Velocity = linearVelocityFromLocalPoint2 - linearVelocityFromLocalPoint; contactPoint.Normal = this._manifold.Normal; contactPoint.Separation = manifoldPoint.Separation; contactPoint.ID = manifoldPoint.ID; listener.Persist(contactPoint); } } } else { this._manifoldCount = 0; if (manifold.PointCount > 0 && listener != null) { ManifoldPoint manifoldPoint2 = manifold.Points[0]; contactPoint.Position = body.GetWorldPoint(manifoldPoint2.LocalPoint1); Vec2 linearVelocityFromLocalPoint = body.GetLinearVelocityFromLocalPoint(manifoldPoint2.LocalPoint1); Vec2 linearVelocityFromLocalPoint2 = body2.GetLinearVelocityFromLocalPoint(manifoldPoint2.LocalPoint2); contactPoint.Velocity = linearVelocityFromLocalPoint2 - linearVelocityFromLocalPoint; contactPoint.Normal = manifold.Normal; contactPoint.Separation = manifoldPoint2.Separation; contactPoint.ID = manifoldPoint2.ID; listener.Remove(contactPoint); } } }
public override void Evaluate(ContactListener listener) { Body b1 = _shape1.GetBody(); Body b2 = _shape2.GetBody(); #warning "needfix" //memcpy(&m0, &m_manifold, sizeof(b2Manifold)); Manifold m0 = _manifold.Clone(); Collision.Collision.CollidePolygons(ref _manifold, (PolygonShape)_shape1, b1.GetXForm(), (PolygonShape)_shape2, b2.GetXForm()); bool[] persisted = new bool[] { false, false }; 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); // Match contact ids to facilitate warm starting. if (_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 mp = _manifold.Points[i]; mp.NormalImpulse = 0.0f; mp.TangentImpulse = 0.0f; bool found = false; ContactID id = mp.ID; for (int j = 0; j < m0.PointCount; ++j) { if (persisted[j] == true) { continue; } ManifoldPoint mp0 = m0.Points[j]; if (mp0.ID.Key == id.Key) { persisted[j] = true; mp.NormalImpulse = mp0.NormalImpulse; mp.TangentImpulse = mp0.TangentImpulse; // A persistent point. found = true; // Report persistent point. if (listener != null) { cp.Position = b1.GetWorldPoint(mp.LocalPoint1); Vec2 v1 = b1.GetLinearVelocityFromLocalPoint(mp.LocalPoint1); Vec2 v2 = b2.GetLinearVelocityFromLocalPoint(mp.LocalPoint2); cp.Velocity = v2 - v1; cp.Normal = _manifold.Normal; cp.Separation = mp.Separation; cp.ID = id; listener.Persist(cp); } break; } } // Report added point. if (found == false && listener != null) { cp.Position = b1.GetWorldPoint(mp.LocalPoint1); Vec2 v1 = b1.GetLinearVelocityFromLocalPoint(mp.LocalPoint1); Vec2 v2 = b2.GetLinearVelocityFromLocalPoint(mp.LocalPoint2); cp.Velocity = v2 - v1; cp.Normal = _manifold.Normal; cp.Separation = mp.Separation; cp.ID = id; listener.Add(cp); } } _manifoldCount = 1; } else { _manifoldCount = 0; } if (listener == null) { return; } // Report removed points. for (int i = 0; i < m0.PointCount; ++i) { if (persisted[i]) { continue; } ManifoldPoint mp0 = m0.Points[i]; cp.Position = b1.GetWorldPoint(mp0.LocalPoint1); Vec2 v1 = b1.GetLinearVelocityFromLocalPoint(mp0.LocalPoint1); Vec2 v2 = b2.GetLinearVelocityFromLocalPoint(mp0.LocalPoint2); cp.Velocity = v2 - v1; cp.Normal = m0.Normal; cp.Separation = mp0.Separation; cp.ID = mp0.ID; listener.Remove(cp); } }
public override void Evaluate(ContactListener listener) { Body body = this._shape1.GetBody(); Body body2 = this._shape2.GetBody(); Manifold manifold = this._manifold.Clone(); Collision.Collision.CollidePolygonAndCircle(ref this._manifold, (PolygonShape)this._shape1, body.GetXForm(), (CircleShape)this._shape2, body2.GetXForm()); bool[] array = new bool[2]; bool[] array2 = array; ContactPoint contactPoint = new ContactPoint(); contactPoint.Shape1 = this._shape1; contactPoint.Shape2 = this._shape2; contactPoint.Friction = Settings.MixFriction(this._shape1.Friction, this._shape2.Friction); contactPoint.Restitution = Settings.MixRestitution(this._shape1.Restitution, this._shape2.Restitution); if (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; bool flag = false; ContactID iD = manifoldPoint.ID; for (int j = 0; j < manifold.PointCount; j++) { if (!array2[j]) { ManifoldPoint manifoldPoint2 = manifold.Points[j]; if (manifoldPoint2.ID.Key == iD.Key) { array2[j] = true; manifoldPoint.NormalImpulse = manifoldPoint2.NormalImpulse; manifoldPoint.TangentImpulse = manifoldPoint2.TangentImpulse; flag = true; if (listener != null) { contactPoint.Position = body.GetWorldPoint(manifoldPoint.LocalPoint1); Vec2 linearVelocityFromLocalPoint = body.GetLinearVelocityFromLocalPoint(manifoldPoint.LocalPoint1); Vec2 linearVelocityFromLocalPoint2 = body2.GetLinearVelocityFromLocalPoint(manifoldPoint.LocalPoint2); contactPoint.Velocity = linearVelocityFromLocalPoint2 - linearVelocityFromLocalPoint; contactPoint.Normal = this._manifold.Normal; contactPoint.Separation = manifoldPoint.Separation; contactPoint.ID = iD; listener.Persist(contactPoint); } break; } } } if (!flag && listener != null) { contactPoint.Position = body.GetWorldPoint(manifoldPoint.LocalPoint1); Vec2 linearVelocityFromLocalPoint = body.GetLinearVelocityFromLocalPoint(manifoldPoint.LocalPoint1); Vec2 linearVelocityFromLocalPoint2 = body2.GetLinearVelocityFromLocalPoint(manifoldPoint.LocalPoint2); contactPoint.Velocity = linearVelocityFromLocalPoint2 - linearVelocityFromLocalPoint; contactPoint.Normal = this._manifold.Normal; contactPoint.Separation = manifoldPoint.Separation; contactPoint.ID = iD; listener.Add(contactPoint); } } this._manifoldCount = 1; } else { this._manifoldCount = 0; } if (listener != null) { for (int i = 0; i < manifold.PointCount; i++) { if (!array2[i]) { ManifoldPoint manifoldPoint2 = manifold.Points[i]; contactPoint.Position = body.GetWorldPoint(manifoldPoint2.LocalPoint1); Vec2 linearVelocityFromLocalPoint = body.GetLinearVelocityFromLocalPoint(manifoldPoint2.LocalPoint1); Vec2 linearVelocityFromLocalPoint2 = body2.GetLinearVelocityFromLocalPoint(manifoldPoint2.LocalPoint2); contactPoint.Velocity = linearVelocityFromLocalPoint2 - linearVelocityFromLocalPoint; contactPoint.Normal = manifold.Normal; contactPoint.Separation = manifoldPoint2.Separation; contactPoint.ID = manifoldPoint2.ID; listener.Remove(contactPoint); } } } }
public override void Evaluate(ContactListener listener) { Body b1 = _shape1.GetBody(); Body b2 = _shape2.GetBody(); #warning "needfix" //memcpy(&m0, &m_manifold, sizeof(b2Manifold)); Manifold m0 = _manifold.Clone(); Collision.Collision.CollideCircles(ref _manifold, (CircleShape)_shape1, b1.GetXForm(), (CircleShape)_shape2, b2.GetXForm()); 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); if (_manifold.PointCount > 0) { _manifoldCount = 1; ManifoldPoint mp = _manifold.Points[0]; if (m0.PointCount == 0) { mp.NormalImpulse = 0.0f; mp.TangentImpulse = 0.0f; if (listener!=null) { cp.Position = b1.GetWorldPoint(mp.LocalPoint1); Vec2 v1 = b1.GetLinearVelocityFromLocalPoint(mp.LocalPoint1); Vec2 v2 = b2.GetLinearVelocityFromLocalPoint(mp.LocalPoint2); cp.Velocity = v2 - v1; cp.Normal = _manifold.Normal; cp.Separation = mp.Separation; cp.ID = mp.ID; listener.Add(cp); } } else { ManifoldPoint mp0 = m0.Points[0]; mp.NormalImpulse = mp0.NormalImpulse; mp.TangentImpulse = mp0.TangentImpulse; if (listener!=null) { cp.Position = b1.GetWorldPoint(mp.LocalPoint1); Vec2 v1 = b1.GetLinearVelocityFromLocalPoint(mp.LocalPoint1); Vec2 v2 = b2.GetLinearVelocityFromLocalPoint(mp.LocalPoint2); cp.Velocity = v2 - v1; cp.Normal = _manifold.Normal; cp.Separation = mp.Separation; cp.ID = mp.ID; listener.Persist(cp); } } } else { _manifoldCount = 0; if (m0.PointCount > 0 && listener!=null) { ManifoldPoint mp0 = m0.Points[0]; cp.Position = b1.GetWorldPoint(mp0.LocalPoint1); Vec2 v1 = b1.GetLinearVelocityFromLocalPoint(mp0.LocalPoint1); Vec2 v2 = b2.GetLinearVelocityFromLocalPoint(mp0.LocalPoint2); cp.Velocity = v2 - v1; cp.Normal = m0.Normal; cp.Separation = mp0.Separation; cp.ID = mp0.ID; listener.Remove(cp); } } }
public override void Evaluate(ContactListener listener) { Body b1 = _shape1.GetBody(); Body b2 = _shape2.GetBody(); #warning "needfix" //memcpy(&m0, &m_manifold, sizeof(b2Manifold)); Manifold m0 = _manifold.Clone(); Collision.Collision.CollideCircles(ref _manifold, (CircleShape)_shape1, b1.GetXForm(), (CircleShape)_shape2, b2.GetXForm()); 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); if (_manifold.PointCount > 0) { _manifoldCount = 1; ManifoldPoint mp = _manifold.Points[0]; if (m0.PointCount == 0) { mp.NormalImpulse = 0.0f; mp.TangentImpulse = 0.0f; if (listener != null) { cp.Position = b1.GetWorldPoint(mp.LocalPoint1); Vec2 v1 = b1.GetLinearVelocityFromLocalPoint(mp.LocalPoint1); Vec2 v2 = b2.GetLinearVelocityFromLocalPoint(mp.LocalPoint2); cp.Velocity = v2 - v1; cp.Normal = _manifold.Normal; cp.Separation = mp.Separation; cp.ID = mp.ID; listener.Add(cp); } } else { ManifoldPoint mp0 = m0.Points[0]; mp.NormalImpulse = mp0.NormalImpulse; mp.TangentImpulse = mp0.TangentImpulse; if (listener != null) { cp.Position = b1.GetWorldPoint(mp.LocalPoint1); Vec2 v1 = b1.GetLinearVelocityFromLocalPoint(mp.LocalPoint1); Vec2 v2 = b2.GetLinearVelocityFromLocalPoint(mp.LocalPoint2); cp.Velocity = v2 - v1; cp.Normal = _manifold.Normal; cp.Separation = mp.Separation; cp.ID = mp.ID; listener.Persist(cp); } } } else { _manifoldCount = 0; if (m0.PointCount > 0 && listener != null) { ManifoldPoint mp0 = m0.Points[0]; cp.Position = b1.GetWorldPoint(mp0.LocalPoint1); Vec2 v1 = b1.GetLinearVelocityFromLocalPoint(mp0.LocalPoint1); Vec2 v2 = b2.GetLinearVelocityFromLocalPoint(mp0.LocalPoint2); cp.Velocity = v2 - v1; cp.Normal = m0.Normal; cp.Separation = mp0.Separation; cp.ID = mp0.ID; listener.Remove(cp); } } }
public override void Evaluate(ContactListener listener) { Body body = this._shape1.GetBody(); Body body2 = this._shape2.GetBody(); Manifold manifold = this._manifold.Clone(); Collision.Collision.CollidePolygons(ref this._manifold, (PolygonShape)this._shape1, body.GetXForm(), (PolygonShape)this._shape2, body2.GetXForm()); bool[] array = new bool[2]; bool[] array2 = array; ContactPoint contactPoint = new ContactPoint(); contactPoint.Shape1 = this._shape1; contactPoint.Shape2 = this._shape2; contactPoint.Friction = Settings.MixFriction(this._shape1.Friction, this._shape2.Friction); contactPoint.Restitution = Settings.MixRestitution(this._shape1.Restitution, this._shape2.Restitution); if (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; bool flag = false; ContactID iD = manifoldPoint.ID; for (int j = 0; j < manifold.PointCount; j++) { if (!array2[j]) { ManifoldPoint manifoldPoint2 = manifold.Points[j]; if (manifoldPoint2.ID.Key == iD.Key) { array2[j] = true; manifoldPoint.NormalImpulse = manifoldPoint2.NormalImpulse; manifoldPoint.TangentImpulse = manifoldPoint2.TangentImpulse; flag = true; if (listener != null) { contactPoint.Position = body.GetWorldPoint(manifoldPoint.LocalPoint1); Vec2 linearVelocityFromLocalPoint = body.GetLinearVelocityFromLocalPoint(manifoldPoint.LocalPoint1); Vec2 linearVelocityFromLocalPoint2 = body2.GetLinearVelocityFromLocalPoint(manifoldPoint.LocalPoint2); contactPoint.Velocity = linearVelocityFromLocalPoint2 - linearVelocityFromLocalPoint; contactPoint.Normal = this._manifold.Normal; contactPoint.Separation = manifoldPoint.Separation; contactPoint.ID = iD; listener.Persist(contactPoint); } break; } } } if (!flag && listener != null) { contactPoint.Position = body.GetWorldPoint(manifoldPoint.LocalPoint1); Vec2 linearVelocityFromLocalPoint = body.GetLinearVelocityFromLocalPoint(manifoldPoint.LocalPoint1); Vec2 linearVelocityFromLocalPoint2 = body2.GetLinearVelocityFromLocalPoint(manifoldPoint.LocalPoint2); contactPoint.Velocity = linearVelocityFromLocalPoint2 - linearVelocityFromLocalPoint; contactPoint.Normal = this._manifold.Normal; contactPoint.Separation = manifoldPoint.Separation; contactPoint.ID = iD; listener.Add(contactPoint); } } this._manifoldCount = 1; } else { this._manifoldCount = 0; } if (listener != null) { for (int i = 0; i < manifold.PointCount; i++) { if (!array2[i]) { ManifoldPoint manifoldPoint2 = manifold.Points[i]; contactPoint.Position = body.GetWorldPoint(manifoldPoint2.LocalPoint1); Vec2 linearVelocityFromLocalPoint = body.GetLinearVelocityFromLocalPoint(manifoldPoint2.LocalPoint1); Vec2 linearVelocityFromLocalPoint2 = body2.GetLinearVelocityFromLocalPoint(manifoldPoint2.LocalPoint2); contactPoint.Velocity = linearVelocityFromLocalPoint2 - linearVelocityFromLocalPoint; contactPoint.Normal = manifold.Normal; contactPoint.Separation = manifoldPoint2.Separation; contactPoint.ID = manifoldPoint2.ID; listener.Remove(contactPoint); } } } }