Example #1
0
        /// <summary>
        /// Inverses the direction of a vector.
        /// </summary>
        /// <param name="value">The vector to inverse.</param>
        /// <returns>The negated vector.</returns>
        public static TSVector Negate(TSVector value)
        {
            TSVector result;

            TSVector.Negate(ref value, out result);
            return(result);
        }
Example #2
0
        public override void GetBoundingBox(ref TSMatrix orientation, out TSBBox box)
        {
            TSMatrix tSMatrix;

            TSMath.Absolute(ref orientation, out tSMatrix);
            TSVector max;

            TSVector.Transform(ref this.halfSize, ref tSMatrix, out max);
            box.max = max;
            TSVector.Negate(ref max, out box.min);
        }
Example #3
0
        public static bool ClosestPoints(ISupportMappable support1, ISupportMappable support2, ref TSMatrix orientation1, ref TSMatrix orientation2, ref TSVector position1, ref TSVector position2, out TSVector p1, out TSVector p2, out TSVector normal)
        {
            VoronoiSimplexSolver @new = GJKCollide.simplexSolverPool.GetNew();

            @new.Reset();
            p1 = (p2 = TSVector.zero);
            TSVector value    = position1 - position2;
            TSVector tSVector = TSVector.Negate(value);
            TSVector tSVector2;

            GJKCollide.SupportMapTransformed(support1, ref orientation1, ref position1, ref tSVector, out tSVector2);
            TSVector tSVector3;

            GJKCollide.SupportMapTransformed(support2, ref orientation2, ref position2, ref value, out tSVector3);
            TSVector tSVector4 = tSVector2 - tSVector3;

            normal = TSVector.zero;
            int num = 15;
            FP  x   = tSVector4.sqrMagnitude;
            FP  eN  = FP.EN5;

            while (x > eN && num-- != 0)
            {
                TSVector tSVector5 = TSVector.Negate(tSVector4);
                GJKCollide.SupportMapTransformed(support1, ref orientation1, ref position1, ref tSVector5, out tSVector2);
                GJKCollide.SupportMapTransformed(support2, ref orientation2, ref position2, ref tSVector4, out tSVector3);
                TSVector w    = tSVector2 - tSVector3;
                bool     flag = [email protected](w);
                if (flag)
                {
                    @new.AddVertex(w, tSVector2, tSVector3);
                }
                bool flag2 = @new.Closest(out tSVector4);
                if (flag2)
                {
                    x      = tSVector4.sqrMagnitude;
                    normal = tSVector4;
                }
                else
                {
                    x = FP.Zero;
                }
            }
            @new.ComputePoints(out p1, out p2);
            bool flag3 = normal.sqrMagnitude > TSMath.Epsilon * TSMath.Epsilon;

            if (flag3)
            {
                normal.Normalize();
            }
            GJKCollide.simplexSolverPool.GiveBack(@new);
            return(true);
        }
Example #4
0
        private void FindSupportPoints(RigidBody body1, RigidBody body2, Shape shape1, Shape shape2, ref TSVector point, ref TSVector normal, out TSVector point1, out TSVector point2)
        {
            TSVector tSVector;

            TSVector.Negate(ref normal, out tSVector);
            TSVector tSVector2;

            this.SupportMapping(body1, shape1, ref tSVector, out tSVector2);
            TSVector tSVector3;

            this.SupportMapping(body2, shape2, ref normal, out tSVector3);
            TSVector.Subtract(ref tSVector2, ref point, out tSVector2);
            TSVector.Subtract(ref tSVector3, ref point, out tSVector3);
            FP scaleFactor  = TSVector.Dot(ref tSVector2, ref normal);
            FP scaleFactor2 = TSVector.Dot(ref tSVector3, ref normal);

            TSVector.Multiply(ref normal, scaleFactor, out tSVector2);
            TSVector.Multiply(ref normal, scaleFactor2, out tSVector3);
            TSVector.Add(ref point, ref tSVector2, out point1);
            TSVector.Add(ref point, ref tSVector3, out point2);
        }
