public void GetClosestPoints(ref ClosestPointInput input, IDiscreteCollisionDetectorInterfaceResult output, IDebugDraw debugDraw, bool swapResults)
        {
            IndexedMatrix transformA = input.m_transformA;
            IndexedMatrix transformB = input.m_transformB;

            IndexedVector3 point, normal;
            float          timeOfImpact = 1f;
            float          depth        = 0f;
            //	output.m_distance = float(1e30);
            //move sphere into triangle space
            IndexedMatrix sphereInTr = transformB.InverseTimes(ref transformA);

            IndexedVector3 temp = sphereInTr._origin;

            if (Collide(ref temp, out point, out normal, ref depth, ref timeOfImpact, m_contactBreakingThreshold))
            {
                if (swapResults)
                {
                    IndexedVector3 normalOnB = transformB._basis * normal;
                    IndexedVector3 normalOnA = -normalOnB;
                    IndexedVector3 pointOnA  = transformB * point + normalOnB * depth;
                    output.AddContactPoint(ref normalOnA, ref pointOnA, depth);
                }
                else
                {
                    IndexedVector3 p  = transformB._basis * normal;
                    IndexedVector3 p2 = transformB * point;
                    output.AddContactPoint(ref p, ref p2, depth);
                }
            }
        }
        public static void ClipHullAgainstHull(ref IndexedVector3 separatingNormal1, ConvexPolyhedron hullA, ConvexPolyhedron hullB, ref IndexedMatrix transA, ref IndexedMatrix transB, float minDist, float maxDist, IDiscreteCollisionDetectorInterfaceResult resultOut)
        {

            IndexedVector3 separatingNormal = separatingNormal1.Normalized();
            IndexedVector3 c0 = transA * hullA.m_localCenter;
            IndexedVector3 c1 = transB * hullB.m_localCenter;
            IndexedVector3 DeltaC2 = c0 - c1;
            float curMaxDist = maxDist;
            int closestFaceB = -1;
            float dmax = float.MinValue;

            {
                for (int face = 0; face < hullB.m_faces.Count; face++)
                {
                    IndexedVector3 Normal = new IndexedVector3(hullB.m_faces[face].m_plane[0], hullB.m_faces[face].m_plane[1], hullB.m_faces[face].m_plane[2]);
                    IndexedVector3 WorldNormal = transB._basis * Normal;

                    float d = IndexedVector3.Dot(WorldNormal, separatingNormal);
                    if (d > dmax)
                    {
                        dmax = d;
                        closestFaceB = face;
                    }
                }
            }


            // setup initial clip face (minimizing face from hull B)
            ObjectArray<IndexedVector3> worldVertsB1 = new ObjectArray<IndexedVector3>();
            {
                Face polyB = hullB.m_faces[closestFaceB];
                int numVertices = polyB.m_indices.Count;
                for (int e0 = 0; e0 < numVertices; e0++)
                {
                    IndexedVector3 b = hullB.m_vertices[polyB.m_indices[e0]];
                    // check this to see if it is transposed version
                    worldVertsB1.Add(transB * b);
                }
            }
            if (closestFaceB >= 0)
            {
                ClipFaceAgainstHull(ref separatingNormal, hullA, ref transA, worldVertsB1, minDist, maxDist, resultOut);
            }

        }
 public static void ClipFaceAgainstHull(IndexedVector3 separatingNormal, ConvexPolyhedron hullA, IndexedMatrix transA, ObjectArray<IndexedVector3> worldVertsB1, float minDist, float maxDist, IDiscreteCollisionDetectorInterfaceResult resultOut)
 {
     ClipFaceAgainstHull(ref separatingNormal, hullA, ref transA, worldVertsB1, minDist, maxDist, resultOut);
 }
Exemple #4
0
        public static void ClipHullAgainstHull(ref IndexedVector3 separatingNormal1, ConvexPolyhedron hullA, ConvexPolyhedron hullB, ref IndexedMatrix transA, ref IndexedMatrix transB, float minDist, float maxDist, IDiscreteCollisionDetectorInterfaceResult resultOut)
        {
            IndexedVector3 separatingNormal = separatingNormal1.Normalized();
            IndexedVector3 c0           = transA * hullA.m_localCenter;
            IndexedVector3 c1           = transB * hullB.m_localCenter;
            IndexedVector3 DeltaC2      = c0 - c1;
            float          curMaxDist   = maxDist;
            int            closestFaceB = -1;
            float          dmax         = float.MinValue;

            {
                for (int face = 0; face < hullB.m_faces.Count; face++)
                {
                    IndexedVector3 Normal      = new IndexedVector3(hullB.m_faces[face].m_plane[0], hullB.m_faces[face].m_plane[1], hullB.m_faces[face].m_plane[2]);
                    IndexedVector3 WorldNormal = transB._basis * Normal;

                    float d = IndexedVector3.Dot(WorldNormal, separatingNormal);
                    if (d > dmax)
                    {
                        dmax         = d;
                        closestFaceB = face;
                    }
                }
            }


            // setup initial clip face (minimizing face from hull B)
            ObjectArray <IndexedVector3> worldVertsB1 = new ObjectArray <IndexedVector3>();

            {
                Face polyB       = hullB.m_faces[closestFaceB];
                int  numVertices = polyB.m_indices.Count;
                for (int e0 = 0; e0 < numVertices; e0++)
                {
                    IndexedVector3 b = hullB.m_vertices[polyB.m_indices[e0]];
                    // check this to see if it is transposed version
                    worldVertsB1.Add(transB * b);
                }
            }
            if (closestFaceB >= 0)
            {
                ClipFaceAgainstHull(ref separatingNormal, hullA, ref transA, worldVertsB1, minDist, maxDist, resultOut);
            }
        }
Exemple #5
0
 public static void ClipHullAgainstHull(IndexedVector3 separatingNormal, ConvexPolyhedron hullA, ConvexPolyhedron hullB, IndexedMatrix transA, IndexedMatrix transB, float minDist, float maxDist, IDiscreteCollisionDetectorInterfaceResult resultOut)
 {
     ClipHullAgainstHull(ref separatingNormal, hullA, hullB, ref transA, ref transB, minDist, maxDist, resultOut);
 }
Exemple #6
0
        public static void ClipFaceAgainstHull(ref IndexedVector3 separatingNormal, ConvexPolyhedron hullA, ref IndexedMatrix transA, ObjectArray <IndexedVector3> worldVertsB1, float minDist, float maxDist, IDiscreteCollisionDetectorInterfaceResult resultOut)
        {
            ObjectArray <IndexedVector3> worldVertsB2 = new ObjectArray <IndexedVector3>();
            ObjectArray <IndexedVector3> pVtxIn       = worldVertsB1;
            ObjectArray <IndexedVector3> pVtxOut      = worldVertsB2;

            pVtxOut.Capacity = pVtxIn.Count;

            int closestFaceA = -1;

            {
                float dmin = float.MaxValue;
                for (int face = 0; face < hullA.m_faces.Count; face++)
                {
                    IndexedVector3 Normal        = new IndexedVector3(hullA.m_faces[face].m_plane[0], hullA.m_faces[face].m_plane[1], hullA.m_faces[face].m_plane[2]);
                    IndexedVector3 faceANormalWS = transA._basis * Normal;

                    float d = IndexedVector3.Dot(faceANormalWS, separatingNormal);
                    if (d < dmin)
                    {
                        dmin         = d;
                        closestFaceA = face;
                    }
                }
            }
            if (closestFaceA < 0)
            {
                return;
            }

            Face polyA = hullA.m_faces[closestFaceA];

            // clip polygon to back of planes of all faces of hull A that are adjacent to witness face
            int numContacts  = pVtxIn.Count;
            int numVerticesA = polyA.m_indices.Count;

            for (int e0 = 0; e0 < numVerticesA; e0++)
            {
                IndexedVector3 a                  = hullA.m_vertices[polyA.m_indices[e0]];
                IndexedVector3 b                  = hullA.m_vertices[polyA.m_indices[(e0 + 1) % numVerticesA]];
                IndexedVector3 edge0              = a - b;
                IndexedVector3 WorldEdge0         = transA._basis * edge0;
                IndexedVector3 worldPlaneAnormal1 = transA._basis * new IndexedVector3(polyA.m_plane[0], polyA.m_plane[1], polyA.m_plane[2]);

                IndexedVector3 planeNormalWS1 = -WorldEdge0.Cross(worldPlaneAnormal1);//.cross(WorldEdge0);
                IndexedVector3 worldA1        = transA * a;
                float          planeEqWS1     = -worldA1.Dot(planeNormalWS1);

//int otherFace=0;
#if BLA1
                int otherFace = polyA.m_connectedFaces[e0];
                btVector3 localPlaneNormal(hullA.m_faces[otherFace].m_plane[0], hullA.m_faces[otherFace].m_plane[1], hullA.m_faces[otherFace].m_plane[2]);

                btScalar localPlaneEq = hullA.m_faces[otherFace].m_plane[3];

                btVector3 planeNormalWS = transA.getBasis() * localPlaneNormal;
                btScalar  planeEqWS     = localPlaneEq - planeNormalWS.dot(transA.getOrigin());
#else
                IndexedVector3 planeNormalWS = planeNormalWS1;
                float          planeEqWS     = planeEqWS1;
#endif                //clip face

                ClipFace(pVtxIn, pVtxOut, ref planeNormalWS, planeEqWS);

                //btSwap(pVtxIn,pVtxOut);
                ObjectArray <IndexedVector3> temp = pVtxIn;
                pVtxIn  = pVtxOut;
                pVtxOut = temp;

                pVtxOut.Clear();
            }



            //#define ONLY_REPORT_DEEPEST_POINT

            IndexedVector3 point;


            // only keep points that are behind the witness face
            {
                IndexedVector3 localPlaneNormal = new IndexedVector3(polyA.m_plane[0], polyA.m_plane[1], polyA.m_plane[2]);
                float          localPlaneEq     = polyA.m_plane[3];
                IndexedVector3 planeNormalWS    = transA._basis * localPlaneNormal;
                float          planeEqWS        = localPlaneEq - IndexedVector3.Dot(planeNormalWS, transA._origin);
                for (int i = 0; i < pVtxIn.Count; i++)
                {
                    float depth = IndexedVector3.Dot(planeNormalWS, pVtxIn[i]) + planeEqWS;
                    if (depth <= minDist)
                    {
                        //				printf("clamped: depth=%f to minDist=%f\n",depth,minDist);
                        depth = minDist;
                    }

                    if (depth <= maxDist && depth >= minDist)
                    {
                        IndexedVector3 point2 = pVtxIn[i];
#if ONLY_REPORT_DEEPEST_POINT
                        curMaxDist = depth;
#else
#if false
                        if (depth < -3)
                        {
                            printf("error in btPolyhedralContactClipping depth = %f\n", depth);
                            printf("likely wrong separatingNormal passed in\n");
                        }
#endif
                        resultOut.AddContactPoint(ref separatingNormal, ref point2, depth);
#endif
                    }
                }
            }
#if ONLY_REPORT_DEEPEST_POINT
            if (curMaxDist < maxDist)
            {
                resultOut.AddContactPoint(ref separatingNormal, ref point, curMaxDist);
            }
#endif //ONLY_REPORT_DEEPEST_POINT
        }
