Example #1
0
        private void DetectRigidRigid(RigidBody body1, RigidBody body2)
        {
            bool b1IsMulti = (body1.Shape is Multishape);
            bool b2IsMulti = (body2.Shape is Multishape);

            bool speculative = speculativeContacts ||
                               (body1.EnableSpeculativeContacts || body2.EnableSpeculativeContacts);

            TSVector point, normal;
            FP       penetration;

            if (!b1IsMulti && !b2IsMulti)
            {
                TSVector  point1, point2;
                ShapeType type = body1.shape.shapeType | body2.shape.shapeType;
                switch (type)
                {
                case ShapeType.Sphere | ShapeType.Sphere:
                    SphereSpherePair sphereSpherePair = SphereSpherePair.pool.GetNew();
                    sphereSpherePair.Shape1 = body1.Shape;
                    sphereSpherePair.Shape2 = body2.Shape;

                    if (sphereSpherePair.IsColliding(ref body1.orientation, ref body2.orientation, ref body1.position, ref body2.position,
                                                     out point, out point1, out point2, out normal, out penetration))
                    {
                        RaiseCollisionDetected(body1, body2, ref point1, ref point2, ref normal, penetration);
                    }
                    SphereSpherePair.pool.GiveBack(sphereSpherePair);
                    break;

                case ShapeType.Box | ShapeType.Sphere:
                    BoxSpherePair boxSpherePair = BoxSpherePair.pool.GetNew();

                    RigidBody b1, b2;
                    if (body1.Shape is BoxShape)
                    {
                        b1 = body1;
                        b2 = body2;
                    }
                    else
                    {
                        b1 = body2;
                        b2 = body1;
                    }
                    boxSpherePair.Shape1 = b1.Shape;
                    boxSpherePair.Shape2 = b2.Shape;

                    if (boxSpherePair.IsColliding(ref b1.orientation, ref b2.orientation, ref b1.position, ref b2.position,
                                                  out point, out point1, out point2, out normal, out penetration))
                    {
                        RaiseCollisionDetected(b1, b2, ref point1, ref point2, ref normal, penetration);
                    }
                    BoxSpherePair.pool.GiveBack(boxSpherePair);
                    break;

                case ShapeType.Capusle | ShapeType.Capusle:
                    CapsuleCapsulePair capsuleCapsulePair = CapsuleCapsulePair.pool.GetNew();
                    capsuleCapsulePair.Shape1 = body1.Shape;
                    capsuleCapsulePair.Shape2 = body2.Shape;

                    if (capsuleCapsulePair.IsColliding(ref body1.orientation, ref body2.orientation, ref body1.position, ref body2.position,
                                                       out point, out point1, out point2, out normal, out penetration))
                    {
                        RaiseCollisionDetected(body1, body2, ref point1, ref point2, ref normal, penetration);
                    }
                    CapsuleCapsulePair.pool.GiveBack(capsuleCapsulePair);
                    break;

                case ShapeType.Capusle | ShapeType.Sphere:
                    CapsuleSpherePair capsuleSpherePair = CapsuleSpherePair.pool.GetNew();

                    if (body1.Shape is CapsuleShape)
                    {
                        b1 = body1;
                        b2 = body2;
                    }
                    else
                    {
                        b1 = body2;
                        b2 = body1;
                    }
                    capsuleSpherePair.Shape1 = b1.Shape;
                    capsuleSpherePair.Shape2 = b2.Shape;

                    if (capsuleSpherePair.IsColliding(ref b1.orientation, ref b2.orientation, ref b1.position, ref b2.position,
                                                      out point, out point1, out point2, out normal, out penetration))
                    {
                        RaiseCollisionDetected(b1, b2, ref point1, ref point2, ref normal, penetration);
                    }
                    CapsuleSpherePair.pool.GiveBack(capsuleSpherePair);
                    break;

                case ShapeType.Box | ShapeType.Box:
                //BoxBoxPair boxBoxPair = BoxBoxPair.pool.GetNew();
                //boxBoxPair.Shape1 = body1.Shape;
                //boxBoxPair.Shape2 = body2.Shape;

                //if (boxBoxPair.IsColliding(ref body1.orientation, ref body2.orientation, ref body1.position, ref body2.position,
                //    out point, out point1, out point2, out normal, out penetration))
                //{
                //    RaiseCollisionDetected(body1, body2, ref point1, ref point2, ref normal, penetration);
                //}
                //BoxBoxPair.pool.GiveBack(boxBoxPair);
                //break;
                default:
                    if (XenoCollide.Detect(body1.Shape, body2.Shape, ref body1.orientation,
                                           ref body2.orientation, ref body1.position, ref body2.position,
                                           out point, out normal, out penetration))
                    {
                        //normal = JVector.Up;
                        //UnityEngine.Debug.Log("FINAL  --- >>> normal: " + normal);
                        FindSupportPoints(body1, body2, body1.Shape, body2.Shape, ref point, ref normal, out point1, out point2);
                        RaiseCollisionDetected(body1, body2, ref point1, ref point2, ref normal, penetration);
                    }
                    else if (speculative)
                    {
                        TSVector hit1, hit2;

                        if (GJKCollide.ClosestPoints(body1.Shape, body2.Shape, ref body1.orientation, ref body2.orientation,
                                                     ref body1.position, ref body2.position, out hit1, out hit2, out normal))
                        {
                            TSVector delta = hit2 - hit1;

                            if (delta.sqrMagnitude < (body1.sweptDirection - body2.sweptDirection).sqrMagnitude)
                            {
                                penetration = delta * normal;

                                if (penetration < FP.Zero)
                                {
                                    RaiseCollisionDetected(body1, body2, ref hit1, ref hit2, ref normal, penetration);
                                }
                            }
                        }
                    }
                    break;
                }



                //UnityEngine.Debug.Log("-----------------------: " + normal);
            }
            else if (b1IsMulti && b2IsMulti)
            {
                Multishape ms1 = (body1.Shape as Multishape);
                Multishape ms2 = (body2.Shape as Multishape);

                ms1 = ms1.RequestWorkingClone();
                ms2 = ms2.RequestWorkingClone();

                TSBBox transformedBoundingBox = body2.boundingBox;
                transformedBoundingBox.InverseTransform(ref body1.position, ref body1.orientation);

                int ms1Length = ms1.Prepare(ref transformedBoundingBox);

                transformedBoundingBox = body1.boundingBox;
                transformedBoundingBox.InverseTransform(ref body2.position, ref body2.orientation);

                int ms2Length = ms2.Prepare(ref transformedBoundingBox);

                if (ms1Length == 0 || ms2Length == 0)
                {
                    ms1.ReturnWorkingClone();
                    ms2.ReturnWorkingClone();
                    return;
                }

                for (int i = 0; i < ms1Length; i++)
                {
                    ms1.SetCurrentShape(i);

                    for (int e = 0; e < ms2Length; e++)
                    {
                        ms2.SetCurrentShape(e);

                        if (XenoCollide.Detect(ms1, ms2, ref body1.orientation,
                                               ref body2.orientation, ref body1.position, ref body2.position,
                                               out point, out normal, out penetration))
                        {
                            TSVector point1, point2;
                            FindSupportPoints(body1, body2, ms1, ms2, ref point, ref normal, out point1, out point2);
                            RaiseCollisionDetected(body1, body2, ref point1, ref point2, ref normal, penetration);
                        }
                        else if (speculative)
                        {
                            TSVector hit1, hit2;

                            if (GJKCollide.ClosestPoints(ms1, ms2, ref body1.orientation, ref body2.orientation,
                                                         ref body1.position, ref body2.position, out hit1, out hit2, out normal))
                            {
                                TSVector delta = hit2 - hit1;

                                if (delta.sqrMagnitude < (body1.sweptDirection - body2.sweptDirection).sqrMagnitude)
                                {
                                    penetration = delta * normal;

                                    if (penetration < FP.Zero)
                                    {
                                        RaiseCollisionDetected(body1, body2, ref hit1, ref hit2, ref normal, penetration);
                                    }
                                }
                            }
                        }
                    }
                }

                ms1.ReturnWorkingClone();
                ms2.ReturnWorkingClone();
            }
            else
            {
                RigidBody b1, b2;

                if (body2.Shape is Multishape)
                {
                    b1 = body2;
                    b2 = body1;
                }
                else
                {
                    b2 = body2;
                    b1 = body1;
                }

                Multishape ms = (b1.Shape as Multishape);

                ms = ms.RequestWorkingClone();

                TSBBox transformedBoundingBox = b2.boundingBox;
                transformedBoundingBox.InverseTransform(ref b1.position, ref b1.orientation);

                int msLength = ms.Prepare(ref transformedBoundingBox);

                if (msLength == 0)
                {
                    ms.ReturnWorkingClone();
                    return;
                }

                ShapeType type = body1.shape.shapeType | body2.shape.shapeType;
                switch (type)
                {
                case ShapeType.Sphere | ShapeType.TriangleMesh:
                    for (int i = 0; i < msLength; i++)
                    {
                        ms.SetCurrentShape(i);
                        TSVector           point1, point2;
                        SphereTrianglePair pair = SphereTrianglePair.pool.GetNew();
                        pair.Shape1 = ms;
                        pair.Shape2 = b2.shape;
                        if (pair.IsColliding(ref b1.orientation, ref b2.orientation, ref b1.position, ref b2.position,
                                             out point, out point1, out point2, out normal, out penetration))
                        {
                            RaiseCollisionDetected(b1, b2, ref point1, ref point2, ref normal, penetration);
                        }
                        SphereTrianglePair.pool.GiveBack(pair);
                    }
                    break;

                case ShapeType.Box | ShapeType.TriangleMesh:
                default:
                    for (int i = 0; i < msLength; i++)
                    {
                        ms.SetCurrentShape(i);

                        if (XenoCollide.Detect(ms, b2.Shape, ref b1.orientation,
                                               ref b2.orientation, ref b1.position, ref b2.position,
                                               out point, out normal, out penetration))
                        {
                            TSVector point1, point2;
                            FindSupportPoints(b1, b2, ms, b2.Shape, ref point, ref normal, out point1, out point2);

                            if (useTerrainNormal && ms is TerrainShape)
                            {
                                (ms as TerrainShape).CollisionNormal(out normal);
                                TSVector.Transform(ref normal, ref b1.orientation, out normal);
                            }
                            else if (useTriangleMeshNormal && ms is TriangleMeshShape)
                            {
                                (ms as TriangleMeshShape).CollisionNormal(out normal);
                                TSVector.Transform(ref normal, ref b1.orientation, out normal);
                            }

                            RaiseCollisionDetected(b1, b2, ref point1, ref point2, ref normal, penetration);
                        }
                        else if (speculative)
                        {
                            TSVector hit1, hit2;

                            if (GJKCollide.ClosestPoints(ms, b2.Shape, ref b1.orientation, ref b2.orientation,
                                                         ref b1.position, ref b2.position, out hit1, out hit2, out normal))
                            {
                                TSVector delta = hit2 - hit1;

                                if (delta.sqrMagnitude < (body1.sweptDirection - body2.sweptDirection).sqrMagnitude)
                                {
                                    penetration = delta * normal;

                                    if (penetration < FP.Zero)
                                    {
                                        RaiseCollisionDetected(b1, b2, ref hit1, ref hit2, ref normal, penetration);
                                    }
                                }
                            }
                        }
                    }
                    break;
                }
                ms.ReturnWorkingClone();
            }
        }