Example #5
0
        private void CollisionDetected(RigidBody body1, RigidBody body2, TSVector point1, TSVector point2, TSVector normal, FP penetration)
        {
            bool       flag    = body1.IsColliderOnly || body2.IsColliderOnly;
            Arbiter    arbiter = null;
            bool       flag2   = flag;
            ArbiterMap arbiterMap;

            if (flag2)
            {
                arbiterMap = this.arbiterTriggerMap;
            }
            else
            {
                arbiterMap = this.arbiterMap;
            }
            bool       flag3 = false;
            ArbiterMap obj   = arbiterMap;

            lock (obj)
            {
                arbiterMap.LookUpArbiter(body1, body2, out arbiter);
                bool flag4 = arbiter == null;
                if (flag4)
                {
                    arbiter       = Arbiter.Pool.GetNew();
                    arbiter.body1 = body1;
                    arbiter.body2 = body2;
                    arbiterMap.Add(new ArbiterKey(body1, body2), arbiter);
                    flag3 = true;
                }
            }
            bool    flag5 = arbiter.body1 == body1;
            Contact contact;

            if (flag5)
            {
                TSVector.Negate(ref normal, out normal);
                contact = arbiter.AddContact(point1, point2, normal, penetration, this.contactSettings);
            }
            else
            {
                contact = arbiter.AddContact(point2, point1, normal, penetration, this.contactSettings);
            }
            bool flag6 = flag3;

            if (flag6)
            {
                bool flag7 = flag;
                if (flag7)
                {
                    this.events.RaiseTriggerBeginCollide(contact);
                    body1.arbitersTrigger.Add(arbiter);
                    body2.arbitersTrigger.Add(arbiter);
                    OverlapPairContact overlapPairContact = new OverlapPairContact(body1, body2);
                    overlapPairContact.contact = contact;
                    this.initialTriggers.Add(overlapPairContact);
                }
                else
                {
                    this.events.RaiseBodiesBeginCollide(contact);
                    this.addedArbiterQueue.Enqueue(arbiter);
                    OverlapPairContact overlapPairContact2 = new OverlapPairContact(body1, body2);
                    overlapPairContact2.contact = contact;
                    this.initialCollisions.Add(overlapPairContact2);
                }
            }
            bool flag8 = !flag && contact != null;

            if (flag8)
            {
                this.events.RaiseContactCreated(contact);
            }
        }