Exemple #7
0
 public static void ClipFaceAgainstHull(IndexedVector3 separatingNormal, ConvexPolyhedron hullA, IndexedMatrix transA, ObjectArray <IndexedVector3> worldVertsB1, float minDist, float maxDist, IDiscreteCollisionDetectorInterfaceResult resultOut)
 {
     ClipFaceAgainstHull(ref separatingNormal, hullA, ref transA, worldVertsB1, minDist, maxDist, resultOut);
 }
        public void GetClosestPoints(ClosestPointInput input, IDiscreteCollisionDetectorInterfaceResult output, IDebugDraw debugDraw, bool swapResults)
        {
	        Matrix transformA = input.m_transformA;
	        Matrix transformB = input.m_transformB;

            Vector3 point = Vector3.Zero, normal = Vector3.Up;
	        float timeOfImpact = 1f;
            float depth = 0f;
            //	output.m_distance = float(1e30);
	        //move sphere into triangle space
			Matrix sphereInTr = MathUtil.InverseTimes(transformB,transformA);

            Vector3 temp = sphereInTr.Translation;
	        if (Collide(ref temp,ref point,ref normal,ref depth,ref timeOfImpact,m_contactBreakingThreshold))
	        {
		        if (swapResults)
		        {
			        Vector3 normalOnB = Vector3.TransformNormal(normal,transformB);
			        Vector3 normalOnA = -normalOnB;
			        Vector3 pointOnA = Vector3.Transform(point,transformB)+normalOnB*depth;
			        output.AddContactPoint(ref normalOnA,ref pointOnA,depth);
		        } 
                else
		        {
                    Vector3 p = Vector3.TransformNormal(normal, transformB);
                    Vector3 p2 = Vector3.Transform(point, transformB);
			        output.AddContactPoint(ref p,ref p2,depth);
		        }
	        }
        }
        public void GetClosestPointsNonVirtual(ref ClosestPointInput input, IDiscreteCollisionDetectorInterfaceResult output, IDebugDraw debugDraw)
        {
            m_cachedSeparatingDistance = 0f;

            float          distance = 0f;
            IndexedVector3 normalInB = IndexedVector3.Zero;
            IndexedVector3 pointOnA = IndexedVector3.Zero, pointOnB = IndexedVector3.Zero;
            IndexedMatrix  localTransA    = input.m_transformA;
            IndexedMatrix  localTransB    = input.m_transformB;
            IndexedVector3 positionOffset = (localTransA._origin + localTransB._origin) * .5f;


            IndexedVector3.Subtract(out localTransA._origin, ref localTransA._origin, ref positionOffset);
            IndexedVector3.Subtract(out localTransB._origin, ref localTransB._origin, ref positionOffset);
            //localTransB._origin -= positionOffset;

            bool check2d = m_minkowskiA.IsConvex2d() && m_minkowskiB.IsConvex2d();

            float marginA = m_marginA;
            float marginB = m_marginB;

#if TEST_NON_VIRTUAL
            float marginAv = m_minkowskiA.getMarginNonVirtual();
            float marginBv = m_minkowskiB.getMarginNonVirtual();
            Debug.Assert(marginA == marginAv);
            Debug.Assert(marginB == marginBv);
#endif //TEST_NON_VIRTUAL

            gNumGjkChecks++;

#if DEBUG_SPU_COLLISION_DETECTION
            spu_printf("inside gjk\n");
#endif
            //for CCD we don't use margins
            if (m_ignoreMargin)
            {
                marginA = 0f;
                marginB = 0f;
#if DEBUG_SPU_COLLISION_DETECTION
                spu_printf("ignoring margin\n");
#endif
            }

            m_curIter = 0;
            int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
            m_cachedSeparatingAxis = new IndexedVector3(0, 1, 0);

            bool isValid          = false;
            bool checkSimplex     = false;
            bool checkPenetration = true;
            m_degenerateSimplex = 0;

            m_lastUsedMethod = -1;

            if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJKDetector)
            {
                MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjk::getClosestPointsNonVirtual transA", localTransA);
                MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjk::getClosestPointsNonVirtual transB", localTransB);
            }


            {
                float squaredDistance = MathUtil.BT_LARGE_FLOAT;
                float delta           = 0f;

                float margin = marginA + marginB;

                m_simplexSolver.Reset();

                int count = 0;
                for (; ;)
                //while (true)
                {
                    count++;

                    IndexedVector3 seperatingAxisInA = (-m_cachedSeparatingAxis) * input.m_transformA._basis;
                    IndexedVector3 seperatingAxisInB = m_cachedSeparatingAxis * input.m_transformB._basis;


                    IndexedVector3 pInA = m_minkowskiA.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInA);
                    IndexedVector3 qInB = m_minkowskiB.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInB);

                    IndexedVector3 pWorld = localTransA * pInA;
                    IndexedVector3 qWorld = localTransB * qInB;

                    if (check2d)
                    {
                        pWorld.Z = 0.0f;
                        qWorld.Z = 0.0f;
                    }

                    IndexedVector3 w = new IndexedVector3(pWorld.X - qWorld.X, pWorld.Y - qWorld.Y, pWorld.Z - qWorld.Z);
                    IndexedVector3.Dot(ref m_cachedSeparatingAxis, ref w, out delta);

                    if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJKDetector)
                    {
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "m_cachedSeparatingAxis", m_cachedSeparatingAxis);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "w", w);
                        BulletGlobals.g_streamWriter.WriteLine(String.Format("simplex num vertices [{0}]", m_simplexSolver.NumVertices()));
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxisA", seperatingAxisInA);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxisB", seperatingAxisInB);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "pInA", pInA);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "qInB", qInB);
                        MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "localTransA", localTransA);
                        MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "localTransB", localTransB);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "pWorld", pWorld);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "qWorld", qWorld);
                    }

                    // potential exit, they don't overlap
                    if ((delta > 0f) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared))
                    {
                        m_degenerateSimplex = 10;
                        checkSimplex        = true;
                        //checkPenetration = false;
                        break;
                    }

                    //exit 0: the new point is already in the simplex, or we didn't come any closer
                    if (m_simplexSolver.InSimplex(ref w))
                    {
                        m_degenerateSimplex = 1;
                        checkSimplex        = true;
                        break;
                    }
                    // are we getting any closer ?
                    float f0 = squaredDistance - delta;
                    float f1 = squaredDistance * REL_ERROR2;

                    if (f0 <= f1)
                    {
                        if (f0 <= 0f)
                        {
                            m_degenerateSimplex = 2;
                        }
                        else
                        {
                            m_degenerateSimplex = 11;
                        }
                        checkSimplex = true;
                        break;
                    }
                    //add current vertex to simplex
                    m_simplexSolver.AddVertex(ref w, ref pWorld, ref qWorld);

                    //calculate the closest point to the origin (update vector v)
                    IndexedVector3 newCachedSeparatingAxis;

                    if (!m_simplexSolver.Closest(out newCachedSeparatingAxis))
                    {
                        m_degenerateSimplex = 3;
                        checkSimplex        = true;
                        break;
                    }

                    if (newCachedSeparatingAxis.LengthSquared() < REL_ERROR2)
                    {
                        m_cachedSeparatingAxis = newCachedSeparatingAxis;
                        m_degenerateSimplex    = 6;
                        checkSimplex           = true;
                        break;
                    }

                    float previousSquaredDistance = squaredDistance;
                    squaredDistance = newCachedSeparatingAxis.LengthSquared();

                    if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJKDetector)
                    {
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxisA", seperatingAxisInA);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxisB", seperatingAxisInB);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "pInA", pInA);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "qInB", qInB);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "pWorld", pWorld);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "qWorld", qWorld);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "newSeperatingAxis", newCachedSeparatingAxis);
                        BulletGlobals.g_streamWriter.WriteLine(String.Format("f0[{0:0.00000000}] f1[{1:0.00000000}] checkSimplex[{2}] degen[{3}]", f0, f1, checkSimplex, m_degenerateSimplex));
                    }


#if false
                    ///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo
                    if (squaredDistance > previousSquaredDistance)
                    {
                        m_degenerateSimplex = 7;
                        squaredDistance     = previousSquaredDistance;
                        checkSimplex        = false;
                        break;
                    }
