コード例 #1
0
        public bool CalcPenDepth(ISimplexSolverInterface simplexSolver, ConvexShape convexA, ConvexShape convexB, ref IndexedMatrix transA, ref IndexedMatrix transB,
                                 ref IndexedVector3 v, ref IndexedVector3 pa, ref IndexedVector3 pb, IDebugDraw debugDraw)
        {
            bool check2d = convexA.IsConvex2d() && convexB.IsConvex2d();


            float          minProj = float.MaxValue;
            IndexedVector3 minNorm = IndexedVector3.Zero;
            IndexedVector3 minA = IndexedVector3.Zero, minB = IndexedVector3.Zero;
            IndexedVector3 seperatingAxisInA, seperatingAxisInB;
            IndexedVector3 pInA, qInB, pWorld, qWorld, w;

#if USE_BATCHED_SUPPORT
            IndexedVector4[] supportVerticesABatch  = new IndexedVector4[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
            IndexedVector4[] supportVerticesBBatch  = new IndexedVector4[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
            IndexedVector3[] seperatingAxisInABatch = new IndexedVector3[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
            IndexedVector3[] seperatingAxisInBBatch = new IndexedVector3[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];


            int numSampleDirections = NUM_UNITSPHERE_POINTS;

            for (int i = 0; i < numSampleDirections; i++)
            {
                IndexedVector3 norm    = sPenetrationDirections[i];
                IndexedVector3 negNorm = -norm;

                IndexedBasisMatrix.Multiply(ref seperatingAxisInABatch[i], ref negNorm, ref transA._basis);
                IndexedBasisMatrix.Multiply(ref seperatingAxisInBBatch[i], ref norm, ref transB._basis);
                //seperatingAxisInABatch[i] = (-norm) * transA._basis;
                //seperatingAxisInBBatch[i] = norm * transB._basis;
            }

            {
                int numPDA = convexA.GetNumPreferredPenetrationDirections();
                if (numPDA > 0)
                {
                    for (int i = 0; i < numPDA; i++)
                    {
                        IndexedVector3 norm;
                        convexA.GetPreferredPenetrationDirection(i, out norm);
                        IndexedBasisMatrix.Multiply(ref norm, ref transA._basis, ref norm);
                        sPenetrationDirections[numSampleDirections] = norm;
                        IndexedVector3 negNorm = -norm;
                        IndexedBasisMatrix.Multiply(ref seperatingAxisInABatch[numSampleDirections], ref negNorm, ref transA._basis);
                        IndexedBasisMatrix.Multiply(ref seperatingAxisInBBatch[numSampleDirections], ref norm, ref transB._basis);
                        numSampleDirections++;
                    }
                }
            }

            {
                int numPDB = convexB.GetNumPreferredPenetrationDirections();
                if (numPDB > 0)
                {
                    for (int i = 0; i < numPDB; i++)
                    {
                        IndexedVector3 norm;
                        convexB.GetPreferredPenetrationDirection(i, out norm);
                        IndexedBasisMatrix.Multiply(ref norm, ref transB._basis, ref norm);
                        sPenetrationDirections[numSampleDirections] = norm;
                        IndexedVector3 negNorm = -norm;
                        IndexedBasisMatrix.Multiply(ref seperatingAxisInABatch[numSampleDirections], ref negNorm, ref transA._basis);
                        IndexedBasisMatrix.Multiply(ref seperatingAxisInBBatch[numSampleDirections], ref norm, ref transB._basis);
                        numSampleDirections++;
                    }
                }
            }

            convexA.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch, numSampleDirections);
            convexB.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch, numSampleDirections);

            for (int i = 0; i < numSampleDirections; i++)
            {
                IndexedVector3 norm = sPenetrationDirections[i];
                if (check2d)
                {
                    // shouldn't this be Y ?
                    norm.Z = 0;
                }
                if (norm.LengthSquared() > 0.01f)
                {
                    seperatingAxisInA = seperatingAxisInABatch[i];
                    seperatingAxisInB = seperatingAxisInBBatch[i];

                    pInA = new IndexedVector3(supportVerticesABatch[i].X, supportVerticesABatch[i].Y, supportVerticesABatch[i].Z);
                    qInB = new IndexedVector3(supportVerticesBBatch[i].X, supportVerticesBBatch[i].Y, supportVerticesBBatch[i].Z);

                    IndexedMatrix.Multiply(out pWorld, ref transA, ref pInA);
                    IndexedMatrix.Multiply(out qWorld, ref transB, ref qInB);
                    if (check2d)
                    {
                        // shouldn't this be Y ?
                        pWorld.Z = 0f;
                        qWorld.Z = 0f;
                    }

                    IndexedVector3.Subtract(out w, ref qWorld, ref pWorld);
                    float delta = IndexedVector3.Dot(ref norm, ref w);
                    //find smallest delta
                    if (delta < minProj)
                    {
                        minProj = delta;
                        minNorm = norm;
                        minA    = pWorld;
                        minB    = qWorld;
                    }
                }
            }
#else
            int numSampleDirections = NUM_UNITSPHERE_POINTS;

            {
                int numPDA = convexA.GetNumPreferredPenetrationDirections();
                if (numPDA > 0)
                {
                    for (int i = 0; i < numPDA; i++)
                    {
                        IndexedVector3 norm;
                        convexA.GetPreferredPenetrationDirection(i, out norm);
                        norm = IndexedVector3.TransformNormal(norm, transA);
                        sPenetrationDirections[numSampleDirections] = norm;
                        numSampleDirections++;
                    }
                }
            }

            {
                int numPDB = convexB.GetNumPreferredPenetrationDirections();
                if (numPDB > 0)
                {
                    for (int i = 0; i < numPDB; i++)
                    {
                        IndexedVector3 norm = IndexedVector3.Zero;
                        convexB.GetPreferredPenetrationDirection(i, out norm);
                        norm = IndexedVector3.TransformNormal(norm, transB);
                        sPenetrationDirections[numSampleDirections] = norm;
                        numSampleDirections++;
                    }
                }
            }

            for (int i = 0; i < numSampleDirections; i++)
            {
                IndexedVector3 norm = sPenetrationDirections[i];
                if (check2d)
                {
                    norm.Z = 0f;
                }
                if (norm.LengthSquared() > 0.01f)
                {
                    seperatingAxisInA = IndexedVector3.TransformNormal(-norm, transA);
                    seperatingAxisInB = IndexedVector3.TransformNormal(norm, transB);
                    pInA   = convexA.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInA);
                    qInB   = convexB.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInB);
                    pWorld = IndexedVector3.Transform(pInA, transA);
                    qWorld = IndexedVector3.Transform(qInB, transB);
                    if (check2d)
                    {
                        pWorld.Z = 0.0f;
                        qWorld.Z = 0.0f;
                    }

                    w = qWorld - pWorld;
                    float delta = IndexedVector3.Dot(norm, w);
                    //find smallest delta
                    if (delta < minProj)
                    {
                        minProj = delta;
                        minNorm = norm;
                        minA    = pWorld;
                        minB    = qWorld;
                    }
                }
            }
