Revision control
++ // Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
++ // Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
++ // SPDX-FileCopyrightText: Copyright (c) 2008-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
++ // SPDX-License-Identifier: Apache-2.0
++
++ #include "geomutils/PxContactBuffer.h"
++ #include "GuGJKPenetration.h"
++ #include "GuEPA.h"
++ #include "GuVecConvexHull.h"
++ #include "GuVecConvexHullNoScale.h"
++ #include "GuContactMethodImpl.h"
++ #include "GuPCMShapeConvex.h"
++ #include "GuPCMContactGen.h"
++
++ using namespace physx;
++ using namespace Gu;
++ using namespace aos;
++
++ static bool fullContactsGenerationConvexConvex(const GjkConvex* relativeConvex, const GjkConvex* localConvex, const PxTransformV& transf0, const PxTransformV& transf1,
++ bool idtScale0, bool idtScale1, PersistentContact* manifoldContacts, PxContactBuffer& contactBuffer,
++ PersistentContactManifold& manifold, Vec3VArg normal, const Vec3VArg closestA, const Vec3VArg closestB,
++ const FloatVArg contactDist, bool doOverlapTest, PxRenderOutput* renderOutput, PxReal toleranceLength)
++ {
++ PolygonalData polyData0, polyData1;
++ const ConvexHullV& convexHull0 = relativeConvex->getConvex<ConvexHullV>();
++ const ConvexHullV& convexHull1 = localConvex->getConvex<ConvexHullV>();
++ getPCMConvexData(convexHull0, idtScale0, polyData0);
++ getPCMConvexData(convexHull1, idtScale1, polyData1);
++
++ PX_ALIGN(16, PxU8 buff0[sizeof(SupportLocalImpl<ConvexHullV>)]);
++ PX_ALIGN(16, PxU8 buff1[sizeof(SupportLocalImpl<ConvexHullV>)]);
++
++ SupportLocal* map0 = (idtScale0 ? static_cast<SupportLocal*>(PX_PLACEMENT_NEW(buff0, SupportLocalImpl<ConvexHullNoScaleV>)(static_cast<const ConvexHullNoScaleV&>(convexHull0), transf0, convexHull0.vertex2Shape, convexHull0.shape2Vertex, idtScale0)) :
++ static_cast<SupportLocal*>(PX_PLACEMENT_NEW(buff0, SupportLocalImpl<ConvexHullV>)(convexHull0, transf0, convexHull0.vertex2Shape, convexHull0.shape2Vertex, idtScale0)));
++
++ SupportLocal* map1 = (idtScale1 ? static_cast<SupportLocal*>(PX_PLACEMENT_NEW(buff1, SupportLocalImpl<ConvexHullNoScaleV>)(static_cast<const ConvexHullNoScaleV&>(convexHull1), transf1, convexHull1.vertex2Shape, convexHull1.shape2Vertex, idtScale1)) :
++ static_cast<SupportLocal*>(PX_PLACEMENT_NEW(buff1, SupportLocalImpl<ConvexHullV>)(convexHull1, transf1, convexHull1.vertex2Shape, convexHull1.shape2Vertex, idtScale1)));
++
++ PxU32 numContacts = 0;
++
++ if(generateFullContactManifold(polyData0, polyData1, map0, map1, manifoldContacts, numContacts, contactDist, normal, closestA, closestB, convexHull0.getMarginF(),
++ convexHull1.getMarginF(), doOverlapTest, renderOutput, toleranceLength))
++ {
++ if(numContacts > 0)
++ {
++ //reduce contacts
++ manifold.addBatchManifoldContacts(manifoldContacts, numContacts, toleranceLength);
++
++ const Vec3V worldNormal = manifold.getWorldNormal(transf1);
++
++ //add the manifold contacts;
++ manifold.addManifoldContactsToContactBuffer(contactBuffer, worldNormal, transf1, contactDist);
++
++ #if PCM_LOW_LEVEL_DEBUG
++ manifold.drawManifold(*renderOutput, transf0, transf1);
++ #endif
++ }
++ else
++ {
++ //if doOverlapTest is true, which means GJK/EPA degenerate so we won't have any contact in the manifoldContacts array
++ if(!doOverlapTest)
++ {
++ const Vec3V worldNormal = manifold.getWorldNormal(transf1);
++
++ manifold.addManifoldContactsToContactBuffer(contactBuffer, worldNormal, transf1, contactDist);
++ }
++ }
++ return true;
++ }
++ return false;
++ }
++
++ static bool generateOrProcessContactsConvexConvex( const GjkConvex* relativeConvex, const GjkConvex* localConvex, const PxTransformV& transf0, const PxTransformV& transf1,
++ const PxMatTransformV& aToB, GjkStatus status, GjkOutput& output, PersistentContactManifold& manifold, PxContactBuffer& contactBuffer,
++ PxU32 initialContacts, const FloatV minMargin, const FloatV contactDist,
++ bool idtScale0, bool idtScale1, PxReal toleranceLength, PxRenderOutput* renderOutput)
++ {
++ if(status == GJK_NON_INTERSECT)
++ {
++ return false;
++ }
++ else
++ {
++ PersistentContact* manifoldContacts = PX_CP_TO_PCP(contactBuffer.contacts);
++
++ const Vec3V localNor = manifold.mNumContacts ? manifold.getLocalNormal() : V3Zero();
++
++ const FloatV replaceBreakingThreshold = FMul(minMargin, FLoad(0.05f));
++
++ //addGJKEPAContacts will increase the number of contacts in manifold. If status == EPA_CONTACT, we need to run epa algorithm and generate closest points, normal and
++ //pentration. If epa doesn't degenerate, we will store the contacts information in the manifold. Otherwise, we will return true to do the fallback test
++ const bool doOverlapTest = addGJKEPAContacts(relativeConvex, localConvex, aToB, status, manifoldContacts, replaceBreakingThreshold, FLoad(toleranceLength), output, manifold);
++
++ #if PCM_LOW_LEVEL_DEBUG
++ manifold.drawManifold(*renderOutput, transf0, transf1);
++ #endif
++ //ML: after we refresh the contacts(newContacts) and generate a GJK/EPA contacts(we will store that in the manifold), if the number of contacts is still less than the original contacts,
++ //which means we lose too many contacts and we should regenerate all the contacts in the current configuration
++ //Also, we need to look at the existing contacts, if the existing contacts has very different normal than the GJK/EPA contacts,
++ //which means we should throw away the existing contacts and do full contact gen
++ const bool fullContactGen = FAllGrtr(FLoad(0.707106781f), V3Dot(localNor, output.normal)) || (manifold.mNumContacts < initialContacts);
++
++ if(fullContactGen || doOverlapTest || initialContacts > 1)
++ {
++ return fullContactsGenerationConvexConvex(relativeConvex, localConvex, transf0, transf1, idtScale0, idtScale1, manifoldContacts, contactBuffer,
++ manifold, output.normal, output.closestA, output.closestB, contactDist, doOverlapTest, renderOutput, toleranceLength);
++ }
++ else
++ {
++ const Vec3V newLocalNor = V3Add(localNor, output.normal);
++ const Vec3V worldNormal = V3Normalize(transf1.rotate(newLocalNor));
++ //const Vec3V worldNormal = transf1.rotate(normal);
++ manifold.addManifoldContactsToContactBuffer(contactBuffer, worldNormal, transf1, contactDist);
++ return true;
++ }
++ }
++ }
++
++ static bool convexHullNoScale0( const ConvexHullV& convexHull0, const ConvexHullV& convexHull1, const PxTransformV& transf0, const PxTransformV& transf1,
++ const PxMatTransformV& aToB, GjkOutput& output, PersistentContactManifold& manifold, PxContactBuffer& contactBuffer,
++ PxU32 initialContacts, const FloatV minMargin, const FloatV contactDist,
++ bool idtScale1, PxReal toleranceLength, PxRenderOutput* renderOutput)
++ {
++ const RelativeConvex<ConvexHullNoScaleV> convexA(static_cast<const ConvexHullNoScaleV&>(convexHull0), aToB);
++ if(idtScale1)
++ {
++ const LocalConvex<ConvexHullNoScaleV> convexB(static_cast<const ConvexHullNoScaleV&>(convexHull1));
++ GjkStatus status = gjkPenetration<RelativeConvex<ConvexHullNoScaleV>, LocalConvex<ConvexHullNoScaleV> >(convexA, convexB, aToB.p, contactDist, true,
++ manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, output);
++
++ return generateOrProcessContactsConvexConvex(&convexA, &convexB, transf0, transf1, aToB, status, output, manifold,
++ contactBuffer, initialContacts, minMargin, contactDist, true, true, toleranceLength, renderOutput);
++ }
++ else
++ {
++ const LocalConvex<ConvexHullV> convexB(convexHull1);
++ GjkStatus status = gjkPenetration<RelativeConvex<ConvexHullNoScaleV>, LocalConvex<ConvexHullV> >(convexA, convexB, aToB.p, contactDist, true,
++ manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, output);
++
++ return generateOrProcessContactsConvexConvex(&convexA, &convexB, transf0, transf1, aToB, status, output, manifold,
++ contactBuffer, initialContacts, minMargin, contactDist, true, false, toleranceLength, renderOutput);
++ }
++ }
++
++ static bool convexHullHasScale0(const ConvexHullV& convexHull0, const ConvexHullV& convexHull1, const PxTransformV& transf0, const PxTransformV& transf1,
++ const PxMatTransformV& aToB, GjkOutput& output, PersistentContactManifold& manifold, PxContactBuffer& contactBuffer,
++ PxU32 initialContacts, const FloatV minMargin, const FloatV contactDist,
++ bool idtScale1, PxReal toleranceLength, PxRenderOutput* renderOutput)
++ {
++ RelativeConvex<ConvexHullV> convexA(convexHull0, aToB);
++ if(idtScale1)
++ {
++ const LocalConvex<ConvexHullNoScaleV> convexB(static_cast<const ConvexHullNoScaleV&>(convexHull1));
++ GjkStatus status = gjkPenetration<RelativeConvex<ConvexHullV>, LocalConvex<ConvexHullNoScaleV> >(convexA, convexB, aToB.p, contactDist, true,
++ manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints,output);
++
++ return generateOrProcessContactsConvexConvex(&convexA, &convexB, transf0, transf1, aToB, status, output, manifold,
++ contactBuffer, initialContacts, minMargin, contactDist, false, true, toleranceLength, renderOutput);
++ }
++ else
++ {
++ const LocalConvex<ConvexHullV> convexB(convexHull1);
++ GjkStatus status = gjkPenetration<RelativeConvex<ConvexHullV>, LocalConvex<ConvexHullV> >(convexA, convexB, aToB.p, contactDist, true,
++ manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, output);
++
++ return generateOrProcessContactsConvexConvex(&convexA, &convexB, transf0, transf1, aToB, status, output, manifold,
++ contactBuffer, initialContacts, minMargin, contactDist, false, false, toleranceLength, renderOutput);
++ }
++ }
++
++ bool Gu::pcmContactConvexConvex(GU_CONTACT_METHOD_ARGS)
++ {
++ const PxConvexMeshGeometry& shapeConvex0 = checkedCast<PxConvexMeshGeometry>(shape0);
++ const PxConvexMeshGeometry& shapeConvex1 = checkedCast<PxConvexMeshGeometry>(shape1);
++
++ PersistentContactManifold& manifold = cache.getManifold();
++
++ const ConvexHullData* hullData0 = _getHullData(shapeConvex0);
++ const ConvexHullData* hullData1 = _getHullData(shapeConvex1);
++ PxPrefetchLine(hullData0);
++ PxPrefetchLine(hullData1);
++
++ PX_ASSERT(transform1.q.isSane());
++ PX_ASSERT(transform0.q.isSane());
++
++ const Vec3V vScale0 = V3LoadU_SafeReadW(shapeConvex0.scale.scale); // PT: safe because 'rotation' follows 'scale' in PxMeshScale
++ const Vec3V vScale1 = V3LoadU_SafeReadW(shapeConvex1.scale.scale); // PT: safe because 'rotation' follows 'scale' in PxMeshScale
++ const FloatV contactDist = FLoad(params.mContactDistance);
++
++ //Transfer A into the local space of B
++ const PxTransformV transf0 = loadTransformA(transform0);
++ const PxTransformV transf1 = loadTransformA(transform1);
++ const PxTransformV curRTrans(transf1.transformInv(transf0));
++ const PxMatTransformV aToB(curRTrans);
++
++ const PxReal toleranceLength = params.mToleranceLength;
++ const FloatV convexMargin0 = CalculatePCMConvexMargin(hullData0, vScale0, toleranceLength);
++ const FloatV convexMargin1 = CalculatePCMConvexMargin(hullData1, vScale1, toleranceLength);
++
++ const PxU32 initialContacts = manifold.mNumContacts;
++
++ const FloatV minMargin = FMin(convexMargin0, convexMargin1);
++ const FloatV projectBreakingThreshold = FMul(minMargin, FLoad(0.8f));
++
++ manifold.refreshContactPoints(aToB, projectBreakingThreshold, contactDist);
++
++ //ML: after refreshContactPoints, we might lose some contacts
++ const bool bLostContacts = (manifold.mNumContacts != initialContacts);
++
++ const Vec3V extent0 = V3Mul(V3LoadU_SafeReadW(hullData0->mInternal.mInternalExtents), vScale0);
++ const Vec3V extent1 = V3Mul(V3LoadU_SafeReadW(hullData1->mInternal.mInternalExtents), vScale1);
++
++ const FloatV radiusA = V3Length(extent0);
++ const FloatV radiusB = V3Length(extent1);
++
++ if(bLostContacts || manifold.invalidate_BoxConvex(curRTrans, transf0.q, transf1.q, minMargin, radiusA, radiusB))
++ {
++ manifold.setRelativeTransform(curRTrans, transf0.q, transf1.q);
++
++ const bool idtScale0 = shapeConvex0.scale.isIdentity();
++ const bool idtScale1 = shapeConvex1.scale.isIdentity();
++ const QuatV vQuat0 = QuatVLoadU(&shapeConvex0.scale.rotation.x);
++ const QuatV vQuat1 = QuatVLoadU(&shapeConvex1.scale.rotation.x);
++
++ // PT: safe loads because mCenterOfMass isn't the last data in the structure
++ const ConvexHullV convexHull0(hullData0, V3LoadU_SafeReadW(hullData0->mCenterOfMass), vScale0, vQuat0, idtScale0);
++ const ConvexHullV convexHull1(hullData1, V3LoadU_SafeReadW(hullData1->mCenterOfMass), vScale1, vQuat1, idtScale1);
++
++ GjkOutput output;
++
++ if(idtScale0)
++ {
++ return convexHullNoScale0(convexHull0, convexHull1, transf0, transf1, aToB, output, manifold,
++ contactBuffer, initialContacts, minMargin, contactDist, idtScale1, toleranceLength, renderOutput);
++ }
++ else
++ {
++ return convexHullHasScale0(convexHull0, convexHull1, transf0, transf1, aToB, output, manifold,
++ contactBuffer, initialContacts, minMargin, contactDist, idtScale1, toleranceLength, renderOutput);
++ }
++ }
++ else if(manifold.getNumContacts()> 0)
++ {
++ const Vec3V worldNormal = manifold.getWorldNormal(transf1);
++ manifold.addManifoldContactsToContactBuffer(contactBuffer, worldNormal, transf1, contactDist);
++ #if PCM_LOW_LEVEL_DEBUG
++ manifold.drawManifold(*renderOutput, transf0, transf1);
++ #endif
++ return true;
++ }
++
++ return false;
++ }