#endif //

                    //redundant m_simplexSolver->compute_points(pointOnA, pointOnB);

                    //are we getting any closer ?
                    if (previousSquaredDistance - squaredDistance <= MathUtil.SIMD_EPSILON * previousSquaredDistance)
                    {
                        //m_simplexSolver.BackupClosest(ref m_cachedSeparatingAxis);
                        checkSimplex        = true;
                        m_degenerateSimplex = 12;
                        break;
                    }

                    m_cachedSeparatingAxis = newCachedSeparatingAxis;

                    //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
                    if (m_curIter++ > gGjkMaxIter)
                    {
                        //#if defined(DEBUG) || defined (_DEBUG) || defined (DEBUG_SPU_COLLISION_DETECTION)

                        //        printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter);
                        //        printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",
                        //        m_cachedSeparatingAxis.getX(),
                        //        m_cachedSeparatingAxis.getY(),
                        //        m_cachedSeparatingAxis.getZ(),
                        //        squaredDistance,
                        //        m_minkowskiA->getShapeType(),
                        //        m_minkowskiB->getShapeType());

                        //#endif
                        break;
                    }

                    bool check = (!m_simplexSolver.FullSimplex());
                    //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());

                    if (!check)
                    {
                        //do we need this backup_closest here ?
                        //m_simplexSolver.BackupClosest(ref m_cachedSeparatingAxis);
                        m_degenerateSimplex = 13;

                        break;
                    }
                }

                if (checkSimplex)
                {
                    m_simplexSolver.ComputePoints(out pointOnA, out pointOnB);
                    normalInB = m_cachedSeparatingAxis;
                    float lenSqr = m_cachedSeparatingAxis.LengthSquared();
                    //valid normal
                    if (lenSqr < 0.0001f)
                    {
                        m_degenerateSimplex = 5;
                    }
                    if (lenSqr > MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON)
                    {
                        //float rlen = 1 / normalInB.Length();
                        float rlen = 1.0f / (float)Math.Sqrt((float)lenSqr);
                        //normalInB.Normalize();
                        normalInB *= rlen;
                        float s = (float)Math.Sqrt((float)squaredDistance);

                        Debug.Assert(s > 0f);
                        pointOnA -= m_cachedSeparatingAxis * (marginA / s);
                        pointOnB += m_cachedSeparatingAxis * (marginB / s);
                        distance  = ((1f / rlen) - margin);
                        isValid   = true;

                        m_lastUsedMethod = 1;
                    }
                    else
                    {
                        m_lastUsedMethod = 2;
                    }
                }

                bool catchDegeneratePenetrationCase =
                    (m_catchDegeneracies && m_penetrationDepthSolver != null && m_degenerateSimplex > 0 && ((distance + margin) < 0.01));

                //if (checkPenetration && !isValid)
                if (checkPenetration && (!isValid || catchDegeneratePenetrationCase))
                {
                    //penetration case

                    //if there is no way to handle penetrations, bail ref
                    if (m_penetrationDepthSolver != null)
                    {
                        // Penetration depth case.
                        IndexedVector3 tmpPointOnA = IndexedVector3.Zero, tmpPointOnB = IndexedVector3.Zero;

                        gNumDeepPenetrationChecks++;
                        m_cachedSeparatingAxis = IndexedVector3.Zero;
                        bool isValid2 = m_penetrationDepthSolver.CalcPenDepth(
                            m_simplexSolver,
                            m_minkowskiA, m_minkowskiB,
                            ref localTransA, ref localTransB,
                            ref m_cachedSeparatingAxis, ref tmpPointOnA, ref tmpPointOnB,
                            debugDraw
                            );


                        if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJKDetector)
                        {
                            BulletGlobals.g_streamWriter.WriteLine("calcPenDepthResult");
                            BulletGlobals.g_streamWriter.WriteLine("lastMethodUsed : " + m_lastUsedMethod);

                            MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "localTransA", localTransA);
                            MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "localTransB", localTransB);
                            MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxis", m_cachedSeparatingAxis);
                            MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "tmpA", tmpPointOnA);
                            MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "tmpB", tmpPointOnB);
                        }


                        if (isValid2)
                        {
                            IndexedVector3 tmpNormalInB = tmpPointOnB - tmpPointOnA;
                            float          lenSqr       = tmpNormalInB.LengthSquared();
                            if (lenSqr <= (MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON))
                            {
                                tmpNormalInB = m_cachedSeparatingAxis;
                                lenSqr       = m_cachedSeparatingAxis.LengthSquared();
                            }

                            if (lenSqr > (MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON))
                            {
                                tmpNormalInB /= (float)Math.Sqrt(lenSqr);
                                float distance2 = -(tmpPointOnA - tmpPointOnB).Length();
                                //only replace valid penetrations when the result is deeper (check)
                                if (!isValid || (distance2 < distance))
                                {
                                    distance  = distance2;
                                    pointOnA  = tmpPointOnA;
                                    pointOnB  = tmpPointOnB;
                                    normalInB = tmpNormalInB;
                                    isValid   = true;



                                    //FIXME! check2d THIS


                                    m_lastUsedMethod = 3;
                                }
                                else
                                {
                                    m_lastUsedMethod = 8;
                                }
                            }
                            else
                            {
                                //isValid = false;
                                m_lastUsedMethod = 9;
                            }
                        }
                        else
                        {
                            ///this is another degenerate case, where the initial GJK calculation reports a degenerate case
                            ///EPA reports no penetration, and the second GJK (using the supporting vector without margin)
                            ///reports a valid positive distance. Use the results of the second GJK instead of failing.
                            ///thanks to Jacob.Langford for the reproduction case
                            ///http://code.google.com/p/bullet/issues/detail?id=250


                            if (m_cachedSeparatingAxis.LengthSquared() > 0f)
                            {
                                float distance2 = (tmpPointOnA - tmpPointOnB).Length() - margin;
                                //only replace valid distances when the distance is less
                                if (!isValid || (distance2 < distance))
                                {
                                    distance  = distance2;
                                    pointOnA  = tmpPointOnA;
                                    pointOnB  = tmpPointOnB;
                                    pointOnA -= m_cachedSeparatingAxis * marginA;
                                    pointOnB += m_cachedSeparatingAxis * marginB;
                                    normalInB = m_cachedSeparatingAxis;
                                    normalInB.Normalize();
                                    isValid          = true;
                                    m_lastUsedMethod = 6;
                                }
                                else
                                {
                                    m_lastUsedMethod = 5;
                                }
                            }
                        }
                    }
                }
            }


            if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJKDetector)
            {
                BulletGlobals.g_streamWriter.WriteLine("valid [{0}] distance[{1:0000.00000000}][{2:0000.00000000}] maxDistSq[{3:0000.00000000}]", isValid, distance, distance * distance, input.m_maximumDistanceSquared);
            }


            if (isValid && ((distance < 0) || (distance * distance < input.m_maximumDistanceSquared)))
            {
                m_cachedSeparatingAxis     = normalInB;
                m_cachedSeparatingDistance = distance;
                IndexedVector3 temp = pointOnB + positionOffset;
                output.AddContactPoint(
                    ref normalInB,
                    ref temp,
                    distance);
            }
        }
Exemple #10
0
        public void GetClosestPointsNonVirtual(ClosestPointInput input, IDiscreteCollisionDetectorInterfaceResult output, IDebugDraw debugDraw)
        {
            m_cachedSeparatingDistance = 0f;

            float distance = 0f;
            Vector3 normalInB = Vector3.Zero;
            Vector3 pointOnA = Vector3.Zero, pointOnB = Vector3.Zero;
            Matrix localTransA = input.m_transformA;
            Matrix localTransB = input.m_transformB;
            Vector3 positionOffset = (localTransA.Translation + localTransB.Translation) * .5f;
            localTransA.Translation -= positionOffset;
            localTransB.Translation -= positionOffset;

            bool check2d = m_minkowskiA.IsConvex2D() && m_minkowskiB.IsConvex2D();

            float marginA = m_marginA;
            float marginB = m_marginB;

#if TEST_NON_VIRTUAL
            float marginAv = m_minkowskiA.getMarginNonVirtual();
            float marginBv = m_minkowskiB.getMarginNonVirtual();
            Debug.Assert(marginA == marginAv);
            Debug.Assert(marginB == marginBv);
#endif //TEST_NON_VIRTUAL

            gNumGjkChecks++;

#if DEBUG_SPU_COLLISION_DETECTION
           spu_printf("inside gjk\n");
#endif
            //for CCD we don't use margins
            if (m_ignoreMargin)
            {
                marginA = 0f;
                marginB = 0f;
#if DEBUG_SPU_COLLISION_DETECTION
                spu_printf("ignoring margin\n");
#endif
            }

            m_curIter = 0;
            int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
            m_cachedSeparatingAxis = new Vector3(0, 1, 0);

            bool isValid = false;
            bool checkSimplex = false;
            bool checkPenetration = true;
            m_degenerateSimplex = 0;

            m_lastUsedMethod = -1;

            if (BulletGlobals.g_streamWriter != null && debugGJK)
            {
                MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjk::getClosestPointsNonVirtual transA", localTransA);
                MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjk::getClosestPointsNonVirtual transB", localTransB);

            }


            {
                float squaredDistance = MathUtil.BT_LARGE_FLOAT;
                float delta = 0f;

                float margin = marginA + marginB;

                m_simplexSolver.Reset();

                int count = 0;
                for (; ; )
                //while (true)
                {
                    count++;
                    if (gNumGjkChecks == 3 && count == 4)
                    {
                        int ibreak = 0;
                    }
                    Vector3 seperatingAxisInA = MathUtil.TransposeTransformNormal(-m_cachedSeparatingAxis, input.m_transformA);
                    Vector3 seperatingAxisInB = MathUtil.TransposeTransformNormal(m_cachedSeparatingAxis, input.m_transformB);
#if true

                    Vector3 pInA = m_minkowskiA.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInA);
                    Vector3 qInB = m_minkowskiB.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInB);

                    //			btVector3 pInA  = localGetSupportingVertexWithoutMargin(m_shapeTypeA, m_minkowskiA, seperatingAxisInA,input.m_convexVertexData[0]);//, &featureIndexA);
                    //			btVector3 qInB  = localGetSupportingVertexWithoutMargin(m_shapeTypeB, m_minkowskiB, seperatingAxisInB,input.m_convexVertexData[1]);//, &featureIndexB);

#else
                    Vector3 pInA = m_minkowskiA.localGetSupportingVertexWithoutMargin(ref seperatingAxisInA);
                    Vector3 qInB = m_minkowskiB.localGetSupportingVertexWithoutMargin(ref seperatingAxisInB);
#if TEST_NON_VIRTUAL
                    Vector3 pInAv = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
                    Vector3 qInBv = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
                    Debug.Assert((pInAv-pInA).Length() < 0.0001);
                    Debug.Assert((qInBv-qInB).Length() < 0.0001);
#endif //
#endif
                    Vector3 pWorld = Vector3.Transform(pInA, localTransA);
                    Vector3 qWorld = Vector3.Transform(qInB, localTransB);



                    if (check2d)
                    {
                        pWorld.Z = 0.0f;
                        qWorld.Z = 0.0f;
                    }

                    Vector3 w = pWorld - qWorld;
                    delta = Vector3.Dot(m_cachedSeparatingAxis, w);

                    if (BulletGlobals.g_streamWriter != null && debugGJK)
                    {
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "m_cachedSeparatingAxis", m_cachedSeparatingAxis);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "w", w);
                        BulletGlobals.g_streamWriter.WriteLine(String.Format("simplex num vertices [{0}]", m_simplexSolver.NumVertices()));
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxisA", seperatingAxisInA);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxisB", seperatingAxisInB);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "pInA", pInA);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "qInB", qInB);
                        MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "localTransA", localTransA);
                        MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "localTransB", localTransB);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "pWorld", pWorld);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "qWorld", qWorld);
                    }

                    if (m_simplexSolver.NumVertices() == 3)
                    {
                        int ibreak = 0;
                    }


                    // potential exit, they don't overlap
                    if ((delta > 0f) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared))
                    {
                        m_degenerateSimplex = 10;
                        checkSimplex = true;
                        //checkPenetration = false;
                        break;
                    }

                    //exit 0: the new point is already in the simplex, or we didn't come any closer
                    if (m_simplexSolver.InSimplex(ref w))
                    {
                        m_degenerateSimplex = 1;
                        checkSimplex = true;
                        break;
                    }
                    // are we getting any closer ?
                    float f0 = squaredDistance - delta;
                    float f1 = squaredDistance * REL_ERROR2;



                    if (f0 <= f1)
                    {
                        if (f0 <= 0f)
                        {
                            m_degenerateSimplex = 2;
                        }
                        else
                        {
                            m_degenerateSimplex = 11;
                        }
                        checkSimplex = true;
                        break;
                    }
                    //add current vertex to simplex
                    m_simplexSolver.AddVertex(ref w, ref pWorld, ref qWorld);

                    //calculate the closest point to the origin (update vector v)
                    Vector3 newCachedSeparatingAxis = new Vector3();

                    if (!m_simplexSolver.Closest(ref newCachedSeparatingAxis))
                    {
                        m_degenerateSimplex = 3;
                        checkSimplex = true;
                        break;
                    }

                    if (newCachedSeparatingAxis.LengthSquared() < REL_ERROR2)
                    {
                        m_cachedSeparatingAxis = newCachedSeparatingAxis;
                        m_degenerateSimplex = 6;
                        checkSimplex = true;
                        break;
                    }

                    float previousSquaredDistance = squaredDistance;
                    squaredDistance = newCachedSeparatingAxis.LengthSquared();

                    if (BulletGlobals.g_streamWriter != null && debugGJK)
                    {
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxisA", seperatingAxisInA);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxisB", seperatingAxisInB);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "pInA", pInA);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "qInB", qInB);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "pWorld", pWorld);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "qWorld", qWorld);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "newSeperatingAxis", newCachedSeparatingAxis);
                        BulletGlobals.g_streamWriter.WriteLine(String.Format("f0[{0:0.00000000}] f1[{1:0.00000000}] checkSimplex[{2}] degen[{3}]", f0, f1, checkSimplex, m_degenerateSimplex));
                    }


