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);

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

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

                    FPVector 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 FPMatrix.InternalIdentity,
                                                    ref rigidBody.position, ref FPVector.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
            {
                List <int> detected = potentialTriangleLists.GetNew();
                softBody.dynamicTree.Query(detected, ref rigidBody.boundingBox);

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

                    FPVector point, normal;
                    FP       penetration;
                    bool     result;

                    result = XenoCollide.Detect(rigidBody.Shape, t, ref rigidBody.orientation, ref FPMatrix.InternalIdentity,
                                                ref rigidBody.position, ref FPVector.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);

            FPVector 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);
                    FPVector 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)
                {
                    FPVector 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))
                    {
                        FPVector 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))
                        {
                            FPVector 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)
                        {
                            FPVector 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))
                            {
                                FPVector 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))
                    {
                        FPVector 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);
                            FPVector.Transform(ref normal, ref b1.orientation, out normal);
                        }
                        else if (useTriangleMeshNormal && ms is TriangleMeshShape)
                        {
                            (ms as TriangleMeshShape).CollisionNormal(out normal);
                            FPVector.Transform(ref normal, ref b1.orientation, out normal);
                        }

                        RaiseCollisionDetected(b1, b2, ref point1, ref point2, ref normal, penetration);
                    }
                    else if (speculative)
                    {
                        FPVector 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))
                        {
                            FPVector 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();
            }
        }