#endif //USE_BATCHED_SUPPORT

            //add the margins

            minA += minNorm * convexA.GetMarginNonVirtual();
            minB -= minNorm * convexB.GetMarginNonVirtual();
            //no penetration
            if (minProj < 0f)
            {
                return(false);
            }

            float extraSeparation = 0.5f;///scale dependent
            minProj += extraSeparation + (convexA.GetMarginNonVirtual() + convexB.GetMarginNonVirtual());

#if DEBUG_DRAW
            if (debugDraw)
            {
                IndexedVector3 color = new IndexedVector3(0, 1, 0);
                debugDraw.drawLine(minA, minB, color);
                color = new IndexedVector3(1, 1, 1);
                IndexedVector3 vec  = minB - minA;
                float          prj2 = IndexedVector3.Dot(minNorm, vec);
                debugDraw.drawLine(minA, minA + (minNorm * minProj), color);
            }
#endif //DEBUG_DRAW



            GjkPairDetector gjkdet = BulletGlobals.GjkPairDetectorPool.Get();
            gjkdet.Initialize(convexA, convexB, simplexSolver, null);

            float          offsetDist = minProj;
            IndexedVector3 offset     = minNorm * offsetDist;

            ClosestPointInput input = ClosestPointInput.Default();

            IndexedVector3 newOrg = transA._origin + offset;

            IndexedMatrix displacedTrans = transA;
            displacedTrans._origin = newOrg;

            input.m_transformA             = displacedTrans;
            input.m_transformB             = transB;
            input.m_maximumDistanceSquared = float.MaxValue;

            MinkowskiIntermediateResult res = new MinkowskiIntermediateResult();
            gjkdet.SetCachedSeperatingAxis(-minNorm);

            gjkdet.GetClosestPoints(ref input, res, debugDraw, false);

            float correctedMinNorm = minProj - res.m_depth;

            //the penetration depth is over-estimated, relax it
            float penetration_relaxation = 1f;
            minNorm *= penetration_relaxation;

            if (res.m_hasResult)
            {
                pa = res.m_pointInWorld - minNorm * correctedMinNorm;
                pb = res.m_pointInWorld;
                v  = minNorm;

#if DEBUG_DRAW
                if (debugDraw != null)
                {
                    IndexedVector3 color = new IndexedVector3(1, 0, 0);
                    debugDraw.drawLine(pa, pb, color);
                }
#endif//DEBUG_DRAW
            }

            BulletGlobals.GjkPairDetectorPool.Free(gjkdet);
            return(res.m_hasResult);
        }