#if false
                    ///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo
                    if (squaredDistance>previousSquaredDistance)
                    {
                        m_degenerateSimplex = 7;
                        squaredDistance = previousSquaredDistance;
                        checkSimplex = false;
                        break;
                    }
#endif //
                    m_cachedSeparatingAxis = newCachedSeparatingAxis;

                    //redundant m_simplexSolver->compute_points(pointOnA, pointOnB);

                    //are we getting any closer ?
                    if (previousSquaredDistance - squaredDistance <= MathUtil.SIMD_EPSILON * previousSquaredDistance)
                    {
                        m_simplexSolver.BackupClosest(ref m_cachedSeparatingAxis);
                        checkSimplex = true;
                        m_degenerateSimplex = 12;
                        break;
                    }

                    //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject   
                    if (m_curIter++ > gGjkMaxIter)
                    {
                        //#if defined(DEBUG) || defined (_DEBUG) || defined (DEBUG_SPU_COLLISION_DETECTION)

                        //        printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter);   
                        //        printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",   
                        //        m_cachedSeparatingAxis.getX(),   
                        //        m_cachedSeparatingAxis.getY(),   
                        //        m_cachedSeparatingAxis.getZ(),   
                        //        squaredDistance,   
                        //        m_minkowskiA->getShapeType(),   
                        //        m_minkowskiB->getShapeType());   

                        //#endif   
                        break;

                    }

                    bool check = (!m_simplexSolver.FullSimplex());
                    //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());

                    if (!check)
                    {
                        //do we need this backup_closest here ?
                        m_simplexSolver.BackupClosest(ref m_cachedSeparatingAxis);
                        m_degenerateSimplex = 13;

                        break;
                    }
                }

                if (checkSimplex)
                {
                    m_simplexSolver.ComputePoints(ref pointOnA, ref pointOnB);
                    normalInB = pointOnA - pointOnB;
                    float lenSqr = m_cachedSeparatingAxis.LengthSquared();
                    //valid normal
                    if (lenSqr < 0.0001f)
                    {
                        m_degenerateSimplex = 5;
                    }
                    if (lenSqr > MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON)
                    {
                        //float rlen = 1 / normalInB.Length();
                        float rlen = 1.0f / (float)System.Math.Sqrt((float)lenSqr );
                        //normalInB.Normalize();
                        normalInB *= rlen;
                        float s = (float)System.Math.Sqrt((float)squaredDistance);

                        Debug.Assert(s > 0f);
                        pointOnA -= m_cachedSeparatingAxis * (marginA / s);
                        pointOnB += m_cachedSeparatingAxis * (marginB / s);
                        distance = ((1f / rlen) - margin);
                        isValid = true;

                        m_lastUsedMethod = 1;
                    }
                    else
                    {
                        m_lastUsedMethod = 2;
                    }
                }

                bool catchDegeneratePenetrationCase =
                    (m_catchDegeneracies && m_penetrationDepthSolver != null && m_degenerateSimplex > 0 && ((distance + margin) < 0.01));

                //if (checkPenetration && !isValid)
                if (checkPenetration && (!isValid || catchDegeneratePenetrationCase))
                {
                    //penetration case

                    //if there is no way to handle penetrations, bail ref
                    if (m_penetrationDepthSolver != null)
                    {
                        // Penetration depth case.
                        Vector3 tmpPointOnA = Vector3.Zero, tmpPointOnB = Vector3.Zero;

                        gNumDeepPenetrationChecks++;
                        m_cachedSeparatingAxis = Vector3.Zero;
                        bool isValid2 = m_penetrationDepthSolver.CalcPenDepth(
                            m_simplexSolver,
                            m_minkowskiA, m_minkowskiB,
                            ref localTransA, ref localTransB,
                            ref m_cachedSeparatingAxis, ref tmpPointOnA, ref tmpPointOnB,
                            debugDraw
                            );


                        if (BulletGlobals.g_streamWriter != null && debugGJK)
                        {
                            BulletGlobals.g_streamWriter.WriteLine("calcPenDepthResult");
                            MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "localTransA", localTransA);
                            MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "localTransB", localTransB);
                            MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxis", m_cachedSeparatingAxis);
                            MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "tmpA", tmpPointOnA);
                            MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "tmpB", tmpPointOnB);
                        }


                        if (isValid2)
                        {
                            Vector3 tmpNormalInB = tmpPointOnB - tmpPointOnA;
                            float lenSqr = tmpNormalInB.LengthSquared();
                            if (lenSqr <= (MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON))
                            {
                                tmpNormalInB = m_cachedSeparatingAxis;
                                lenSqr = m_cachedSeparatingAxis.LengthSquared();
                            }

                            if (lenSqr > (MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON))
                            {
                                tmpNormalInB.Normalize();
                                float distance2 = -(tmpPointOnA - tmpPointOnB).Length();
                                //only replace valid penetrations when the result is deeper (check)
                                if (!isValid || (distance2 < distance))
                                {
                                    distance = distance2;
                                    pointOnA = tmpPointOnA;
                                    pointOnB = tmpPointOnB;
                                    normalInB = tmpNormalInB;
                                    isValid = true;
                                    m_lastUsedMethod = 3;
                                }
                                else
                                {
                                    m_lastUsedMethod = 8;
                                }
                            }
                            else
                            {
                                //isValid = false;
                                m_lastUsedMethod = 9;
                            }
                        }
                        else
                        {
                            ///this is another degenerate case, where the initial GJK calculation reports a degenerate case
                            ///EPA reports no penetration, and the second GJK (using the supporting vector without margin)
                            ///reports a valid positive distance. Use the results of the second GJK instead of failing.
                            ///thanks to Jacob.Langford for the reproduction case
                            ///http://code.google.com/p/bullet/issues/detail?id=250


                            if (m_cachedSeparatingAxis.LengthSquared() > 0f)
                            {
                                float distance2 = (tmpPointOnA - tmpPointOnB).Length() - margin;
                                //only replace valid distances when the distance is less
                                if (!isValid || (distance2 < distance))
                                {
                                    distance = distance2;
                                    pointOnA = tmpPointOnA;
                                    pointOnB = tmpPointOnB;
                                    pointOnA -= m_cachedSeparatingAxis * marginA;
                                    pointOnB += m_cachedSeparatingAxis * marginB;
                                    normalInB = m_cachedSeparatingAxis;
                                    normalInB.Normalize();
                                    isValid = true;
                                    m_lastUsedMethod = 6;
                                }
                                else
                                {
                                    m_lastUsedMethod = 5;
                                }
                            }
                        }
                    }
                }
            }

            if (isValid && ((distance < 0) || (distance * distance < input.m_maximumDistanceSquared)))
            {
                m_cachedSeparatingAxis = normalInB;
                m_cachedSeparatingDistance = distance;
                Vector3 temp = pointOnB + positionOffset;
                output.AddContactPoint(
                    ref normalInB,
                    ref temp,
                    distance);
            }
        }
Exemple #11
0
 public virtual void GetClosestPoints(ClosestPointInput input, IDiscreteCollisionDetectorInterfaceResult output, IDebugDraw debugDraw, bool swapResults)
 {
     GetClosestPointsNonVirtual(input, output, debugDraw);
 }
Exemple #12
0
 public virtual void GetClosestPoints(ClosestPointInput input, IDiscreteCollisionDetectorInterfaceResult output, IDebugDraw debugDraw)
 {
     GetClosestPoints(input, output, debugDraw, false);
 }
