GetWorldPoint() public method

Get the world coordinates of a point given the local coordinates.
public GetWorldPoint ( System.Vector2 localPoint ) : System.Vector2
localPoint System.Vector2 A point on the body measured relative the the body's origin.
return System.Vector2
Example #1
0
 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);
                 }
             }
         }
     }
 }
Example #2
0
        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));
        }
Example #3
0
        /// <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);
        }
Example #4
0
        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;
        }
Example #5
0
        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);
                }
            }
        }
Example #6
0
		/// <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));
		}
Example #7
0
        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--;
        }
Example #8
0
        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);
            }
        }
Example #9
0
        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);
                }
            }
        }
Example #10
0
        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);
                    }
                }
            }
        }