GetLinearVelocityFromLocalPoint() public method

Get the world velocity of a local point.
public GetLinearVelocityFromLocalPoint ( System.Vector2 localPoint ) : System.Vector2
localPoint System.Vector2 A point in local coordinates.
return System.Vector2
Ejemplo n.º 1
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;
        }
Ejemplo n.º 2
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--;
        }
Ejemplo n.º 3
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);
                }
            }
        }
Ejemplo n.º 4
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);
            }
        }
Ejemplo n.º 5
0
        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;
        }
Ejemplo n.º 6
0
        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);
        }
Ejemplo n.º 7
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);
                }
            }
        }
Ejemplo n.º 8
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);
                    }
                }
            }
        }