Exemple #13
0
        private static int DBoxBox2(ref IndexedVector3 p1, float[] R1,
        ref IndexedVector3 side1, ref IndexedVector3 p2,
        float[] R2, ref IndexedVector3 side2,
        ref IndexedVector3 normal, ref float depth, ref int return_code,
        int maxc, Object contact, int skip, IDiscreteCollisionDetectorInterfaceResult output)
        {
            Vector3 centerDifference = Vector3.Zero, ppv = Vector3.Zero;
            float[] normalR = null;
            int normalROffsetResult = 0;


            IndexedVector3 A = side1 * 0.5f;
            IndexedVector3 B = side2 * 0.5f;

            int code;
            bool invert_normal;
            // get vector from centers of box 1 to box 2, relative to box 1
            IndexedVector3 p = p2 - p1;

            IndexedVector3 pp = new IndexedVector3();

            DMULTIPLY1_331(ref pp, R1, ref p);		// get pp = p relative to body 1

            // for all 15 possible separating axes:
            //   * see if the axis separates the boxes. if so, return 0.
            //   * find the depth of the penetration along the separating axis (s2)
            //   * if this is the largest depth so far, record it.
            // the normal vector will be set to the separating axis with the smallest
            // depth. note: normalR is set to point to a column of R1 or R2 if that is
            // the smallest depth normal so far. otherwise normalR is 0 and normalC is
            // set to a vector relative to body 1. invert_normal is 1 if the sign of
            // the normal should be flipped.

            float R11 = DDOT44(R1, 0, R2, 0);
            float R12 = DDOT44(R1, 0, R2, 1);
            float R13 = DDOT44(R1, 0, R2, 2);
            float R21 = DDOT44(R1, 1, R2, 0);
            float R22 = DDOT44(R1, 1, R2, 1);
            float R23 = DDOT44(R1, 1, R2, 2);
            float R31 = DDOT44(R1, 2, R2, 0);
            float R32 = DDOT44(R1, 2, R2, 1);
            float R33 = DDOT44(R1, 2, R2, 2);

            float Q11 = System.Math.Abs(R11);
            float Q12 = System.Math.Abs(R12);
            float Q13 = System.Math.Abs(R13);

            float Q21 = System.Math.Abs(R21);
            float Q22 = System.Math.Abs(R22);
            float Q23 = System.Math.Abs(R23);

            float Q31 = System.Math.Abs(R31);
            float Q32 = System.Math.Abs(R32);
            float Q33 = System.Math.Abs(R33);

            float s = -float.MaxValue;
            invert_normal = false;
            code = 0;

            int normalROffset = 0;
            // separating axis = u1,u2,u3
            if (TST(pp[0], (A[0] + B[0] * Q11 + B[1] * Q12 + B[2] * Q13), R1, ref normalR, 0, ref normalROffset, 1, ref code, ref s, ref invert_normal)) return 0;
            if (TST(pp[1], (A[1] + B[0] * Q21 + B[1] * Q22 + B[2] * Q23), R1, ref normalR, 1, ref normalROffset, 2, ref code, ref s, ref invert_normal)) return 0;
            if (TST(pp[2], (A[2] + B[0] * Q31 + B[1] * Q32 + B[2] * Q33), R1, ref normalR, 2, ref normalROffset, 3, ref code, ref s, ref invert_normal)) return 0;

            // separating axis = v1,v2,v3
            if (TST(DDOT41(R2, 0, ref p, 0), (A[0] * Q11 + A[1] * Q21 + A[2] * Q31 + B[0]), R2, ref normalR, 0, ref normalROffset, 4, ref code, ref s, ref invert_normal)) return 0;
            if (TST(DDOT41(R2, 1, ref p, 0), (A[0] * Q12 + A[1] * Q22 + A[2] * Q32 + B[1]), R2, ref normalR, 1, ref normalROffset, 5, ref code, ref s, ref invert_normal)) return 0;
            if (TST(DDOT41(R2, 2, ref p, 0), (A[0] * Q13 + A[1] * Q23 + A[2] * Q33 + B[2]), R2, ref normalR, 2, ref normalROffset, 6, ref code, ref s, ref invert_normal)) return 0;

            // note: cross product axes need to be scaled when s is computed.
            // normal (n1,n2,n3) is relative to box 1.
            // separating axis = u1 x (v1,v2,v3)
            //private static bool TST2(float expr1,float expr2,ref Vector3 normal, ref Vector3 normalC,int cc,ref int code)

            IndexedVector3 normalC = new IndexedVector3();

            // separating axis = u1 x (v1,v2,v3)
            if (TST2(pp[2] * R21 - pp[1] * R31, (A[1] * Q31 + A[2] * Q21 + B[1] * Q13 + B[2] * Q12), 0, -R31, R21, ref normalC, ref normalR, 7, ref code, ref s, ref invert_normal)) return 0;
            if (TST2(pp[2] * R22 - pp[1] * R32, (A[1] * Q32 + A[2] * Q22 + B[0] * Q13 + B[2] * Q11), 0, -R32, R22, ref normalC, ref normalR, 8, ref code, ref s, ref invert_normal)) return 0;
            if (TST2(pp[2] * R23 - pp[1] * R33, (A[1] * Q33 + A[2] * Q23 + B[0] * Q12 + B[1] * Q11), 0, -R33, R23, ref normalC, ref normalR, 9, ref code, ref s, ref invert_normal)) return 0;

            // separating axis = u2 x (v1,v2,v3)
            if (TST2(pp[0] * R31 - pp[2] * R11, (A[0] * Q31 + A[2] * Q11 + B[1] * Q23 + B[2] * Q22), R31, 0, -R11, ref normalC, ref normalR, 10, ref code, ref s, ref invert_normal)) return 0;
            if (TST2(pp[0] * R32 - pp[2] * R12, (A[0] * Q32 + A[2] * Q12 + B[0] * Q23 + B[2] * Q21), R32, 0, -R12, ref normalC, ref normalR, 11, ref code, ref s, ref invert_normal)) return 0;
            if (TST2(pp[0] * R33 - pp[2] * R13, (A[0] * Q33 + A[2] * Q13 + B[0] * Q22 + B[1] * Q21), R33, 0, -R13, ref normalC, ref normalR, 12, ref code, ref s, ref invert_normal)) return 0;

            // separating axis = u3 x (v1,v2,v3)
            if (TST2(pp[1] * R11 - pp[0] * R21, (A[0] * Q21 + A[1] * Q11 + B[1] * Q33 + B[2] * Q32), -R21, R11, 0, ref normalC, ref normalR, 13, ref code, ref s, ref invert_normal)) return 0;
            if (TST2(pp[1] * R12 - pp[0] * R22, (A[0] * Q22 + A[1] * Q12 + B[0] * Q33 + B[2] * Q31), -R22, R12, 0, ref normalC, ref normalR, 14, ref code, ref s, ref invert_normal)) return 0;
            if (TST2(pp[1] * R13 - pp[0] * R23, (A[0] * Q23 + A[1] * Q13 + B[0] * Q32 + B[1] * Q31), -R23, R13, 0, ref normalC, ref normalR, 15, ref code, ref s, ref invert_normal)) return 0;

            if (code == 0)
            {
                return 0;
            }
            // if we get to this point, the boxes interpenetrate. compute the normal
            // in global coordinates.
            if (normalR != null)
            {
                normal[0] = normalR[0 + normalROffset];
                normal[1] = normalR[4 + normalROffset];
                normal[2] = normalR[8 + normalROffset];
            }
            else
            {
                DMULTIPLY0_331(ref normal, R1, ref normalC);
            }
            if (invert_normal)
            {
                normal = -normal;
            }
            depth = -s;

            // compute contact point(s)

            if (code > 6)
            {
                // an edge from box 1 touches an edge from box 2.
                // find a point pa on the intersecting edge of box 1
                IndexedVector3 pa1 = p1;

                for (int j = 0; j < 3; j++)
                {
                    float sign = (DDOT14(ref normal, 0, R1, j) > 0) ? 1.0f : -1.0f;
                    for (int i = 0; i < 3; i++)
                    {
                        pa1[i] += sign * A[j] * R1[i * 4 + j];
                    }
                }

                // find a point pb on the intersecting edge of box 2
                IndexedVector3 pb1 = p2;
                //for (i = 0; i < 3; i++) pb[i] = p2[i];
                for (int j = 0; j < 3; j++)
                {
                    float sign = (DDOT14(ref normal, 0, R2, j) > 0) ? -1.0f : 1.0f;
                    for (int i = 0; i < 3; i++)
                    {
                        pb1[i] += sign * B[j] * R2[i * 4 + j];
                    }
                }


                float alpha = 0f, beta = 0f;
                IndexedVector3 ua = new IndexedVector3();
                IndexedVector3 ub = new IndexedVector3();
                for (int i = 0; i < 3; i++)
                {
                    ua[i] = R1[((code) - 7) / 3 + i * 4];
                }
                for (int i = 0; i < 3; i++)
                {
                    ub[i] = R2[((code) - 7) % 3 + i * 4];
                }

                DLineClosestApproach(ref pa1, ref ua, ref pb1, ref ub, ref alpha, ref beta);

                for (int i = 0; i < 3; i++)
                {
                    pa1[i] += ua[i] * alpha;
                }
                for (int i = 0; i < 3; i++)
                {
                    pb1[i] += ub[i] * beta;
                }

                {
                    //contact[0].pos[i] = float(0.5)*(pa[i]+pb[i]);
                    //contact[0].depth = *depth;

#if USE_CENTER_POINT
            pointInWorld = (pa + pb) * 0.5f;
            output.addContactPoint(-normal,pointInWorld,-depth);
#else
                    Vector3 pbv = pb1.ToVector3();
                    output.AddContactPoint((-normal).ToVector3(), pbv, -depth);
#endif //
                    return_code = code;
                }
                return 1;
            }

            // okay, we have a face-something intersection (because the separating
            // axis is perpendicular to a face). define face 'a' to be the reference
            // face (i.e. the normal vector is perpendicular to this) and face 'b' to be
            // the incident face (the closest face of the other box).


            float[] Ra;
            float[] Rb;
            IndexedVector3 pa;
            IndexedVector3 pb;
            IndexedVector3 Sa;
            IndexedVector3 Sb;

            if (code <= 3)
            {
                Ra = R1;
                Rb = R2;
                pa = p1;
                pb = p2;
                Sa = A;
                Sb = B;
            }
            else
            {
                Ra = R2;
                Rb = R1;
                pa = p2;
                pb = p1;
                Sa = B;
                Sb = A;
            }

            // nr = normal vector of reference face dotted with axes of incident box.
            // anr = absolute values of nr.
            IndexedVector3 normal2;
            IndexedVector3 nr = new IndexedVector3();
            IndexedVector3 anr;


            if (code <= 3)
            {
                normal2 = normal;
            }
            else
            {
                normal2 = -normal;
            }
            DMULTIPLY1_331(ref nr, Rb, ref normal2);

            nr.Abs(out anr);

            // find the largest compontent of anr: this corresponds to the normal
            // for the indident face. the other axis numbers of the indicent face
            // are stored in a1,a2.
            int lanr, a1, a2;
            if (anr[1] > anr[0])
            {
                if (anr[1] > anr[2])
                {
                    a1 = 0;
                    lanr = 1;
                    a2 = 2;
                }
                else
                {
                    a1 = 0;
                    a2 = 1;
                    lanr = 2;
                }
            }
            else
            {
                if (anr[0] > anr[2])
                {
                    lanr = 0;
                    a1 = 1;
                    a2 = 2;
                }
                else
                {
                    a1 = 0;
                    a2 = 1;
                    lanr = 2;
                }
            }

            // compute center point of incident face, in reference-face coordinates
            IndexedVector3 center = new IndexedVector3(); ;
            if (nr[lanr] < 0)
            {
                for (int i = 0; i < 3; i++)
                {
                    center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i * 4 + lanr];
                }
            }
            else
            {
                for (int i = 0; i < 3; i++)
                {
                    center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i * 4 + lanr];
                }
            }


            // find the normal and non-normal axis numbers of the reference box
            int codeN, code1, code2;
            if (code <= 3)
            {
                codeN = code - 1;
            }
            else
            {
                codeN = code - 4;
            }
            if (codeN == 0)
            {
                code1 = 1;
                code2 = 2;
            }
            else if (codeN == 1)
            {
                code1 = 0;
                code2 = 2;
            }
            else
            {
                code1 = 0;
                code2 = 1;
            }

            // find the four corners of the incident face, in reference-face coordinates
            float[] quad = s_quad;	// 2D coordinate of incident face (x,y pairs)
            float c1, c2, m11, m12, m21, m22;
            c1 = DDOT14(ref center, 0, Ra, code1);
            c2 = DDOT14(ref center, 0, Ra, code2);

            // optimize this? - we have already computed this data above, but it is not
            // stored in an easy-to-index format. for now it's quicker just to recompute
            // the four dot products.
            m11 = DDOT44(Ra, code1, Rb, a1);
            m12 = DDOT44(Ra, code1, Rb, a2);
            m21 = DDOT44(Ra, code2, Rb, a1);
            m22 = DDOT44(Ra, code2, Rb, a2);
            {
                float k1 = m11 * Sb[a1];
                float k2 = m21 * Sb[a1];
                float k3 = m12 * Sb[a2];
                float k4 = m22 * Sb[a2];
                quad[0] = c1 - k1 - k3;
                quad[1] = c2 - k2 - k4;
                quad[2] = c1 - k1 + k3;
                quad[3] = c2 - k2 + k4;
                quad[4] = c1 + k1 + k3;
                quad[5] = c2 + k2 + k4;
                quad[6] = c1 + k1 - k3;
                quad[7] = c2 + k2 - k4;
            }

            // find the size of the reference face
            float[] rect = new float[2];
            rect[0] = Sa[code1];
            rect[1] = Sa[code2];

            // intersect the incident and reference faces
            float[] ret = s_ret;
            int n = IntersectRectQuad2(rect, quad, ret);
            if (n < 1)
            {
                return 0;		// this should never happen
            }

            // convert the intersection points into reference-face coordinates,
            // and compute the contact position and depth for each point. only keep
            // those points that have a positive (penetrating) depth. delete points in
            // the 'ret' array as necessary so that 'point' and 'ret' correspond.
            float[] point = s_point;		// penetrating contact points
            float[] dep = s_dep;			// depths for those points
            float det1 = 1f / (m11 * m22 - m12 * m21);
            m11 *= det1;
            m12 *= det1;
            m21 *= det1;
            m22 *= det1;
            int cnum = 0;			// number of penetrating contact points found
            for (int j = 0; j < n; j++)
            {
                float k1 = m22 * (ret[j * 2] - c1) - m12 * (ret[j * 2 + 1] - c2);
                float k2 = -m21 * (ret[j * 2] - c1) + m11 * (ret[j * 2 + 1] - c2);
                for (int i = 0; i < 3; i++)
                {
                    point[cnum * 3 + i] = center[i] + k1 * Rb[i * 4 + a1] + k2 * Rb[i * 4 + a2];
                }
                dep[cnum] = Sa[codeN] - DDOT(ref normal2, 0, point, cnum * 3);
                if (dep[cnum] >= 0)
                {
                    ret[cnum * 2] = ret[j * 2];
                    ret[cnum * 2 + 1] = ret[j * 2 + 1];
                    cnum++;
                }
            }
            if (cnum < 1)
            {
                return 0;	// this should never happen
            }

            // we can't generate more contacts than we actually have
            if (maxc > cnum)
            {
                maxc = cnum;
            }
            if (maxc < 1)
            {
                maxc = 1;
            }

            if (cnum <= maxc)
            {
                if (code < 4)
                {
                    // we have less contacts than we need, so we use them all
                    for (int j = 0; j < cnum; j++)
                    {
                        IndexedVector3 pointInWorldFA = new IndexedVector3(); ;
                        for (int i = 0; i < 3; i++)
                        {
                            pointInWorldFA[i] = point[j * 3 + i] + pa[i];
                        }
                        Vector3 pointInWorld = pointInWorldFA.ToVector3();
                        if (BulletGlobals.g_streamWriter != null && debugBoxBox)
                        {
                            MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "boxbox get closest", pointInWorld);
                        }

                        output.AddContactPoint((-normal).ToVector3(), pointInWorld, -dep[j]);
                    }
                }
                else
                {
                    // we have less contacts than we need, so we use them all
                    for (int j = 0; j < cnum; j++)
                    {
                        IndexedVector3 pointInWorld = new IndexedVector3(); ;
                        for (int i = 0; i < 3; i++)
                        {
                            pointInWorld[i] = point[j * 3 + i] + pa[i];

                        }

                        if (BulletGlobals.g_streamWriter != null && debugBoxBox)
                        {
                            MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "boxbox get closest", pointInWorld.ToVector3());
                        }
                        output.AddContactPoint((-normal).ToVector3(), pointInWorld.ToVector3(), -dep[j]);
                    }
                }
            }
            else
            {
                // we have more contacts than are wanted, some of them must be culled.
                // find the deepest point, it is always the first contact.
                int i1 = 0;
                float maxdepth = dep[0];
                for (int i = 1; i < cnum; i++)
                {
                    if (dep[i] > maxdepth)
                    {
                        maxdepth = dep[i];
                        i1 = i;
                    }
                }

                int[] iret = new int[8];
                CullPoints2(cnum, ret, maxc, i1, iret);

                for (int j = 0; j < maxc; j++)
                {
                    //      dContactGeom *con = CONTACT(contact,skip*j);
                    //    for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i];
                    //  con->depth = dep[iret[j]];
                    IndexedVector3 posInWorldFA = new IndexedVector3(); ;
                    for (int i = 0; i < 3; i++)
                    {
                        posInWorldFA[i] = point[iret[j] * 3 + i] + pa[i];
                    }
                    Vector3 pointInWorld = posInWorldFA.ToVector3();

                    if (BulletGlobals.g_streamWriter != null && debugBoxBox)
                    {
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "boxbox get closest", pointInWorld);
                    }

                    output.AddContactPoint((-normal).ToVector3(), pointInWorld, -dep[iret[j]]);

                }
                cnum = maxc;
            }
            return_code = code;
            return cnum;
        }
        public static void ClipFaceAgainstHull(ref IndexedVector3 separatingNormal, ConvexPolyhedron hullA, ref IndexedMatrix transA, ObjectArray<IndexedVector3> worldVertsB1, float minDist, float maxDist, IDiscreteCollisionDetectorInterfaceResult resultOut)
        {
            ObjectArray<IndexedVector3> worldVertsB2 = new ObjectArray<IndexedVector3>();
            ObjectArray<IndexedVector3> pVtxIn = worldVertsB1;
            ObjectArray<IndexedVector3> pVtxOut = worldVertsB2;
            pVtxOut.Capacity = pVtxIn.Count;

            int closestFaceA = -1;
            {
                float dmin = float.MaxValue;
                for (int face = 0; face < hullA.m_faces.Count; face++)
                {
                    IndexedVector3 Normal = new IndexedVector3(hullA.m_faces[face].m_plane[0], hullA.m_faces[face].m_plane[1], hullA.m_faces[face].m_plane[2]);
                    IndexedVector3 faceANormalWS = transA._basis * Normal;

                    float d = IndexedVector3.Dot(faceANormalWS, separatingNormal);
                    if (d < dmin)
                    {
                        dmin = d;
                        closestFaceA = face;
                    }
                }
            }
            if (closestFaceA < 0)
                return;

            Face polyA = hullA.m_faces[closestFaceA];

            // clip polygon to back of planes of all faces of hull A that are adjacent to witness face
            int numContacts = pVtxIn.Count;
            int numVerticesA = polyA.m_indices.Count;
            for (int e0 = 0; e0 < numVerticesA; e0++)
            {
		        IndexedVector3 a = hullA.m_vertices[polyA.m_indices[e0]];
                IndexedVector3 b = hullA.m_vertices[polyA.m_indices[(e0 + 1) % numVerticesA]];
                IndexedVector3 edge0 = a - b;
                IndexedVector3 WorldEdge0 = transA._basis * edge0;
                IndexedVector3 worldPlaneAnormal1 = transA._basis * new IndexedVector3(polyA.m_plane[0], polyA.m_plane[1], polyA.m_plane[2]);

                IndexedVector3 planeNormalWS1 = -WorldEdge0.Cross(worldPlaneAnormal1);//.cross(WorldEdge0);
                IndexedVector3 worldA1 = transA * a;
		        float planeEqWS1 = -worldA1.Dot(planeNormalWS1);
		
//int otherFace=0;
#if BLA1
		int otherFace = polyA.m_connectedFaces[e0];
		btVector3 localPlaneNormal (hullA.m_faces[otherFace].m_plane[0],hullA.m_faces[otherFace].m_plane[1],hullA.m_faces[otherFace].m_plane[2]);
		btScalar localPlaneEq = hullA.m_faces[otherFace].m_plane[3];

		btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal;
		btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin());
#else 
                IndexedVector3 planeNormalWS = planeNormalWS1;
		float planeEqWS=planeEqWS1;
		
#endif                //clip face

                ClipFace(pVtxIn, pVtxOut, ref planeNormalWS, planeEqWS);

                //btSwap(pVtxIn,pVtxOut);
                ObjectArray<IndexedVector3> temp = pVtxIn;
                pVtxIn = pVtxOut;
                pVtxOut = temp;

                pVtxOut.Clear();
            }



            //#define ONLY_REPORT_DEEPEST_POINT

            IndexedVector3 point;


            // only keep points that are behind the witness face
            {
                IndexedVector3 localPlaneNormal = new IndexedVector3(polyA.m_plane[0], polyA.m_plane[1], polyA.m_plane[2]);
                float localPlaneEq = polyA.m_plane[3];
                IndexedVector3 planeNormalWS = transA._basis * localPlaneNormal;
                float planeEqWS = localPlaneEq - IndexedVector3.Dot(planeNormalWS, transA._origin);
                for (int i = 0; i < pVtxIn.Count; i++)
                {

                    float depth = IndexedVector3.Dot(planeNormalWS, pVtxIn[i]) + planeEqWS;
                    if (depth <= minDist)
                    {
                        //				printf("clamped: depth=%f to minDist=%f\n",depth,minDist);
                        depth = minDist;
                    }

                    if (depth <= maxDist && depth >= minDist)
                    {
                        IndexedVector3 point2 = pVtxIn[i];
#if ONLY_REPORT_DEEPEST_POINT
				curMaxDist = depth;
#else
#if false
				if (depth<-3)
				{
					printf("error in btPolyhedralContactClipping depth = %f\n", depth);
					printf("likely wrong separatingNormal passed in\n");
				} 
#endif
                        resultOut.AddContactPoint(ref separatingNormal, ref point2, depth);
#endif
                    }
                }
            }