コード例 #2
0
        public bool CalcPenDepth(ISimplexSolverInterface simplexSolver, ConvexShape convexA, ConvexShape convexB, ref Matrix transA, ref Matrix transB,
                ref Vector3 v, ref Vector3 pa, ref Vector3 pb, IDebugDraw debugDraw)
        {
            bool check2d = convexA.IsConvex2D() && convexB.IsConvex2D();


            float minProj = float.MaxValue;
            Vector3 minNorm = Vector3.Zero;
            Vector3 minA = Vector3.Zero, minB = Vector3.Zero;
            Vector3 seperatingAxisInA, seperatingAxisInB;
            Vector3 pInA, qInB, pWorld, qWorld, w;

#if USE_BATCHED_SUPPORT


            IList<Vector4> supportVerticesABatch = new ObjectArray<Vector4>(NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2);
            IList<Vector4> supportVerticesBBatch = new ObjectArray<Vector4>(NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2);
            IList<Vector3> seperatingAxisInABatch = new ObjectArray<Vector3>(NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2);
            IList<Vector3> seperatingAxisInBBatch = new ObjectArray<Vector3>(NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2);


            int numSampleDirections = NUM_UNITSPHERE_POINTS;

            for (int i = 0; i < numSampleDirections; i++)
            {
                Vector3 norm = sPenetrationDirections[i];
				seperatingAxisInABatch[i] = MathUtil.TransposeTransformNormal(-norm, transA);
				seperatingAxisInBBatch[i] = MathUtil.TransposeTransformNormal(norm, transB);
            }

            {
                int numPDA = convexA.GetNumPreferredPenetrationDirections();
                if (numPDA > 0)
                {
                    for (int i = 0; i < numPDA; i++)
                    {
                        Vector3 norm = Vector3.Up;
                        convexA.GetPreferredPenetrationDirection(i, ref norm);
                        norm = Vector3.TransformNormal(norm, transA);
                        sPenetrationDirections[numSampleDirections] = norm;
                        seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal(-norm, transA);
                        seperatingAxisInBBatch[numSampleDirections] = Vector3.Transform(norm, transB);
                        numSampleDirections++;
                    }
                }
            }

            {
                int numPDB = convexB.GetNumPreferredPenetrationDirections();
                if (numPDB > 0)
                {
                    for (int i = 0; i < numPDB; i++)
                    {
                        Vector3 norm = Vector3.Up;
                        convexB.GetPreferredPenetrationDirection(i, ref norm);
                        norm = Vector3.TransformNormal(norm, transB);
                        sPenetrationDirections[numSampleDirections] = norm;
                        seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal(-norm, transA);
                        seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transB);
                        numSampleDirections++;
                    }
                }
            }

            convexA.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch, numSampleDirections);
            convexB.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch, numSampleDirections);

            for (int i = 0; i < numSampleDirections; i++)
            {
                Vector3 norm = sPenetrationDirections[i];
                if (check2d)
                {
                    // shouldn't this be Y ?
                    norm.Z = 0;
                }
                seperatingAxisInA = seperatingAxisInABatch[i];
                seperatingAxisInB = seperatingAxisInBBatch[i];

                pInA = new Vector3(supportVerticesABatch[i].X, supportVerticesABatch[i].Y, supportVerticesABatch[i].Z);
                qInB = new Vector3(supportVerticesBBatch[i].X, supportVerticesBBatch[i].Y, supportVerticesBBatch[i].Z);

                pWorld = Vector3.Transform(pInA, transA);
                qWorld = Vector3.Transform(qInB, transB);
                if (check2d)
                {
                    // shouldn't this be Y ?
                    pWorld.Z = 0f;
                    qWorld.Z = 0f;
                }


                w = qWorld - pWorld;
                float delta = Vector3.Dot(norm, w);
                //find smallest delta
                if (delta < minProj)
                {
                    minProj = delta;
                    minNorm = norm;
                    minA = pWorld;
                    minB = qWorld;
                }
            }
