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 "GuConvexUtilsInternal.h"
++ #include "GuInternal.h"
++ #include "GuContactPolygonPolygon.h"
++ #include "GuConvexEdgeFlags.h"
++ #include "GuSeparatingAxes.h"
++ #include "GuContactMethodImpl.h"
++ #include "GuMidphaseInterface.h"
++ #include "GuConvexHelper.h"
++ #include "GuTriangleCache.h"
++ #include "GuHeightFieldUtil.h"
++ #include "GuEntityReport.h"
++ #include "GuIntersectionTriangleBox.h"
++ #include "GuBox.h"
++
++ #include "CmUtils.h"
++ #include "foundation/PxAlloca.h"
++ #include "foundation/PxFPU.h"
++ #include "CmMatrix34.h"
++
++ using namespace physx;
++ using namespace Gu;
++ using namespace Cm;
++ using namespace aos;
++ using namespace intrinsics;
++
++ //sizeof(SavedContactData)/sizeof(PxU32) = 17, 1088/17 = 64 triangles in the local array
++ #define LOCAL_CONTACTS_SIZE 1088
++
++ #define LOCAL_TOUCHED_TRIG_SIZE 192
++
++ //#define USE_TRIANGLE_NORMAL
++ #define TEST_INTERNAL_OBJECTS
++
++ static PX_FORCE_INLINE void projectTriangle(const PxVec3& localSpaceDirection, const PxVec3* PX_RESTRICT triangle, PxReal& min1, PxReal& max1)
++ {
++ const PxReal dp0 = triangle[0].dot(localSpaceDirection);
++ const PxReal dp1 = triangle[1].dot(localSpaceDirection);
++ min1 = selectMin(dp0, dp1);
++ max1 = selectMax(dp0, dp1);
++
++ const PxReal dp2 = triangle[2].dot(localSpaceDirection);
++ min1 = selectMin(min1, dp2);
++ max1 = selectMax(max1, dp2);
++ }
++
++ #ifdef TEST_INTERNAL_OBJECTS
++
++ static PX_FORCE_INLINE void boxSupport(const float extents[3], const PxVec3& sv, float p[3])
++ {
++ const PxU32* iextents = reinterpret_cast<const PxU32*>(extents);
++ const PxU32* isv = reinterpret_cast<const PxU32*>(&sv);
++ PxU32* ip = reinterpret_cast<PxU32*>(p);
++
++ ip[0] = iextents[0]|(isv[0]&PX_SIGN_BITMASK);
++ ip[1] = iextents[1]|(isv[1]&PX_SIGN_BITMASK);
++ ip[2] = iextents[2]|(isv[2]&PX_SIGN_BITMASK);
++ }
++
++ #if PX_DEBUG
++ static const PxReal testInternalObjectsEpsilon = 1.0e-3f;
++ #endif
++
++ static PX_FORCE_INLINE bool testInternalObjects(const PxVec3& localAxis0,
++ const PolygonalData& polyData0,
++ const PxVec3* PX_RESTRICT triangleInHullSpace,
++ float dmin)
++ {
++ PxReal min1, max1;
++ projectTriangle(localAxis0, triangleInHullSpace, min1, max1);
++
++ const float dp = polyData0.mCenter.dot(localAxis0);
++
++ float p0[3];
++ boxSupport(&polyData0.mInternal.mInternalExtents.x, localAxis0, p0);
++ const float Radius0 = p0[0]*localAxis0.x + p0[1]*localAxis0.y + p0[2]*localAxis0.z;
++ const float bestRadius = selectMax(Radius0, polyData0.mInternal.mInternalRadius);
++ const PxReal min0 = dp - bestRadius;
++ const PxReal max0 = dp + bestRadius;
++
++ const PxReal d0 = max0 - min1;
++ const PxReal d1 = max1 - min0;
++
++ const float depth = selectMin(d0, d1);
++ if(depth>dmin)
++ return false;
++ return true;
++ }
++ #endif
++
++ static PX_FORCE_INLINE bool testNormal( const PxVec3& sepAxis, PxReal min0, PxReal max0,
++ const PxVec3* PX_RESTRICT triangle,
++ PxReal& depth, PxReal contactDistance)
++ {
++ PxReal min1, max1;
++ projectTriangle(sepAxis, triangle, min1, max1);
++
++ if(max0+contactDistance<min1 || max1+contactDistance<min0)
++ return false;
++
++ const PxReal d0 = max0 - min1;
++ const PxReal d1 = max1 - min0;
++ depth = selectMin(d0, d1);
++ return true;
++ }
++
++ static PX_FORCE_INLINE bool testSepAxis(const PxVec3& sepAxis,
++ const PolygonalData& polyData0,
++ const PxVec3* PX_RESTRICT triangle,
++ const PxMat34& m0to1,
++ const FastVertex2ShapeScaling& convexScaling,
++ PxReal& depth, PxReal contactDistance)
++ {
++ PxReal min0, max0;
++ (polyData0.mProjectHull)(polyData0, sepAxis, m0to1, convexScaling, min0, max0);
++
++ PxReal min1, max1;
++ projectTriangle(sepAxis, triangle, min1, max1);
++
++ if(max0+contactDistance < min1 || max1+contactDistance < min0)
++ return false;
++
++ const PxReal d0 = max0 - min1;
++ const PxReal d1 = max1 - min0;
++ depth = selectMin(d0, d1);
++ return true;
++ }
++
++ static bool testFacesSepAxesBackface( const PolygonalData& polyData0,
++ const PxMat34& /*world0*/, const PxMat34& /*world1*/,
++ const PxMat34& m0to1, const PxVec3& witness,
++ const PxVec3* PX_RESTRICT triangle,
++ const FastVertex2ShapeScaling& convexScaling,
++ PxU32& numHullIndices,
++ PxU32* hullIndices_, PxReal& dmin, PxVec3& sep, PxU32& id, PxReal contactDistance,
++ bool idtConvexScale
++ )
++ {
++ id = PX_INVALID_U32;
++
++ const PxU32 numHullPolys = polyData0.mNbPolygons;
++ const HullPolygonData* PX_RESTRICT polygons = polyData0.mPolygons;
++ const PxVec3* PX_RESTRICT vertices = polyData0.mVerts;
++ const PxVec3& trans = m0to1.p;
++ {
++ PxU32* hullIndices = hullIndices_;
++
++ // PT: when the center of one object is inside the other object (deep penetrations) this discards everything!
++ // PT: when this happens, the backup procedure is used to come up with good results anyway.
++ // PT: it's worth having a special codepath here for identity scales, to skip all the normalizes/division. Lot faster without.
++
++ if(idtConvexScale)
++ {
++ for(PxU32 i=0; i<numHullPolys; i++)
++ {
++ const HullPolygonData& P = polygons[i];
++ const PxPlane& PL = P.mPlane;
++
++ #ifdef USE_TRIANGLE_NORMAL
++ if(PL.normal.dot(witness) > 0.0f)
++ #else
++ // ### this is dubious since the triangle center is likely to be very close to the hull, if not inside. Why not use the triangle normal?
++ if(PL.distance(witness) < 0.0f)
++ #endif
++ continue; //backface culled
++
++ *hullIndices++ = i;
++
++ const PxVec3 sepAxis = m0to1.rotate(PL.n);
++ const PxReal dp = sepAxis.dot(trans);
++
++ PxReal d;
++ if(!testNormal(sepAxis, P.getMin(vertices) + dp, P.getMax() + dp, triangle,
++ d, contactDistance))
++ return false;
++
++ if(d < dmin)
++ {
++ dmin = d;
++ sep = sepAxis;
++ id = i;
++ }
++ }
++ }
++ else
++ {
++ #ifndef USE_TRIANGLE_NORMAL
++ //transform delta from hull0 shape into vertex space:
++ const PxVec3 vertSpaceWitness = convexScaling % witness;
++ #endif
++ for(PxU32 i=0; i<numHullPolys; i++)
++ {
++ const HullPolygonData& P = polygons[i];
++ const PxPlane& PL = P.mPlane;
++
++ #ifdef USE_TRIANGLE_NORMAL
++ if(PL.normal.dot(witness) > 0.0f)
++ #else
++ // ### this is dubious since the triangle center is likely to be very close to the hull, if not inside. Why not use the triangle normal?
++ if(PL.distance(vertSpaceWitness) < 0.0f)
++ #endif
++ continue; //backface culled
++
++ //normals transform by inverse transpose: (and transpose(skew) == skew as its symmetric)
++ PxVec3 shapeSpaceNormal = convexScaling % PL.n;
++ //renormalize: (Arr!)
++ const PxReal magnitude = shapeSpaceNormal.normalize();
++
++ *hullIndices++ = i;
++ const PxVec3 sepAxis = m0to1.rotate(shapeSpaceNormal);
++ PxReal d;
++ const PxReal dp = sepAxis.dot(trans);
++
++ const float oneOverM = 1.0f / magnitude;
++ if(!testNormal(sepAxis, P.getMin(vertices) * oneOverM + dp, P.getMax() * oneOverM + dp, triangle,
++ d, contactDistance))
++ return false;
++
++ if(d < dmin)
++ {
++ dmin = d;
++ sep = sepAxis;
++ id = i;
++ }
++ }
++ }
++ numHullIndices = PxU32(hullIndices - hullIndices_);
++ }
++
++ // Backup
++ if(id == PX_INVALID_U32)
++ {
++ if(idtConvexScale)
++ {
++ for(PxU32 i=0; i<numHullPolys; i++)
++ {
++ const HullPolygonData& P = polygons[i];
++ const PxPlane& PL = P.mPlane;
++
++ const PxVec3 sepAxis = m0to1.rotate(PL.n);
++ const PxReal dp = sepAxis.dot(trans);
++
++ PxReal d;
++ if(!testNormal(sepAxis, P.getMin(vertices) + dp, P.getMax() + dp, triangle,
++ d, contactDistance))
++ return false;
++
++ if(d < dmin)
++ {
++ dmin = d;
++ sep = sepAxis;
++ id = i;
++ }
++ hullIndices_[i] = i;
++ }
++ }
++ else
++ {
++ for(PxU32 i=0; i<numHullPolys; i++)
++ {
++ const HullPolygonData& P = polygons[i];
++ const PxPlane& PL = P.mPlane;
++
++ PxVec3 shapeSpaceNormal = convexScaling % PL.n;
++ //renormalize: (Arr!)
++ const PxReal magnitude = shapeSpaceNormal.normalize();
++
++ const PxVec3 sepAxis = m0to1.rotate(shapeSpaceNormal);
++
++ PxReal d;
++ const PxReal dp = sepAxis.dot(trans);
++
++ const float oneOverM = 1.0f / magnitude;
++ if(!testNormal(sepAxis, P.getMin(vertices) * oneOverM + dp, P.getMax() * oneOverM + dp, triangle,
++ d, contactDistance))
++ return false;
++
++ if(d < dmin)
++ {
++ dmin = d;
++ sep = sepAxis;
++ id = i;
++ }
++ hullIndices_[i] = i;
++ }
++ }
++ numHullIndices = numHullPolys;
++ }
++ return true;
++ }
++
++ static PX_FORCE_INLINE bool edgeCulling(const PxPlane& plane, const PxVec3& p0, const PxVec3& p1, PxReal contactDistance)
++ {
++ return plane.distance(p0)<=contactDistance || plane.distance(p1)<=contactDistance;
++ }
++
++ static bool performEETests(
++ const PolygonalData& polyData0,
++ const PxU8 triFlags,
++ const PxMat34& m0to1, const PxMat34& m1to0,
++ const PxVec3* PX_RESTRICT triangle,
++ PxU32 numHullIndices, const PxU32* PX_RESTRICT hullIndices,
++ const PxPlane& localTriPlane,
++ const FastVertex2ShapeScaling& convexScaling,
++ PxVec3& vec, PxReal& dmin, PxReal contactDistance, PxReal toleranceLength,
++ PxU32 id0, PxU32 /*triangleIndex*/)
++ {
++ PX_UNUSED(toleranceLength); // Only used in Debug
++
++ // Cull candidate triangle edges vs to hull plane
++ PxU32 nbTriangleAxes = 0;
++ PxVec3 triangleAxes[3];
++ {
++ const HullPolygonData& P = polyData0.mPolygons[id0];
++ const PxPlane& vertSpacePlane = P.mPlane;
++
++ const PxVec3 newN = m1to0.rotate(vertSpacePlane.n);
++ PxPlane hullWitness(convexScaling * newN, vertSpacePlane.d - m1to0.p.dot(newN)); //technically not a fully xformed plane, just use property of x|My == Mx|y for symmetric M.
++
++ if((triFlags & ETD_CONVEX_EDGE_01) && edgeCulling(hullWitness, triangle[0], triangle[1], contactDistance))
++ triangleAxes[nbTriangleAxes++] = (triangle[0] - triangle[1]);
++
++ if((triFlags & ETD_CONVEX_EDGE_12) && edgeCulling(hullWitness, triangle[1], triangle[2], contactDistance))
++ triangleAxes[nbTriangleAxes++] = (triangle[1] - triangle[2]);
++
++ if((triFlags & ETD_CONVEX_EDGE_20) && edgeCulling(hullWitness, triangle[2], triangle[0], contactDistance))
++ triangleAxes[nbTriangleAxes++] = (triangle[2] - triangle[0]);
++ }
++
++ //PxcPlane vertexSpacePlane = localTriPlane.getTransformed(m1to0);
++ //vertexSpacePlane.normal = convexScaling * vertexSpacePlane.normal; //technically not a fully xformed plane, just use property of x|My == Mx|y for symmetric M.
++ const PxVec3 newN = m1to0.rotate(localTriPlane.n);
++ PxPlane vertexSpacePlane(convexScaling * newN, localTriPlane.d - m1to0.p.dot(newN));
++
++ const PxVec3* PX_RESTRICT hullVerts = polyData0.mVerts;
++
++ SeparatingAxes SA;
++ SA.reset();
++
++ const PxU8* PX_RESTRICT vrefBase0 = polyData0.mPolygonVertexRefs;
++ const HullPolygonData* PX_RESTRICT polygons = polyData0.mPolygons;
++ while(numHullIndices--)
++ {
++ const HullPolygonData& P = polygons[*hullIndices++];
++ const PxU8* PX_RESTRICT data = vrefBase0 + P.mVRef8;
++
++ PxU32 numEdges = nbTriangleAxes;
++ const PxVec3* edges = triangleAxes;
++
++ // TODO: cheap edge culling as in convex/convex!
++ while(numEdges--)
++ {
++ const PxVec3& currentPolyEdge = *edges++;
++
++ // Loop through polygon vertices == polygon edges;
++ PxU32 numVerts = P.mNbVerts;
++ for(PxU32 j = 0; j < numVerts; j++)
++ {
++ PxU32 j1 = j+1;
++ if(j1>=numVerts) j1 = 0;
++
++ const PxU32 VRef0 = data[j];
++ const PxU32 VRef1 = data[j1];
++
++ if(edgeCulling(vertexSpacePlane, hullVerts[VRef0], hullVerts[VRef1], contactDistance))
++ {
++ const PxVec3 currentHullEdge = m0to1.rotate(convexScaling * (hullVerts[VRef0] - hullVerts[VRef1])); //matrix mult is distributive!
++
++ PxVec3 sepAxis = currentHullEdge.cross(currentPolyEdge);
++ if(!isAlmostZero(sepAxis))
++ SA.addAxis(sepAxis.getNormalized());
++ }
++ }
++ }
++ }
++
++ dmin = PX_MAX_REAL;
++ PxU32 numAxes = SA.getNumAxes();
++ const PxVec3* PX_RESTRICT axes = SA.getAxes();
++
++ #ifdef TEST_INTERNAL_OBJECTS
++ PxVec3 triangleInHullSpace[3];
++ if(numAxes)
++ {
++ triangleInHullSpace[0] = m1to0.transform(triangle[0]);
++ triangleInHullSpace[1] = m1to0.transform(triangle[1]);
++ triangleInHullSpace[2] = m1to0.transform(triangle[2]);
++ }
++ #endif
++
++ while(numAxes--)
++ {
++ const PxVec3& currentAxis = *axes++;
++
++ #ifdef TEST_INTERNAL_OBJECTS
++ const PxVec3 localAxis0 = m1to0.rotate(currentAxis);
++ if(!testInternalObjects(localAxis0, polyData0, triangleInHullSpace, dmin))
++ {
++ #if PX_DEBUG
++ PxReal dtest;
++ if(testSepAxis(currentAxis, polyData0, triangle, m0to1, convexScaling, dtest, contactDistance))
++ {
++ PX_ASSERT(dtest + testInternalObjectsEpsilon*toleranceLength >= dmin);
++ }
++ #endif
++ continue;
++ }
++ #endif
++
++ PxReal d;
++ if(!testSepAxis(currentAxis, polyData0, triangle,
++ m0to1, convexScaling, d, contactDistance))
++ {
++ return false;
++ }
++
++ if(d < dmin)
++ {
++ dmin = d;
++ vec = currentAxis;
++ }
++ }
++ return true;
++ }
++
++ static bool triangleConvexTest( const PolygonalData& polyData0,
++ const PxU8 triFlags,
++ PxU32 index, const PxVec3* PX_RESTRICT localPoints,
++ const PxPlane& localPlane,
++ const PxVec3& groupCenterHull,
++ const PxMat34& world0, const PxMat34& world1, const PxMat34& m0to1, const PxMat34& m1to0,
++ const FastVertex2ShapeScaling& convexScaling,
++ PxReal contactDistance, PxReal toleranceLength,
++ PxVec3& groupAxis, PxReal& groupMinDepth, bool& faceContact,
++ bool idtConvexScale
++ )
++ {
++ PxU32 id0 = PX_INVALID_U32;
++ PxReal dmin0 = PX_MAX_REAL;
++ PxVec3 vec0;
++
++ PxU32 numHullIndices = 0;
++ PxU32* PX_RESTRICT const hullIndices = reinterpret_cast<PxU32*>(PxAlloca(polyData0.mNbPolygons*sizeof(PxU32)));
++
++ // PT: we test the hull normals first because they don't need any hull projection. If we can early exit thanks
++ // to those, we completely avoid all hull projections.
++ bool status = testFacesSepAxesBackface(polyData0, world0, world1, m0to1, groupCenterHull, localPoints,
++ convexScaling, numHullIndices, hullIndices, dmin0, vec0, id0, contactDistance, idtConvexScale);
++ if(!status)
++ return false;
++
++ groupAxis = PxVec3(0);
++ groupMinDepth = PX_MAX_REAL;
++
++ const PxReal eps = 0.0001f; // DE7748
++
++ //Test in mesh-space
++ PxVec3 sepAxis;
++ PxReal depth;
++
++ {
++ // Test triangle normal
++ PxReal d;
++ if(!testSepAxis(localPlane.n, polyData0, localPoints,
++ m0to1, convexScaling, d, contactDistance))
++ return false;
++
++ if(d<dmin0+eps)
++ // if(d<dmin0)
++ {
++ depth = d;
++ sepAxis = localPlane.n;
++ faceContact = true;
++ }
++ else
++ {
++ depth = dmin0;
++ sepAxis = vec0;
++ faceContact = false;
++ }
++ }
++
++ if(depth < groupMinDepth)
++ {
++ groupMinDepth = depth;
++ groupAxis = world1.rotate(sepAxis);
++ }
++
++ if(!performEETests(polyData0, triFlags, m0to1, m1to0,
++ localPoints,
++ numHullIndices, hullIndices, localPlane,
++ convexScaling, sepAxis, depth, contactDistance, toleranceLength,
++ id0, index))
++ return false;
++
++ if(depth < groupMinDepth)
++ {
++ groupMinDepth = depth;
++ groupAxis = world1.rotate(sepAxis);
++ faceContact = false;
++ }
++
++ return true;
++ }
++
++ namespace
++ {
++ struct ConvexMeshContactGeneration
++ {
++ PxInlineArray<PxU32,LOCAL_CONTACTS_SIZE>& mDelayedContacts;
++ CacheMap<CachedEdge, 128> mEdgeCache;
++ CacheMap<CachedVertex, 128> mVertCache;
++
++ const Matrix34FromTransform m0to1;
++ const Matrix34FromTransform m1to0;
++
++ PxVec3 mHullCenterMesh;
++ PxVec3 mHullCenterWorld;
++
++ const PolygonalData& mPolyData0;
++ const PxMat34& mWorld0;
++ const PxMat34& mWorld1;
++
++ const FastVertex2ShapeScaling& mConvexScaling;
++
++ PxReal mContactDistance;
++ PxReal mToleranceLength;
++ bool mIdtMeshScale, mIdtConvexScale;
++ PxReal mCCDEpsilon;
++ const PxTransform& mTransform0;
++ const PxTransform& mTransform1;
++ PxContactBuffer& mContactBuffer;
++ bool mAnyHits;
++
++ ConvexMeshContactGeneration(
++ PxInlineArray<PxU32,LOCAL_CONTACTS_SIZE>& delayedContacts,
++ const PxTransform& t0to1, const PxTransform& t1to0,
++ const PolygonalData& polyData0, const PxMat34& world0, const PxMat34& world1,
++ const FastVertex2ShapeScaling& convexScaling,
++ PxReal contactDistance,
++ PxReal toleranceLength,
++ bool idtConvexScale,
++ PxReal cCCDEpsilon,
++ const PxTransform& transform0, const PxTransform& transform1,
++ PxContactBuffer& contactBuffer
++ );
++
++ void processTriangle(const PxVec3* verts, PxU32 triangleIndex, PxU8 triFlags, const PxU32* vertInds);
++ void generateLastContacts();
++
++ bool generateContacts(
++ const PxPlane& localPlane,
++ const PxVec3* PX_RESTRICT localPoints,
++ const PxVec3& triCenter, PxVec3& groupAxis,
++ PxReal groupMinDepth, PxU32 index) const;
++
++ private:
++ ConvexMeshContactGeneration& operator=(const ConvexMeshContactGeneration&);
++ };
++
++ // 17 entries. 1088/17 = 64 triangles in the local array
++ struct SavedContactData
++ {
++ PxU32 mTriangleIndex; // 1
++ PxVec3 mVerts[3]; // 10
++ PxU32 mInds[3]; // 13
++ PxVec3 mGroupAxis; // 16
++ PxReal mGroupMinDepth; // 17
++ };
++ }
++
++ ConvexMeshContactGeneration::ConvexMeshContactGeneration(
++ PxInlineArray<PxU32,LOCAL_CONTACTS_SIZE>& delayedContacts,
++ const PxTransform& t0to1, const PxTransform& t1to0,
++ const PolygonalData& polyData0, const PxMat34& world0, const PxMat34& world1,
++ const FastVertex2ShapeScaling& convexScaling,
++ PxReal contactDistance,
++ PxReal toleranceLength,
++ bool idtConvexScale,
++ PxReal cCCDEpsilon,
++ const PxTransform& transform0, const PxTransform& transform1,
++ PxContactBuffer& contactBuffer
++ ) :
++ mDelayedContacts(delayedContacts),
++ m0to1 (t0to1),
++ m1to0 (t1to0),
++ mPolyData0 (polyData0),
++ mWorld0 (world0),
++ mWorld1 (world1),
++ mConvexScaling (convexScaling),
++ mContactDistance(contactDistance),
++ mToleranceLength(toleranceLength),
++ mIdtConvexScale (idtConvexScale),
++ mCCDEpsilon (cCCDEpsilon),
++ mTransform0 (transform0),
++ mTransform1 (transform1),
++ mContactBuffer (contactBuffer)
++ {
++ delayedContacts.forceSize_Unsafe(0);
++ mAnyHits = false;
++
++ // Hull center in local space
++ const PxVec3& hullCenterLocal = mPolyData0.mCenter;
++ // Hull center in mesh space
++ mHullCenterMesh = m0to1.transform(hullCenterLocal);
++ // Hull center in world space
++ mHullCenterWorld = mWorld0.transform(hullCenterLocal);
++ }
++
++ struct ConvexMeshContactGenerationCallback : MeshHitCallback<PxGeomRaycastHit>
++ {
++ ConvexMeshContactGeneration mGeneration;
++ const FastVertex2ShapeScaling& mMeshScaling;
++ const PxU8* PX_RESTRICT mExtraTrigData;
++ bool mIdtMeshScale;
++ const TriangleMesh* mMeshData;
++ const BoxPadded& mBox;
++
++ ConvexMeshContactGenerationCallback(
++ PxInlineArray<PxU32,LOCAL_CONTACTS_SIZE>& delayedContacts,
++ const PxTransform& t0to1, const PxTransform& t1to0,
++ const PolygonalData& polyData0, const PxMat34& world0, const PxMat34& world1,
++ const TriangleMesh* meshData,
++ const PxU8* PX_RESTRICT extraTrigData,
++ const FastVertex2ShapeScaling& meshScaling,
++ const FastVertex2ShapeScaling& convexScaling,
++ PxReal contactDistance,
++ PxReal toleranceLength,
++ bool idtMeshScale, bool idtConvexScale,
++ PxReal cCCDEpsilon,
++ const PxTransform& transform0, const PxTransform& transform1,
++ PxContactBuffer& contactBuffer,
++ const BoxPadded& box
++ ) :
++ MeshHitCallback<PxGeomRaycastHit> (CallbackMode::eMULTIPLE),
++ mGeneration (delayedContacts, t0to1, t1to0, polyData0, world0, world1, convexScaling, contactDistance, toleranceLength, idtConvexScale, cCCDEpsilon, transform0, transform1, contactBuffer),
++ mMeshScaling (meshScaling),
++ mExtraTrigData (extraTrigData),
++ mIdtMeshScale (idtMeshScale),
++ mMeshData (meshData),
++ mBox (box)
++ {
++ }
++
++ virtual PxAgain processHit( // all reported coords are in mesh local space including hit.position
++ const PxGeomRaycastHit& hit, const PxVec3& v0, const PxVec3& v1, const PxVec3& v2, PxReal&, const PxU32* vinds) PX_OVERRIDE
++ {
++ // PT: this one is safe because incoming vertices from midphase are always safe to V4Load (by design)
++ // PT: TODO: is this test really needed? Not done in midphase already?
++ if(!intersectTriangleBox(mBox, v0, v1, v2))
++ return true;
++
++ PxVec3 verts[3];
++ getScaledVertices(verts, v0, v1, v2, mIdtMeshScale, mMeshScaling);
++
++ const PxU32 triangleIndex = hit.faceIndex;
++
++ PxU8 extraData = getConvexEdgeFlags(mExtraTrigData, triangleIndex);
++
++ const PxU32* vertexIndices = vinds;
++ PxU32 localStorage[3];
++ if(mMeshScaling.flipsNormal())
++ {
++ flipConvexEdgeFlags(extraData);
++ localStorage[0] = vinds[0];
++ localStorage[1] = vinds[2];
++ localStorage[2] = vinds[1];
++ vertexIndices = localStorage;
++ }
++
++ mGeneration.processTriangle(verts, triangleIndex, extraData, vertexIndices);
++ return true;
++ }
++
++ protected:
++ ConvexMeshContactGenerationCallback &operator=(const ConvexMeshContactGenerationCallback &);
++ };
++
++ bool ConvexMeshContactGeneration::generateContacts(
++ const PxPlane& localPlane,
++ const PxVec3* PX_RESTRICT localPoints,
++ const PxVec3& triCenter, PxVec3& groupAxis,
++ PxReal groupMinDepth, PxU32 index) const
++ {
++ const PxVec3 worldGroupCenter = mWorld1.transform(triCenter);
++ const PxVec3 deltaC = mHullCenterWorld - worldGroupCenter;
++ if(deltaC.dot(groupAxis) < 0.0f)
++ groupAxis = -groupAxis;
++
++ const PxU32 id = (mPolyData0.mSelectClosestEdgeCB)(mPolyData0, mConvexScaling, mWorld0.rotateTranspose(-groupAxis));
++
++ const HullPolygonData& HP = mPolyData0.mPolygons[id];
++ PX_ALIGN(16, PxPlane) shapeSpacePlane0;
++ if(mIdtConvexScale)
++ V4StoreA(V4LoadU(&HP.mPlane.n.x), &shapeSpacePlane0.n.x);
++ else
++ mConvexScaling.transformPlaneToShapeSpace(HP.mPlane.n, HP.mPlane.d, shapeSpacePlane0.n, shapeSpacePlane0.d);
++
++ const PxVec3 hullNormalWorld = mWorld0.rotate(shapeSpacePlane0.n);
++
++ const PxReal d0 = PxAbs(hullNormalWorld.dot(groupAxis));
++
++ const PxVec3 triNormalWorld = mWorld1.rotate(localPlane.n);
++ const PxReal d1 = PxAbs(triNormalWorld.dot(groupAxis));
++ const bool d0biggerd1 = d0 > d1;
++
++ ////////////////////NEW DIST HANDLING//////////////////////
++ //TODO: skip this if there is no dist involved!
++
++ PxReal separation = - groupMinDepth; //convert to real distance.
++
++ separation = fsel(separation, separation, 0.0f); //don't do anything when penetrating!
++
++ //printf("\nseparation = %f", separation);
++
++ PxReal contactGenPositionShift = separation + mCCDEpsilon; //if we're at a distance, shift so we're within penetration.
++
++ PxVec3 contactGenPositionShiftVec = groupAxis * contactGenPositionShift; //shift one of the bodies this distance toward the other just for Pierre's contact generation. Then the bodies should be penetrating exactly by MIN_SEPARATION_FOR_PENALTY - ideal conditions for this contact generator.
++
++ //note: for some reason this has to change sign!
++
++ //this will make contact gen always generate contacts at about MSP. Shift them back to the true real distance, and then to a solver compliant distance given that
++ //the solver converges to MSP penetration, while we want it to converge to 0 penetration.
++ //to real distance:
++ // PxReal polyPolySeparationShift = separation; //(+ or - depending on which way normal goes)
++
++ //The system: We always shift convex 0 (arbitrary). If the contact is attached to convex 0 then we will need to shift the contact point, otherwise not.
++
++ //TODO: make these overwrite orig location if its safe to do so.
++
++ PxMat34 world0_(mWorld0);
++ PxTransform transform0_(mTransform0);
++
++ world0_.p -= contactGenPositionShiftVec;
++ transform0_.p = world0_.p; //reset this too.
++
++ const PxTransform t0to1_ = mTransform1.transformInv(transform0_);
++ const PxTransform t1to0_ = transform0_.transformInv(mTransform1);
++ const Matrix34FromTransform m0to1_(t0to1_);
++ const Matrix34FromTransform m1to0_(t1to0_);
++
++ PxVec3* scaledVertices0;
++ PxU8* stackIndices0;
++ GET_SCALEX_CONVEX(scaledVertices0, stackIndices0, mIdtConvexScale, HP.mNbVerts, mConvexScaling, mPolyData0.mVerts, mPolyData0.getPolygonVertexRefs(HP))
++
++ const PxU8 indices[3] = {0, 1, 2};
++
++ const PxMat33 RotT0 = findRotationMatrixFromZ(shapeSpacePlane0.n);
++ const PxMat33 RotT1 = findRotationMatrixFromZ(localPlane.n);
++
++ if(d0biggerd1)
++ {
++ if(contactPolygonPolygonExt(
++ HP.mNbVerts, scaledVertices0, stackIndices0, world0_, shapeSpacePlane0,
++ RotT0,
++ 3, localPoints, indices, mWorld1, localPlane,
++ RotT1,
++ hullNormalWorld, m0to1_, m1to0_, PXC_CONTACT_NO_FACE_INDEX, index,
++ mContactBuffer,
++ true,
++ contactGenPositionShiftVec, contactGenPositionShift))
++ {
++ return true;
++ }
++ }
++ else
++ {
++ if(contactPolygonPolygonExt(
++ 3, localPoints, indices, mWorld1, localPlane,
++ RotT1,
++ HP.mNbVerts, scaledVertices0, stackIndices0, world0_, shapeSpacePlane0,
++ RotT0,
++ triNormalWorld, m1to0_, m0to1_, PXC_CONTACT_NO_FACE_INDEX, index,
++ mContactBuffer,
++ false,
++ contactGenPositionShiftVec, contactGenPositionShift))
++ {
++ return true;
++ }
++ }
++ return false;
++ }
++
++ enum FeatureCode
++ {
++ FC_VERTEX0,
++ FC_VERTEX1,
++ FC_VERTEX2,
++ FC_EDGE01,
++ FC_EDGE12,
++ FC_EDGE20,
++ FC_FACE,
++
++ FC_UNDEFINED
++ };
++
++ static FeatureCode computeFeatureCode(const PxVec3& point, const PxVec3* verts)
++ {
++ const PxVec3& triangleOrigin = verts[0];
++ const PxVec3 triangleEdge0 = verts[1] - verts[0];
++ const PxVec3 triangleEdge1 = verts[2] - verts[0];
++
++ const PxVec3 kDiff = triangleOrigin - point;
++ const PxReal fA00 = triangleEdge0.magnitudeSquared();
++ const PxReal fA01 = triangleEdge0.dot(triangleEdge1);
++ const PxReal fA11 = triangleEdge1.magnitudeSquared();
++ const PxReal fB0 = kDiff.dot(triangleEdge0);
++ const PxReal fB1 = kDiff.dot(triangleEdge1);
++ const PxReal fDet = PxAbs(fA00*fA11 - fA01*fA01);
++ const PxReal u = fA01*fB1-fA11*fB0;
++ const PxReal v = fA01*fB0-fA00*fB1;
++
++ FeatureCode fc = FC_UNDEFINED;
++
++ if(u + v <= fDet)
++ {
++ if(u < 0.0f)
++ {
++ if(v < 0.0f) // region 4
++ {
++ if(fB0 < 0.0f)
++ {
++ if(-fB0 >= fA00)
++ fc = FC_VERTEX1;
++ else
++ fc = FC_EDGE01;
++ }
++ else
++ {
++ if(fB1 >= 0.0f)
++ fc = FC_VERTEX0;
++ else if(-fB1 >= fA11)
++ fc = FC_VERTEX2;
++ else
++ fc = FC_EDGE20;
++ }
++ }
++ else // region 3
++ {
++ if(fB1 >= 0.0f)
++ fc = FC_VERTEX0;
++ else if(-fB1 >= fA11)
++ fc = FC_VERTEX2;
++ else
++ fc = FC_EDGE20;
++ }
++ }
++ else if(v < 0.0f) // region 5
++ {
++ if(fB0 >= 0.0f)
++ fc = FC_VERTEX0;
++ else if(-fB0 >= fA00)
++ fc = FC_VERTEX1;
++ else
++ fc = FC_EDGE01;
++ }
++ else // region 0
++ {
++ // minimum at interior PxVec3
++ if(fDet==0.0f)
++ fc = FC_VERTEX0;
++ else
++ fc = FC_FACE;
++ }
++ }
++ else
++ {
++ PxReal fTmp0, fTmp1, fNumer, fDenom;
++
++ if(u < 0.0f) // region 2
++ {
++ fTmp0 = fA01 + fB0;
++ fTmp1 = fA11 + fB1;
++ if(fTmp1 > fTmp0)
++ {
++ fNumer = fTmp1 - fTmp0;
++ fDenom = fA00-2.0f*fA01+fA11;
++ if(fNumer >= fDenom)
++ fc = FC_VERTEX1;
++ else
++ fc = FC_EDGE12;
++ }
++ else
++ {
++ if(fTmp1 <= 0.0f)
++ fc = FC_VERTEX2;
++ else if(fB1 >= 0.0f)
++ fc = FC_VERTEX0;
++ else
++ fc = FC_EDGE20;
++ }
++ }
++ else if(v < 0.0f) // region 6
++ {
++ fTmp0 = fA01 + fB1;
++ fTmp1 = fA00 + fB0;
++ if(fTmp1 > fTmp0)
++ {
++ fNumer = fTmp1 - fTmp0;
++ fDenom = fA00-2.0f*fA01+fA11;
++ if(fNumer >= fDenom)
++ fc = FC_VERTEX2;
++ else
++ fc = FC_EDGE12;
++ }
++ else
++ {
++ if(fTmp1 <= 0.0f)
++ fc = FC_VERTEX1;
++ else if(fB0 >= 0.0f)
++ fc = FC_VERTEX0;
++ else
++ fc = FC_EDGE01;
++ }
++ }
++ else // region 1
++ {
++ fNumer = fA11 + fB1 - fA01 - fB0;
++ if(fNumer <= 0.0f)
++ {
++ fc = FC_VERTEX2;
++ }
++ else
++ {
++ fDenom = fA00-2.0f*fA01+fA11;
++ if(fNumer >= fDenom)
++ fc = FC_VERTEX1;
++ else
++ fc = FC_EDGE12;
++ }
++ }
++ }
++ return fc;
++ }
++
++
++ //static bool validateVertex(PxU32 vref, const PxU32 count, const ContactPoint* PX_RESTRICT contacts, const TriangleMesh& meshData)
++ //{
++ // PxU32 previous = 0xffffffff;
++ // for(PxU32 i=0;i<count;i++)
++ // {
++ // if(contacts[i].internalFaceIndex1==previous)
++ // continue;
++ // previous = contacts[i].internalFaceIndex1;
++ //
++ // const TriangleIndices T(meshData, contacts[i].internalFaceIndex1);
++ // if( T.mVRefs[0]==vref
++ // || T.mVRefs[1]==vref
++ // || T.mVRefs[2]==vref)
++ // return false;
++ // }
++ // return true;
++ //}
++
++
++ //static PX_FORCE_INLINE bool testEdge(PxU32 vref0, PxU32 vref1, PxU32 tvref0, PxU32 tvref1)
++ //{
++ // if(tvref0>tvref1)
++ // PxSwap(tvref0, tvref1);
++ //
++ // if(tvref0==vref0 && tvref1==vref1)
++ // return false;
++ // return true;
++ //}
++
++ //static bool validateEdge(PxU32 vref0, PxU32 vref1, const PxU32 count, const ContactPoint* PX_RESTRICT contacts, const TriangleMesh& meshData)
++ //{
++ // if(vref0>vref1)
++ // PxSwap(vref0, vref1);
++ //
++ // PxU32 previous = 0xffffffff;
++ // for(PxU32 i=0;i<count;i++)
++ // {
++ // if(contacts[i].internalFaceIndex1==previous)
++ // continue;
++ // previous = contacts[i].internalFaceIndex1;
++ //
++ // const TriangleIndices T(meshData, contacts[i].internalFaceIndex1);
++ //
++ ///* if(T.mVRefs[0]==vref0 || T.mVRefs[0]==vref1)
++ // return false;
++ // if(T.mVRefs[1]==vref0 || T.mVRefs[1]==vref1)
++ // return false;
++ // if(T.mVRefs[2]==vref0 || T.mVRefs[2]==vref1)
++ // return false;*/
++ // // PT: wow, this was wrong??? ###FIX
++ // if(!testEdge(vref0, vref1, T.mVRefs[0], T.mVRefs[1]))
++ // return false;
++ // if(!testEdge(vref0, vref1, T.mVRefs[1], T.mVRefs[2]))
++ // return false;
++ // if(!testEdge(vref0, vref1, T.mVRefs[2], T.mVRefs[0]))
++ // return false;
++ // }
++ // return true;
++ //}
++
++ //static bool validateEdge(PxU32 vref0, PxU32 vref1, const PxU32* vertIndices, const PxU32 nbIndices)
++ //{
++ // if(vref0>vref1)
++ // PxSwap(vref0, vref1);
++ //
++ // for(PxU32 i=0;i<nbIndices;i+=3)
++ // {
++ // if(!testEdge(vref0, vref1, vertIndices[i+0], vertIndices[i+1]))
++ // return false;
++ // if(!testEdge(vref0, vref1, vertIndices[i+1], vertIndices[i+2]))
++ // return false;
++ // if(!testEdge(vref0, vref1, vertIndices[i+2], vertIndices[i+0]))
++ // return false;
++ // }
++ // return true;
++ //}
++
++ //#endif
++
++ void ConvexMeshContactGeneration::processTriangle(const PxVec3* verts, PxU32 triangleIndex, PxU8 triFlags, const PxU32* vertInds)
++ {
++ const PxPlane localPlane(verts[0], verts[1], verts[2]);
++
++ // Backface culling
++ if(localPlane.distance(mHullCenterMesh)<0.0f)
++ // if(localPlane.normal.dot(mHullCenterMesh - T.mVerts[0]) <= 0.0f)
++ return;
++
++ //////////////////////////////////////////////////////////////////////////
++
++ const PxVec3 triCenter = (verts[0] + verts[1] + verts[2])*(1.0f/3.0f);
++
++ // Group center in hull space
++ #ifdef USE_TRIANGLE_NORMAL
++ const PxVec3 groupCenterHull = m1to0.rotate(localPlane.normal);
++ #else
++ const PxVec3 groupCenterHull = m1to0.transform(triCenter);
++ #endif
++ //////////////////////////////////////////////////////////////////////////
++
++ PxVec3 groupAxis;
++ PxReal groupMinDepth;
++ bool faceContact;
++ if(!triangleConvexTest( mPolyData0, triFlags,
++ triangleIndex, verts,
++ localPlane,
++ groupCenterHull,
++ mWorld0, mWorld1, m0to1, m1to0,
++ mConvexScaling,
++ mContactDistance, mToleranceLength,
++ groupAxis, groupMinDepth, faceContact,
++ mIdtConvexScale
++ ))
++ return;
++
++ if(faceContact)
++ {
++ // PT: generate face contacts immediately to save memory & avoid recomputing triangle data later
++ if(generateContacts(
++ localPlane,
++ verts,
++ triCenter, groupAxis,
++ groupMinDepth, triangleIndex))
++ {
++ mAnyHits = true;
++ mEdgeCache.addData(CachedEdge(vertInds[0], vertInds[1]));
++ mEdgeCache.addData(CachedEdge(vertInds[0], vertInds[2]));
++ mEdgeCache.addData(CachedEdge(vertInds[1], vertInds[2]));
++ mVertCache.addData(CachedVertex(vertInds[0]));
++ mVertCache.addData(CachedVertex(vertInds[1]));
++ mVertCache.addData(CachedVertex(vertInds[2]));
++ }
++ else
++ {
++ int stop=1;
++ (void)stop;
++ }
++ }
++ else
++ {
++ const PxU32 nb = sizeof(SavedContactData)/sizeof(PxU32);
++
++ // PT: no "pushBack" please (useless data copy + LHS)
++ PxU32 newSize = nb + mDelayedContacts.size();
++ mDelayedContacts.reserve(newSize);
++ SavedContactData* PX_RESTRICT cd = reinterpret_cast<SavedContactData*>(mDelayedContacts.end());
++ mDelayedContacts.forceSize_Unsafe(newSize);
++
++ cd->mTriangleIndex = triangleIndex;
++ cd->mVerts[0] = verts[0];
++ cd->mVerts[1] = verts[1];
++ cd->mVerts[2] = verts[2];
++ cd->mInds[0] = vertInds[0];
++ cd->mInds[1] = vertInds[1];
++ cd->mInds[2] = vertInds[2];
++ cd->mGroupAxis = groupAxis;
++ cd->mGroupMinDepth = groupMinDepth;
++ }
++ }
++
++ void ConvexMeshContactGeneration::generateLastContacts()
++ {
++ // Process delayed contacts
++ PxU32 nbEntries = mDelayedContacts.size();
++ if(nbEntries)
++ {
++ nbEntries /= sizeof(SavedContactData)/sizeof(PxU32);
++
++ // PT: TODO: replicate this fix in sphere-vs-mesh ###FIX
++ //const PxU32 count = mContactBuffer.count;
++ //const ContactPoint* PX_RESTRICT contacts = mContactBuffer.contacts;
++
++ const SavedContactData* PX_RESTRICT cd = reinterpret_cast<const SavedContactData*>(mDelayedContacts.begin());
++ for(PxU32 i=0;i<nbEntries;i++)
++ {
++ const SavedContactData& currentContact = cd[i];
++
++ const PxU32 triangleIndex = currentContact.mTriangleIndex;
++
++ // PT: unfortunately we must recompute this triangle-data here.
++ // PT: TODO: find a way not to
++ // const TriangleVertices T(*mMeshData, mMeshScaling, triangleIndex);
++ // const TriangleIndices T(*mMeshData, triangleIndex);
++
++ const PxU32 ref0 = currentContact.mInds[0];
++ const PxU32 ref1 = currentContact.mInds[1];
++ const PxU32 ref2 = currentContact.mInds[2];
++
++ // PT: TODO: why bother with the feature code at all? Use edge cache directly?
++ // const FeatureCode FC = computeFeatureCode(mHullCenterMesh, T.mVerts);
++ const FeatureCode FC = computeFeatureCode(mHullCenterMesh, currentContact.mVerts);
++
++ bool generateContact = false;
++ switch(FC)
++ {
++ // PT: trying the same as in sphere-mesh here
++ case FC_VERTEX0:
++ generateContact = !mVertCache.contains(CachedVertex(ref0));
++ break;
++ case FC_VERTEX1:
++ generateContact =!mVertCache.contains(CachedVertex(ref1));
++ break;
++ case FC_VERTEX2:
++ generateContact = !mVertCache.contains(CachedVertex(ref2));
++ break;
++ case FC_EDGE01:
++ generateContact = !mEdgeCache.contains(CachedEdge(ref0, ref1));
++ break;
++ case FC_EDGE12:
++ generateContact = !mEdgeCache.contains(CachedEdge(ref1, ref2));
++ break;
++ case FC_EDGE20:
++ generateContact = !mEdgeCache.contains(CachedEdge(ref0, ref2));
++ break;
++ case FC_FACE:
++ generateContact = true;
++ break;
++ case FC_UNDEFINED:
++ break;
++ };
++
++ if(!generateContact)
++ continue;
++
++ // const PxcPlane localPlane(T.mVerts[0], T.mVerts[1], T.mVerts[2]);
++ const PxPlane localPlane(currentContact.mVerts[0], currentContact.mVerts[1], currentContact.mVerts[2]);
++ // const PxVec3 triCenter = (T.mVerts[0] + T.mVerts[1] + T.mVerts[2])*(1.0f/3.0f);
++ const PxVec3 triCenter = (currentContact.mVerts[0] + currentContact.mVerts[1] + currentContact.mVerts[2])*(1.0f/3.0f);
++
++ PxVec3 groupAxis = currentContact.mGroupAxis;
++ if(generateContacts(
++ localPlane,
++ // T.mVerts,
++ currentContact.mVerts,
++ triCenter, groupAxis,
++ currentContact.mGroupMinDepth, triangleIndex))
++ {
++ mAnyHits = true;
++
++ //We don't add the edges to the data - this is important because we don't want to reject triangles
++ //because we generated an edge contact with an adjacent triangle
++ /*mEdgeCache.addData(CachedEdge(ref0, ref1));
++ mEdgeCache.addData(CachedEdge(ref0, ref2));
++ mEdgeCache.addData(CachedEdge(ref1, ref2));
++ mVertCache.addData(CachedVertex(ref0));
++ mVertCache.addData(CachedVertex(ref1));
++ mVertCache.addData(CachedVertex(ref2));*/
++
++ }
++ }
++ }
++ }
++
++ /////////////
++
++ static bool contactHullMesh2(const PolygonalData& polyData0, const PxBounds3& hullAABB, const PxTriangleMeshGeometry& shape1,
++ const PxTransform& transform0, const PxTransform& transform1,
++ const NarrowPhaseParams& params, PxContactBuffer& contactBuffer,
++ const FastVertex2ShapeScaling& convexScaling, const FastVertex2ShapeScaling& meshScaling,
++ bool idtConvexScale, bool idtMeshScale)
++ {
++ //Just a sanity-check in debug-mode
++ PX_ASSERT(shape1.getType() == PxGeometryType::eTRIANGLEMESH);
++ ////////////////////
++
++ // Compute matrices
++ const Matrix34FromTransform world0(transform0);
++ const Matrix34FromTransform world1(transform1);
++
++ // Compute relative transforms
++ const PxTransform t0to1 = transform1.transformInv(transform0);
++ const PxTransform t1to0 = transform0.transformInv(transform1);
++
++ BoxPadded hullOBB;
++ computeHullOBB(hullOBB, hullAABB, params.mContactDistance, world0, world1, meshScaling, idtMeshScale);
++
++ // Setup the collider
++ const TriangleMesh* PX_RESTRICT meshData = _getMeshData(shape1);
++
++ PxInlineArray<PxU32,LOCAL_CONTACTS_SIZE> delayedContacts;
++
++ ConvexMeshContactGenerationCallback blockCallback(
++ delayedContacts,
++ t0to1, t1to0, polyData0, world0, world1, meshData, meshData->getExtraTrigData(), meshScaling,
++ convexScaling, params.mContactDistance, params.mToleranceLength,
++ idtMeshScale, idtConvexScale, params.mMeshContactMargin,
++ transform0, transform1,
++ contactBuffer, hullOBB
++ );
++
++ Midphase::intersectOBB(meshData, hullOBB, blockCallback, false);
++
++ blockCallback.mGeneration.generateLastContacts();
++
++ return blockCallback.mGeneration.mAnyHits;
++ }
++
++ /////////////
++
++ bool Gu::contactConvexMesh(GU_CONTACT_METHOD_ARGS)
++ {
++ PX_UNUSED(cache);
++ PX_UNUSED(renderOutput);
++
++ const PxConvexMeshGeometry& shapeConvex = checkedCast<PxConvexMeshGeometry>(shape0);
++ const PxTriangleMeshGeometry& shapeMesh = checkedCast<PxTriangleMeshGeometry>(shape1);
++
++ const bool idtScaleMesh = shapeMesh.scale.isIdentity();
++
++ FastVertex2ShapeScaling meshScaling;
++ if(!idtScaleMesh)
++ meshScaling.init(shapeMesh.scale);
++
++ FastVertex2ShapeScaling convexScaling;
++ PxBounds3 hullAABB;
++ PolygonalData polyData0;
++ const bool idtScaleConvex = getConvexData(shapeConvex, convexScaling, hullAABB, polyData0);
++
++ return contactHullMesh2(polyData0, hullAABB, shapeMesh, transform0, transform1, params, contactBuffer, convexScaling, meshScaling, idtScaleConvex, idtScaleMesh);
++ }
++
++ bool Gu::contactBoxMesh(GU_CONTACT_METHOD_ARGS)
++ {
++ PX_UNUSED(cache);
++ PX_UNUSED(renderOutput);
++
++ const PxBoxGeometry& shapeBox = checkedCast<PxBoxGeometry>(shape0);
++ const PxTriangleMeshGeometry& shapeMesh = checkedCast<PxTriangleMeshGeometry>(shape1);
++
++ PolygonalData polyData0;
++ PolygonalBox polyBox(shapeBox.halfExtents);
++ polyBox.getPolygonalData(&polyData0);
++
++ const PxBounds3 hullAABB(-shapeBox.halfExtents, shapeBox.halfExtents);
++
++ const bool idtScaleMesh = shapeMesh.scale.isIdentity();
++
++ FastVertex2ShapeScaling meshScaling;
++ if(!idtScaleMesh)
++ meshScaling.init(shapeMesh.scale);
++
++ FastVertex2ShapeScaling idtScaling;
++ return contactHullMesh2(polyData0, hullAABB, shapeMesh, transform0, transform1, params, contactBuffer, idtScaling, meshScaling, true, idtScaleMesh);
++ }
++
++ /////////////
++
++ namespace
++ {
++ struct ConvexVsHeightfieldContactGenerationCallback : OverlapReport
++ {
++ ConvexMeshContactGeneration mGeneration;
++ HeightFieldUtil& mHfUtil;
++
++ ConvexVsHeightfieldContactGenerationCallback(
++ HeightFieldUtil& hfUtil,
++ PxInlineArray<PxU32,LOCAL_CONTACTS_SIZE>& delayedContacts,
++ const PxTransform& t0to1, const PxTransform& t1to0,
++ const PolygonalData& polyData0, const PxMat34& world0, const PxMat34& world1,
++ const FastVertex2ShapeScaling& convexScaling,
++ PxReal contactDistance,
++ PxReal toleranceLength,
++ bool idtConvexScale,
++ PxReal cCCDEpsilon,
++ const PxTransform& transform0, const PxTransform& transform1,
++ PxContactBuffer& contactBuffer
++ ) : mGeneration(delayedContacts, t0to1, t1to0, polyData0, world0, world1, convexScaling, contactDistance, toleranceLength, idtConvexScale, cCCDEpsilon, transform0,
++ transform1, contactBuffer),
++ mHfUtil(hfUtil)
++ {
++ }
++
++ // PT: TODO: refactor/unify with similar code in other places
++ virtual bool reportTouchedTris(PxU32 nb, const PxU32* indices) PX_OVERRIDE
++ {
++ const PxU8 nextInd[] = {2,0,1};
++
++ while(nb--)
++ {
++ const PxU32 triangleIndex = *indices++;
++
++ PxU32 vertIndices[3];
++ PxTriangle currentTriangle; // in world space
++ PxU32 adjInds[3];
++ mHfUtil.getTriangle(mGeneration.mTransform1, currentTriangle, vertIndices, adjInds, triangleIndex, false, false);
++
++ PxVec3 normal;
++ currentTriangle.normal(normal);
++
++ PxU8 triFlags = 0; //KS - temporary until we can calculate triFlags for HF
++
++ for(PxU32 a = 0; a < 3; ++a)
++ {
++ if(adjInds[a] != 0xFFFFFFFF)
++ {
++ PxTriangle adjTri;
++ mHfUtil.getTriangle(mGeneration.mTransform1, adjTri, NULL, NULL, adjInds[a], false, false);
++ //We now compare the triangles to see if this edge is active
++
++ PxVec3 adjNormal;
++ adjTri.denormalizedNormal(adjNormal);
++ PxU32 otherIndex = nextInd[a];
++ PxF32 projD = adjNormal.dot(currentTriangle.verts[otherIndex] - adjTri.verts[0]);
++ if(projD < 0.f)
++ {
++ adjNormal.normalize();
++
++ PxF32 proj = adjNormal.dot(normal);
++
++ if(proj < 0.999f)
++ {
++ triFlags |= 1 << (a+3);
++ }
++ }
++ }
++ else
++ triFlags |= (1 << (a+3));
++ }
++ mGeneration.processTriangle(currentTriangle.verts, triangleIndex, triFlags, vertIndices);
++ }
++ return true;
++ }
++
++ protected:
++ ConvexVsHeightfieldContactGenerationCallback &operator=(const ConvexVsHeightfieldContactGenerationCallback &);
++ };
++ }
++
++ static bool contactHullHeightfield2(const PolygonalData& polyData0, const PxBounds3& hullAABB, const PxHeightFieldGeometry& shape1,
++ const PxTransform& transform0, const PxTransform& transform1,
++ const NarrowPhaseParams& params, PxContactBuffer& contactBuffer,
++ const FastVertex2ShapeScaling& convexScaling,
++ bool idtConvexScale)
++ {
++ //We need to create a callback that fills triangles from the HF
++
++ HeightFieldUtil hfUtil(shape1);
++
++ const Matrix34FromTransform world0(transform0);
++ const Matrix34FromTransform world1(transform1);
++
++ ////////////////////
++
++ // Compute relative transforms
++ const PxTransform t0to1 = transform1.transformInv(transform0);
++ const PxTransform t1to0 = transform0.transformInv(transform1);
++
++ PxInlineArray<PxU32, LOCAL_CONTACTS_SIZE> delayedContacts;
++
++ ConvexVsHeightfieldContactGenerationCallback blockCallback(hfUtil, delayedContacts, t0to1, t1to0, polyData0, world0, world1, convexScaling, params.mContactDistance, params.mToleranceLength,
++ idtConvexScale, params.mMeshContactMargin, transform0, transform1, contactBuffer);
++
++ hfUtil.overlapAABBTriangles0to1(t0to1, hullAABB, blockCallback);
++
++ blockCallback.mGeneration.generateLastContacts();
++
++ return blockCallback.mGeneration.mAnyHits;
++ }
++
++ bool Gu::contactConvexHeightfield(GU_CONTACT_METHOD_ARGS)
++ {
++ PX_UNUSED(cache);
++ PX_UNUSED(renderOutput);
++
++ //Create a triangle cache from the HF triangles and then feed triangles to NP mesh methods
++ const PxConvexMeshGeometry& shapeConvex = checkedCast<PxConvexMeshGeometry>(shape0);
++ const PxHeightFieldGeometry& shapeMesh = checkedCast<PxHeightFieldGeometry>(shape1);
++
++ FastVertex2ShapeScaling convexScaling;
++ PxBounds3 hullAABB;
++ PolygonalData polyData0;
++ const bool idtScaleConvex = getConvexData(shapeConvex, convexScaling, hullAABB, polyData0);
++ const PxVec3 inflation(params.mContactDistance);
++ hullAABB.minimum -= inflation;
++ hullAABB.maximum += inflation;
++
++ return contactHullHeightfield2(polyData0, hullAABB, shapeMesh, transform0, transform1, params, contactBuffer, convexScaling, idtScaleConvex);
++ }
++
++ bool Gu::contactBoxHeightfield(GU_CONTACT_METHOD_ARGS)
++ {
++ PX_UNUSED(cache);
++ PX_UNUSED(renderOutput);
++
++ //Create a triangle cache from the HF triangles and then feed triangles to NP mesh methods
++ const PxHeightFieldGeometry& shapeMesh = checkedCast<PxHeightFieldGeometry>(shape1);
++ const PxBoxGeometry& shapeBox = checkedCast<PxBoxGeometry>(shape0);
++
++ PolygonalData polyData0;
++ PolygonalBox polyBox(shapeBox.halfExtents);
++ polyBox.getPolygonalData(&polyData0);
++
++ const PxVec3 inflatedExtents = shapeBox.halfExtents + PxVec3(params.mContactDistance);
++
++ const PxBounds3 hullAABB = PxBounds3(-inflatedExtents, inflatedExtents);
++
++ const FastVertex2ShapeScaling idtScaling;
++
++ return contactHullHeightfield2(polyData0, hullAABB, shapeMesh, transform0, transform1, params, contactBuffer, idtScaling, true);
++ }