public static void rayTestSingleInternal( ref btTransform rayFromTrans, ref btTransform rayToTrans, btCollisionShape collisionShape, btCollisionObject collisionObject, ref btTransform useTransform, RayResultCallback resultCallback ) { btSphereShape pointShape = BulletGlobals.SphereShapePool.Get(); pointShape.Initialize( btScalar.BT_ZERO ); pointShape.setMargin( 0f ); btConvexShape castShape = pointShape; //btCollisionShape collisionShape = collisionObjectWrap.getCollisionShape(); btTransform colObjWorldTransform = useTransform;// collisionObjectWrap.m_worldTransform; if( collisionShape.isConvex() ) { // CProfileSample sample = new CProfileSample("rayTestConvex"); btConvexCast.CastResult castResult = new btConvexCast.CastResult(); castResult.m_fraction = resultCallback.m_closestHitFraction; btConvexShape convexShape = (btConvexShape)collisionShape; btVoronoiSimplexSolver simplexSolver = BulletGlobals.VoronoiSimplexSolverPool.Get(); //new btVoronoiSimplexSolver(); btSubsimplexConvexCast subSimplexConvexCaster = BulletGlobals.SubSimplexConvexCastPool.Get(); subSimplexConvexCaster.Initialize( castShape, convexShape, simplexSolver ); btGjkConvexCast gjkConvexCaster = BulletGlobals.GjkConvexCastPool.Get(); gjkConvexCaster.Initialize( castShape, convexShape, simplexSolver ); BulletGlobals.SubSimplexConvexCastPool.Free( subSimplexConvexCaster ); BulletGlobals.VoronoiSimplexSolverPool.Free( simplexSolver ); //btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0); btConvexCast convexCasterPtr = null; //use kF_UseSubSimplexConvexCastRaytest by default if( ( resultCallback.m_flags & (uint)btTriangleRaycastCallback.EFlags.kF_UseGjkConvexCastRaytest ) != 0 ) convexCasterPtr = gjkConvexCaster; else convexCasterPtr = subSimplexConvexCaster; btConvexCast convexCaster = convexCasterPtr; if( convexCaster.calcTimeOfImpact( ref rayFromTrans, ref rayToTrans , ref useTransform/*collisionObjectWrap.m_worldTransform*/ , ref useTransform/*collisionObjectWrap.m_worldTransform*/ , castResult ) ) { //add hit if( castResult.m_normal.length2() > (double)( 0.0001 ) ) { if( castResult.m_fraction < resultCallback.m_closestHitFraction ) { //todo: figure out what this is about. When is rayFromTest.getBasis() not identity? #if USE_SUBSIMPLEX_CONVEX_CAST //rotate normal into worldspace castResult.m_normal = rayFromTrans.getBasis() castResult.m_normal; #endif //USE_SUBSIMPLEX_CONVEX_CAST castResult.m_normal.normalize(); LocalRayResult localRayResult = new LocalRayResult ( collisionObject, null, castResult.m_normal, castResult.m_fraction ); bool normalInWorldSpace = true; resultCallback.addSingleResult( localRayResult, normalInWorldSpace ); } } } BulletGlobals.GjkConvexCastPool.Free( gjkConvexCaster ); } else { if( collisionShape.isConcave() ) { btTransform worldTocollisionObject; colObjWorldTransform.inverse( out worldTocollisionObject ); btVector3 tmp; rayFromTrans.getOrigin( out tmp ); btVector3 rayFromLocal; worldTocollisionObject.Apply( ref tmp, out rayFromLocal ); rayToTrans.getOrigin( out tmp ); btVector3 rayToLocal; worldTocollisionObject.Apply( ref tmp, out rayToLocal ); // CProfileSample sample = new CProfileSample("rayTestConcave"); #if SUPPORT_TRIANGLE_MESH if( collisionShape.getShapeType() == BroadphaseNativeTypes.TRIANGLE_MESH_SHAPE_PROXYTYPE ) { ///optimized version for btBvhTriangleMeshShape btBvhTriangleMeshShape triangleMesh = (btBvhTriangleMeshShape)collisionShape; BridgeTriangleRaycastCallback rcb( rayFromLocal, rayToLocal,&resultCallback, collisionObjectWrap.getCollisionObject(), triangleMesh, colObjWorldTransform); rcb.m_hitFraction = resultCallback.m_closestHitFraction; triangleMesh.performRaycast( &rcb, rayFromLocal, rayToLocal ); } else #endif { //generic (slower) case btConcaveShape concaveShape = (btConcaveShape)collisionShape; //ConvexCast::CastResult BridgeTriangleRaycastCallback rcb = new BridgeTriangleRaycastCallback( ref rayFromLocal, ref rayToLocal, resultCallback , collisionObject, concaveShape, ref colObjWorldTransform ); rcb.m_hitFraction = resultCallback.m_closestHitFraction; btVector3 rayAabbMinLocal = rayFromLocal; rayAabbMinLocal.setMin( ref rayToLocal ); btVector3 rayAabbMaxLocal = rayFromLocal; rayAabbMaxLocal.setMax( ref rayToLocal ); concaveShape.processAllTriangles( rcb, ref rayAabbMinLocal, ref rayAabbMaxLocal ); } } else { // CProfileSample sample = new CProfileSample("rayTestCompound"); if( collisionShape.isCompound() ) { btCompoundShape compoundShape = (btCompoundShape)( collisionShape ); btDbvt dbvt = compoundShape.getDynamicAabbTree(); RayTester rayCB = new RayTester( collisionObject, compoundShape, ref colObjWorldTransform, ref rayFromTrans, ref rayToTrans, resultCallback ); #if !DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION if( dbvt != null ) { btTransform tmp; colObjWorldTransform.inverseTimes( ref rayFromTrans, out tmp ); btVector3 localRayFrom; tmp.getOrigin( out localRayFrom ); colObjWorldTransform.inverseTimes( ref rayToTrans, out tmp ); btVector3 localRayTo; tmp.getOrigin( out localRayTo ); btDbvt.rayTest( dbvt.m_root, ref localRayFrom, ref localRayTo, rayCB ); } else #endif //DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION { for( int i = 0, n = compoundShape.getNumChildShapes(); i < n; ++i ) { rayCB.Process( i ); } } } } } BulletGlobals.SphereShapePool.Free( pointShape ); }
// // Convex-Convex collision algorithm // internal override void processCollision( btCollisionObjectWrapper body0Wrap , ref btTransform body0Transform , btCollisionObjectWrapper body1Wrap , ref btTransform body1Transform , btDispatcherInfo dispatchInfo, btManifoldResult resultOut ) { if( m_manifoldPtr == null ) { //swapped? m_manifoldPtr = m_dispatcher.getNewManifold( body0Wrap.m_collisionObject, body1Wrap.m_collisionObject ); m_ownManifold = true; } resultOut.setPersistentManifold( m_manifoldPtr ); //comment-out next line to test multi-contact generation //resultOut.getPersistentManifold().clearManifold(); btConvexShape min0 = (btConvexShape)body0Wrap.getCollisionShape(); btConvexShape min1 = (btConvexShape)body1Wrap.getCollisionShape(); btVector3 normalOnB; btVector3 pointOnBWorld; #if !BT_DISABLE_CAPSULE_CAPSULE_COLLIDER if( ( min0.getShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE ) && ( min1.getShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE ) ) { btCapsuleShape capsuleA = (btCapsuleShape)min0; btCapsuleShape capsuleB = (btCapsuleShape)min1; // btVector3 localScalingA = capsuleA.getLocalScaling(); // btVector3 localScalingB = capsuleB.getLocalScaling(); double threshold = m_manifoldPtr.getContactBreakingThreshold(); double dist = capsuleCapsuleDistance( out normalOnB, out pointOnBWorld , capsuleA.getHalfHeight(), capsuleA.getRadius() , capsuleB.getHalfHeight(), capsuleB.getRadius() , capsuleA.getUpAxis(), capsuleB.getUpAxis() , ref body0Wrap.m_collisionObject.m_worldTransform, ref body1Wrap.m_collisionObject.m_worldTransform, threshold ); if( dist < threshold ) { Debug.Assert( normalOnB.length2() >= ( btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON ) ); resultOut.addContactPoint( ref normalOnB, ref pointOnBWorld, dist ); } resultOut.refreshContactPoints(); return; } #endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER #if USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil) { m_sepDistance.updateSeparatingDistance(body0.getWorldTransform(),body1.getWorldTransform()); } if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0) #endif //USE_SEPDISTANCE_UTIL2 { btGjkPairDetector.ClosestPointInput input = BulletGlobals.ClosestPointInputPool.Get(); input.Initialize(); btGjkPairDetector gjkPairDetector = BulletGlobals.GjkPairDetectorPool.Get(); gjkPairDetector.Initialize( min0, min1, m_simplexSolver, m_pdSolver ); //TODO: if (dispatchInfo.m_useContinuous) gjkPairDetector.setMinkowskiA( min0 ); gjkPairDetector.setMinkowskiB( min1 ); #if USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil) { input.m_maximumDistanceSquared = BT_LARGE_FLOAT; } else #endif //USE_SEPDISTANCE_UTIL2 { //if (dispatchInfo.m_convexMaxDistanceUseCPT) //{ // input.m_maximumDistanceSquared = min0.getMargin() + min1.getMargin() + m_manifoldPtr.getContactProcessingThreshold(); //} else //{ input.m_maximumDistanceSquared = min0.getMargin() + min1.getMargin() + m_manifoldPtr.getContactBreakingThreshold(); // } input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared; } input.m_transformA = body0Transform; input.m_transformB = body1Transform; #if USE_SEPDISTANCE_UTIL2 double sepDist = 0; if (dispatchInfo.m_useConvexConservativeDistanceUtil) { sepDist = gjkPairDetector.getCachedSeparatingDistance(); if (sepDist>SIMD_EPSILON) { sepDist += dispatchInfo.m_convexConservativeDistanceThreshold; //now perturbe directions to get multiple contact points } } #endif //USE_SEPDISTANCE_UTIL2 if( min0.isPolyhedral() && min1.isPolyhedral() ) { btDummyResult dummy = new btDummyResult(); ///btBoxShape is an exception: its vertices are created WITH margin so don't subtract it double min0Margin = min0.getShapeType() == BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE ? 0 : min0.getMargin(); double min1Margin = min1.getShapeType() == BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE ? 0 : min1.getMargin(); btWithoutMarginResult withoutMargin = new btWithoutMarginResult( resultOut, min0Margin, min1Margin ); btPolyhedralConvexShape polyhedronA = (btPolyhedralConvexShape)min0; btPolyhedralConvexShape polyhedronB = (btPolyhedralConvexShape)min1; if( polyhedronA.getConvexPolyhedron() != null && polyhedronB.getConvexPolyhedron() != null ) { double threshold = m_manifoldPtr.getContactBreakingThreshold(); double minDist = -1e30f; btVector3 sepNormalWorldSpace; bool foundSepAxis = true; if( dispatchInfo.m_enableSatConvex ) { foundSepAxis = btPolyhedralContactClipping.findSeparatingAxis( polyhedronA.getConvexPolyhedron(), polyhedronB.getConvexPolyhedron(), ref body0Wrap.m_collisionObject.m_worldTransform, ref body1Wrap.m_collisionObject.m_worldTransform, out sepNormalWorldSpace, resultOut ); } else { #if ZERO_MARGIN gjkPairDetector.setIgnoreMargin(true); gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); #else gjkPairDetector.getClosestPoints( input, withoutMargin, dispatchInfo.m_debugDraw ); //gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw); #endif //ZERO_MARGIN //double l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); //if (l2>SIMD_EPSILON) { sepNormalWorldSpace = withoutMargin.m_reportedNormalOnWorld;//gjkPairDetector.getCachedSeparatingAxis()*(1/l2); //minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance(); minDist = withoutMargin.m_reportedDistance;//gjkPairDetector.getCachedSeparatingDistance()+min0.getMargin()+min1.getMargin(); #if ZERO_MARGIN foundSepAxis = true;//gjkPairDetector.getCachedSeparatingDistance()<0; #else foundSepAxis = withoutMargin.m_foundResult && minDist < 0;//-(min0.getMargin()+min1.getMargin()); #endif } } if( foundSepAxis ) { // Console.WriteLine("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.x,sepNormalWorldSpace.y,sepNormalWorldSpace.z); btPolyhedralContactClipping.clipHullAgainstHull( ref sepNormalWorldSpace, polyhedronA.getConvexPolyhedron(), polyhedronB.getConvexPolyhedron(), ref body0Wrap.m_collisionObject.m_worldTransform, ref body1Wrap.m_collisionObject.m_worldTransform , minDist - threshold, threshold, resultOut ); } if( m_ownManifold ) { resultOut.refreshContactPoints(); } BulletGlobals.ClosestPointInputPool.Free( input ); BulletGlobals.GjkPairDetectorPool.Free( gjkPairDetector ); return; } else { //we can also deal with convex versus triangle (without connectivity data) if( polyhedronA.getConvexPolyhedron() != null && polyhedronB.getShapeType() == BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE ) { btVertexArray vertices = new btVertexArray(); btTriangleShape tri = (btTriangleShape)polyhedronB; btVector3 tmp; body1Transform.Apply( ref tri.m_vertices1, out tmp ); vertices.Add( ref tmp ); body1Transform.Apply( ref tri.m_vertices2, out tmp ); vertices.Add( ref tmp ); body1Transform.Apply( ref tri.m_vertices3, out tmp ); vertices.Add( ref tmp ); //tri.initializePolyhedralFeatures(); double threshold = m_manifoldPtr.getContactBreakingThreshold(); btVector3 sepNormalWorldSpace; double minDist = -btScalar.BT_LARGE_FLOAT; double maxDist = threshold; bool foundSepAxis = false; if( false ) { polyhedronB.initializePolyhedralFeatures(); foundSepAxis = btPolyhedralContactClipping.findSeparatingAxis( polyhedronA.getConvexPolyhedron(), polyhedronB.getConvexPolyhedron(), ref body0Wrap.m_collisionObject.m_worldTransform, ref body1Wrap.m_collisionObject.m_worldTransform, out sepNormalWorldSpace, resultOut ); // Console.WriteLine("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.x,sepNormalWorldSpace.y,sepNormalWorldSpace.z); btPolyhedralContactClipping.clipFaceAgainstHull( ref sepNormalWorldSpace , polyhedronA.getConvexPolyhedron(), ref body0Wrap.m_collisionObject.m_worldTransform, vertices, minDist - threshold, maxDist, resultOut ); } else { #if ZERO_MARGIN gjkPairDetector.setIgnoreMargin(true); gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); #else gjkPairDetector.getClosestPoints( input, dummy, dispatchInfo.m_debugDraw ); #endif//ZERO_MARGIN double l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); if( l2 > btScalar.SIMD_EPSILON ) { gjkPairDetector.getCachedSeparatingAxis().Mult( ( 1 / l2 ), out sepNormalWorldSpace ); //minDist = gjkPairDetector.getCachedSeparatingDistance(); //maxDist = threshold; minDist = gjkPairDetector.getCachedSeparatingDistance() - min0.getMargin() - min1.getMargin(); //foundSepAxis = true; btPolyhedralContactClipping.clipFaceAgainstHull( ref sepNormalWorldSpace , polyhedronA.getConvexPolyhedron(), ref body0Wrap.m_collisionObject.m_worldTransform, vertices, minDist - threshold, maxDist, resultOut ); } else { //sepNormalWorldSpace = btVector3.Zero; } } if( m_ownManifold ) { resultOut.refreshContactPoints(); } BulletGlobals.ClosestPointInputPool.Free( input ); BulletGlobals.GjkPairDetectorPool.Free( gjkPairDetector ); return; } } } gjkPairDetector.getClosestPoints( input, resultOut, dispatchInfo.m_debugDraw ); //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points if( m_numPerturbationIterations != 0 && resultOut.m_manifoldPtr.m_cachedPoints < m_minimumPointsPerturbationThreshold ) { int i; btVector3 v0, v1; btVector3 sepNormalWorldSpace; double l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); if( l2 > btScalar.SIMD_EPSILON ) { gjkPairDetector.getCachedSeparatingAxis().Mult( ( 1 / l2 ), out sepNormalWorldSpace ); btVector3.btPlaneSpace1( ref sepNormalWorldSpace, out v0, out v1 ); bool perturbeA = true; double angleLimit = 0.125f * btScalar.SIMD_PI; double perturbeAngle; double radiusA = min0.getAngularMotionDisc(); double radiusB = min1.getAngularMotionDisc(); if( radiusA < radiusB ) { perturbeAngle = btPersistentManifold.gContactBreakingThreshold / radiusA; perturbeA = true; } else { perturbeAngle = btPersistentManifold.gContactBreakingThreshold / radiusB; perturbeA = false; } if( perturbeAngle > angleLimit ) perturbeAngle = angleLimit; btTransform unPerturbedTransform; if( perturbeA ) { unPerturbedTransform = input.m_transformA; } else { unPerturbedTransform = input.m_transformB; } for( i = 0; i < m_numPerturbationIterations; i++ ) { if( v0.length2() > btScalar.SIMD_EPSILON ) { btQuaternion perturbeRot = new btQuaternion( ref v0, perturbeAngle ); double iterationAngle = i * ( btScalar.SIMD_2_PI / (double)( m_numPerturbationIterations ) ); btQuaternion rotq = new btQuaternion( ref sepNormalWorldSpace, iterationAngle ); if( perturbeA ) { btQuaternion tmpq; btQuaternion tmpq2; rotq.inverse( out tmpq ); btQuaternion.Mult( ref tmpq, ref perturbeRot, out tmpq2 ); btQuaternion.Mult( ref tmpq2, ref rotq, out tmpq ); btMatrix3x3 m = new btMatrix3x3( ref tmpq ); btMatrix3x3 m2; body0Transform.getBasis( out m2 ); btMatrix3x3 m3; btMatrix3x3.Mult( ref m, ref m2, out m3 ); input.m_transformA.setBasis( ref m3 ); input.m_transformB = body1Transform; #if DEBUG_CONTACTS dispatchInfo.m_debugDraw.drawTransform(input.m_transformA,10.0); #endif //DEBUG_CONTACTS } else { btQuaternion tmpq; btQuaternion tmpq2; rotq.inverse( out tmpq ); btQuaternion.Mult( ref tmpq, ref perturbeRot, out tmpq2 ); btQuaternion.Mult( ref tmpq2, ref rotq, out tmpq ); btMatrix3x3 m = new btMatrix3x3( ref tmpq ); btMatrix3x3 m2; body1Transform.getBasis( out m2 ); btMatrix3x3 m3; btMatrix3x3.Mult( ref m, ref m2, out m3 ); input.m_transformA = body0Transform; input.m_transformB.setBasis( ref m3 ); #if DEBUG_CONTACTS dispatchInfo.m_debugDraw.drawTransform(input.m_transformB,10.0); #endif } btPerturbedContactResult perturbedResultOut = BulletGlobals.PerturbedContactResultPool.Get(); if( perturbeA ) perturbedResultOut.Initialize( resultOut , ref input.m_transformA, ref input.m_transformB , ref input.m_transformA , perturbeA , dispatchInfo.m_debugDraw ); else perturbedResultOut.Initialize( resultOut , ref input.m_transformA, ref input.m_transformB , ref input.m_transformB , perturbeA , dispatchInfo.m_debugDraw ); gjkPairDetector.getClosestPoints( input, perturbedResultOut, dispatchInfo.m_debugDraw ); BulletGlobals.PerturbedContactResultPool.Free( perturbedResultOut ); } } } } #if USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist>SIMD_EPSILON)) { m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0.getWorldTransform(),body1.getWorldTransform()); } #endif //USE_SEPDISTANCE_UTIL2 BulletGlobals.ClosestPointInputPool.Free( input ); BulletGlobals.GjkPairDetectorPool.Free( gjkPairDetector ); } if( m_ownManifold ) { resultOut.refreshContactPoints(); } }
public virtual bool calcPenDepth( btSimplexSolverInterface simplexSolver, btConvexShape convexA, btConvexShape convexB, ref btTransform transA, ref btTransform transB, ref btVector3 v, ref btVector3 pa, ref btVector3 pb, btIDebugDraw debugDraw ) { //(void)v; bool check2d = convexA.isConvex2d() && convexB.isConvex2d(); //just take fixed number of orientation, and sample the penetration depth in that direction double minProj = btScalar.BT_LARGE_FLOAT; btVector3 minNorm = btVector3.Zero; btVector3 minA, minB; btVector3 seperatingAxisInA, seperatingAxisInB; btVector3 pInA, qInB, pWorld, qWorld, w; #if USE_BATCHED_SUPPORT btVector3[] supportVerticesABatch = new btVector3[NUM_UNITSPHERE_POINTS + btConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2]; btVector3[] supportVerticesBBatch = new btVector3[NUM_UNITSPHERE_POINTS + btConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2]; btVector3[] seperatingAxisInABatch = new btVector3[NUM_UNITSPHERE_POINTS + btConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2]; btVector3[] seperatingAxisInBBatch = new btVector3[NUM_UNITSPHERE_POINTS + btConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2]; int i; int numSampleDirections = NUM_UNITSPHERE_POINTS; for( i = 0; i < numSampleDirections; i++ ) { btVector3 norm = getPenetrationDirections()[i]; seperatingAxisInABatch[i] = ( -norm ) * transA.getBasis(); seperatingAxisInBBatch[i] = norm * transB.getBasis(); } { int numPDA = convexA.getNumPreferredPenetrationDirections(); if( numPDA ) { for( int i = 0; i < numPDA; i++ ) { btVector3 norm; convexA.getPreferredPenetrationDirection( i, norm ); norm = transA.getBasis() * norm; getPenetrationDirections()[numSampleDirections] = norm; seperatingAxisInABatch[numSampleDirections] = ( -norm ) * transA.getBasis(); seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis(); numSampleDirections++; } } } { int numPDB = convexB.getNumPreferredPenetrationDirections(); if( numPDB ) { for( int i = 0; i < numPDB; i++ ) { btVector3 norm; convexB.getPreferredPenetrationDirection( i, norm ); norm = transB.getBasis() * norm; getPenetrationDirections()[numSampleDirections] = norm; seperatingAxisInABatch[numSampleDirections] = ( -norm ) * transA.getBasis(); seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis(); numSampleDirections++; } } } convexA.batchedUnitVectorGetSupportingVertexWithoutMargin( seperatingAxisInABatch, supportVerticesABatch, numSampleDirections ); convexB.batchedUnitVectorGetSupportingVertexWithoutMargin( seperatingAxisInBBatch, supportVerticesBBatch, numSampleDirections ); for( i = 0; i < numSampleDirections; i++ ) { btVector3 norm = getPenetrationDirections()[i]; if( check2d ) { norm[2] = 0; } if( norm.length2() > 0.01 ) { seperatingAxisInA = seperatingAxisInABatch[i]; seperatingAxisInB = seperatingAxisInBBatch[i]; pInA = supportVerticesABatch[i]; qInB = supportVerticesBBatch[i]; pWorld = transA( pInA ); qWorld = transB( qInB ); if( check2d ) { pWorld[2] = 0; qWorld[2] = 0; } w = qWorld - pWorld; double delta = norm.dot( w ); //find smallest delta if( delta < minProj ) { minProj = delta; minNorm = norm; minA = pWorld; minB = qWorld; } } } #else int numSampleDirections = NUM_UNITSPHERE_POINTS; #if !__SPU__ { int numPDA = convexA.getNumPreferredPenetrationDirections(); if (numPDA) { for (int i=0;i<numPDA;i++) { btVector3 norm; convexA.getPreferredPenetrationDirection(i,norm); norm = transA.getBasis() * norm; getPenetrationDirections()[numSampleDirections] = norm; numSampleDirections++; } } } { int numPDB = convexB.getNumPreferredPenetrationDirections(); if (numPDB) { for (int i=0;i<numPDB;i++) { btVector3 norm; convexB.getPreferredPenetrationDirection(i,norm); norm = transB.getBasis() * norm; getPenetrationDirections()[numSampleDirections] = norm; numSampleDirections++; } } } #endif // __SPU__ for (int i=0;i<numSampleDirections;i++) { ref btVector3 norm = getPenetrationDirections()[i]; seperatingAxisInA = (-norm)* transA.getBasis(); seperatingAxisInB = norm* transB.getBasis(); pInA = convexA.localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); qInB = convexB.localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); pWorld = transA(pInA); qWorld = transB(qInB); w = qWorld - pWorld; double delta = norm.dot(w); //find smallest delta if (delta < minProj) { minProj = delta; minNorm = norm; minA = pWorld; minB = qWorld; } }