#else
            int numSampleDirections = NUM_UNITSPHERE_POINTS;

	        {
		        int numPDA = convexA.getNumPreferredPenetrationDirections();
		        if (numPDA > 0)
		        {
			        for (int i=0;i<numPDA;i++)
			        {
				        Vector3 norm = Vector3.Zero;
				        convexA.getPreferredPenetrationDirection(i,ref norm);
				        norm  = Vector3.TransformNormal(norm,transA);
				        sPenetrationDirections[numSampleDirections] = norm;
				        numSampleDirections++;
			        }
		        }
	        }

	        {
		        int numPDB = convexB.getNumPreferredPenetrationDirections();
		        if (numPDB > 0)
		        {
			        for (int i=0;i<numPDB;i++)
			        {
                        Vector3 norm = Vector3.Zero;
				        convexB.getPreferredPenetrationDirection(i,ref norm);
				        norm  = Vector3.TransformNormal(norm,transB);
				        sPenetrationDirections[numSampleDirections] = norm;
				        numSampleDirections++;
			        }
		        }
	        }

	        for (int i=0;i<numSampleDirections;i++)
	        {
		        Vector3 norm = sPenetrationDirections[i];
		        if (check2d)
		        {
			        norm.Z = 0f;
		        }
                if (norm.LengthSquared() > 0.01f)
                {
                    seperatingAxisInA = Vector3.TransformNormal(-norm, transA);
                    seperatingAxisInB = Vector3.TransformNormal(norm, transB);
                    pInA = convexA.localGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInA);
                    qInB = convexB.localGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInB);
                    pWorld = Vector3.Transform(pInA, transA);
                    qWorld = Vector3.Transform(qInB, transB);
                    if (check2d)
                    {
                        pWorld.Z = 0.0f;
                        qWorld.Z = 0.0f;
                    }

                    w = qWorld - pWorld;
                    float delta = Vector3.Dot(norm, w);
                    //find smallest delta
                    if (delta < minProj)
                    {
                        minProj = delta;
                        minNorm = norm;
                        minA = pWorld;
                        minB = qWorld;
                    }
                }
	        }