Example #2
0
        private void DetectSoftRigid(RigidBody rigidBody, SoftBody softBody)
        {
            if (rigidBody.Shape is Multishape)
            {
                Multishape ms = (rigidBody.Shape as Multishape);
                ms = ms.RequestWorkingClone();

                TSBBox transformedBoundingBox = softBody.BoundingBox;
                transformedBoundingBox.InverseTransform(ref rigidBody.position, ref rigidBody.orientation);

                int msLength = ms.Prepare(ref transformedBoundingBox);

                ResourcePoolItemList <int> detected = potentialTriangleLists.GetNew();
                softBody.dynamicTree.Query(detected, ref rigidBody.boundingBox);

                foreach (int i in detected)
                {
                    SoftBody.Triangle t = softBody.dynamicTree.GetUserData(i);

                    TSVector point, normal;
                    FP       penetration;
                    bool     result;

                    for (int e = 0; e < msLength; e++)
                    {
                        ms.SetCurrentShape(e);

                        result = XenoCollide.Detect(ms, t, ref rigidBody.orientation, ref TSMatrix.InternalIdentity,
                                                    ref rigidBody.position, ref TSVector.InternalZero, out point, out normal, out penetration);

                        if (result)
                        {
                            int minIndex = FindNearestTrianglePoint(softBody, i, ref point);

                            RaiseCollisionDetected(rigidBody,
                                                   softBody.VertexBodies[minIndex], ref point, ref point, ref normal, penetration);
                        }
                    }
                }

                detected.Clear(); potentialTriangleLists.GiveBack(detected);
                ms.ReturnWorkingClone();
            }
            else
            {
                ResourcePoolItemList <int> detected = potentialTriangleLists.GetNew();
                softBody.dynamicTree.Query(detected, ref rigidBody.boundingBox);

                foreach (int i in detected)
                {
                    SoftBody.Triangle t = softBody.dynamicTree.GetUserData(i);

                    TSVector point, normal;
                    FP       penetration;
                    bool     result;

                    result = XenoCollide.Detect(rigidBody.Shape, t, ref rigidBody.orientation, ref TSMatrix.InternalIdentity,
                                                ref rigidBody.position, ref TSVector.InternalZero, out point, out normal, out penetration);

                    if (result)
                    {
                        int minIndex = FindNearestTrianglePoint(softBody, i, ref point);

                        RaiseCollisionDetected(rigidBody,
                                               softBody.VertexBodies[minIndex], ref point, ref point, ref normal, penetration);
                    }
                }

                detected.Clear();
                potentialTriangleLists.GiveBack(detected);
            }
        }
        private void DetectRigidRigid(RigidBody body1, RigidBody body2)
        {
            bool b1IsMulti = (body1.Shape is Multishape);
            bool b2IsMulti = (body2.Shape is Multishape);

            bool speculative = speculativeContacts ||
                               (body1.EnableSpeculativeContacts || body2.EnableSpeculativeContacts);

            TSVector point, normal;
            FP       penetration;

            if (!b1IsMulti && !b2IsMulti)
            {
                if (XenoCollide.Detect(body1.Shape, body2.Shape, ref body1.orientation,
                                       ref body2.orientation, ref body1.position, ref body2.position,
                                       out point, out normal, out penetration))
                {
                    //normal = JVector.Up;
                    //UnityEngine.Debug.Log("FINAL  --- >>> normal: " + normal);
                    TSVector point1, point2;
                    FindSupportPoints(body1, body2, body1.Shape, body2.Shape, ref point, ref normal, out point1, out point2);
                    RaiseCollisionDetected(body1, body2, ref point1, ref point2, ref normal, penetration);
                }
                else if (speculative)
                {
                    TSVector hit1, hit2;

                    if (GJKCollide.ClosestPoints(body1.Shape, body2.Shape, ref body1.orientation, ref body2.orientation,
                                                 ref body1.position, ref body2.position, out hit1, out hit2, out normal))
                    {
                        TSVector delta = hit2 - hit1;

                        if (delta.sqrMagnitude < (body1.sweptDirection - body2.sweptDirection).sqrMagnitude)
                        {
                            penetration = delta * normal;

                            if (penetration < FP.Zero)
                            {
                                RaiseCollisionDetected(body1, body2, ref hit1, ref hit2, ref normal, penetration);
                            }
                        }
                    }
                }

                //UnityEngine.Debug.Log("-----------------------: " + normal);
            }
            else if (b1IsMulti && b2IsMulti)
            {
                Multishape ms1 = (body1.Shape as Multishape);
                Multishape ms2 = (body2.Shape as Multishape);

                ms1 = ms1.RequestWorkingClone();
                ms2 = ms2.RequestWorkingClone();

                TSBBox transformedBoundingBox = body2.boundingBox;
                transformedBoundingBox.InverseTransform(ref body1.position, ref body1.orientation);

                int ms1Length = ms1.Prepare(ref transformedBoundingBox);

                transformedBoundingBox = body1.boundingBox;
                transformedBoundingBox.InverseTransform(ref body2.position, ref body2.orientation);

                int ms2Length = ms2.Prepare(ref transformedBoundingBox);

                if (ms1Length == 0 || ms2Length == 0)
                {
                    ms1.ReturnWorkingClone();
                    ms2.ReturnWorkingClone();
                    return;
                }

                for (int i = 0; i < ms1Length; i++)
                {
                    ms1.SetCurrentShape(i);

                    for (int e = 0; e < ms2Length; e++)
                    {
                        ms2.SetCurrentShape(e);

                        if (XenoCollide.Detect(ms1, ms2, ref body1.orientation,
                                               ref body2.orientation, ref body1.position, ref body2.position,
                                               out point, out normal, out penetration))
                        {
                            TSVector point1, point2;
                            FindSupportPoints(body1, body2, ms1, ms2, ref point, ref normal, out point1, out point2);
                            RaiseCollisionDetected(body1, body2, ref point1, ref point2, ref normal, penetration);
                        }
                        else if (speculative)
                        {
                            TSVector hit1, hit2;

                            if (GJKCollide.ClosestPoints(ms1, ms2, ref body1.orientation, ref body2.orientation,
                                                         ref body1.position, ref body2.position, out hit1, out hit2, out normal))
                            {
                                TSVector delta = hit2 - hit1;

                                if (delta.sqrMagnitude < (body1.sweptDirection - body2.sweptDirection).sqrMagnitude)
                                {
                                    penetration = delta * normal;

                                    if (penetration < FP.Zero)
                                    {
                                        RaiseCollisionDetected(body1, body2, ref hit1, ref hit2, ref normal, penetration);
                                    }
                                }
                            }
                        }
                    }
                }

                ms1.ReturnWorkingClone();
                ms2.ReturnWorkingClone();
            }
            else
            {
                RigidBody b1, b2;

                if (body2.Shape is Multishape)
                {
                    b1 = body2; b2 = body1;
                }
                else
                {
                    b2 = body2; b1 = body1;
                }

                Multishape ms = (b1.Shape as Multishape);

                ms = ms.RequestWorkingClone();

                TSBBox transformedBoundingBox = b2.boundingBox;
                transformedBoundingBox.InverseTransform(ref b1.position, ref b1.orientation);

                int msLength = ms.Prepare(ref transformedBoundingBox);

                if (msLength == 0)
                {
                    ms.ReturnWorkingClone();
                    return;
                }

                for (int i = 0; i < msLength; i++)
                {
                    ms.SetCurrentShape(i);

                    if (XenoCollide.Detect(ms, b2.Shape, ref b1.orientation,
                                           ref b2.orientation, ref b1.position, ref b2.position,
                                           out point, out normal, out penetration))
                    {
                        TSVector point1, point2;
                        FindSupportPoints(b1, b2, ms, b2.Shape, ref point, ref normal, out point1, out point2);

                        if (useTerrainNormal && ms is TerrainShape)
                        {
                            (ms as TerrainShape).CollisionNormal(out normal);
                            TSVector.Transform(ref normal, ref b1.orientation, out normal);
                        }
                        else if (useTriangleMeshNormal && ms is TriangleMeshShape)
                        {
                            (ms as TriangleMeshShape).CollisionNormal(out normal);
                            TSVector.Transform(ref normal, ref b1.orientation, out normal);
                        }

                        RaiseCollisionDetected(b1, b2, ref point1, ref point2, ref normal, penetration);
                    }
                    else if (speculative)
                    {
                        TSVector hit1, hit2;

                        if (GJKCollide.ClosestPoints(ms, b2.Shape, ref b1.orientation, ref b2.orientation,
                                                     ref b1.position, ref b2.position, out hit1, out hit2, out normal))
                        {
                            TSVector delta = hit2 - hit1;

                            if (delta.sqrMagnitude < (body1.sweptDirection - body2.sweptDirection).sqrMagnitude)
                            {
                                penetration = delta * normal;

                                if (penetration < FP.Zero)
                                {
                                    RaiseCollisionDetected(b1, b2, ref hit1, ref hit2, ref normal, penetration);
                                }
                            }
                        }
                    }
                }

                ms.ReturnWorkingClone();
            }
        }