static bool TestEPA(const MyConvex& hull0, const MyConvex& hull1) { static btSimplexSolverInterface simplexSolver; #ifdef COMPARE_WITH_SOLID35_AND_OTHER_EPA // static Solid3JohnsonSimplexSolver simplexSolver2; #endif //COMPARE_WITH_SOLID35_AND_OTHER_EPA simplexSolver.reset(); btConvexHullShape convexA((btScalar*)hull0.mVerts, hull0.mNbVerts, sizeof(btVector3)); btConvexHullShape convexB((btScalar*)hull1.mVerts, hull1.mNbVerts, sizeof(btVector3)); static btGjkEpaPenetrationDepthSolver Solver0; static btMinkowskiPenetrationDepthSolver Solver1; #ifdef COMPARE_WITH_SOLID35_AND_OTHER_EPA static Solid3EpaPenetrationDepth Solver2; static EpaPenetrationDepthSolver Solver3; #endif btConvexPenetrationDepthSolver* Solver = NULL ; if(gMethod==0) Solver = &Solver0; else if(gMethod==1) Solver = &Solver1; #ifdef COMPARE_WITH_SOLID35_AND_OTHER_EPA else if(gMethod==2) Solver = &Solver2; else Solver = &Solver3; #endif //COMPARE_WITH_SOLID35_AND_OTHER_EPA #ifdef USE_ORIGINAL btGjkPairDetector GJK(&convexA, &convexB, &simplexSolver, Solver); GJK.m_catchDegeneracies = 1; convexA.setMargin(0.01f); convexB.setMargin(0.01f); btDiscreteCollisionDetectorInterface::ClosestPointInput input; input.m_transformA = hull0.mTransform; input.m_transformB = hull1.mTransform; input.m_stackAlloc = &gStackAlloc; MyResult output; GJK.getClosestPoints(input, output, 0); gLastUsedMethod = GJK.m_lastUsedMethod; gNumGjkIterations = GJK.m_curIter; gLastDegenerateSimplex= GJK.m_degenerateSimplex; #else MyResult output; btVector3 witnesses[2]; btVector3 normal; btScalar depth; btGjkEpaSolver::sResults results; btScalar radialMargin = 0.01f; btGjkEpaSolver::Collide(&convexA,hull0.mTransform, &convexB,hull1.mTransform, radialMargin, results); if (results.depth>0) { output.addContactPoint(results.normal,results.witnesses[1],-results.depth); } #endif return true; }
bool pcmContactCapsuleConvex(GU_CONTACT_METHOD_ARGS) { PX_UNUSED(renderOutput); const PxConvexMeshGeometryLL& shapeConvex = shape1.get<const PxConvexMeshGeometryLL>(); const PxCapsuleGeometry& shapeCapsule = shape0.get<const PxCapsuleGeometry>(); PersistentContactManifold& manifold = cache.getManifold(); Ps::prefetchLine(shapeConvex.hullData); PX_ASSERT(transform1.q.isSane()); PX_ASSERT(transform0.q.isSane()); const Vec3V zeroV = V3Zero(); const Vec3V vScale = V3LoadU_SafeReadW(shapeConvex.scale.scale); // PT: safe because 'rotation' follows 'scale' in PxMeshScale const FloatV contactDist = FLoad(params.mContactDistance); const FloatV capsuleHalfHeight = FLoad(shapeCapsule.halfHeight); const FloatV capsuleRadius = FLoad(shapeCapsule.radius); const ConvexHullData* hullData =shapeConvex.hullData; //Transfer A into the local space of B const PsTransformV transf0 = loadTransformA(transform0); const PsTransformV transf1 = loadTransformA(transform1); const PsTransformV curRTrans(transf1.transformInv(transf0)); const PsMatTransformV aToB(curRTrans); const FloatV convexMargin = Gu::CalculatePCMConvexMargin(hullData, vScale); const FloatV capsuleMinMargin = Gu::CalculateCapsuleMinMargin(capsuleRadius); const FloatV minMargin = FMin(convexMargin, capsuleMinMargin); const PxU32 initialContacts = manifold.mNumContacts; const FloatV projectBreakingThreshold = FMul(minMargin, FLoad(1.25f)); const FloatV refreshDist = FAdd(contactDist, capsuleRadius); manifold.refreshContactPoints(aToB, projectBreakingThreshold, refreshDist); //ML: after refreshContactPoints, we might lose some contacts const bool bLostContacts = (manifold.mNumContacts != initialContacts); GjkStatus status = manifold.mNumContacts > 0 ? GJK_UNDEFINED : GJK_NON_INTERSECT; Vec3V closestA(zeroV), closestB(zeroV), normal(zeroV); // from a to b const FloatV zero = FZero(); FloatV penDep = zero; PX_UNUSED(bLostContacts); if(bLostContacts || manifold.invalidate_SphereCapsule(curRTrans, minMargin)) { const bool idtScale = shapeConvex.scale.isIdentity(); manifold.setRelativeTransform(curRTrans); const QuatV vQuat = QuatVLoadU(&shapeConvex.scale.rotation.x); ConvexHullV convexHull(hullData, zeroV, vScale, vQuat, idtScale); convexHull.setMargin(zero); //transform capsule(a) into the local space of convexHull(b) CapsuleV capsule(aToB.p, aToB.rotate(V3Scale(V3UnitX(), capsuleHalfHeight)), capsuleRadius); LocalConvex<CapsuleV> convexA(capsule); const Vec3V initialSearchDir = V3Sub(capsule.getCenter(), convexHull.getCenter()); if(idtScale) { LocalConvex<ConvexHullNoScaleV> convexB(*PX_CONVEX_TO_NOSCALECONVEX(&convexHull)); status = gjkPenetration<LocalConvex<CapsuleV>, LocalConvex<ConvexHullNoScaleV> >(convexA, convexB, initialSearchDir, contactDist, closestA, closestB, normal, penDep, manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, true); } else { LocalConvex<ConvexHullV> convexB(convexHull); status = gjkPenetration<LocalConvex<CapsuleV>, LocalConvex<ConvexHullV> >(convexA, convexB, initialSearchDir, contactDist, closestA, closestB, normal, penDep, manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, true); } Gu::PersistentContact* manifoldContacts = PX_CP_TO_PCP(contactBuffer.contacts); bool doOverlapTest = false; if(status == GJK_NON_INTERSECT) { return false; } else if(status == GJK_DEGENERATE) { return fullContactsGenerationCapsuleConvex(capsule, convexHull, aToB, transf0, transf1, manifoldContacts, contactBuffer, idtScale, manifold, normal, closestB, convexHull.getMargin(), contactDist, true, renderOutput, FLoad(params.mToleranceLength)); } else { const FloatV replaceBreakingThreshold = FMul(minMargin, FLoad(0.05f)); if(status == GJK_CONTACT) { const Vec3V localPointA = aToB.transformInv(closestA);//curRTrans.transformInv(closestA); const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(normal), penDep); //Add contact to contact stream manifoldContacts[0].mLocalPointA = localPointA; manifoldContacts[0].mLocalPointB = closestB; manifoldContacts[0].mLocalNormalPen = localNormalPen; //Add contact to manifold manifold.addManifoldPoint2(localPointA, closestB, localNormalPen, replaceBreakingThreshold); } else { PX_ASSERT(status == EPA_CONTACT); if(idtScale) { LocalConvex<ConvexHullNoScaleV> convexB(*PX_CONVEX_TO_NOSCALECONVEX(&convexHull)); status= Gu::epaPenetration(convexA, convexB, manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, closestA, closestB, normal, penDep, true); } else { LocalConvex<ConvexHullV> convexB(convexHull); status= Gu::epaPenetration(convexA, convexB, manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, closestA, closestB, normal, penDep, true); } if(status == EPA_CONTACT) { const Vec3V localPointA = aToB.transformInv(closestA);//curRTrans.transformInv(closestA); const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(normal), penDep); //Add contact to contact stream manifoldContacts[0].mLocalPointA = localPointA; manifoldContacts[0].mLocalPointB = closestB; manifoldContacts[0].mLocalNormalPen = localNormalPen; //Add contact to manifold manifold.addManifoldPoint2(localPointA, closestB, localNormalPen, replaceBreakingThreshold); } else { doOverlapTest = true; } } if(initialContacts == 0 || bLostContacts || doOverlapTest) { return fullContactsGenerationCapsuleConvex(capsule, convexHull, aToB, transf0, transf1, manifoldContacts, contactBuffer, idtScale, manifold, normal, closestB, convexHull.getMargin(), contactDist, doOverlapTest, renderOutput, FLoad(params.mToleranceLength)); } else { //This contact is either come from GJK or EPA normal = transf1.rotate(normal); manifold.addManifoldContactsToContactBuffer(contactBuffer, normal, transf0, capsuleRadius, contactDist); #if PCM_LOW_LEVEL_DEBUG manifold.drawManifold(*renderOutput, transf0, transf1); #endif return true; } } } else if (manifold.getNumContacts() > 0) { normal = manifold.getWorldNormal(transf1); manifold.addManifoldContactsToContactBuffer(contactBuffer, normal, transf0, capsuleRadius, contactDist); #if PCM_LOW_LEVEL_DEBUG manifold.drawManifold(*renderOutput, transf0, transf1); #endif return true; } return false; }