#endif //USE_BATCHED_SUPPORT

            //add the margins

            minA += minNorm * convexA.GetMarginNonVirtual();
            minB -= minNorm * convexB.GetMarginNonVirtual();
            //no penetration
            if (minProj < 0f)
            {
                return false;
            }

            float extraSeparation = 0.5f;///scale dependent
            minProj += extraSeparation + (convexA.GetMarginNonVirtual() + convexB.GetMarginNonVirtual());

#if DEBUG_DRAW
	        if (debugDraw)
	        {
		        Vector3 color = new Vector3(0,1,0);
		        debugDraw.drawLine(minA,minB,color);
		        color = new Vector3(1,1,1);
		        Vector3 vec = minB-minA;
		        float prj2 = Vector3.Dot(minNorm,vec);
		        debugDraw.drawLine(minA,minA+(minNorm*minProj),color);

	        }
#endif //DEBUG_DRAW



            GjkPairDetector gjkdet = new GjkPairDetector(convexA, convexB, simplexSolver, null);

            float offsetDist = minProj;
            Vector3 offset = minNorm * offsetDist;

            ClosestPointInput input = new ClosestPointInput();

            Vector3 newOrg = transA.Translation + offset;

            Matrix displacedTrans = transA;
            displacedTrans.Translation = newOrg;

            input.m_transformA = displacedTrans;
            input.m_transformB = transB;
            input.m_maximumDistanceSquared = float.MaxValue;

            MinkowskiIntermediateResult res = new MinkowskiIntermediateResult();
            Vector3 temp = -minNorm;
            gjkdet.SetCachedSeperatingAxis(-minNorm);

            gjkdet.GetClosestPoints(input, res, debugDraw,false);

            float correctedMinNorm = minProj - res.m_depth;

            //the penetration depth is over-estimated, relax it
            float penetration_relaxation = 1f;
            minNorm *= penetration_relaxation;

            if (res.m_hasResult)
            {

                pa = res.m_pointInWorld - minNorm * correctedMinNorm;
                pb = res.m_pointInWorld;
                v = minNorm;

#if DEBUG_DRAW
		        if (debugDraw != null)
		        {
			        Vector3 color = new Vector3(1,0,0);
			        debugDraw.drawLine(pa,pb,color);
		        }
#endif//DEBUG_DRAW


            }
            return res.m_hasResult;
        }
