예제 #1
0
 public void StoreImpulses()
 {
     for (int i = 0; i < this._count; i++)
     {
         ContactVelocityConstraint contactVelocityConstraint = this._velocityConstraints[i];
         Manifold manifold = this._contacts[contactVelocityConstraint.contactIndex].Manifold;
         for (int j = 0; j < contactVelocityConstraint.pointCount; j++)
         {
             ManifoldPoint value = manifold.Points[j];
             value.NormalImpulse  = contactVelocityConstraint.points[j].normalImpulse;
             value.TangentImpulse = contactVelocityConstraint.points[j].tangentImpulse;
             manifold.Points[j]   = value;
         }
         this._contacts[contactVelocityConstraint.contactIndex].Manifold = manifold;
     }
 }
        public void StoreImpulses()
        {
            for (int i = 0; i < _count; ++i)
            {
                ContactVelocityConstraint vc = _velocityConstraints[i];
                Manifold manifold            = _contacts[vc.contactIndex].Manifold;

                for (int j = 0; j < vc.pointCount; ++j)
                {
                    ManifoldPoint point = manifold.Points[j];
                    point.NormalImpulse  = vc.points[j].normalImpulse;
                    point.TangentImpulse = vc.points[j].tangentImpulse;
                    manifold.Points[j]   = point;
                }

                _contacts[vc.contactIndex].Manifold = manifold;
            }
        }
예제 #3
0
        public void Reset(TimeStep step, int count, Contact[] contacts, Position[] positions, Velocity[] velocities, bool warmstarting = true)
        {
            this._step       = step;
            this._count      = count;
            this._positions  = positions;
            this._velocities = velocities;
            this._contacts   = contacts;
            bool flag = this._velocityConstraints == null || this._velocityConstraints.Length < count;

            if (flag)
            {
                this._velocityConstraints = new ContactVelocityConstraint[count * 2];
                this._positionConstraints = new ContactPositionConstraint[count * 2];
                for (int i = 0; i < this._velocityConstraints.Length; i++)
                {
                    this._velocityConstraints[i] = new ContactVelocityConstraint();
                }
                for (int j = 0; j < this._positionConstraints.Length; j++)
                {
                    this._positionConstraints[j] = new ContactPositionConstraint();
                }
            }
            for (int k = 0; k < this._count; k++)
            {
                Contact  contact    = contacts[k];
                Fixture  fixtureA   = contact.FixtureA;
                Fixture  fixtureB   = contact.FixtureB;
                Shape    shape      = fixtureA.Shape;
                Shape    shape2     = fixtureB.Shape;
                FP       radius     = shape.Radius;
                FP       radius2    = shape2.Radius;
                Body     body       = fixtureA.Body;
                Body     body2      = fixtureB.Body;
                Manifold manifold   = contact.Manifold;
                int      pointCount = manifold.PointCount;
                Debug.Assert(pointCount > 0);
                ContactVelocityConstraint contactVelocityConstraint = this._velocityConstraints[k];
                contactVelocityConstraint.friction     = contact.Friction;
                contactVelocityConstraint.restitution  = contact.Restitution;
                contactVelocityConstraint.tangentSpeed = contact.TangentSpeed;
                contactVelocityConstraint.indexA       = body.IslandIndex;
                contactVelocityConstraint.indexB       = body2.IslandIndex;
                contactVelocityConstraint.invMassA     = body._invMass;
                contactVelocityConstraint.invMassB     = body2._invMass;
                contactVelocityConstraint.invIA        = body._invI;
                contactVelocityConstraint.invIB        = body2._invI;
                contactVelocityConstraint.contactIndex = k;
                contactVelocityConstraint.pointCount   = pointCount;
                contactVelocityConstraint.K.SetZero();
                contactVelocityConstraint.normalMass.SetZero();
                ContactPositionConstraint contactPositionConstraint = this._positionConstraints[k];
                contactPositionConstraint.indexA       = body.IslandIndex;
                contactPositionConstraint.indexB       = body2.IslandIndex;
                contactPositionConstraint.invMassA     = body._invMass;
                contactPositionConstraint.invMassB     = body2._invMass;
                contactPositionConstraint.localCenterA = body._sweep.LocalCenter;
                contactPositionConstraint.localCenterB = body2._sweep.LocalCenter;
                contactPositionConstraint.invIA        = body._invI;
                contactPositionConstraint.invIB        = body2._invI;
                contactPositionConstraint.localNormal  = manifold.LocalNormal;
                contactPositionConstraint.localPoint   = manifold.LocalPoint;
                contactPositionConstraint.pointCount   = pointCount;
                contactPositionConstraint.radiusA      = radius;
                contactPositionConstraint.radiusB      = radius2;
                contactPositionConstraint.type         = manifold.Type;
                for (int l = 0; l < pointCount; l++)
                {
                    ManifoldPoint           manifoldPoint           = manifold.Points[l];
                    VelocityConstraintPoint velocityConstraintPoint = contactVelocityConstraint.points[l];
                    velocityConstraintPoint.normalImpulse    = this._step.dtRatio * manifoldPoint.NormalImpulse;
                    velocityConstraintPoint.tangentImpulse   = this._step.dtRatio * manifoldPoint.TangentImpulse;
                    velocityConstraintPoint.rA               = TSVector2.zero;
                    velocityConstraintPoint.rB               = TSVector2.zero;
                    velocityConstraintPoint.normalMass       = 0f;
                    velocityConstraintPoint.tangentMass      = 0f;
                    velocityConstraintPoint.velocityBias     = 0f;
                    contactPositionConstraint.localPoints[l] = manifoldPoint.LocalPoint;
                }
            }
        }