#if ONLY_REPORT_DEEPEST_POINT
	if (curMaxDist<maxDist)
	{
		resultOut.AddContactPoint(ref separatingNormal,ref point,curMaxDist);
	}
#endif //ONLY_REPORT_DEEPEST_POINT


        }
 public virtual void GetClosestPoints(ref ClosestPointInput input, IDiscreteCollisionDetectorInterfaceResult output, IDebugDraw debugDraw)
 {
     GetClosestPoints(ref input, output, debugDraw, false);
 }
 public static void ClipHullAgainstHull(IndexedVector3 separatingNormal, ConvexPolyhedron hullA, ConvexPolyhedron hullB, IndexedMatrix transA, IndexedMatrix transB, float minDist, float maxDist, IDiscreteCollisionDetectorInterfaceResult resultOut)
 {
     ClipHullAgainstHull(ref separatingNormal, hullA, hullB, ref transA, ref transB, minDist, maxDist, resultOut);
 }
 public virtual void GetClosestPoints(ref ClosestPointInput input, IDiscreteCollisionDetectorInterfaceResult output, IDebugDraw debugDraw, bool swapResults)
 {
     GetClosestPointsNonVirtual(ref input, output, debugDraw);
 }
Exemple #18
0
        private static int DBoxBox2(ref IndexedVector3 p1, float[] R1,
                                    ref IndexedVector3 side1, ref IndexedVector3 p2,
                                    float[] R2, ref IndexedVector3 side2,
                                    ref IndexedVector3 normal, ref float depth, ref int return_code,
                                    int maxc, Object contact, int skip, IDiscreteCollisionDetectorInterfaceResult output)
        {
            //IndexedVector3 centerDifference = IndexedVector3.Zero, ppv = IndexedVector3.Zero;
            float[] normalR             = null;
            int     normalROffsetResult = 0;


            IndexedVector3 A = side1 * 0.5f;
            IndexedVector3 B = side2 * 0.5f;

            int  code;
            bool invert_normal;
            // get vector from centers of box 1 to box 2, relative to box 1
            IndexedVector3 p = p2 - p1;

            IndexedVector3 pp = new IndexedVector3();

            DMULTIPLY1_331(ref pp, R1, ref p);          // get pp = p relative to body 1

            // for all 15 possible separating axes:
            //   * see if the axis separates the boxes. if so, return 0.
            //   * find the depth of the penetration along the separating axis (s2)
            //   * if this is the largest depth so far, record it.
            // the normal vector will be set to the separating axis with the smallest
            // depth. note: normalR is set to point to a column of R1 or R2 if that is
            // the smallest depth normal so far. otherwise normalR is 0 and normalC is
            // set to a vector relative to body 1. invert_normal is 1 if the sign of
            // the normal should be flipped.

            float R11 = DDOT44(R1, 0, R2, 0);
            float R12 = DDOT44(R1, 0, R2, 1);
            float R13 = DDOT44(R1, 0, R2, 2);
            float R21 = DDOT44(R1, 1, R2, 0);
            float R22 = DDOT44(R1, 1, R2, 1);
            float R23 = DDOT44(R1, 1, R2, 2);
            float R31 = DDOT44(R1, 2, R2, 0);
            float R32 = DDOT44(R1, 2, R2, 1);
            float R33 = DDOT44(R1, 2, R2, 2);

            float Q11 = Math.Abs(R11);
            float Q12 = Math.Abs(R12);
            float Q13 = Math.Abs(R13);

            float Q21 = Math.Abs(R21);
            float Q22 = Math.Abs(R22);
            float Q23 = Math.Abs(R23);

            float Q31 = Math.Abs(R31);
            float Q32 = Math.Abs(R32);
            float Q33 = Math.Abs(R33);

            float s = -float.MaxValue;

            invert_normal = false;
            code          = 0;

            int normalROffset = 0;

            // separating axis = u1,u2,u3
            if (TST(pp.X, (A.X + B.X * Q11 + B.Y * Q12 + B.Z * Q13), R1, ref normalR, 0, ref normalROffset, 1, ref code, ref s, ref invert_normal))
            {
                return(0);
            }
            if (TST(pp.Y, (A.Y + B.X * Q21 + B.Y * Q22 + B.Z * Q23), R1, ref normalR, 1, ref normalROffset, 2, ref code, ref s, ref invert_normal))
            {
                return(0);
            }
            if (TST(pp.Z, (A.Z + B.X * Q31 + B.Y * Q32 + B.Z * Q33), R1, ref normalR, 2, ref normalROffset, 3, ref code, ref s, ref invert_normal))
            {
                return(0);
            }

            // separating axis = v1,v2,v3
            if (TST(DDOT41(R2, 0, ref p, 0), (A.X * Q11 + A.Y * Q21 + A.Z * Q31 + B.X), R2, ref normalR, 0, ref normalROffset, 4, ref code, ref s, ref invert_normal))
            {
                return(0);
            }
            if (TST(DDOT41(R2, 1, ref p, 0), (A.X * Q12 + A.Y * Q22 + A.Z * Q32 + B.Y), R2, ref normalR, 1, ref normalROffset, 5, ref code, ref s, ref invert_normal))
            {
                return(0);
            }
            if (TST(DDOT41(R2, 2, ref p, 0), (A.X * Q13 + A.Y * Q23 + A.Z * Q33 + B.Z), R2, ref normalR, 2, ref normalROffset, 6, ref code, ref s, ref invert_normal))
            {
                return(0);
            }

            // note: cross product axes need to be scaled when s is computed.
            // normal (n1,n2,n3) is relative to box 1.
            // separating axis = u1 x (v1,v2,v3)
            //private static bool TST2(float expr1,float expr2,ref IndexedVector3 normal, ref IndexedVector3 normalC,int cc,ref int code)

            IndexedVector3 normalC = new IndexedVector3();

            float fudge2 = 1.0e-5f;

            Q11 += fudge2;
            Q12 += fudge2;
            Q13 += fudge2;

            Q21 += fudge2;
            Q22 += fudge2;
            Q23 += fudge2;

            Q31 += fudge2;
            Q32 += fudge2;
            Q33 += fudge2;

            // separating axis = u1 x (v1,v2,v3)
            if (TST2(pp.Z * R21 - pp.Y * R31, (A.Y * Q31 + A.Z * Q21 + B.Y * Q13 + B.Z * Q12), 0, -R31, R21, ref normalC, ref normalR, 7, ref code, ref s, ref invert_normal))
            {
                return(0);
            }
            if (TST2(pp.Z * R22 - pp.Y * R32, (A.Y * Q32 + A.Z * Q22 + B.X * Q13 + B.Z * Q11), 0, -R32, R22, ref normalC, ref normalR, 8, ref code, ref s, ref invert_normal))
            {
                return(0);
            }
            if (TST2(pp.Z * R23 - pp.Y * R33, (A.Y * Q33 + A.Z * Q23 + B.X * Q12 + B.Y * Q11), 0, -R33, R23, ref normalC, ref normalR, 9, ref code, ref s, ref invert_normal))
            {
                return(0);
            }

            // separating axis = u2 x (v1,v2,v3)
            if (TST2(pp.X * R31 - pp.Z * R11, (A.X * Q31 + A.Z * Q11 + B.Y * Q23 + B.Z * Q22), R31, 0, -R11, ref normalC, ref normalR, 10, ref code, ref s, ref invert_normal))
            {
                return(0);
            }
            if (TST2(pp.X * R32 - pp.Z * R12, (A.X * Q32 + A.Z * Q12 + B.X * Q23 + B.Z * Q21), R32, 0, -R12, ref normalC, ref normalR, 11, ref code, ref s, ref invert_normal))
            {
                return(0);
            }
            if (TST2(pp.X * R33 - pp.Z * R13, (A.X * Q33 + A.Z * Q13 + B.X * Q22 + B.Y * Q21), R33, 0, -R13, ref normalC, ref normalR, 12, ref code, ref s, ref invert_normal))
            {
                return(0);
            }

            // separating axis = u3 x (v1,v2,v3)
            if (TST2(pp.Y * R11 - pp.X * R21, (A.X * Q21 + A.Y * Q11 + B.Y * Q33 + B.Z * Q32), -R21, R11, 0, ref normalC, ref normalR, 13, ref code, ref s, ref invert_normal))
            {
                return(0);
            }
            if (TST2(pp.Y * R12 - pp.X * R22, (A.X * Q22 + A.Y * Q12 + B.X * Q33 + B.Z * Q31), -R22, R12, 0, ref normalC, ref normalR, 14, ref code, ref s, ref invert_normal))
            {
                return(0);
            }
            if (TST2(pp.Y * R13 - pp.X * R23, (A.X * Q23 + A.Y * Q13 + B.X * Q32 + B.Y * Q31), -R23, R13, 0, ref normalC, ref normalR, 15, ref code, ref s, ref invert_normal))
            {
                return(0);
            }

            if (code == 0)
            {
                return(0);
            }
            // if we get to this point, the boxes interpenetrate. compute the normal
            // in global coordinates.
            if (normalR != null)
            {
                normal.X = normalR[0 + normalROffset];
                normal.Y = normalR[4 + normalROffset];
                normal.Z = normalR[8 + normalROffset];
            }
            else
            {
                DMULTIPLY0_331(ref normal, R1, ref normalC);
            }
            if (invert_normal)
            {
                normal = -normal;
            }
            depth = -s;

            // compute contact point(s)

            if (code > 6)
            {
                // an edge from box 1 touches an edge from box 2.
                // find a point pa on the intersecting edge of box 1
                IndexedVector3 pa1 = p1;

                for (int j = 0; j < 3; j++)
                {
                    float sign = (DDOT14(ref normal, 0, R1, j) > 0) ? 1.0f : -1.0f;
                    for (int i = 0; i < 3; i++)
                    {
                        pa1[i] += sign * A[j] * R1[i * 4 + j];
                    }
                }

                // find a point pb on the intersecting edge of box 2
                IndexedVector3 pb1 = p2;
                //for (i = 0; i < 3; i++) pb[i] = p2[i];
                for (int j = 0; j < 3; j++)
                {
                    float sign = (DDOT14(ref normal, 0, R2, j) > 0) ? -1.0f : 1.0f;
                    for (int i = 0; i < 3; i++)
                    {
                        pb1[i] += sign * B[j] * R2[i * 4 + j];
                    }
                }


                float          alpha, beta;
                IndexedVector3 ua = new IndexedVector3();
                IndexedVector3 ub = new IndexedVector3();
                for (int i = 0; i < 3; i++)
                {
                    ua[i] = R1[((code) - 7) / 3 + i * 4];
                }
                for (int i = 0; i < 3; i++)
                {
                    ub[i] = R2[((code) - 7) % 3 + i * 4];
                }

                DLineClosestApproach(ref pa1, ref ua, ref pb1, ref ub, out alpha, out beta);

                for (int i = 0; i < 3; i++)
                {
                    pa1[i] += ua[i] * alpha;
                }
                for (int i = 0; i < 3; i++)
                {
                    pb1[i] += ub[i] * beta;
                }

                {
                    //contact[0].pos[i] = float(0.5)*(pa[i]+pb[i]);
                    //contact[0].depth = *depth;

#if USE_CENTER_POINT
                    pointInWorld = (pa + pb) * 0.5f;
                    output.addContactPoint(-normal, pointInWorld, -depth);
#else
                    output.AddContactPoint(-normal, pb1, -depth);
#endif //
                    return_code = code;
                }
                return(1);
            }

            // okay, we have a face-something intersection (because the separating
            // axis is perpendicular to a face). define face 'a' to be the reference
            // face (i.e. the normal vector is perpendicular to this) and face 'b' to be
            // the incident face (the closest face of the other box).


            float[]        Ra;
            float[]        Rb;
            IndexedVector3 pa;
            IndexedVector3 pb;
            IndexedVector3 Sa;
            IndexedVector3 Sb;

            if (code <= 3)
            {
                Ra = R1;
                Rb = R2;
                pa = p1;
                pb = p2;
                Sa = A;
                Sb = B;
            }
            else
            {
                Ra = R2;
                Rb = R1;
                pa = p2;
                pb = p1;
                Sa = B;
                Sb = A;
            }

            // nr = normal vector of reference face dotted with axes of incident box.
            // anr = absolute values of nr.
            IndexedVector3 normal2;
            IndexedVector3 nr = new IndexedVector3();
            IndexedVector3 anr;


            if (code <= 3)
            {
                normal2 = normal;
            }
            else
            {
                normal2 = -normal;
            }
            DMULTIPLY1_331(ref nr, Rb, ref normal2);

            nr.Abs(out anr);

            // find the largest compontent of anr: this corresponds to the normal
            // for the indident face. the other axis numbers of the indicent face
            // are stored in a1,a2.
            int lanr, a1, a2;
            if (anr.Y > anr.X)
            {
                if (anr.Y > anr.Z)
                {
                    a1   = 0;
                    lanr = 1;
                    a2   = 2;
                }
                else
                {
                    a1   = 0;
                    a2   = 1;
                    lanr = 2;
                }
            }
            else
            {
                if (anr.X > anr.Z)
                {
                    lanr = 0;
                    a1   = 1;
                    a2   = 2;
                }
                else
                {
                    a1   = 0;
                    a2   = 1;
                    lanr = 2;
                }
            }

            // compute center point of incident face, in reference-face coordinates
            IndexedVector3 center;
            if (nr[lanr] < 0)
            {
                center = new IndexedVector3(
                    pb.X - pa.X + Sb[lanr] * Rb[lanr],
                    pb.Y - pa.Y + Sb[lanr] * Rb[lanr + 4],
                    pb.Z - pa.Z + Sb[lanr] * Rb[lanr + 8]
                    );
            }
            else
            {
                center = new IndexedVector3(
                    pb.X - pa.X - Sb[lanr] * Rb[lanr],
                    pb.Y - pa.Y - Sb[lanr] * Rb[lanr + 4],
                    pb.Z - pa.Z - Sb[lanr] * Rb[lanr + 8]
                    );
            }


            // find the normal and non-normal axis numbers of the reference box
            int codeN, code1, code2;
            if (code <= 3)
            {
                codeN = code - 1;
            }
            else
            {
                codeN = code - 4;
            }
            if (codeN == 0)
            {
                code1 = 1;
                code2 = 2;
            }
            else if (codeN == 1)
            {
                code1 = 0;
                code2 = 2;
            }
            else
            {
                code1 = 0;
                code2 = 1;
            }

            // find the four corners of the incident face, in reference-face coordinates
            float[] quad = s_quad;      // 2D coordinate of incident face (x,y pairs)
            float   c1, c2, m11, m12, m21, m22;
            c1 = DDOT14(ref center, 0, Ra, code1);
            c2 = DDOT14(ref center, 0, Ra, code2);

            // optimize this? - we have already computed this data above, but it is not
            // stored in an easy-to-index format. for now it's quicker just to recompute
            // the four dot products.
            m11 = DDOT44(Ra, code1, Rb, a1);
            m12 = DDOT44(Ra, code1, Rb, a2);
            m21 = DDOT44(Ra, code2, Rb, a1);
            m22 = DDOT44(Ra, code2, Rb, a2);
            {
                float k1 = m11 * Sb[a1];
                float k2 = m21 * Sb[a1];
                float k3 = m12 * Sb[a2];
                float k4 = m22 * Sb[a2];
                quad[0] = c1 - k1 - k3;
                quad[1] = c2 - k2 - k4;
                quad[2] = c1 - k1 + k3;
                quad[3] = c2 - k2 + k4;
                quad[4] = c1 + k1 + k3;
                quad[5] = c2 + k2 + k4;
                quad[6] = c1 + k1 - k3;
                quad[7] = c2 + k2 - k4;
            }

            // find the size of the reference face
            //float[] s_rectReferenceFace = new float[2];
            s_rectReferenceFace[0] = Sa[code1];
            s_rectReferenceFace[1] = Sa[code2];

            // intersect the incident and reference faces
            float[] ret = s_ret;
            int     n   = IntersectRectQuad2(s_rectReferenceFace, quad, ret);
            if (n < 1)
            {
                return(0);               // this should never happen
            }

            // convert the intersection points into reference-face coordinates,
            // and compute the contact position and depth for each point. only keep
            // those points that have a positive (penetrating) depth. delete points in
            // the 'ret' array as necessary so that 'point' and 'ret' correspond.
            float[] point = s_point;            // penetrating contact points
            float[] dep   = s_dep;              // depths for those points
            float   det1  = 1f / (m11 * m22 - m12 * m21);
            m11 *= det1;
            m12 *= det1;
            m21 *= det1;
            m22 *= det1;
            int cnum = 0;                       // number of penetrating contact points found
            for (int j = 0; j < n; j++)
            {
                float k1 = m22 * (ret[j * 2] - c1) - m12 * (ret[j * 2 + 1] - c2);
                float k2 = -m21 * (ret[j * 2] - c1) + m11 * (ret[j * 2 + 1] - c2);
                for (int i = 0; i < 3; i++)
                {
                    point[cnum * 3 + i] = center[i] + k1 * Rb[i * 4 + a1] + k2 * Rb[i * 4 + a2];
                }
                dep[cnum] = Sa[codeN] - DDOT(ref normal2, 0, point, cnum * 3);
                if (dep[cnum] >= 0)
                {
                    ret[cnum * 2]     = ret[j * 2];
                    ret[cnum * 2 + 1] = ret[j * 2 + 1];
                    cnum++;
                }
            }
            if (cnum < 1)
            {
                return(0);       // this should never happen
            }

            // we can't generate more contacts than we actually have
            if (maxc > cnum)
            {
                maxc = cnum;
            }
            if (maxc < 1)
            {
                maxc = 1;
            }

            if (cnum <= maxc)
            {
                if (code < 4)
                {
                    // we have less contacts than we need, so we use them all
                    for (int j = 0; j < cnum; j++)
                    {
                        IndexedVector3 pointInWorldFA = new IndexedVector3();;
                        for (int i = 0; i < 3; i++)
                        {
                            pointInWorldFA[i] = point[j * 3 + i] + pa[i];
                        }
                        if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugBoxBoxDetector)
                        {
                            MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "boxbox get closest", pointInWorldFA);
                        }

                        output.AddContactPoint(-normal, pointInWorldFA, -dep[j]);
                    }
                }
                else
                {
                    // we have less contacts than we need, so we use them all
                    for (int j = 0; j < cnum; j++)
                    {
                        IndexedVector3 pointInWorld = new IndexedVector3();;
                        for (int i = 0; i < 3; i++)
                        {
                            pointInWorld[i] = point[j * 3 + i] + pa[i];
                        }

                        if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugBoxBoxDetector)
                        {
                            MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "boxbox get closest", pointInWorld);
                        }
                        output.AddContactPoint(-normal, pointInWorld, -dep[j]);
                    }
                }
            }
            else
            {
                // we have more contacts than are wanted, some of them must be culled.
                // find the deepest point, it is always the first contact.
                int   i1       = 0;
                float maxdepth = dep[0];
                for (int i = 1; i < cnum; i++)
                {
                    if (dep[i] > maxdepth)
                    {
                        maxdepth = dep[i];
                        i1       = i;
                    }
                }

                int[] iret = new int[8];
                CullPoints2(cnum, ret, maxc, i1, iret);

                for (int j = 0; j < maxc; j++)
                {
                    //      dContactGeom *con = CONTACT(contact,skip*j);
                    //    for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i];
                    //  con->depth = dep[iret[j]];
                    IndexedVector3 posInWorldFA = new IndexedVector3();;
                    for (int i = 0; i < 3; i++)
                    {
                        posInWorldFA[i] = point[iret[j] * 3 + i] + pa[i];
                    }
                    IndexedVector3 pointInWorld = posInWorldFA;

                    if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugBoxBoxDetector)
                    {
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "boxbox get closest", pointInWorld);
                    }

                    output.AddContactPoint((-normal), pointInWorld, -dep[iret[j]]);
                }
                cnum = maxc;
            }
            return_code = code;
            return(cnum);
        }
        public void GetClosestPoints(ref ClosestPointInput input, IDiscreteCollisionDetectorInterfaceResult output, IDebugDraw debugDraw, bool swapResults)
        {
            IndexedMatrix transformA = input.m_transformA;
            IndexedMatrix transformB = input.m_transformB;

            IndexedVector3 point, normal;
            float timeOfImpact = 1f;
            float depth = 0f;
            //	output.m_distance = float(1e30);
            //move sphere into triangle space
            IndexedMatrix sphereInTr = transformB.InverseTimes(ref transformA);

            IndexedVector3 temp = sphereInTr._origin;
            if (Collide(ref temp, out point, out normal, ref depth, ref timeOfImpact, m_contactBreakingThreshold))
            {
                if (swapResults)
                {
                    IndexedVector3 normalOnB = transformB._basis * normal;
                    IndexedVector3 normalOnA = -normalOnB;
                    IndexedVector3 pointOnA = transformB * point + normalOnB * depth;
                    output.AddContactPoint(ref normalOnA, ref pointOnA, depth);
                }
                else
                {
                    IndexedVector3 p = transformB._basis * normal;
                    IndexedVector3 p2 = transformB * point;
                    output.AddContactPoint(ref p, ref p2, depth);
                }
            }
        }