コード例 #3
0
        public bool CalcPenDepth(ISimplexSolverInterface simplexSolver, ConvexShape convexA, ConvexShape convexB, ref IndexedMatrix transA, ref IndexedMatrix transB,
                ref IndexedVector3 v, ref IndexedVector3 pa, ref IndexedVector3 pb, IDebugDraw debugDraw)
        {
            bool check2d = convexA.IsConvex2d() && convexB.IsConvex2d();


            float minProj = float.MaxValue;
            IndexedVector3 minNorm = IndexedVector3.Zero;
            IndexedVector3 minA = IndexedVector3.Zero, minB = IndexedVector3.Zero;
            IndexedVector3 seperatingAxisInA, seperatingAxisInB;
            IndexedVector3 pInA, qInB, pWorld, qWorld, w;

#if USE_BATCHED_SUPPORT


            IndexedVector4[] supportVerticesABatch = new IndexedVector4[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
            IndexedVector4[] supportVerticesBBatch = new IndexedVector4[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
            IndexedVector3[] seperatingAxisInABatch = new IndexedVector3[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
            IndexedVector3[] seperatingAxisInBBatch = new IndexedVector3[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];


            int numSampleDirections = NUM_UNITSPHERE_POINTS;

            for (int i = 0; i < numSampleDirections; i++)
            {
                IndexedVector3 norm = sPenetrationDirections[i];
                IndexedVector3 negNorm = -norm;

                IndexedBasisMatrix.Multiply(ref seperatingAxisInABatch[i], ref negNorm, ref transA._basis);
                IndexedBasisMatrix.Multiply(ref seperatingAxisInBBatch[i], ref norm, ref transB._basis);
                //seperatingAxisInABatch[i] = (-norm) * transA._basis;
                //seperatingAxisInBBatch[i] = norm * transB._basis;
            }

            {
                int numPDA = convexA.GetNumPreferredPenetrationDirections();
                if (numPDA > 0)
                {
                    for (int i = 0; i < numPDA; i++)
                    {
                        IndexedVector3 norm;
                        convexA.GetPreferredPenetrationDirection(i, out norm);
                        IndexedBasisMatrix.Multiply(ref norm ,ref transA._basis ,ref norm);
                        sPenetrationDirections[numSampleDirections] = norm;
                        IndexedVector3 negNorm = -norm;
                        IndexedBasisMatrix.Multiply(ref seperatingAxisInABatch[numSampleDirections], ref negNorm,ref transA._basis);
                        IndexedBasisMatrix.Multiply(ref seperatingAxisInBBatch[numSampleDirections] ,ref norm ,ref transB._basis);
                        numSampleDirections++;
                    }
                }
            }

            {
                int numPDB = convexB.GetNumPreferredPenetrationDirections();
                if (numPDB > 0)
                {
                    for (int i = 0; i < numPDB; i++)
                    {
                        IndexedVector3 norm;
                        convexB.GetPreferredPenetrationDirection(i, out norm);
                        IndexedBasisMatrix.Multiply(ref norm, ref transB._basis, ref norm);
                        sPenetrationDirections[numSampleDirections] = norm;
                        IndexedVector3 negNorm = -norm;
                        IndexedBasisMatrix.Multiply(ref seperatingAxisInABatch[numSampleDirections],ref negNorm,ref transA._basis);
                        IndexedBasisMatrix.Multiply(ref seperatingAxisInBBatch[numSampleDirections],ref norm ,ref transB._basis);
                        numSampleDirections++;
                    }
                }
            }

            convexA.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch, numSampleDirections);
            convexB.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch, numSampleDirections);

            for (int i = 0; i < numSampleDirections; i++)
            {
                IndexedVector3 norm = sPenetrationDirections[i];
                if (check2d)
                {
                    // shouldn't this be Y ?
                    norm.Z = 0;
                }
                if (norm.LengthSquared() > 0.01f)
                {
                    seperatingAxisInA = seperatingAxisInABatch[i];
                    seperatingAxisInB = seperatingAxisInBBatch[i];

                    pInA = new IndexedVector3(supportVerticesABatch[i].X, supportVerticesABatch[i].Y, supportVerticesABatch[i].Z);
                    qInB = new IndexedVector3(supportVerticesBBatch[i].X, supportVerticesBBatch[i].Y, supportVerticesBBatch[i].Z);

                    IndexedMatrix.Multiply(out  pWorld, ref transA, ref pInA);
                    IndexedMatrix.Multiply(out  qWorld, ref transB, ref qInB);
                    if (check2d)
                    {
                        // shouldn't this be Y ?
                        pWorld.Z = 0f;
                        qWorld.Z = 0f;
                    }

                    IndexedVector3.Subtract(out w, ref qWorld, ref pWorld);
                    float delta = IndexedVector3.Dot(ref norm, ref w);
                    //find smallest delta
                    if (delta < minProj)
                    {
                        minProj = delta;
                        minNorm = norm;
                        minA = pWorld;
                        minB = qWorld;
                    }
                }
            }
#else
            int numSampleDirections = NUM_UNITSPHERE_POINTS;

	        {
		        int numPDA = convexA.GetNumPreferredPenetrationDirections();
		        if (numPDA > 0)
		        {
			        for (int i=0;i<numPDA;i++)
			        {
				        IndexedVector3 norm;
				        convexA.GetPreferredPenetrationDirection(i, out norm);
				        norm  = IndexedVector3.TransformNormal(norm,transA);
				        sPenetrationDirections[numSampleDirections] = norm;
				        numSampleDirections++;
			        }
		        }
	        }

	        {
		        int numPDB = convexB.GetNumPreferredPenetrationDirections();
		        if (numPDB > 0)
		        {
			        for (int i=0;i<numPDB;i++)
			        {
                        IndexedVector3 norm = IndexedVector3.Zero;
				        convexB.GetPreferredPenetrationDirection(i, out norm);
				        norm  = IndexedVector3.TransformNormal(norm,transB);
				        sPenetrationDirections[numSampleDirections] = norm;
				        numSampleDirections++;
			        }
		        }
	        }

	        for (int i=0;i<numSampleDirections;i++)
	        {
		        IndexedVector3 norm = sPenetrationDirections[i];
		        if (check2d)
		        {
			        norm.Z = 0f;
		        }
                if (norm.LengthSquared() > 0.01f)
                {
                    seperatingAxisInA = IndexedVector3.TransformNormal(-norm, transA);
                    seperatingAxisInB = IndexedVector3.TransformNormal(norm, transB);
                    pInA = convexA.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInA);
                    qInB = convexB.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInB);
                    pWorld = IndexedVector3.Transform(pInA, transA);
                    qWorld = IndexedVector3.Transform(qInB, transB);
                    if (check2d)
                    {
                        pWorld.Z = 0.0f;
                        qWorld.Z = 0.0f;
                    }

                    w = qWorld - pWorld;
                    float delta = IndexedVector3.Dot(norm, w);
                    //find smallest delta
                    if (delta < minProj)
                    {
                        minProj = delta;
                        minNorm = norm;
                        minA = pWorld;
                        minB = qWorld;
                    }
                }
	        }