예제 #4
0
        /// <summary>
        /// Update the contact manifold and touching status.
        /// Note: do not assume the fixture AABBs are overlapping or are valid.
        /// </summary>
        /// <param name="contactManager">The contact manager.</param>
        internal void Update(ContactManager contactManager)
        {
            Body bodyA = FixtureA.Body;
            Body bodyB = FixtureB.Body;

            if (FixtureA == null || FixtureB == null)
            {
                return;
            }

            if (!ContactManager.CheckCollisionConditions(FixtureA, FixtureB))
            {
                Enabled = false;
                return;
            }

            Manifold oldManifold = Manifold;

            // Re-enable this contact.
            Enabled = true;

            bool touching;
            bool wasTouching = IsTouching;

            bool sensor = FixtureA.IsSensor || FixtureB.IsSensor;

            // Is this contact a sensor?
            if (sensor)
            {
                Shape shapeA = FixtureA.Shape;
                Shape shapeB = FixtureB.Shape;
                touching = Collision.TestOverlap(shapeA, ChildIndexA, shapeB, ChildIndexB, ref bodyA._xf, ref bodyB._xf);

                // Sensors don't generate manifolds.
                Manifold.PointCount = 0;
            }
            else
            {
                Evaluate(ref Manifold, ref bodyA._xf, ref bodyB._xf);
                touching = 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 mp2 = Manifold.Points[i];
                    mp2.NormalImpulse  = 0.0f;
                    mp2.TangentImpulse = 0.0f;
                    ContactID id2 = mp2.Id;

                    for (int j = 0; j < oldManifold.PointCount; ++j)
                    {
                        ManifoldPoint mp1 = oldManifold.Points[j];

                        if (mp1.Id.Key == id2.Key)
                        {
                            mp2.NormalImpulse  = mp1.NormalImpulse;
                            mp2.TangentImpulse = mp1.TangentImpulse;
                            break;
                        }
                    }

                    Manifold.Points[i] = mp2;
                }

                if (touching != wasTouching)
                {
                    bodyA.Awake = true;
                    bodyB.Awake = true;
                }
            }

            IsTouching = touching;

            if (wasTouching == false)
            {
                if (touching)
                {
                    if (Settings.AllCollisionCallbacksAgree)
                    {
                        bool enabledA = true, enabledB = true;

                        // Report the collision to both participants. Track which ones returned true so we can
                        // later call OnSeparation if the contact is disabled for a different reason.
                        if (FixtureA.OnCollision != null)
                        {
                            foreach (OnCollisionEventHandler handler in FixtureA.OnCollision.GetInvocationList())
                            {
                                enabledA = handler(FixtureA, FixtureB, this) && enabledA;
                            }
                        }

                        // Reverse the order of the reported fixtures. The first fixture is always the one that the
                        // user subscribed to.
                        if (FixtureB.OnCollision != null)
                        {
                            foreach (OnCollisionEventHandler handler in FixtureB.OnCollision.GetInvocationList())
                            {
                                enabledB = handler(FixtureB, FixtureA, this) && enabledB;
                            }
                        }

                        Enabled = enabledA && enabledB;

                        // BeginContact can also return false and disable the contact
                        if (enabledA && enabledB && contactManager.BeginContact != null)
                        {
                            Enabled = contactManager.BeginContact(this);
                        }
                    }
                    else
                    {
                        //Report the collision to both participants:
                        if (FixtureA.OnCollision != null)
                        {
                            foreach (OnCollisionEventHandler handler in FixtureA.OnCollision.GetInvocationList())
                            {
                                Enabled = handler(FixtureA, FixtureB, this);
                            }
                        }

                        //Reverse the order of the reported fixtures. The first fixture is always the one that the
                        //user subscribed to.
                        if (FixtureB.OnCollision != null)
                        {
                            foreach (OnCollisionEventHandler handler in FixtureB.OnCollision.GetInvocationList())
                            {
                                Enabled = handler(FixtureB, FixtureA, this);
                            }
                        }

                        //BeginContact can also return false and disable the contact
                        if (contactManager.BeginContact != null)
                        {
                            Enabled = contactManager.BeginContact(this);
                        }
                    }

                    // If the user disabled the contact (needed to exclude it in TOI solver) at any point by
                    // any of the callbacks, we need to mark it as not touching and call any separation
                    // callbacks for fixtures that didn't explicitly disable the collision.
                    if (!Enabled)
                    {
                        IsTouching = false;
                    }
                }
            }
            else
            {
                if (touching == false)
                {
                    //Report the separation to both participants:
                    if (FixtureA != null && FixtureA.OnSeparation != null)
                    {
                        FixtureA.OnSeparation(FixtureA, FixtureB);
                    }

                    //Reverse the order of the reported fixtures. The first fixture is always the one that the
                    //user subscribed to.
                    if (FixtureB != null && FixtureB.OnSeparation != null)
                    {
                        FixtureB.OnSeparation(FixtureB, FixtureA);
                    }

                    if (contactManager.EndContact != null)
                    {
                        contactManager.EndContact(this);
                    }
                }
                else
                {
                    if (contactManager.StayContact != null)
                    {
                        contactManager.StayContact(this);
                    }
                }
            }

            if (sensor)
            {
                return;
            }

            if (contactManager.PreSolve != null)
            {
                contactManager.PreSolve(this, ref oldManifold);
            }
        }
        public void Reset(TimeStep step, int count, Contact[] contacts, Position[] positions, Velocity[] velocities, bool warmstarting = Settings.EnableWarmstarting)
        {
            _step       = step;
            _count      = count;
            _positions  = positions;
            _velocities = velocities;
            _contacts   = contacts;

            // grow the array
            if (_velocityConstraints == null || _velocityConstraints.Length < count)
            {
                _velocityConstraints = new ContactVelocityConstraint[count * 2];
                _positionConstraints = new ContactPositionConstraint[count * 2];

                for (int i = 0; i < _velocityConstraints.Length; i++)
                {
                    _velocityConstraints[i] = new ContactVelocityConstraint();
                }

                for (int i = 0; i < _positionConstraints.Length; i++)
                {
                    _positionConstraints[i] = new ContactPositionConstraint();
                }
            }

            // Initialize position independent portions of the constraints.
            for (int i = 0; i < _count; ++i)
            {
                Contact contact = contacts[i];

                Fixture  fixtureA = contact.FixtureA;
                Fixture  fixtureB = contact.FixtureB;
                Shape    shapeA   = fixtureA.Shape;
                Shape    shapeB   = fixtureB.Shape;
                FP       radiusA  = shapeA.Radius;
                FP       radiusB  = shapeB.Radius;
                Body     bodyA    = fixtureA.Body;
                Body     bodyB    = fixtureB.Body;
                Manifold manifold = contact.Manifold;

                int pointCount = manifold.PointCount;
                Debug.Assert(pointCount > 0);

                ContactVelocityConstraint vc = _velocityConstraints[i];
                vc.friction     = contact.Friction;
                vc.restitution  = contact.Restitution;
                vc.tangentSpeed = contact.TangentSpeed;
                vc.indexA       = bodyA.IslandIndex;
                vc.indexB       = bodyB.IslandIndex;
                vc.invMassA     = bodyA._invMass;
                vc.invMassB     = bodyB._invMass;
                vc.invIA        = bodyA._invI;
                vc.invIB        = bodyB._invI;
                vc.contactIndex = i;
                vc.pointCount   = pointCount;
                vc.K.SetZero();
                vc.normalMass.SetZero();

                ContactPositionConstraint pc = _positionConstraints[i];
                pc.indexA       = bodyA.IslandIndex;
                pc.indexB       = bodyB.IslandIndex;
                pc.invMassA     = bodyA._invMass;
                pc.invMassB     = bodyB._invMass;
                pc.localCenterA = bodyA._sweep.LocalCenter;
                pc.localCenterB = bodyB._sweep.LocalCenter;
                pc.invIA        = bodyA._invI;
                pc.invIB        = bodyB._invI;
                pc.localNormal  = manifold.LocalNormal;
                pc.localPoint   = manifold.LocalPoint;
                pc.pointCount   = pointCount;
                pc.radiusA      = radiusA;
                pc.radiusB      = radiusB;
                pc.type         = manifold.Type;

                for (int j = 0; j < pointCount; ++j)
                {
                    ManifoldPoint           cp  = manifold.Points[j];
                    VelocityConstraintPoint vcp = vc.points[j];

                    if (Settings.EnableWarmstarting)
                    {
                        vcp.normalImpulse  = _step.dtRatio * cp.NormalImpulse;
                        vcp.tangentImpulse = _step.dtRatio * cp.TangentImpulse;
                    }
                    else
                    {
                        vcp.normalImpulse  = 0.0f;
                        vcp.tangentImpulse = 0.0f;
                    }

                    vcp.rA           = TSVector2.zero;
                    vcp.rB           = TSVector2.zero;
                    vcp.normalMass   = 0.0f;
                    vcp.tangentMass  = 0.0f;
                    vcp.velocityBias = 0.0f;

                    pc.localPoints[j] = cp.LocalPoint;
                }
            }
        }
