public GetLinearVelocityFromLocalPoint ( System.Vector2 localPoint ) : System.Vector2 | ||
localPoint | System.Vector2 | A point in local coordinates. |
return | System.Vector2 |
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 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 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); } }
float limitVelocity(Body hull, float max) { Vec2 velocity = hull.GetLinearVelocityFromLocalPoint(Box2DX.Common.Vec2.Zero); var vec1 = hull.GetXForm().R.Col2; var projection = Vec2.Dot(velocity, vec1); vec1 *= projection; var A = vec1.Length(); if (A > max) { var k = max / A; hull.SetLinearVelocity(vec1 * k); } return A; }
void killWheelOrthogonalVelocity(Body hull, Body wheel) { var p_hull = hull.GetPosition(); var p_wheel = wheel.GetPosition(); var velocity = wheel.GetLinearVelocityFromLocalPoint(Vec2.Zero); var vec1 = wheel.GetXForm().R.Col1; var projection = Vec2.Dot(velocity, vec1); vec1 *= projection; var k = vec1.Length() / 0.001f; // превышение боковой скорости 1 мм / с if (k < 0.1f) k = 0.1f; if (k > 10f) k = 10f; float force = -k * 3000; //warning - mn vec1.Normalize(); hull.ApplyForce(force * vec1, p_wheel); }
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); } } } }