#endif //USE_BATCHED_SUPPORT

            //add the margins

            minA += minNorm * convexA.GetMarginNonVirtual();
            minB -= minNorm * convexB.GetMarginNonVirtual();
            //no penetration
            if (minProj < 0f)
            {
                return false;
            }

            float extraSeparation = 0.5f;///scale dependent
            minProj += extraSeparation + (convexA.GetMarginNonVirtual() + convexB.GetMarginNonVirtual());

#if DEBUG_DRAW
	        if (debugDraw)
	        {
		        IndexedVector3 color = new IndexedVector3(0,1,0);
		        debugDraw.drawLine(minA,minB,color);
		        color = new IndexedVector3(1,1,1);
		        IndexedVector3 vec = minB-minA;
		        float prj2 = IndexedVector3.Dot(minNorm,vec);
		        debugDraw.drawLine(minA,minA+(minNorm*minProj),color);

	        }
#endif //DEBUG_DRAW



            GjkPairDetector gjkdet = BulletGlobals.GjkPairDetectorPool.Get();
            gjkdet.Initialize(convexA, convexB, simplexSolver, null);

            float offsetDist = minProj;
            IndexedVector3 offset = minNorm * offsetDist;

            ClosestPointInput input = ClosestPointInput.Default();

            IndexedVector3 newOrg = transA._origin + offset;

            IndexedMatrix displacedTrans = transA;
            displacedTrans._origin = newOrg;

            input.m_transformA = displacedTrans;
            input.m_transformB = transB;
            input.m_maximumDistanceSquared = float.MaxValue;

            MinkowskiIntermediateResult res = new MinkowskiIntermediateResult();
            gjkdet.SetCachedSeperatingAxis(-minNorm);

            gjkdet.GetClosestPoints(ref input, res, debugDraw, false);

            float correctedMinNorm = minProj - res.m_depth;

            //the penetration depth is over-estimated, relax it
            float penetration_relaxation = 1f;
            minNorm *= penetration_relaxation;

            if (res.m_hasResult)
            {

                pa = res.m_pointInWorld - minNorm * correctedMinNorm;
                pb = res.m_pointInWorld;
                v = minNorm;

#if DEBUG_DRAW
		        if (debugDraw != null)
		        {
			        IndexedVector3 color = new IndexedVector3(1,0,0);
			        debugDraw.drawLine(pa,pb,color);
		        }
#endif//DEBUG_DRAW


            }

            BulletGlobals.GjkPairDetectorPool.Free(gjkdet);
            return res.m_hasResult;
        }