예제 #6
0
        internal void Update(ContactManager contactManager)
        {
            Body body  = this.FixtureA.Body;
            Body body2 = this.FixtureB.Body;
            bool flag  = this.FixtureA == null || this.FixtureB == null;

            if (!flag)
            {
                bool flag2 = !ContactManager.CheckCollisionConditions(this.FixtureA, this.FixtureB);
                if (flag2)
                {
                    this.Enabled = false;
                }
                else
                {
                    Manifold manifold = this.Manifold;
                    this.Enabled = true;
                    bool isTouching = this.IsTouching;
                    bool flag3      = this.FixtureA.IsSensor || this.FixtureB.IsSensor;
                    bool flag4      = flag3;
                    bool flag5;
                    if (flag4)
                    {
                        Shape shape  = this.FixtureA.Shape;
                        Shape shape2 = this.FixtureB.Shape;
                        flag5 = Collision.TestOverlap(shape, this.ChildIndexA, shape2, this.ChildIndexB, ref body._xf, ref body2._xf);
                        this.Manifold.PointCount = 0;
                    }
                    else
                    {
                        this.Evaluate(ref this.Manifold, ref body._xf, ref body2._xf);
                        flag5 = (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;
                            ContactID id = manifoldPoint.Id;
                            for (int j = 0; j < manifold.PointCount; j++)
                            {
                                ManifoldPoint manifoldPoint2 = manifold.Points[j];
                                bool          flag6          = manifoldPoint2.Id.Key == id.Key;
                                if (flag6)
                                {
                                    manifoldPoint.NormalImpulse  = manifoldPoint2.NormalImpulse;
                                    manifoldPoint.TangentImpulse = manifoldPoint2.TangentImpulse;
                                    break;
                                }
                            }
                            this.Manifold.Points[i] = manifoldPoint;
                        }
                        bool flag7 = flag5 != isTouching;
                        if (flag7)
                        {
                            body.Awake  = true;
                            body2.Awake = true;
                        }
                    }
                    this.IsTouching = flag5;
                    bool flag8 = !isTouching;
                    if (flag8)
                    {
                        bool flag9 = flag5;
                        if (flag9)
                        {
                            bool flag10 = true;
                            bool flag11 = true;
                            bool flag12 = this.FixtureA.OnCollision != null;
                            if (flag12)
                            {
                                Delegate[] invocationList = this.FixtureA.OnCollision.GetInvocationList();
                                for (int k = 0; k < invocationList.Length; k++)
                                {
                                    OnCollisionEventHandler onCollisionEventHandler = (OnCollisionEventHandler)invocationList[k];
                                    flag10 = (onCollisionEventHandler(this.FixtureA, this.FixtureB, this) & flag10);
                                }
                            }
                            bool flag13 = this.FixtureB.OnCollision != null;
                            if (flag13)
                            {
                                Delegate[] invocationList2 = this.FixtureB.OnCollision.GetInvocationList();
                                for (int l = 0; l < invocationList2.Length; l++)
                                {
                                    OnCollisionEventHandler onCollisionEventHandler2 = (OnCollisionEventHandler)invocationList2[l];
                                    flag11 = (onCollisionEventHandler2(this.FixtureB, this.FixtureA, this) & flag11);
                                }
                            }
                            this.Enabled = (flag10 & flag11);
                            bool flag14 = (flag10 & flag11) && contactManager.BeginContact != null;
                            if (flag14)
                            {
                                this.Enabled = contactManager.BeginContact(this);
                            }
                            bool flag15 = !this.Enabled;
                            if (flag15)
                            {
                                this.IsTouching = false;
                            }
                        }
                    }
                    else
                    {
                        bool flag16 = !flag5;
                        if (flag16)
                        {
                            bool flag17 = this.FixtureA != null && this.FixtureA.OnSeparation != null;
                            if (flag17)
                            {
                                this.FixtureA.OnSeparation(this.FixtureA, this.FixtureB);
                            }
                            bool flag18 = this.FixtureB != null && this.FixtureB.OnSeparation != null;
                            if (flag18)
                            {
                                this.FixtureB.OnSeparation(this.FixtureB, this.FixtureA);
                            }
                            bool flag19 = contactManager.EndContact != null;
                            if (flag19)
                            {
                                contactManager.EndContact(this);
                            }
                        }
                        else
                        {
                            bool flag20 = contactManager.StayContact != null;
                            if (flag20)
                            {
                                contactManager.StayContact(this);
                            }
                        }
                    }
                    bool flag21 = flag3;
                    if (!flag21)
                    {
                        bool flag22 = contactManager.PreSolve != null;
                        if (flag22)
                        {
                            contactManager.PreSolve(this, ref manifold);
                        }
                    }
                }
            }
        }