Example #6
0
        public static bool Detect(ISupportMappable support1, ISupportMappable support2, ref TSMatrix orientation1, ref TSMatrix orientation2, ref TSVector position1, ref TSVector position2, out TSVector point, out TSVector normal, out FP penetration)
        {
            TSVector zero  = TSVector.zero;
            TSVector zero2 = TSVector.zero;
            TSVector zero3 = TSVector.zero;

            point       = (normal = TSVector.zero);
            penetration = FP.Zero;
            TSVector tSVector;

            support1.SupportCenter(out tSVector);
            TSVector.Transform(ref tSVector, ref orientation1, out tSVector);
            TSVector.Add(ref position1, ref tSVector, out tSVector);
            TSVector tSVector2;

            support2.SupportCenter(out tSVector2);
            TSVector.Transform(ref tSVector2, ref orientation2, out tSVector2);
            TSVector.Add(ref position2, ref tSVector2, out tSVector2);
            TSVector tSVector3;

            TSVector.Subtract(ref tSVector2, ref tSVector, out tSVector3);
            bool flag = tSVector3.IsNearlyZero();

            if (flag)
            {
                tSVector3 = new TSVector(FP.EN4, 0, 0);
            }
            TSVector tSVector4 = tSVector3;

            TSVector.Negate(ref tSVector3, out normal);
            TSVector tSVector5;

            XenoCollide.SupportMapTransformed(support1, ref orientation1, ref position1, ref tSVector4, out tSVector5);
            TSVector tSVector6;

            XenoCollide.SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out tSVector6);
            TSVector tSVector7;

            TSVector.Subtract(ref tSVector6, ref tSVector5, out tSVector7);
            bool flag2 = TSVector.Dot(ref tSVector7, ref normal) <= FP.Zero;
            bool result;

            if (flag2)
            {
                result = false;
            }
            else
            {
                TSVector.Cross(ref tSVector7, ref tSVector3, out normal);
                bool flag3 = normal.IsNearlyZero();
                if (flag3)
                {
                    TSVector.Subtract(ref tSVector7, ref tSVector3, out normal);
                    normal.Normalize();
                    point = tSVector5;
                    TSVector.Add(ref point, ref tSVector6, out point);
                    TSVector.Multiply(ref point, FP.Half, out point);
                    TSVector tSVector8;
                    TSVector.Subtract(ref tSVector6, ref tSVector5, out tSVector8);
                    penetration = TSVector.Dot(ref tSVector8, ref normal);
                    result      = true;
                }
                else
                {
                    TSVector.Negate(ref normal, out tSVector4);
                    TSVector tSVector9;
                    XenoCollide.SupportMapTransformed(support1, ref orientation1, ref position1, ref tSVector4, out tSVector9);
                    TSVector tSVector10;
                    XenoCollide.SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out tSVector10);
                    TSVector tSVector11;
                    TSVector.Subtract(ref tSVector10, ref tSVector9, out tSVector11);
                    bool flag4 = TSVector.Dot(ref tSVector11, ref normal) <= FP.Zero;
                    if (flag4)
                    {
                        result = false;
                    }
                    else
                    {
                        TSVector tSVector8;
                        TSVector.Subtract(ref tSVector7, ref tSVector3, out tSVector8);
                        TSVector tSVector12;
                        TSVector.Subtract(ref tSVector11, ref tSVector3, out tSVector12);
                        TSVector.Cross(ref tSVector8, ref tSVector12, out normal);
                        FP   x     = TSVector.Dot(ref normal, ref tSVector3);
                        bool flag5 = x > FP.Zero;
                        if (flag5)
                        {
                            TSVector.Swap(ref tSVector7, ref tSVector11);
                            TSVector.Swap(ref tSVector5, ref tSVector9);
                            TSVector.Swap(ref tSVector6, ref tSVector10);
                            TSVector.Negate(ref normal, out normal);
                            Debug.Log("normal: " + normal);
                        }
                        int  num   = 0;
                        int  num2  = 0;
                        bool flag6 = false;
                        while (true)
                        {
                            bool flag7 = num2 > 34;
                            if (flag7)
                            {
                                break;
                            }
                            num2++;
                            TSVector.Negate(ref normal, out tSVector4);
                            TSVector tSVector13;
                            XenoCollide.SupportMapTransformed(support1, ref orientation1, ref position1, ref tSVector4, out tSVector13);
                            TSVector tSVector14;
                            XenoCollide.SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out tSVector14);
                            TSVector tSVector15;
                            TSVector.Subtract(ref tSVector14, ref tSVector13, out tSVector15);
                            bool flag8 = TSVector.Dot(ref tSVector15, ref normal) <= FP.Zero;
                            if (flag8)
                            {
                                goto Block_7;
                            }
                            TSVector.Cross(ref tSVector7, ref tSVector15, out tSVector8);
                            bool flag9 = TSVector.Dot(ref tSVector8, ref tSVector3) < FP.Zero;
                            if (flag9)
                            {
                                tSVector11 = tSVector15;
                                tSVector9  = tSVector13;
                                tSVector10 = tSVector14;
                                TSVector.Subtract(ref tSVector7, ref tSVector3, out tSVector8);
                                TSVector.Subtract(ref tSVector15, ref tSVector3, out tSVector12);
                                TSVector.Cross(ref tSVector8, ref tSVector12, out normal);
                            }
                            else
                            {
                                TSVector.Cross(ref tSVector15, ref tSVector11, out tSVector8);
                                bool flag10 = TSVector.Dot(ref tSVector8, ref tSVector3) < FP.Zero;
                                if (!flag10)
                                {
                                    goto IL_385;
                                }
                                tSVector7 = tSVector15;
                                tSVector5 = tSVector13;
                                tSVector6 = tSVector14;
                                TSVector.Subtract(ref tSVector15, ref tSVector3, out tSVector8);
                                TSVector.Subtract(ref tSVector11, ref tSVector3, out tSVector12);
                                TSVector.Cross(ref tSVector8, ref tSVector12, out normal);
                            }
                        }
                        result = false;
                        return(result);

Block_7:
                        result = false;
                        return(result);

IL_385:
                        while (true)
                        {
                            num++;
                            TSVector.Subtract(ref tSVector11, ref tSVector7, out tSVector8);
                            TSVector tSVector15;
                            TSVector.Subtract(ref tSVector15, ref tSVector7, out tSVector12);
                            TSVector.Cross(ref tSVector8, ref tSVector12, out normal);
                            bool flag11 = normal.IsNearlyZero();
                            if (flag11)
                            {
                                break;
                            }
                            normal.Normalize();
                            FP   x2     = TSVector.Dot(ref normal, ref tSVector7);
                            bool flag12 = x2 >= 0 && !flag6;
                            if (flag12)
                            {
                                flag6 = true;
                            }
                            TSVector.Negate(ref normal, out tSVector4);
                            XenoCollide.SupportMapTransformed(support1, ref orientation1, ref position1, ref tSVector4, out zero);
                            XenoCollide.SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out zero2);
                            TSVector.Subtract(ref zero2, ref zero, out zero3);
                            TSVector.Subtract(ref zero3, ref tSVector15, out tSVector8);
                            FP x3 = TSVector.Dot(ref tSVector8, ref normal);
                            penetration = TSVector.Dot(ref zero3, ref normal);
                            bool flag13 = x3 <= XenoCollide.CollideEpsilon || penetration <= FP.Zero || num > 34;
                            if (flag13)
                            {
                                goto Block_15;
                            }
                            TSVector.Cross(ref zero3, ref tSVector3, out tSVector8);
                            FP   x4     = TSVector.Dot(ref tSVector8, ref tSVector7);
                            bool flag14 = x4 >= FP.Zero;
                            if (flag14)
                            {
                                x4 = TSVector.Dot(ref tSVector8, ref tSVector11);
                                bool flag15 = x4 >= FP.Zero;
                                if (flag15)
                                {
                                    tSVector7 = zero3;
                                    tSVector5 = zero;
                                    tSVector6 = zero2;
                                }
                                else
                                {
                                    tSVector15 = zero3;
                                    TSVector tSVector13 = zero;
                                    TSVector tSVector14 = zero2;
                                }
                            }
                            else
                            {
                                x4 = TSVector.Dot(ref tSVector8, ref tSVector15);
                                bool flag16 = x4 >= FP.Zero;
                                if (flag16)
                                {
                                    tSVector11 = zero3;
                                    tSVector9  = zero;
                                    tSVector10 = zero2;
                                }
                                else
                                {
                                    tSVector7 = zero3;
                                    tSVector5 = zero;
                                    tSVector6 = zero2;
                                }
                            }
                        }
                        result = true;
                        return(result);

