public GetWorldPoint ( System.Vector2 localPoint ) : System.Vector2 | ||
localPoint | System.Vector2 | A point on the body measured relative the the body's origin. |
리턴 | System.Vector2 |
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 float GetJointTranslation() { Body body = this._body1; Body body2 = this._body2; Vec2 worldPoint = body.GetWorldPoint(this._localAnchor1); Vec2 worldPoint2 = body2.GetWorldPoint(this._localAnchor2); Vec2 a = worldPoint2 - worldPoint; Vec2 worldVector = body.GetWorldVector(this._localXAxis1); return(Vec2.Dot(a, worldVector)); }
/// <summary> /// Get the current joint translation, usually in meters. /// </summary> public float GetJointTranslation() { Body b1 = _body1; Body b2 = _body2; Vector2 p1 = b1.GetWorldPoint(_localAnchor1); Vector2 p2 = b2.GetWorldPoint(_localAnchor2); Vector2 d = p2 - p1; Vector2 axis = b1.GetWorldVector(_localXAxis1); float translation = Vector2.Dot(d, axis); return(translation); }
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 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); } } }
/// <summary> /// Add a spring force /// </summary> void AddSpringForce(Body bA, Vec2 localA, Body bB, Vec2 localB, float k, float friction, float desiredDist) { Vec2 pA = bA.GetWorldPoint(localA); Vec2 pB = bB.GetWorldPoint(localB); Vec2 diff = pB - pA; //Find velocities of attach points Vec2 vA = bA.GetLinearVelocity() - Vec2.Cross(bA.GetWorldVector(localA), bA.GetAngularVelocity()); Vec2 vB = bB.GetLinearVelocity() - Vec2.Cross(bB.GetWorldVector(localB), bB.GetAngularVelocity()); Vec2 vdiff = vB - vA; float dx = diff.Normalize(); //normalizes diff and puts length into dx float vrel = vdiff.X * diff.X + vdiff.Y * diff.Y; float forceMag = -k * (dx - desiredDist) - friction * vrel; diff *= forceMag; // diff *= forceMag bB.ApplyForce(diff, bA.GetWorldPoint(localA)); diff *= -1.0f; bA.ApplyForce(diff, bB.GetWorldPoint(localB)); }
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 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 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); } } } }