Block_15:
                        bool flag17 = flag6;
                        if (flag17)
                        {
                            TSVector.Cross(ref tSVector7, ref tSVector11, out tSVector8);
                            TSVector tSVector15;
                            FP       fP = TSVector.Dot(ref tSVector8, ref tSVector15);
                            TSVector.Cross(ref tSVector15, ref tSVector11, out tSVector8);
                            FP fP2 = TSVector.Dot(ref tSVector8, ref tSVector3);
                            TSVector.Cross(ref tSVector3, ref tSVector7, out tSVector8);
                            FP fP3 = TSVector.Dot(ref tSVector8, ref tSVector15);
                            TSVector.Cross(ref tSVector11, ref tSVector7, out tSVector8);
                            FP   fP4    = TSVector.Dot(ref tSVector8, ref tSVector3);
                            FP   fP5    = fP + fP2 + fP3 + fP4;
                            bool flag18 = fP5 <= 0;
                            if (flag18)
                            {
                                fP = 0;
                                TSVector.Cross(ref tSVector11, ref tSVector15, out tSVector8);
                                fP2 = TSVector.Dot(ref tSVector8, ref normal);
                                TSVector.Cross(ref tSVector15, ref tSVector7, out tSVector8);
                                fP3 = TSVector.Dot(ref tSVector8, ref normal);
                                TSVector.Cross(ref tSVector7, ref tSVector11, out tSVector8);
                                fP4 = TSVector.Dot(ref tSVector8, ref normal);
                                fP5 = fP2 + fP3 + fP4;
                            }
                            FP x5 = FP.One / fP5;
                            TSVector.Multiply(ref tSVector, fP, out point);
                            TSVector.Multiply(ref tSVector5, fP2, out tSVector8);
                            TSVector.Add(ref point, ref tSVector8, out point);
                            TSVector.Multiply(ref tSVector9, fP3, out tSVector8);
                            TSVector.Add(ref point, ref tSVector8, out point);
                            TSVector tSVector13;
                            TSVector.Multiply(ref tSVector13, fP4, out tSVector8);
                            TSVector.Add(ref point, ref tSVector8, out point);
                            TSVector.Multiply(ref tSVector2, fP, out tSVector12);
                            TSVector.Add(ref tSVector12, ref point, out point);
                            TSVector.Multiply(ref tSVector6, fP2, out tSVector8);
                            TSVector.Add(ref point, ref tSVector8, out point);
                            TSVector.Multiply(ref tSVector10, fP3, out tSVector8);
                            TSVector.Add(ref point, ref tSVector8, out point);
                            TSVector tSVector14;
                            TSVector.Multiply(ref tSVector14, fP4, out tSVector8);
                            TSVector.Add(ref point, ref tSVector8, out point);
                            TSVector.Multiply(ref point, x5 * FP.Half, out point);
                        }
                        result = flag6;
                    }
                }
            }
            return(result);
        }