diff options
Diffstat (limited to 'PhysX_3.4/Source/GeomUtils/src')
33 files changed, 698 insertions, 584 deletions
diff --git a/PhysX_3.4/Source/GeomUtils/src/GuMeshFactory.cpp b/PhysX_3.4/Source/GeomUtils/src/GuMeshFactory.cpp index 0ce25e11..d55193d5 100644 --- a/PhysX_3.4/Source/GeomUtils/src/GuMeshFactory.cpp +++ b/PhysX_3.4/Source/GeomUtils/src/GuMeshFactory.cpp @@ -367,10 +367,11 @@ static TriangleMeshData* loadMeshData(PxInputStream& stream) stream.read(data->mExtraTrigData, nb*sizeof(PxU8)); } - if(serialFlags & IMSF_GRB_DATA) + if (serialFlags & IMSF_GRB_DATA) { - data->mGRB_meshAdjVerticiesTotal = readDword(mismatch, stream); - + PxU32 GRB_meshAdjVerticiesTotal = 0; + if (version < 15) + GRB_meshAdjVerticiesTotal = readDword(mismatch, stream); //read grb triangle indices PX_ASSERT(data->mGRB_triIndices); @@ -402,7 +403,7 @@ static TriangleMeshData* loadMeshData(PxInputStream& stream) if (data->has16BitIndices()) { PxU16* tris16 = reinterpret_cast<PxU16*>(data->mGRB_triIndices); - stream.read(tris16, nbIndices*sizeof(PxU16)); + stream.read(tris16, nbIndices * sizeof(PxU16)); if (mismatch) { for (PxU32 i = 0; i<nbIndices; i++) @@ -441,7 +442,7 @@ static TriangleMeshData* loadMeshData(PxInputStream& stream) else { PxU32* tris32 = reinterpret_cast<PxU32*>(data->mGRB_triIndices); - stream.read(tris32, nbIndices*sizeof(PxU32)); + stream.read(tris32, nbIndices * sizeof(PxU32)); if (mismatch) { @@ -450,35 +451,33 @@ static TriangleMeshData* loadMeshData(PxInputStream& stream) } } } - - data->mGRB_triAdjacencies = static_cast<void *>(PX_NEW(PxU32)[data->mNbTriangles*4]); - data->mGRB_vertValency = PX_NEW(PxU32)[data->mNbVertices]; - data->mGRB_adjVertStart = PX_NEW(PxU32)[data->mNbVertices]; - data->mGRB_adjVertices = PX_NEW(PxU32)[data->mGRB_meshAdjVerticiesTotal]; + + data->mGRB_triAdjacencies = static_cast<void *>(PX_NEW(PxU32)[data->mNbTriangles * 4]); data->mGRB_faceRemap = PX_NEW(PxU32)[data->mNbTriangles]; - stream.read(data->mGRB_triAdjacencies, sizeof(PxU32)*data->mNbTriangles*4); - stream.read(data->mGRB_vertValency, sizeof(PxU32)*data->mNbVertices); - stream.read(data->mGRB_adjVertStart, sizeof(PxU32)*data->mNbVertices); - stream.read(data->mGRB_adjVertices, sizeof(PxU32)*data->mGRB_meshAdjVerticiesTotal); + stream.read(data->mGRB_triAdjacencies, sizeof(PxU32)*data->mNbTriangles * 4); + if (version < 15) + { + //stream.read(data->mGRB_vertValency, sizeof(PxU32)*data->mNbVertices); + for (PxU32 i = 0; i < data->mNbVertices; ++i) + readDword(mismatch, stream); + //stream.read(data->mGRB_adjVertStart, sizeof(PxU32)*data->mNbVertices); + for (PxU32 i = 0; i < data->mNbVertices; ++i) + readDword(mismatch, stream); + //stream.read(data->mGRB_adjVertices, sizeof(PxU32)*GRB_meshAdjVerticiesTotal); + for (PxU32 i = 0; i < GRB_meshAdjVerticiesTotal; ++i) + readDword(mismatch, stream); + } stream.read(data->mGRB_faceRemap, sizeof(PxU32)*data->mNbTriangles); - if(mismatch) + if (mismatch) { - for(PxU32 i=0;i<data->mNbTriangles*4;i++) + for (PxU32 i = 0; i<data->mNbTriangles * 4; i++) flip(reinterpret_cast<PxU32 *>(data->mGRB_triIndices)[i]); - for(PxU32 i=0;i<data->mNbTriangles*4;i++) + for (PxU32 i = 0; i<data->mNbTriangles * 4; i++) flip(reinterpret_cast<PxU32 *>(data->mGRB_triAdjacencies)[i]); - for(PxU32 i=0;i<data->mNbVertices;i++) - { - flip(data->mGRB_vertValency[i]); - flip(data->mGRB_adjVertStart[i]); - } - for(PxU32 i=0;i<data->mGRB_meshAdjVerticiesTotal;i++) - flip(data->mGRB_adjVertices[i]); - } //read BV32 diff --git a/PhysX_3.4/Source/GeomUtils/src/GuMetaData.cpp b/PhysX_3.4/Source/GeomUtils/src/GuMetaData.cpp index db2bf366..f7194c4d 100644 --- a/PhysX_3.4/Source/GeomUtils/src/GuMetaData.cpp +++ b/PhysX_3.4/Source/GeomUtils/src/GuMetaData.cpp @@ -358,10 +358,6 @@ void Gu::TriangleMesh::getBinaryMetaData(PxOutputStream& stream) PX_DEF_BIN_METADATA_ITEM(stream, TriangleMesh, void, mGRB_triIndices, PxMetaDataFlag::ePTR) PX_DEF_BIN_METADATA_ITEM(stream, TriangleMesh, void, mGRB_triAdjacencies, PxMetaDataFlag::ePTR) - PX_DEF_BIN_METADATA_ITEM(stream, TriangleMesh, PxU32, mGRB_vertValency, PxMetaDataFlag::ePTR) - PX_DEF_BIN_METADATA_ITEM(stream, TriangleMesh, PxU32, mGRB_adjVertStart, PxMetaDataFlag::ePTR) - PX_DEF_BIN_METADATA_ITEM(stream, TriangleMesh, PxU32, mGRB_adjVertices, PxMetaDataFlag::ePTR) - PX_DEF_BIN_METADATA_ITEM(stream, TriangleMesh, PxU32, mGRB_meshAdjVerticiesTotal, 0) PX_DEF_BIN_METADATA_ITEM(stream, TriangleMesh, PxU32, mGRB_faceRemap, PxMetaDataFlag::ePTR) PX_DEF_BIN_METADATA_ITEM(stream, TriangleMesh, void, mGRB_BV32Tree, PxMetaDataFlag::ePTR) @@ -464,6 +460,7 @@ void Gu::GeometryUnion::getBinaryMetaData(PxOutputStream& stream) PX_DEF_BIN_METADATA_ITEM(stream_, ShadowConvexMeshGeometry, PxGeometryType::Enum, mType, 0) PX_DEF_BIN_METADATA_ITEM(stream_, ShadowConvexMeshGeometry, PxMeshScale, scale, 0) PX_DEF_BIN_METADATA_ITEM(stream_, ShadowConvexMeshGeometry, PxConvexMesh, convexMesh, PxMetaDataFlag::ePTR) + PX_DEF_BIN_METADATA_ITEM(stream_, ShadowConvexMeshGeometry, PxReal, maxMargin, 0) PX_DEF_BIN_METADATA_ITEM(stream_, ShadowConvexMeshGeometry, PxConvexMeshGeometryFlags, meshFlags, 0) PX_DEF_BIN_METADATA_ITEMS(stream_, ShadowConvexMeshGeometry, PxU8, paddingFromFlags, PxMetaDataFlag::ePADDING, 3) PX_DEF_BIN_METADATA_ITEM(stream_, ShadowConvexMeshGeometry, ConvexHullData, hullData, PxMetaDataFlag::ePTR) diff --git a/PhysX_3.4/Source/GeomUtils/src/GuRaycastTests.cpp b/PhysX_3.4/Source/GeomUtils/src/GuRaycastTests.cpp index 3cb1ad92..fa7056ad 100644 --- a/PhysX_3.4/Source/GeomUtils/src/GuRaycastTests.cpp +++ b/PhysX_3.4/Source/GeomUtils/src/GuRaycastTests.cpp @@ -168,7 +168,7 @@ PxU32 raycast_capsule(GU_RAY_FUNC_PARAMS) getCapsuleSegment(pose, capsuleGeom, capsule); capsule.radius = capsuleGeom.radius; - PxReal t; + PxReal t = 0.0f; if(!intersectRayCapsule(rayOrigin, rayDir, capsule, t)) return 0; diff --git a/PhysX_3.4/Source/GeomUtils/src/GuSweepMTD.cpp b/PhysX_3.4/Source/GeomUtils/src/GuSweepMTD.cpp index b0a98c50..ff4acd10 100644 --- a/PhysX_3.4/Source/GeomUtils/src/GuSweepMTD.cpp +++ b/PhysX_3.4/Source/GeomUtils/src/GuSweepMTD.cpp @@ -489,7 +489,7 @@ bool physx::Gu::computeBox_TriangleMeshMTD(const PxTriangleMeshGeometry& triMesh const Vec3V p0 = V3LoadU(&boxTransform.p.x); const Vec3V boxExtents = V3LoadU(box.extents); - const FloatV minMargin = CalculatePCMBoxMargin(boxExtents); + const FloatV minMargin = CalculateMTDBoxMargin(boxExtents); const FloatV inflationV = FAdd(FLoad(inflation), minMargin); PxReal boundInflation; FStore(inflationV, &boundInflation); @@ -632,7 +632,7 @@ bool physx::Gu::computeBox_HeightFieldMTD( const PxHeightFieldGeometry& heightFi const Vec3V p0 = V3LoadU(&boxTransform.p.x); const Vec3V boxExtents = V3LoadU(box.extents); - const FloatV minMargin = CalculatePCMBoxMargin(boxExtents); + const FloatV minMargin = CalculateMTDBoxMargin(boxExtents); const FloatV inflationV = FAdd(FLoad(inflation), minMargin); //const FloatV inflationV = FLoad(inflation); @@ -790,7 +790,7 @@ bool physx::Gu::computeConvex_TriangleMeshMTD( const PxTriangleMeshGeometry& tri ConvexHullV convexHull(hullData, V3Zero(), vScale, vQuat, idtScaleConvex); PX_ALIGN(16, PxU8 convexBuff[sizeof(SupportLocalImpl<ConvexHullV>)]); - const FloatV convexMargin = CalculatePCMConvexMargin(hullData, vScale); + const FloatV convexMargin = CalculateMTDConvexMargin(hullData, vScale); const FloatV inflationV = FAdd(FLoad(inflation), convexMargin); PxReal boundInflation; FStore(inflationV, &boundInflation); @@ -951,7 +951,7 @@ bool physx::Gu::computeConvex_HeightFieldMTD( const PxHeightFieldGeometry& heigh ConvexHullV convexHull(hullData, zeroV, vScale, vQuat, idtScaleConvex); PX_ALIGN(16, PxU8 convexBuff[sizeof(SupportLocalImpl<ConvexHullV>)]); - const FloatV convexMargin = CalculatePCMConvexMargin(hullData, vScale); + const FloatV convexMargin = CalculateMTDConvexMargin(hullData, vScale); const FloatV inflationV = FAdd(FLoad(inflation), convexMargin); PxReal boundInflation; FStore(inflationV, &boundInflation); diff --git a/PhysX_3.4/Source/GeomUtils/src/gjk/GuEPA.cpp b/PhysX_3.4/Source/GeomUtils/src/gjk/GuEPA.cpp index 54cded49..db3908e8 100644 --- a/PhysX_3.4/Source/GeomUtils/src/gjk/GuEPA.cpp +++ b/PhysX_3.4/Source/GeomUtils/src/gjk/GuEPA.cpp @@ -36,7 +36,6 @@ #define EPA_DEBUG 0 - namespace physx { namespace Gu @@ -65,11 +64,11 @@ namespace Gu public: EPA(){} GjkStatus PenetrationDepth(const GjkConvex& a, const GjkConvex& b, const Ps::aos::Vec3V* PX_RESTRICT Q, const Ps::aos::Vec3V* PX_RESTRICT A, const Ps::aos::Vec3V* PX_RESTRICT B, const PxI32 size, Ps::aos::Vec3V& pa, Ps::aos::Vec3V& pb, Ps::aos::Vec3V& normal, Ps::aos::FloatV& penDepth, const bool takeCoreShape = false); - bool expandPoint(const GjkConvex& a, const GjkConvex& b, PxI32& numVerts, const FloatVArg lowerBound, const FloatVArg upperBound); - bool expandSegment(const GjkConvex& a, const GjkConvex& b, PxI32& numVerts, const FloatVArg lowerBound, const FloatVArg upperBound); - bool expandTriangle(PxI32& numVerts, const FloatVArg lowerBound, const FloatVArg upperBound); + bool expandPoint(const GjkConvex& a, const GjkConvex& b, PxI32& numVerts, const FloatVArg upperBound); + bool expandSegment(const GjkConvex& a, const GjkConvex& b, PxI32& numVerts, const FloatVArg upperBound); + bool expandTriangle(PxI32& numVerts, const FloatVArg upperBound); - Facet* addFacet(const PxU32 i0, const PxU32 i1, const PxU32 i2, const Ps::aos::FloatVArg lower2, const Ps::aos::FloatVArg upper2); + Facet* addFacet(const PxU32 i0, const PxU32 i1, const PxU32 i2, const Ps::aos::FloatVArg upper); bool originInTetrahedron(const Ps::aos::Vec3VArg p1, const Ps::aos::Vec3VArg p2, const Ps::aos::Vec3VArg p3, const Ps::aos::Vec3VArg p4); @@ -104,32 +103,6 @@ namespace Gu support = V3Sub(tSupportA, tSupportB); } - static FloatV calculatePlaneDist(const PxU32 i0, const PxU32 i1, const PxU32 i2, const Ps::aos::Vec3V* PX_RESTRICT aBuf, const Ps::aos::Vec3V* PX_RESTRICT bBuf) - { - const Vec3V pa0(aBuf[i0]); - const Vec3V pa1(aBuf[i1]); - const Vec3V pa2(aBuf[i2]); - - const Vec3V pb0(bBuf[i0]); - const Vec3V pb1(bBuf[i1]); - const Vec3V pb2(bBuf[i2]); - - const Vec3V p0 = V3Sub(pa0, pb0); - const Vec3V p1 = V3Sub(pa1, pb1); - const Vec3V p2 = V3Sub(pa2, pb2); - const Vec3V v1 = V3Sub(p1, p0); - const Vec3V v2 = V3Sub(p2, p0); - const Vec3V v3 = V3Sub(p2, p1); - - const FloatV v1v1 = V3Dot(v1, v1); - const FloatV v2v2 = V3Dot(v2, v2); - const Vec3V v = V3Sel(FIsGrtr(v1v1, v2v2), v2, v1); - - const Vec3V planeNormal = V3Normalize(V3Cross(v, v3)); - return V3Dot(planeNormal, p0); - } - - GjkStatus epaPenetration(const GjkConvex& a, const GjkConvex& b, PxU8* PX_RESTRICT aInd, PxU8* PX_RESTRICT bInd, PxU8 _size, Ps::aos::Vec3V& contactA, Ps::aos::Vec3V& contactB, Ps::aos::Vec3V& normal, Ps::aos::FloatV& penetrationDepth, const bool takeCoreShape) { using namespace Ps::aos; @@ -224,9 +197,9 @@ namespace Gu //ML: This function: // (1)calculates the distance from orign((0, 0, 0)) to a triangle plane // (2) rejects triangle if the triangle is degenerate (two points are identical) - // (3) rejects triangle to be added into the heap if the plane distance is outside of the boundary[lower, upper] - PX_EPA_FORCE_INLINE Ps::aos::BoolV Facet::isValid2(const PxU32 i0, const PxU32 i1, const PxU32 i2, const Ps::aos::Vec3V* PX_RESTRICT aBuf, const Ps::aos::Vec3V* PX_RESTRICT bBuf, - const Ps::aos::FloatVArg lower, const Ps::aos::FloatVArg upper) + // (3) rejects triangle to be added into the heap if the plane distance is large than upper + Ps::aos::BoolV Facet::isValid2(const PxU32 i0, const PxU32 i1, const PxU32 i2, const Ps::aos::Vec3V* PX_RESTRICT aBuf, const Ps::aos::Vec3V* PX_RESTRICT bBuf, + const Ps::aos::FloatVArg upper) { using namespace Ps::aos; const FloatV eps = FEps(); @@ -245,13 +218,8 @@ namespace Gu const Vec3V v1 = V3Sub(p1, p0); const Vec3V v2 = V3Sub(p2, p0); - const Vec3V v3 = V3Sub(p2, p1); - const FloatV v1v1 = V3Dot(v1, v1); - const FloatV v2v2 = V3Dot(v2, v2); - - const Vec3V v = V3Sel(FIsGrtr(v1v1, v2v2), v2, v1); - const Vec3V denormalizedNormal = V3Cross(v, v3); + const Vec3V denormalizedNormal = V3Cross(v1, v2); FloatV norValue = V3Dot(denormalizedNormal, denormalizedNormal); //if norValue < eps, this triangle is degenerate const BoolV con = FIsGrtr(norValue, eps); @@ -263,12 +231,13 @@ namespace Gu FStore(planeDist, &m_planeDist); - return BAnd(con, BAnd(FIsGrtrOrEq(planeDist, lower), FIsGrtrOrEq(upper, planeDist))); + return BAnd(con, FIsGrtrOrEq(upper, planeDist)); + //return BAnd(con, BAnd(FIsGrtrOrEq(planeDist, lower), FIsGrtrOrEq(upper, planeDist))); } //ML: if the triangle is valid(not degenerate and within lower and upper bound), we need to add it into the heap. Otherwise, we just return //the triangle so that the facet can be linked to other facets in the expanded polytope. - Facet* EPA::addFacet(const PxU32 i0, const PxU32 i1, const PxU32 i2, const Ps::aos::FloatVArg lower2, const Ps::aos::FloatVArg upper2) + Facet* EPA::addFacet(const PxU32 i0, const PxU32 i1, const PxU32 i2, const Ps::aos::FloatVArg upper) { using namespace Ps::aos; PX_ASSERT(i0 != i1 && i0 != i2 && i1 != i2); @@ -281,7 +250,7 @@ namespace Gu Facet * facet = PX_PLACEMENT_NEW(&facetBuf[facetId],Facet(i0, i1, i2)); facet->m_FacetId = PxU8(facetId); - const BoolV validTriangle = facet->isValid2(i0, i1, i2, aBuf, bBuf, lower2, upper2); + const BoolV validTriangle = facet->isValid2(i0, i1, i2, aBuf, bBuf, upper); if(BAllEqTTTT(validTriangle)) { @@ -351,7 +320,7 @@ namespace Gu } } - bool EPA::expandPoint(const GjkConvex& a, const GjkConvex& b, PxI32& numVerts, const FloatVArg lowerBound, const FloatVArg upperBound) + bool EPA::expandPoint(const GjkConvex& a, const GjkConvex& b, PxI32& numVerts, const FloatVArg upperBound) { const Vec3V x = V3UnitX(); Vec3V q0 = V3Sub(aBuf[0], bBuf[0]); @@ -359,11 +328,11 @@ namespace Gu doSupport(a, b, x, aBuf[1], bBuf[1], q1); if (V3AllEq(q0, q1)) return false; - return expandSegment(a, b, numVerts, lowerBound, upperBound); + return expandSegment(a, b, numVerts, upperBound); } //ML: this function use the segement to create a triangle - bool EPA::expandSegment(const GjkConvex& a, const GjkConvex& b, PxI32& numVerts, const FloatVArg lowerBound, const FloatVArg upperBound) + bool EPA::expandSegment(const GjkConvex& a, const GjkConvex& b, PxI32& numVerts, const FloatVArg upperBound) { const Vec3V q0 = V3Sub(aBuf[0], bBuf[0]); const Vec3V q1 = V3Sub(aBuf[1], bBuf[1]); @@ -389,23 +358,23 @@ namespace Gu Vec3V q2; doSupport(a, b, n, aBuf[2], bBuf[2], q2); - return expandTriangle(numVerts, lowerBound, upperBound); + return expandTriangle(numVerts, upperBound); } - bool EPA::expandTriangle(PxI32& numVerts, const FloatVArg lowerBound, const FloatVArg upperBound) + bool EPA::expandTriangle(PxI32& numVerts, const FloatVArg upperBound) { numVerts = 3; - Facet * PX_RESTRICT f0 = addFacet(0, 1, 2, lowerBound, upperBound); - Facet * PX_RESTRICT f1 = addFacet(1, 0, 2, lowerBound, upperBound); + Facet * PX_RESTRICT f0 = addFacet(0, 1, 2, upperBound); + Facet * PX_RESTRICT f1 = addFacet(1, 0, 2, upperBound); - if(f0 == NULL || f1 == NULL || heap.empty()) + if(heap.empty()) return false; f0->link(0, f1, 0); f0->link(1, f1, 2); f0->link(2, f1, 1); - + return true; } @@ -414,21 +383,16 @@ namespace Gu //For example, we treat sphere/capsule as a point/segment in the support function for GJK/EPA, so that the core shape for sphere/capsule is a point/segment. For PCM, we need //to take the point from the core shape because this will allows us recycle the contacts more stably. For SQ sweeps, we need to take the point on the surface of the sphere/capsule //when we calculate MTD because this is what will be reported to the user. Therefore, the takeCoreShape flag will be set to be false in SQ. - static void calculateContactInformation(const Ps::aos::Vec3V* PX_RESTRICT aBuf, const Ps::aos::Vec3V* PX_RESTRICT bBuf, const Facet* facet, const GjkConvex& a, const GjkConvex& b, Vec3V& pa, Vec3V& pb, Vec3V& normal, FloatV& penDepth, const bool takeCoreShape) + static void calculateContactInformation(const Ps::aos::Vec3V* PX_RESTRICT aBuf, const Ps::aos::Vec3V* PX_RESTRICT bBuf, Facet* facet, const GjkConvex& a, const GjkConvex& b, Vec3V& pa, Vec3V& pb, Vec3V& normal, FloatV& penDepth, const bool takeCoreShape) { const FloatV zero = FZero(); Vec3V _pa, _pb; facet->getClosestPoint(aBuf, bBuf, _pa, _pb); //dist > 0 means two shapes are penetrated. If dist < 0(when origin isn't inside the polytope), two shapes status are unknown const FloatV dist = facet->getPlaneDist(); - //planeNormal is pointing from B to A, however, the normal we are expecting is from A to B which match the GJK margin intersect case, therefore, - //we need to flip the normal - Vec3V planeNormal = facet->getPlaneNormal(); - - const Vec3V normalDir = V3Sub(a.getCenter(), b.getCenter()); - if (FAllGrtr(zero, V3Dot(normalDir, planeNormal))) - planeNormal = V3Neg(planeNormal); + const Vec3V planeNormal = V3Neg(facet->getPlaneNormal()); + if(takeCoreShape) { pa = _pa; @@ -496,7 +460,6 @@ namespace Gu const FloatV _max = FMax(); FloatV upper_bound(_max); - FloatV lower_bound(FNeg(_max)); aBuf[0]=A[0]; aBuf[1]=A[1]; aBuf[2]=A[2]; aBuf[3]=A[3]; bBuf[0]=B[0]; bBuf[1]=B[1]; bBuf[2]=B[2]; bBuf[3]=B[3]; @@ -511,7 +474,7 @@ namespace Gu case 1: { // Touching contact. Yes, we have a collision and the penetration will be zero - if(!expandPoint(a, b, numVertsLocal, lower_bound, upper_bound)) + if(!expandPoint(a, b, numVertsLocal, upper_bound)) return EPA_FAIL; break; } @@ -519,7 +482,7 @@ namespace Gu { // We have a line segment inside the Minkowski sum containing the // origin. we need to construct two back to back triangles which link to each other - if(!expandSegment(a, b, numVertsLocal, lower_bound, upper_bound)) + if(!expandSegment(a, b, numVertsLocal, upper_bound)) return EPA_FAIL; break; } @@ -527,7 +490,7 @@ namespace Gu { // We have a triangle inside the Minkowski sum containing // the origin. We need to construct two back to back triangles which link to each other - if(!expandTriangle(numVertsLocal, lower_bound, upper_bound)) + if(!expandTriangle(numVertsLocal, upper_bound)) return EPA_FAIL; break; @@ -537,9 +500,29 @@ namespace Gu { //check for input face normal. All face normals in this tetrahedron should be all pointing either inwards or outwards. If all face normals are pointing outward, we are good to go. Otherwise, we need to //shuffle the input vertexes and make sure all face normals are pointing outward - const FloatV planeDistf0 = calculatePlaneDist(0, 1, 2, aBuf, bBuf); - - if(FAllGrtr(zero, planeDistf0)) + const Vec3V pa0(aBuf[0]); + const Vec3V pa1(aBuf[1]); + const Vec3V pa2(aBuf[2]); + const Vec3V pa3(aBuf[3]); + + const Vec3V pb0(bBuf[0]); + const Vec3V pb1(bBuf[1]); + const Vec3V pb2(bBuf[2]); + const Vec3V pb3(bBuf[3]); + + const Vec3V p0 = V3Sub(pa0, pb0); + const Vec3V p1 = V3Sub(pa1, pb1); + const Vec3V p2 = V3Sub(pa2, pb2); + const Vec3V p3 = V3Sub(pa3, pb3); + + const Vec3V v1 = V3Sub(p1, p0); + const Vec3V v2 = V3Sub(p2, p0); + + const Vec3V planeNormal = V3Normalize(V3Cross(v1, v2)); + + const FloatV signDist = V3Dot(planeNormal, V3Sub(p3, p0)); + + if (FAllGrtr(signDist, zero)) { //shuffle the input vertexes const Vec3V tempA0 = aBuf[2]; @@ -549,19 +532,20 @@ namespace Gu aBuf[1] = tempA0; bBuf[1] = tempB0; } - Facet * PX_RESTRICT f0 = addFacet(0, 1, 2, lower_bound, upper_bound); - Facet * PX_RESTRICT f1 = addFacet(0, 3, 1, lower_bound, upper_bound); - Facet * PX_RESTRICT f2 = addFacet(0, 2, 3, lower_bound, upper_bound); - Facet * PX_RESTRICT f3 = addFacet(1, 3, 2, lower_bound, upper_bound); - PX_ASSERT(f0->m_planeDist >= 0.f); - PX_ASSERT(f1->m_planeDist >= 0.f); - PX_ASSERT(f2->m_planeDist >= 0.f); - PX_ASSERT(f3->m_planeDist >= 0.f); - - if((f0==NULL) || (f1==NULL) || (f2==NULL) || (f3==NULL) || heap.empty()) - return EPA_FAIL; + Facet * PX_RESTRICT f0 = addFacet(0, 1, 2, upper_bound); + Facet * PX_RESTRICT f1 = addFacet(0, 3, 1, upper_bound); + Facet * PX_RESTRICT f2 = addFacet(0, 2, 3, upper_bound); + Facet * PX_RESTRICT f3 = addFacet(1, 3, 2, upper_bound); + if (heap.empty()) + return EPA_FAIL; + + PX_ASSERT(f0->m_planeDist >= -1e-2f); + PX_ASSERT(f1->m_planeDist >= -1e-2f); + PX_ASSERT(f2->m_planeDist >= -1e-2f); + PX_ASSERT(f3->m_planeDist >= -1e-2f); + f0->link(0, f1, 2); f0->link(1, f3, 2); f0->link(2, f2, 0); @@ -580,7 +564,7 @@ namespace Gu const FloatV eps2 = FMul(minMargin, tenPerc); Facet* PX_RESTRICT facet = NULL; - + Vec3V tempa, tempb, q; do @@ -612,29 +596,16 @@ namespace Gu //might be negative const FloatV dist = V3Dot(q, planeNormal); - //update the upper bound to the minimum between exisiting upper bound and the distance if distance is positive - upper_bound = FSel(FIsGrtrOrEq(dist, zero), FMin(upper_bound, dist), upper_bound); - //lower bound is the plane distance, which will be the smallest among all the facets in the prority queue - lower_bound = planeDist; - //if the plane distance and the upper bound is within a small tolerance, which means we found the MTD - const BoolV con0 = FIsGrtrOrEq(eps2, FAbs(FSub(upper_bound, lower_bound))); + const BoolV con0 = FIsGrtrOrEq(eps2, FAbs(FSub(dist, planeDist))); - if(BAllEqTTTT(con0)) + if (BAllEqTTTT(con0)) { calculateContactInformation(aBuf, bBuf, facet, a, b, pa, pb, normal, penDepth, takeCoreShape); return EPA_CONTACT; } - //if planeDist is the same as dist, which means the support point we get out from the Mincowsky sum is one of the point - //in the triangle facet, we should exist because we can't progress further. We can use the fact as contact information - const FloatV dif = FSub(dist, planeDist); - const BoolV degeneratedCondition = FIsGrtr(eps2, dif); - if (BAllEqTTTT(degeneratedCondition)) - { - calculateContactInformation(aBuf, bBuf, facet, a, b, pa, pb, normal, penDepth, takeCoreShape); - - return EPA_CONTACT; - } + //update the upper bound to the minimum between existing upper bound and the distance + upper_bound = FMin(upper_bound, dist); aBuf[numVertsLocal]=tempa; bBuf[numVertsLocal]=tempb; @@ -668,17 +639,18 @@ namespace Gu return EPA_DEGENERATE; } - Facet *firstFacet = addFacet(edge->getTarget(), edge->getSource(),index, lower_bound, upper_bound); + Facet *firstFacet = addFacet(edge->getTarget(), edge->getSource(),index, upper_bound); PX_ASSERT(firstFacet); firstFacet->link(0, edge->getFacet(), edge->getIndex()); Facet * PX_RESTRICT lastFacet = firstFacet; +#if EPA_DEBUG bool degenerate = false; for(PxU32 i=1; (i<bufferSize) && (!degenerate); ++i) { edge=edgeBuffer.Get(i); - Facet* PX_RESTRICT newFacet = addFacet(edge->getTarget(), edge->getSource(),index, lower_bound, upper_bound); + Facet* PX_RESTRICT newFacet = addFacet(edge->getTarget(), edge->getSource(),index, upper_bound); PX_ASSERT(newFacet); const bool b0 = newFacet->link(0, edge->getFacet(), edge->getIndex()); const bool b1 = newFacet->link(2, lastFacet, 1); @@ -686,11 +658,18 @@ namespace Gu lastFacet = newFacet; } - if(degenerate) + if (degenerate) + Ps::debugBreak(); +#else + for (PxU32 i = 1; i<bufferSize; ++i) { - calculateContactInformation(aBuf, bBuf, facet, a, b, pa, pb, normal, penDepth, takeCoreShape); - return EPA_DEGENERATE; + edge = edgeBuffer.Get(i); + Facet* PX_RESTRICT newFacet = addFacet(edge->getTarget(), edge->getSource(), index, upper_bound); + newFacet->link(0, edge->getFacet(), edge->getIndex()); + newFacet->link(2, lastFacet, 1); + lastFacet = newFacet; } +#endif firstFacet->link(2, lastFacet, 1); } diff --git a/PhysX_3.4/Source/GeomUtils/src/gjk/GuEPAFacet.h b/PhysX_3.4/Source/GeomUtils/src/gjk/GuEPAFacet.h index dee06e90..690a0b81 100644 --- a/PhysX_3.4/Source/GeomUtils/src/gjk/GuEPAFacet.h +++ b/PhysX_3.4/Source/GeomUtils/src/gjk/GuEPAFacet.h @@ -42,6 +42,8 @@ #define PX_EPA_FORCE_INLINE PX_FORCE_INLINE #endif +#define EPA_DEBUG 0 + namespace physx { #define MaxEdges 32 @@ -103,8 +105,8 @@ namespace Gu PX_FORCE_INLINE Ps::aos::FloatV getPlaneDist(const Ps::aos::Vec3VArg p, const Ps::aos::Vec3V* PX_RESTRICT aBuf, const Ps::aos::Vec3V* PX_RESTRICT bBuf) const; //check to see whether the triangle is a valid triangle, calculate plane normal and plane distance at the same time - PX_EPA_FORCE_INLINE Ps::aos::BoolV isValid2(const PxU32 i0, const PxU32 i1, const PxU32 i2, const Ps::aos::Vec3V* PX_RESTRICT aBuf, const Ps::aos::Vec3V* PX_RESTRICT bBuf, - const Ps::aos::FloatVArg lower, const Ps::aos::FloatVArg upper); + Ps::aos::BoolV isValid2(const PxU32 i0, const PxU32 i1, const PxU32 i2, const Ps::aos::Vec3V* PX_RESTRICT aBuf, const Ps::aos::Vec3V* PX_RESTRICT bBuf, + const Ps::aos::FloatVArg upper); //return the absolute value for the plane distance from origin PX_FORCE_INLINE Ps::aos::FloatV getPlaneDist() const @@ -119,7 +121,7 @@ namespace Gu } //calculate the closest points for a shape pair - void getClosestPoint(const Ps::aos::Vec3V* PX_RESTRICT aBuf, const Ps::aos::Vec3V* PX_RESTRICT bBuf, Ps::aos::Vec3V& closestA, Ps::aos::Vec3V& closestB) const; + void getClosestPoint(const Ps::aos::Vec3V* PX_RESTRICT aBuf, const Ps::aos::Vec3V* PX_RESTRICT bBuf, Ps::aos::Vec3V& closestA, Ps::aos::Vec3V& closestB); //performs a flood fill over the boundary of the current polytope. void silhouette(const Ps::aos::Vec3VArg w, const Ps::aos::Vec3V* PX_RESTRICT aBuf, const Ps::aos::Vec3V* PX_RESTRICT bBuf, EdgeBuffer& edgeBuffer, EPAFacetManager& manager); @@ -136,7 +138,11 @@ namespace Gu EPAFacetManager& manager); Ps::aos::Vec3V m_planeNormal; //16 - PxF32 m_planeDist; //20 + PxF32 m_planeDist; +#if EPA_DEBUG + PxF32 m_lambda1; + PxF32 m_lambda2; +#endif Facet* PX_RESTRICT m_adjFacets[3]; //the triangle adjacent to edge i in this triangle //32 PxI8 m_adjEdges[3]; //the edge connected with the corresponding triangle //35 @@ -234,10 +240,10 @@ namespace Gu }; //ML: calculate MTD points for a shape pair - PX_FORCE_INLINE void Facet::getClosestPoint(const Ps::aos::Vec3V* PX_RESTRICT aBuf, const Ps::aos::Vec3V* PX_RESTRICT bBuf, Ps::aos::Vec3V& closestA, Ps::aos::Vec3V& closestB) const + PX_FORCE_INLINE void Facet::getClosestPoint(const Ps::aos::Vec3V* PX_RESTRICT aBuf, const Ps::aos::Vec3V* PX_RESTRICT bBuf, Ps::aos::Vec3V& closestA, Ps::aos::Vec3V& closestB) { using namespace Ps::aos; - + const Vec3V pa0(aBuf[m_indices[0]]); const Vec3V pa1(aBuf[m_indices[1]]); const Vec3V pa2(aBuf[m_indices[2]]); @@ -250,28 +256,34 @@ namespace Gu const Vec3V p1 = V3Sub(pa1, pb1); const Vec3V p2 = V3Sub(pa2, pb2); - const Vec3V v1 = V3Sub(p1, p0); - const Vec3V v2 = V3Sub(p2, p0); + const Vec3V v0 = V3Sub(p1, p0); + const Vec3V v1 = V3Sub(p2, p0); - const FloatV v1dv1 = V3Dot(v1, v1); - const FloatV v1dv2 = V3Dot(v1, v2); - const FloatV v2dv2 = V3Dot(v2, v2); + const Vec3V closestP = V3Scale(m_planeNormal, FLoad(m_planeDist)); + const Vec3V v2 = V3Sub(closestP, p0); + //calculate barycentric coordinates + const FloatV d00 = V3Dot(v0, v0); + const FloatV d01 = V3Dot(v0, v1); + const FloatV d11 = V3Dot(v1, v1); - const FloatV p0dv1 = V3Dot(p0, v1); //V3Dot(V3Sub(p0, origin), v1); - const FloatV p0dv2 = V3Dot(p0, v2); + const FloatV d20 = V3Dot(v2, v0); + const FloatV d21 = V3Dot(v2, v1); - const FloatV det = FNegScaleSub(v1dv2, v1dv2, FMul(v1dv1, v2dv2));//FSub( FMul(v1dv1, v2dv2), FMul(v1dv2, v1dv2) ); // non-negative + const FloatV det = FNegScaleSub(d01, d01, FMul(d00, d11));//FSub( FMul(v1dv1, v2dv2), FMul(v1dv2, v1dv2) ); // non-negative const FloatV recip = FSel(FIsGrtr(det, FEps()), FRecip(det), FZero()); - const FloatV lambda1 = FMul(FNegScaleSub(p0dv1, v2dv2, FMul(p0dv2, v1dv2)), recip); - const FloatV lambda2 = FMul(FNegScaleSub(p0dv2, v1dv1, FMul(p0dv1, v1dv2)), recip); + const FloatV lambda1 = FMul(FNegScaleSub(d01, d21, FMul(d11, d20)), recip); + const FloatV lambda2 = FMul(FNegScaleSub(d01, d20, FMul(d00, d21)), recip); + +#if EPA_DEBUG + FStore(lambda1, &m_lambda1); + FStore(lambda2, &m_lambda2); +#endif const Vec3V a0 = V3Scale(V3Sub(pa1, pa0), lambda1); const Vec3V a1 = V3Scale(V3Sub(pa2, pa0), lambda2); - const Vec3V b0 = V3Scale(V3Sub(pb1, pb0), lambda1); - const Vec3V b1 = V3Scale(V3Sub(pb2, pb0), lambda2); closestA = V3Add(V3Add(a0, a1), pa0); - closestB = V3Add(V3Add(b0, b1), pb0); + closestB = V3Sub(closestA, closestP); } //ML: create adjacency informations for both facets diff --git a/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecBox.h b/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecBox.h index 6787a93e..fb90d9c6 100644 --- a/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecBox.h +++ b/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecBox.h @@ -59,7 +59,7 @@ namespace Gu PX_FORCE_INLINE void CalculateBoxMargin(const Ps::aos::Vec3VArg extent, PxReal& minExtent, PxReal& minMargin, PxReal& sweepMargin, - const PxReal minMarginR = BOX_MIN_MARGIN_RATIO) + const PxReal minMarginR = BOX_MIN_MARGIN_RATIO) { using namespace Ps::aos; @@ -80,11 +80,20 @@ namespace Gu } //This method is called in the PCM contact gen for the refreshing contacts - PX_FORCE_INLINE Ps::aos::FloatV CalculatePCMBoxMargin(const Ps::aos::Vec3VArg extent) + PX_FORCE_INLINE Ps::aos::FloatV CalculatePCMBoxMargin(const Ps::aos::Vec3VArg extent, const PxReal toleranceLength, const PxReal toleranceMarginRatio = BOX_MARGIN_RATIO) { using namespace Ps::aos; const FloatV min = V3ExtractMin(extent);//FMin(V3GetX(extent), FMin(V3GetY(extent), V3GetZ(extent))); + const FloatV toleranceMargin = FLoad(toleranceLength * toleranceMarginRatio); + return FMin(FMul(min, FLoad(BOX_MARGIN_RATIO)), toleranceMargin); + } + + PX_FORCE_INLINE Ps::aos::FloatV CalculateMTDBoxMargin(const Ps::aos::Vec3VArg extent) + { + using namespace Ps::aos; + + const FloatV min = V3ExtractMin(extent);//FMin(V3GetX(extent), FMin(V3GetY(extent), V3GetZ(extent))); return FMul(min, FLoad(BOX_MARGIN_RATIO)); } @@ -106,7 +115,8 @@ namespace Gu marginDif = Ps::aos::FZero(); margin = 0.f; } - + + //this constructor is used by the CCD system PX_FORCE_INLINE BoxV(const PxGeometry& geom) : ConvexV(ConvexType::eBOX, Ps::aos::V3Zero()) { using namespace Ps::aos; @@ -125,8 +135,13 @@ namespace Gu { } + PX_FORCE_INLINE void resetMargin(const PxReal toleranceLength) + { + minMargin = PxMin(toleranceLength * BOX_MIN_MARGIN_RATIO, minMargin); + } + //! Assignment operator - PX_INLINE const BoxV& operator=(const BoxV& other) + PX_FORCE_INLINE const BoxV& operator=(const BoxV& other) { center = other.center; extents = other.extents; diff --git a/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecConvexHull.h b/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecConvexHull.h index 1c2fd660..527d1fbd 100644 --- a/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecConvexHull.h +++ b/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecConvexHull.h @@ -48,35 +48,51 @@ namespace Gu #define CONVEX_MARGIN_RATIO 0.1f #define CONVEX_MIN_MARGIN_RATIO 0.05f #define CONVEX_SWEEP_MARGIN_RATIO 0.025f +#define TOLERANCE_MARGIN_RATIO 0.08f +#define TOLERANCE_MIN_MARGIN_RATIO 0.05f + //This margin is used in Persistent contact manifold - PX_SUPPORT_FORCE_INLINE Ps::aos::FloatV CalculatePCMConvexMargin(const Gu::ConvexHullData* hullData, const Ps::aos::Vec3VArg scale) + PX_SUPPORT_FORCE_INLINE Ps::aos::FloatV CalculatePCMConvexMargin(const Gu::ConvexHullData* hullData, const Ps::aos::Vec3VArg scale, + const PxReal toleranceLength, const PxReal toleranceRatio = TOLERANCE_MIN_MARGIN_RATIO) { + using namespace Ps::aos; const Vec3V extents= V3Mul(V3LoadU(hullData->mInternal.mExtents), scale); const FloatV min = V3ExtractMin(extents); + const FloatV toleranceMargin = FLoad(toleranceLength * toleranceRatio); //ML: 25% of the minimum extents of the internal AABB as this convex hull's margin - return FMul(min, FLoad(0.25f)); + return FMin(FMul(min, FLoad(0.25f)), toleranceMargin); } - - //This margin is used in PCM contact gen - PX_SUPPORT_FORCE_INLINE void CalculateConvexMargin(const Gu::ConvexHullData* hullData, PxReal& margin, PxReal& minMargin, PxReal& sweepMargin, const Ps::aos::Vec3VArg scale) + PX_SUPPORT_FORCE_INLINE Ps::aos::FloatV CalculateMTDConvexMargin(const Gu::ConvexHullData* hullData, const Ps::aos::Vec3VArg scale) { using namespace Ps::aos; const Vec3V extents = V3Mul(V3LoadU(hullData->mInternal.mExtents), scale); const FloatV min = V3ExtractMin(extents); + //ML: 25% of the minimum extents of the internal AABB as this convex hull's margin + return FMul(min, FLoad(0.25f)); + } - //Margin is used in the plane shifting for the shrunk convex hull - const FloatV margin_ = FMul(min, FLoad(CONVEX_MARGIN_RATIO)); - //minMargin is used in the GJK termination condition - const FloatV minMargin_ = FMul(min, FLoad(CONVEX_MIN_MARGIN_RATIO)); - const FloatV sweepMargin_ = FMul(min, FLoad(CONVEX_SWEEP_MARGIN_RATIO)); + //This minMargin is used in PCM contact gen + PX_SUPPORT_FORCE_INLINE void CalculateConvexMargin(const Gu::ConvexHullData* hullData, PxReal& margin, PxReal& minMargin, PxReal& sweepMargin, + const Ps::aos::Vec3VArg scale) + { + using namespace Ps::aos; + + const Vec3V extents = V3Mul(V3LoadU(hullData->mInternal.mExtents), scale); + const FloatV min_ = V3ExtractMin(extents); - FStore(margin_, &margin); - FStore(minMargin_, &minMargin); - FStore(sweepMargin_, &sweepMargin); + PxReal minExtent; + FStore(min_, &minExtent); + + //Margin is used in the plane shifting for the shrunk convex hull and acceptanceTolerance for overlap + margin = minExtent * CONVEX_MARGIN_RATIO; + //minMargin is used in the GJK termination condition + minMargin = minExtent * CONVEX_MIN_MARGIN_RATIO; + //this is used for GJKRaycast + sweepMargin = minExtent * CONVEX_SWEEP_MARGIN_RATIO; } PX_SUPPORT_FORCE_INLINE Ps::aos::Mat33V ConstructSkewMatrix(const Ps::aos::Vec3VArg scale, const Ps::aos::QuatVArg rotation) @@ -170,39 +186,36 @@ namespace Gu { public: PxU32 m[8]; - PX_FORCE_INLINE TinyBitMap() { m[0] = m[1] = m[2] = m[3] = m[4] = m[5] = m[6] = m[7] = 0; } - PX_FORCE_INLINE void set(PxU8 v) { m[v>>5] |= 1<<(v&31); } - PX_FORCE_INLINE bool get(PxU8 v) const { return (m[v>>5] & 1<<(v&31)) != 0; } + PX_FORCE_INLINE TinyBitMap() { m[0] = m[1] = m[2] = m[3] = m[4] = m[5] = m[6] = m[7] = 0; } + PX_FORCE_INLINE void set(PxU8 v) { m[v >> 5] |= 1 << (v & 31); } + PX_FORCE_INLINE bool get(PxU8 v) const { return (m[v >> 5] & 1 << (v & 31)) != 0; } }; - public: + public: /** \brief Constructor */ - PX_SUPPORT_INLINE ConvexHullV(): ConvexV(ConvexType::eCONVEXHULL) + PX_SUPPORT_INLINE ConvexHullV() : ConvexV(ConvexType::eCONVEXHULL) { } - PX_SUPPORT_INLINE ConvexHullV(const Gu::ConvexHullData* _hullData, const Ps::aos::Vec3VArg _center, const Ps::aos::Vec3VArg scale, const Ps::aos::QuatVArg scaleRot, const bool idtScale): + PX_SUPPORT_INLINE ConvexHullV(const Gu::ConvexHullData* _hullData, const Ps::aos::Vec3VArg _center, const Ps::aos::Vec3VArg scale, const Ps::aos::QuatVArg scaleRot, + const bool idtScale) : ConvexV(ConvexType::eCONVEXHULL, _center) { using namespace Ps::aos; - hullData = _hullData; + hullData = _hullData; const PxVec3* PX_RESTRICT tempVerts = _hullData->getHullVertices(); - //const PxU8* PX_RESTRICT polyInds = _hullData->getFacesByVertices8(); - //const HullPolygonData* PX_RESTRICT polygons = _hullData->mPolygons; verts = tempVerts; numVerts = _hullData->mNbHullVertices; - CalculateConvexMargin( _hullData, margin, minMargin, sweepMargin, scale); + CalculateConvexMargin(_hullData, margin, minMargin, sweepMargin, scale); ConstructSkewMatrix(scale, scaleRot, vertex2Shape, shape2Vertex, center, idtScale); - /*skewScale = Mat33V temp(V3Scale(trans.col0, V3GetX(scale)), V3Scale(trans.col1, V3GetY(scale)), V3Scale(trans.col2, V3GetZ(scale))); - skewRot = QuatGetMat33V(scaleRot);*/ - data = _hullData->mBigConvexRawData; } + //this is used by CCD system PX_SUPPORT_INLINE ConvexHullV(const PxGeometry& geom) : ConvexV(ConvexType::eCONVEXHULL, Ps::aos::V3Zero()) { using namespace Ps::aos; @@ -213,20 +226,22 @@ namespace Gu const QuatV vRot = QuatVLoadU(&convexGeom.scale.rotation.x); const bool idtScale = convexGeom.scale.isIdentity(); - hullData = hData; + hullData = hData; const PxVec3* PX_RESTRICT tempVerts = hData->getHullVertices(); verts = tempVerts; numVerts = hData->mNbHullVertices; - CalculateConvexMargin( hData, margin, minMargin, sweepMargin, vScale); + CalculateConvexMargin(hData, margin, minMargin, sweepMargin, vScale); ConstructSkewMatrix(vScale, vRot, vertex2Shape, shape2Vertex, center, idtScale); data = hData->mBigConvexRawData; + } - PX_SUPPORT_INLINE void initialize(const Gu::ConvexHullData* _hullData, const Ps::aos::Vec3VArg _center, const Ps::aos::Vec3VArg scale, const Ps::aos::QuatVArg scaleRot, const bool idtScale) - { + PX_SUPPORT_INLINE void initialize(const Gu::ConvexHullData* _hullData, const Ps::aos::Vec3VArg _center, const Ps::aos::Vec3VArg scale, + const Ps::aos::QuatVArg scaleRot, const bool idtScale) + { using namespace Ps::aos; - + const PxVec3* tempVerts = _hullData->getHullVertices(); CalculateConvexMargin(_hullData, margin, minMargin, sweepMargin, scale); ConstructSkewMatrix(scale, scaleRot, vertex2Shape, shape2Vertex, center, idtScale); @@ -234,21 +249,31 @@ namespace Gu verts = tempVerts; numVerts = _hullData->mNbHullVertices; //rot = _rot; - + center = _center; - // searchIndex = 0; + // searchIndex = 0; data = _hullData->mBigConvexRawData; hullData = _hullData; - if(_hullData->mBigConvexRawData) + if (_hullData->mBigConvexRawData) { Ps::prefetchLine(hullData->mBigConvexRawData->mValencies); - Ps::prefetchLine(hullData->mBigConvexRawData->mValencies,128); + Ps::prefetchLine(hullData->mBigConvexRawData->mValencies, 128); Ps::prefetchLine(hullData->mBigConvexRawData->mAdjacentVerts); } } + + PX_FORCE_INLINE void resetMargin(const PxReal toleranceLength) + { + const PxReal toleranceMinMargin = toleranceLength * TOLERANCE_MIN_MARGIN_RATIO; + const PxReal toleranceMargin = toleranceLength * TOLERANCE_MARGIN_RATIO; + + margin = PxMin(margin, toleranceMargin); + minMargin = PxMin(minMargin, toleranceMinMargin); + } + PX_FORCE_INLINE Ps::aos::Vec3V supportPoint(const PxI32 index, Ps::aos::FloatV* /*marginDif*/)const { using namespace Ps::aos; diff --git a/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecConvexHullNoScale.h b/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecConvexHullNoScale.h index d4a09435..dd910b66 100644 --- a/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecConvexHullNoScale.h +++ b/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecConvexHullNoScale.h @@ -52,78 +52,6 @@ namespace Gu { } - PX_SUPPORT_INLINE ConvexHullNoScaleV(const Gu::ConvexHullData* _hullData, const Ps::aos::Vec3VArg _center, const Ps::aos::Vec3VArg scale, const Ps::aos::QuatVArg scaleRot) - { - PX_UNUSED(scaleRot); - PX_UNUSED(_center); - - using namespace Ps::aos; - - hullData = _hullData; - const PxVec3* PX_RESTRICT tempVerts = _hullData->getHullVertices(); - verts = tempVerts; - numVerts = _hullData->mNbHullVertices; - CalculateConvexMargin( _hullData, margin, minMargin, sweepMargin, scale); - data = _hullData->mBigConvexRawData; - - PxU8* startAddress = reinterpret_cast<PxU8*>(_hullData->mPolygons); - PxI32 totalPrefetchBytes = PxI32((_hullData->getFacesByVertices8() + _hullData->mNbHullVertices * 3) - startAddress); - - //Prefetch core data - - while(totalPrefetchBytes > 0) - { - totalPrefetchBytes -= 128; - Ps::prefetchLine(startAddress); - startAddress += 128; - } - - if(data) - { - PxI32 totalSize = PxI32(data->mNbSamples + data->mNbVerts * sizeof(Gu::Valency) + data->mNbAdjVerts); - startAddress = data->mSamples; - while(totalSize > 0) - { - totalSize -= 128; - Ps::prefetchLine(startAddress); - startAddress += 128; - } - } - - } - - - /* PX_SUPPORT_INLINE ConvexHullNoScaleV(const Gu::ConvexHullData* _hullData, const Ps::aos::Vec3VArg _center, const Ps::aos::FloatVArg _margin, const Ps::aos::FloatVArg _minMargin, const Ps::aos::Vec3VArg scale, const Ps::aos::QuatVArg scaleRot) - { - PX_UNUSED(scaleRot); - PX_UNUSED(_center); - PX_UNUSED(scale); - - using namespace Ps::aos; - - hullData = _hullData; - margin = _margin; - minMargin = _minMargin; - - const PxVec3* tempVerts = _hullData->getHullVertices(); - const PxU8* PX_RESTRICT polyInds = _hullData->getFacesByVertices8(); - const HullPolygonData* PX_RESTRICT polygons = _hullData->mPolygons; - verts = tempVerts; - numVerts = _hullData->mNbHullVertices; - - Ps::prefetchLine(tempVerts); - Ps::prefetchLine(tempVerts,128); - Ps::prefetchLine(tempVerts,256); - - Ps::prefetchLine(polyInds); - Ps::prefetchLine(polyInds,128); - - Ps::prefetchLine(polygons); - Ps::prefetchLine(polygons, 128); - Ps::prefetchLine(polygons, 256); - }*/ - - PX_FORCE_INLINE Ps::aos::Vec3V supportPoint(const PxI32 index, Ps::aos::FloatV* /*marginDif*/)const { using namespace Ps::aos; diff --git a/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecShrunkBox.h b/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecShrunkBox.h index f8a214d1..4f3686a6 100644 --- a/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecShrunkBox.h +++ b/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecShrunkBox.h @@ -77,6 +77,7 @@ namespace Gu { } + //this is used by CCD system PX_INLINE ShrunkBoxV(const PxGeometry& geom) : BoxV(geom) { margin = minExtent * BOX_MARGIN_CCD_RATIO; @@ -90,8 +91,8 @@ namespace Gu \param extent Extents/radii of the obb. */ - PX_FORCE_INLINE ShrunkBoxV(const Ps::aos::Vec3VArg origin, const Ps::aos::Vec3VArg extent) : - BoxV(origin, extent) + PX_FORCE_INLINE ShrunkBoxV(const Ps::aos::Vec3VArg origin, const Ps::aos::Vec3VArg extent) + : BoxV(origin, extent) { //calculate margin margin = minExtent * BOX_MARGIN_RATIO; @@ -105,6 +106,13 @@ namespace Gu { } + PX_FORCE_INLINE void resetMargin(const PxReal toleranceLength) + { + BoxV::resetMargin(toleranceLength); + margin = PxMin(toleranceLength * BOX_MARGIN_RATIO, margin); + initialiseMarginDif(); + } + //! Assignment operator PX_INLINE const ShrunkBoxV& operator=(const ShrunkBoxV& other) { diff --git a/PhysX_3.4/Source/GeomUtils/src/mesh/GuMeshData.h b/PhysX_3.4/Source/GeomUtils/src/mesh/GuMeshData.h index c4d7bb9d..e7af8aee 100644 --- a/PhysX_3.4/Source/GeomUtils/src/mesh/GuMeshData.h +++ b/PhysX_3.4/Source/GeomUtils/src/mesh/GuMeshData.h @@ -62,8 +62,9 @@ namespace Gu { // 12: isLeaf is now the lowest bit in ptrs // 13: TA30159 removed deprecated convexEdgeThreshold and bumped version // 14: added midphase ID +// 15: GPU data simplification -#define PX_MESH_VERSION 14 +#define PX_MESH_VERSION 15 // these flags are used to indicate/validate the contents of a cooked mesh file enum InternalMeshSerialFlag @@ -107,11 +108,6 @@ enum InternalMeshSerialFlag // TODO avoroshilov: adjacency info - duplicated, remove it and use 'mAdjacencies' and 'mExtraTrigData' see GuTriangleMesh.cpp:325 void * mGRB_triAdjacencies; //!< GRB: adjacency data, with BOUNDARY and NONCONVEX flags (flags replace adj indices where applicable) [uin4] - PxU32 * mGRB_vertValency; //!< GRB: number of adjacent vertices to a vertex - PxU32 * mGRB_adjVertStart; //!< GRB: offset for each vertex in the adjacency list - PxU32 * mGRB_adjVertices; //!< GRB: list of adjacent vertices - - PxU32 mGRB_meshAdjVerticiesTotal; //!< GRB: total number of indices in the 'mGRB_adjVertices' PxU32* mGRB_faceRemap; //!< GRB: this remap the GPU triangle indices to CPU triangle indices void* mGRB_BV32Tree; @@ -132,11 +128,6 @@ enum InternalMeshSerialFlag mGRB_triIndices (NULL), mGRB_triAdjacencies (NULL), - mGRB_vertValency (NULL), - mGRB_adjVertStart (NULL), - mGRB_adjVertices (NULL), - - mGRB_meshAdjVerticiesTotal (0), mGRB_faceRemap (NULL), mGRB_BV32Tree (NULL) @@ -163,12 +154,6 @@ enum InternalMeshSerialFlag PX_FREE(mGRB_triIndices); if (mGRB_triAdjacencies) PX_DELETE_POD(mGRB_triAdjacencies); - if (mGRB_vertValency) - PX_DELETE_POD(mGRB_vertValency); - if (mGRB_adjVertStart) - PX_DELETE_POD(mGRB_adjVertStart); - if (mGRB_adjVertices) - PX_DELETE_POD(mGRB_adjVertices); if (mGRB_faceRemap) PX_DELETE_POD(mGRB_faceRemap); diff --git a/PhysX_3.4/Source/GeomUtils/src/mesh/GuTriangleMesh.cpp b/PhysX_3.4/Source/GeomUtils/src/mesh/GuTriangleMesh.cpp index f0cb286e..59775508 100644 --- a/PhysX_3.4/Source/GeomUtils/src/mesh/GuTriangleMesh.cpp +++ b/PhysX_3.4/Source/GeomUtils/src/mesh/GuTriangleMesh.cpp @@ -67,11 +67,7 @@ Gu::TriangleMesh::TriangleMesh(GuMeshFactory& factory, TriangleMeshData& d) , mGRB_triIndices (d.mGRB_triIndices) , mGRB_triAdjacencies (d.mGRB_triAdjacencies) -, mGRB_vertValency (d.mGRB_vertValency) -, mGRB_adjVertStart (d.mGRB_adjVertStart) -, mGRB_adjVertices (d.mGRB_adjVertices) -, mGRB_meshAdjVerticiesTotal (d.mGRB_meshAdjVerticiesTotal) , mGRB_faceRemap (d.mGRB_faceRemap) , mGRB_BV32Tree (d.mGRB_BV32Tree) { @@ -86,9 +82,6 @@ Gu::TriangleMesh::TriangleMesh(GuMeshFactory& factory, TriangleMeshData& d) d.mGRB_triIndices = 0; d.mGRB_triAdjacencies = 0; - d.mGRB_vertValency = 0; - d.mGRB_adjVertStart = 0; - d.mGRB_adjVertices = 0; d.mGRB_faceRemap = 0; d.mGRB_BV32Tree = 0; @@ -111,9 +104,6 @@ Gu::TriangleMesh::~TriangleMesh() PX_FREE_AND_RESET(mGRB_triIndices); PX_FREE_AND_RESET(mGRB_triAdjacencies); - PX_FREE_AND_RESET(mGRB_vertValency); - PX_FREE_AND_RESET(mGRB_adjVertStart); - PX_FREE_AND_RESET(mGRB_adjVertices); PX_FREE_AND_RESET(mGRB_faceRemap); BV32Tree* bv32Tree = reinterpret_cast<BV32Tree*>(mGRB_BV32Tree); @@ -197,10 +187,6 @@ void Gu::TriangleMesh::importExtraData(PxDeserializationContext& context) mGRB_triIndices = NULL; mGRB_triAdjacencies = NULL; - mGRB_vertValency = NULL; - mGRB_adjVertStart = NULL; - mGRB_adjVertices = NULL; - mGRB_meshAdjVerticiesTotal = 0; mGRB_faceRemap = NULL; mGRB_BV32Tree = NULL; } diff --git a/PhysX_3.4/Source/GeomUtils/src/mesh/GuTriangleMesh.h b/PhysX_3.4/Source/GeomUtils/src/mesh/GuTriangleMesh.h index 94d4ec54..5ecdc358 100644 --- a/PhysX_3.4/Source/GeomUtils/src/mesh/GuTriangleMesh.h +++ b/PhysX_3.4/Source/GeomUtils/src/mesh/GuTriangleMesh.h @@ -173,11 +173,7 @@ public: // TODO avoroshilov: cooking - adjacency info - duplicated, remove it and use 'mAdjacencies' and 'mExtraTrigData' see GuTriangleMesh.cpp:325 void * mGRB_triAdjacencies; //!< GRB: adjacency data, with BOUNDARY and NONCONVEX flags (flags replace adj indices where applicable) - PxU32 * mGRB_vertValency; //!< GRB: number of adjacent vertices to a vertex - PxU32 * mGRB_adjVertStart; //!< GRB: offset for each vertex in the adjacency list - PxU32 * mGRB_adjVertices; //!< GRB: list of adjacent vertices - PxU32 mGRB_meshAdjVerticiesTotal; //!< GRB: total number of indices in the 'mGRB_adjVertices' PxU32* mGRB_faceRemap; //!< GRB : gpu to cpu triangle indice remap void* mGRB_BV32Tree; //!< GRB: BV32 tree // End of GRB data ------------------ diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactBoxBox.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactBoxBox.cpp index b93df8e3..e66f601e 100644 --- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactBoxBox.cpp +++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactBoxBox.cpp @@ -927,8 +927,9 @@ bool pcmContactBoxBox(GU_CONTACT_METHOD_ARGS) const PsTransformV curRTrans(transf1.transformInv(transf0)); const PsMatTransformV aToB(curRTrans); - const FloatV boxMargin0 = Gu::CalculatePCMBoxMargin(boxExtents0); - const FloatV boxMargin1 = Gu::CalculatePCMBoxMargin(boxExtents1); + const PxReal toleranceLength = params.mToleranceLength; + const FloatV boxMargin0 = Gu::CalculatePCMBoxMargin(boxExtents0, toleranceLength); + const FloatV boxMargin1 = Gu::CalculatePCMBoxMargin(boxExtents1, toleranceLength); const FloatV minMargin = FMin(boxMargin0, boxMargin1); const PxU32 initialContacts = manifold.mNumContacts; @@ -958,7 +959,7 @@ bool pcmContactBoxBox(GU_CONTACT_METHOD_ARGS) if(numContacts > 0) { - manifold.addBatchManifoldContacts(manifoldContacts, numContacts); + manifold.addBatchManifoldContacts(manifoldContacts, numContacts, toleranceLength); const Vec3V worldNormal = transfV1.rotate(Vec3V_From_Vec4V(manifold.mContactPoints[0].mLocalNormalPen)); manifold.addManifoldContactsToContactBuffer(contactBuffer, worldNormal, transfV1); #if PCM_LOW_LEVEL_DEBUG @@ -971,6 +972,7 @@ bool pcmContactBoxBox(GU_CONTACT_METHOD_ARGS) const Vec3V zeroV = V3Zero(); BoxV box0(zeroV, boxExtents0); BoxV box1(zeroV, boxExtents1); + Vec3V closestA(zeroV), closestB(zeroV), normal(zeroV); // these will be in the local space of B FloatV penDep = FZero(); manifold.mNumWarmStartPoints = 0; diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactBoxConvex.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactBoxConvex.cpp index 2f3965eb..03126c61 100644 --- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactBoxConvex.cpp +++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactBoxConvex.cpp @@ -42,6 +42,7 @@ #include "GuPCMShapeConvex.h" #include "GuContactBuffer.h" + namespace physx { @@ -53,7 +54,7 @@ namespace Gu static bool fullContactsGenerationBoxConvex(const PxVec3& halfExtents, const BoxV& box, ConvexHullV& convexHull, const PsTransformV& transf0, const PsTransformV& transf1, PersistentContact* manifoldContacts, ContactBuffer& contactBuffer, Gu::PersistentContactManifold& manifold, Vec3VArg normal, const Vec3VArg closestA, const Vec3VArg closestB, const FloatVArg contactDist, const bool idtScale, const bool doOverlapTest, Cm::RenderOutput* renderOutput, - const FloatVArg toleranceScale) + const PxReal toleranceScale) { Gu::PolygonalData polyData0; PCMPolygonalBox polyBox0(halfExtents); @@ -72,13 +73,13 @@ static bool fullContactsGenerationBoxConvex(const PxVec3& halfExtents, const Box static_cast<SupportLocal*>(PX_PLACEMENT_NEW(buff1, SupportLocalImpl<ConvexHullV>)(convexHull, transf1, convexHull.vertex2Shape, convexHull.shape2Vertex, idtScale))); PxU32 numContacts = 0; - if(generateFullContactManifold(polyData0, polyData1, &map0, map1, manifoldContacts, numContacts, contactDist, normal, closestA, closestB, box.getMargin(), convexHull.getMargin(), + if(generateFullContactManifold(polyData0, polyData1, &map0, map1, manifoldContacts, numContacts, contactDist, normal, closestA, closestB, box.getMarginF(), convexHull.getMarginF(), doOverlapTest, renderOutput, toleranceScale)) { if (numContacts > 0) { //reduce contacts - manifold.addBatchManifoldContacts(manifoldContacts, numContacts); + manifold.addBatchManifoldContacts(manifoldContacts, numContacts, toleranceScale); #if PCM_LOW_LEVEL_DEBUG manifold.drawManifold(*renderOutput, transf0, transf1); @@ -185,9 +186,10 @@ bool pcmContactBoxConvex(GU_CONTACT_METHOD_ARGS) const PsTransformV curRTrans(transf1.transformInv(transf0)); const PsMatTransformV aToB(curRTrans); + const PxReal tolerenceLength = params.mToleranceLength; const Gu::ConvexHullData* hullData = shapeConvex.hullData; - const FloatV convexMargin = Gu::CalculatePCMConvexMargin(hullData, vScale); - const FloatV boxMargin = Gu::CalculatePCMBoxMargin(boxExtents); + const FloatV convexMargin = Gu::CalculatePCMConvexMargin(hullData, vScale, tolerenceLength); + const FloatV boxMargin = Gu::CalculatePCMBoxMargin(boxExtents, tolerenceLength); const FloatV minMargin = FMin(convexMargin, boxMargin);//FMin(boxMargin, convexMargin); const FloatV projectBreakingThreshold = FMul(minMargin, FLoad(0.8f)); @@ -212,6 +214,7 @@ bool pcmContactBoxConvex(GU_CONTACT_METHOD_ARGS) const QuatV vQuat = QuatVLoadU(&shapeConvex.scale.rotation.x); const bool idtScale = shapeConvex.scale.isIdentity(); Gu::ShrunkConvexHullV convexHull(hullData, V3LoadU(hullData->mCenterOfMass), vScale, vQuat, idtScale); + convexHull.setMaxMargin(shapeConvex.maxMargin); Gu::BoxV box(zeroV, boxExtents); const RelativeConvex<BoxV> convexA(box, aToB); @@ -236,7 +239,7 @@ bool pcmContactBoxConvex(GU_CONTACT_METHOD_ARGS) if(status == GJK_DEGENERATE) { return fullContactsGenerationBoxConvex(shapeBox.halfExtents, box, convexHull, transf0, transf1, manifoldContacts, contactBuffer, - manifold, normal, closestA, closestB, contactDist, idtScale, true, renderOutput, FLoad(params.mToleranceLength)); + manifold, normal, closestA, closestB, contactDist, idtScale, true, renderOutput, params.mToleranceLength); } else if(status == GJK_NON_INTERSECT) { @@ -261,7 +264,7 @@ bool pcmContactBoxConvex(GU_CONTACT_METHOD_ARGS) if (fullContactGen || doOverlapTest) { return fullContactsGenerationBoxConvex(shapeBox.halfExtents, box, convexHull, transf0, transf1, manifoldContacts, contactBuffer, - manifold, normal, closestA, closestB, contactDist, idtScale, doOverlapTest, renderOutput, FLoad(params.mToleranceLength)); + manifold, normal, closestA, closestB, contactDist, idtScale, doOverlapTest, renderOutput, params.mToleranceLength); } else { diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactCapsuleBox.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactCapsuleBox.cpp index ecb94a66..9606b959 100644 --- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactCapsuleBox.cpp +++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactCapsuleBox.cpp @@ -48,7 +48,7 @@ namespace Gu static bool fullContactsGenerationCapsuleBox(const CapsuleV& capsule, const BoxV& box, const PxVec3 halfExtents, const PsMatTransformV& aToB, const PsTransformV& transf0, const PsTransformV& transf1, PersistentContact* manifoldContacts, PxU32& numContacts, ContactBuffer& contactBuffer, PersistentContactManifold& manifold, Vec3V& normal, const Vec3VArg closest, - const FloatVArg boxMargin, const FloatVArg contactDist, const bool doOverlapTest, const FloatVArg toleranceScale) + const PxReal boxMargin, const FloatVArg contactDist, const bool doOverlapTest, const PxReal toleranceScale) { PolygonalData polyData; @@ -112,7 +112,8 @@ bool pcmContactCapsuleBox(GU_CONTACT_METHOD_ARGS) const PxU32 initialContacts = manifold.mNumContacts; - const FloatV boxMargin = Gu::CalculatePCMBoxMargin(boxExtents); + const PxReal toleranceLength = params.mToleranceLength; + const FloatV boxMargin = Gu::CalculatePCMBoxMargin(boxExtents, toleranceLength); const FloatV minMargin = FMin(boxMargin, capsuleRadius); @@ -124,7 +125,6 @@ bool pcmContactCapsuleBox(GU_CONTACT_METHOD_ARGS) const bool bLostContacts = (manifold.mNumContacts != initialContacts); - PX_UNUSED(bLostContacts); if(bLostContacts || manifold.invalidate_SphereCapsule(curRTrans, minMargin)) { @@ -139,8 +139,6 @@ bool pcmContactCapsuleBox(GU_CONTACT_METHOD_ARGS) const PsMatTransformV aToB(curRTrans); BoxV box(transf1.p, boxExtents); - box.setMargin(zero); - //transform capsule into the local space of box CapsuleV capsule(aToB.p, aToB.rotate(V3Scale(V3UnitX(), capsuleHalfHeight)), capsuleRadius); LocalConvex<CapsuleV> convexA(capsule); @@ -159,7 +157,7 @@ bool pcmContactCapsuleBox(GU_CONTACT_METHOD_ARGS) else if(status == GJK_DEGENERATE) { return fullContactsGenerationCapsuleBox(capsule, box, shapeBox.halfExtents, aToB, transf0, transf1, manifoldContacts, numContacts, contactBuffer, - manifold, normal, closestB, box.getMargin(), contactDist, true, FLoad(params.mToleranceLength)); + manifold, normal, closestB, box.getMarginF(), contactDist, true, params.mToleranceLength); } else { @@ -199,7 +197,7 @@ bool pcmContactCapsuleBox(GU_CONTACT_METHOD_ARGS) if(initialContacts == 0 || bLostContacts || doOverlapTest) { return fullContactsGenerationCapsuleBox(capsule, box, shapeBox.halfExtents, aToB, transf0, transf1, manifoldContacts, numContacts, contactBuffer, manifold, normal, - closestB, box.getMargin(), contactDist, doOverlapTest, FLoad(params.mToleranceLength)); + closestB, box.getMarginF(), contactDist, doOverlapTest, params.mToleranceLength); } else { diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactCapsuleConvex.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactCapsuleConvex.cpp index 1daf1bd6..5079b32a 100644 --- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactCapsuleConvex.cpp +++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactCapsuleConvex.cpp @@ -50,7 +50,7 @@ namespace Gu static bool fullContactsGenerationCapsuleConvex(const CapsuleV& capsule, const ConvexHullV& convexHull, const PsMatTransformV& aToB, const PsTransformV& transf0,const PsTransformV& transf1, PersistentContact* manifoldContacts, ContactBuffer& contactBuffer, const bool idtScale, PersistentContactManifold& manifold, Vec3VArg normal, - const Vec3VArg closest, const FloatVArg tolerance, const FloatVArg contactDist, const bool doOverlapTest, Cm::RenderOutput* renderOutput, const FloatVArg toleranceScale) + const Vec3VArg closest, const PxReal tolerance, const FloatVArg contactDist, const bool doOverlapTest, Cm::RenderOutput* renderOutput, const PxReal toleranceScale) { PX_UNUSED(renderOutput); @@ -122,8 +122,8 @@ bool pcmContactCapsuleConvex(GU_CONTACT_METHOD_ARGS) const PsTransformV curRTrans(transf1.transformInv(transf0)); const PsMatTransformV aToB(curRTrans); - - const FloatV convexMargin = Gu::CalculatePCMConvexMargin(hullData, vScale); + const PxReal toleranceLength = params.mToleranceLength ; + const FloatV convexMargin = Gu::CalculatePCMConvexMargin(hullData, vScale, toleranceLength); const FloatV capsuleMinMargin = Gu::CalculateCapsuleMinMargin(capsuleRadius); const FloatV minMargin = FMin(convexMargin, capsuleMinMargin); @@ -150,7 +150,7 @@ bool pcmContactCapsuleConvex(GU_CONTACT_METHOD_ARGS) manifold.setRelativeTransform(curRTrans); const QuatV vQuat = QuatVLoadU(&shapeConvex.scale.rotation.x); ConvexHullV convexHull(hullData, V3LoadU(hullData->mCenterOfMass), vScale, vQuat, idtScale); - convexHull.setMargin(zero); + convexHull.setMargin(0.f); //transform capsule(a) into the local space of convexHull(b) CapsuleV capsule(aToB.p, aToB.rotate(V3Scale(V3UnitX(), capsuleHalfHeight)), capsuleRadius); @@ -181,7 +181,7 @@ bool pcmContactCapsuleConvex(GU_CONTACT_METHOD_ARGS) 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)); + closestB, convexHull.getMarginF(), contactDist, true, renderOutput, params.mToleranceLength); } else { @@ -242,7 +242,7 @@ bool pcmContactCapsuleConvex(GU_CONTACT_METHOD_ARGS) 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)); + closestB, convexHull.getMarginF(), contactDist, doOverlapTest, renderOutput, params.mToleranceLength); } else { diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexConvex.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexConvex.cpp index 82636e10..10e014a5 100644 --- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexConvex.cpp +++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexConvex.cpp @@ -40,6 +40,7 @@ #include "GuPCMContactGen.h" #include "GuContactBuffer.h" + namespace physx { @@ -52,7 +53,7 @@ namespace Gu static bool fullContactsGenerationConvexConvex(const ConvexHullV& convexHull0, Gu::ConvexHullV& convexHull1, const PsTransformV& transf0, const PsTransformV& transf1, const bool idtScale0, const bool idtScale1, PersistentContact* manifoldContacts, ContactBuffer& contactBuffer, PersistentContactManifold& manifold, Vec3VArg normal, const Vec3VArg closestA, const Vec3VArg closestB, - const FloatVArg contactDist, const bool doOverlapTest, Cm::RenderOutput* renderOutput, const FloatVArg toleranceScale) + const FloatVArg contactDist, const bool doOverlapTest, Cm::RenderOutput* renderOutput, const PxReal toleranceScale) { Gu::PolygonalData polyData0, polyData1; getPCMConvexData(convexHull0, idtScale0, polyData0); @@ -70,14 +71,14 @@ static bool fullContactsGenerationConvexConvex(const ConvexHullV& convexHull0, G PxU32 numContacts = 0; - if(generateFullContactManifold(polyData0, polyData1, map0, map1, manifoldContacts, numContacts, contactDist, normal, closestA, closestB, convexHull0.getMargin(), - convexHull1.getMargin(), doOverlapTest, renderOutput, toleranceScale)) + if(generateFullContactManifold(polyData0, polyData1, map0, map1, manifoldContacts, numContacts, contactDist, normal, closestA, closestB, convexHull0.getMarginF(), + convexHull1.getMarginF(), doOverlapTest, renderOutput, toleranceScale)) { if (numContacts > 0) { //reduce contacts - manifold.addBatchManifoldContacts(manifoldContacts, numContacts); + manifold.addBatchManifoldContacts(manifoldContacts, numContacts, toleranceScale); const Vec3V worldNormal = manifold.getWorldNormal(transf1); @@ -185,6 +186,7 @@ static bool addGJKEPAContacts(Gu::ShrunkConvexHullV& convexHull0, Gu::ShrunkConv //Add contact to manifold manifold.addManifoldPoint(localPointA, closestB, localNormalPen, replaceBreakingThreshold); + } else { @@ -220,11 +222,12 @@ bool pcmContactConvexConvex(GU_CONTACT_METHOD_ARGS) const Gu::ConvexHullData* hullData0 = shapeConvex0.hullData; const Gu::ConvexHullData* hullData1 = shapeConvex1.hullData; - const FloatV convexMargin0 = Gu::CalculatePCMConvexMargin(hullData0, vScale0); - const FloatV convexMargin1 = Gu::CalculatePCMConvexMargin(hullData1, vScale1); + const PxReal toleranceLength = params.mToleranceLength; + const FloatV convexMargin0 = Gu::CalculatePCMConvexMargin(hullData0, vScale0, toleranceLength); + const FloatV convexMargin1 = Gu::CalculatePCMConvexMargin(hullData1, vScale1, toleranceLength); const PxU32 initialContacts = manifold.mNumContacts; - + const FloatV minMargin = FMin(convexMargin0, convexMargin1); const FloatV projectBreakingThreshold = FMul(minMargin, FLoad(0.8f)); @@ -233,12 +236,15 @@ bool pcmContactConvexConvex(GU_CONTACT_METHOD_ARGS) //ML: after refreshContactPoints, we might lose some contacts const bool bLostContacts = (manifold.mNumContacts != initialContacts); - if(bLostContacts || manifold.invalidate_BoxConvex(curRTrans, minMargin)) + if(bLostContacts || manifold.invalidate_BoxConvex(curRTrans, minMargin) ) { + GjkStatus status = manifold.mNumContacts > 0 ? GJK_UNDEFINED : GJK_NON_INTERSECT; const bool idtScale0 = shapeConvex0.scale.isIdentity(); const bool idtScale1 = shapeConvex1.scale.isIdentity(); + const float maxMargin0 = shapeConvex0.maxMargin; + const float maxMargin1 = shapeConvex1.maxMargin; const QuatV vQuat0 = QuatVLoadU(&shapeConvex0.scale.rotation.x); const QuatV vQuat1 = QuatVLoadU(&shapeConvex1.scale.rotation.x); const Vec3V zeroV = V3Zero(); @@ -246,7 +252,10 @@ bool pcmContactConvexConvex(GU_CONTACT_METHOD_ARGS) Gu::ShrunkConvexHullV convexHull0(hullData0, V3LoadU(hullData0->mCenterOfMass), vScale0, vQuat0, idtScale0); Gu::ShrunkConvexHullV convexHull1(hullData1, V3LoadU(hullData1->mCenterOfMass), vScale1, vQuat1, idtScale1); - + + convexHull0.setMaxMargin(maxMargin0); + convexHull1.setMaxMargin(maxMargin1); + Vec3V closestA(zeroV), closestB(zeroV), normal(zeroV); // from a to b FloatV penDep = FZero(); @@ -266,7 +275,7 @@ bool pcmContactConvexConvex(GU_CONTACT_METHOD_ARGS) if(status == GJK_DEGENERATE) { return fullContactsGenerationConvexConvex(convexHull0, convexHull1, transf0, transf1, idtScale0, idtScale1, manifoldContacts, contactBuffer, - manifold, normal, closestA, closestB, contactDist, true, renderOutput, FLoad(params.mToleranceLength)); + manifold, normal, closestA, closestB, contactDist, true, renderOutput, params.mToleranceLength); } else if(status == GJK_NON_INTERSECT) { @@ -281,6 +290,7 @@ bool pcmContactConvexConvex(GU_CONTACT_METHOD_ARGS) const bool doOverlapTest = addGJKEPAContacts(convexHull0, convexHull1, aToB, status, manifoldContacts, replaceBreakingThreshold, closestA, closestB, normal, penDep, manifold); + //manifold.drawManifold(*renderOutput, transf0, transf1); //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 mang 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, @@ -289,7 +299,7 @@ bool pcmContactConvexConvex(GU_CONTACT_METHOD_ARGS) if (fullContactGen || doOverlapTest) { return fullContactsGenerationConvexConvex(convexHull0, convexHull1, transf0, transf1, idtScale0, idtScale1, manifoldContacts, contactBuffer, - manifold, normal, closestA, closestB, contactDist, doOverlapTest, renderOutput, FLoad(params.mToleranceLength)); + manifold, normal, closestA, closestB, contactDist, doOverlapTest, renderOutput, params.mToleranceLength); } else { @@ -316,7 +326,5 @@ bool pcmContactConvexConvex(GU_CONTACT_METHOD_ARGS) return false; } - - } } diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexHeightField.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexHeightField.cpp index 8ba4e2d7..979d75be 100644 --- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexHeightField.cpp +++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexHeightField.cpp @@ -208,11 +208,9 @@ bool Gu::pcmContactConvexHeightField(GU_CONTACT_METHOD_ARGS) const bool idtScaleConvex = getPCMConvexData(shape0, convexScaling, hullAABB, polyData); const Vec3V vScale = V3LoadU_SafeReadW(shapeConvex.scale.scale); // PT: safe because 'rotation' follows 'scale' in PxMeshScale - const FloatV convexMargin = Gu::CalculatePCMConvexMargin(hullData, vScale); - const FloatV epsilon = FLoad(GU_PCM_MESH_MANIFOLD_EPSILON); - const FloatV toleranceLength = FLoad(params.mToleranceLength); - const FloatV toleranceMargin = FMul(epsilon, toleranceLength); - const FloatV minMargin = FMin(convexMargin, toleranceMargin); + + const PxReal toleranceLength = params.mToleranceLength; + const FloatV minMargin = Gu::CalculatePCMConvexMargin(hullData, vScale, toleranceLength, GU_PCM_MESH_MANIFOLD_EPSILON); const QuatV vQuat = QuatVLoadU(&shapeConvex.scale.rotation.x); Gu::ConvexHullV convexHull(hullData, V3Zero(), vScale, vQuat, shapeConvex.scale.isIdentity()); @@ -249,12 +247,9 @@ bool Gu::pcmContactBoxHeightField(GU_CONTACT_METHOD_ARGS) const Vec3V p0 = V3LoadA(&transform0.p.x); const Vec3V boxExtents = V3LoadU(shapeBox.halfExtents); - const FloatV boxMargin = Gu::CalculatePCMBoxMargin(boxExtents); - const FloatV epsilon = FLoad(GU_PCM_MESH_MANIFOLD_EPSILON); - const FloatV toleranceLength = FLoad(params.mToleranceLength); - const FloatV toleranceMargin = FMul(epsilon, toleranceLength); - const FloatV minMargin = FMin(boxMargin, toleranceMargin); + const PxReal toranceLength = params.mToleranceLength; + const FloatV minMargin = Gu::CalculatePCMBoxMargin(boxExtents, toranceLength, GU_PCM_MESH_MANIFOLD_EPSILON); Gu::BoxV boxV(V3Zero(), boxExtents); diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexMesh.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexMesh.cpp index ca8f9028..ea12eb4e 100644 --- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexMesh.cpp +++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexMesh.cpp @@ -192,19 +192,15 @@ bool Gu::pcmContactConvexMesh(GU_CONTACT_METHOD_ARGS) PxBounds3 hullAABB; PolygonalData polyData; const bool idtScaleConvex = getPCMConvexData(shape0, convexScaling, hullAABB, polyData); - - const Vec3V vScale = V3LoadU_SafeReadW(shapeConvex.scale.scale); // PT: safe because 'rotation' follows 'scale' in PxMeshScale - const FloatV convexMargin = CalculatePCMConvexMargin(hullData, vScale); const QuatV vQuat = QuatVLoadU(&shapeConvex.scale.rotation.x); - const FloatV epsilon = FLoad(GU_PCM_MESH_MANIFOLD_EPSILON); - const FloatV toleranceLength = FLoad(params.mToleranceLength); - const FloatV toleranceMargin = FMul(epsilon, toleranceLength); - const FloatV minMargin = FMin(convexMargin, toleranceMargin); + const Vec3V vScale = V3LoadU_SafeReadW(shapeConvex.scale.scale); // PT: safe because 'rotation' follows 'scale' in PxMeshScale + + const PxReal toleranceScale = params.mToleranceLength; + const FloatV minMargin = CalculatePCMConvexMargin(hullData, vScale, toleranceScale, GU_PCM_MESH_MANIFOLD_EPSILON); ConvexHullV convexHull(hullData, V3Zero(), vScale, vQuat, idtScaleConvex); - if(idtScaleConvex) { SupportLocalImpl<Gu::ConvexHullNoScaleV> convexMap(static_cast<ConvexHullNoScaleV&>(convexHull), convexTransform, convexHull.vertex2Shape, convexHull.shape2Vertex, true); @@ -240,11 +236,8 @@ bool Gu::pcmContactBoxMesh(GU_CONTACT_METHOD_ARGS) Cm::FastVertex2ShapeScaling idtScaling; const Vec3V boxExtents = V3LoadU(shapeBox.halfExtents); - const FloatV boxMargin = Gu::CalculatePCMBoxMargin(boxExtents); - const FloatV epsilon = FLoad(GU_PCM_MESH_MANIFOLD_EPSILON); - const FloatV toleranceLength = FLoad(params.mToleranceLength); - const FloatV toleranceMargin = FMul(epsilon, toleranceLength); - const FloatV minMargin = FMin(boxMargin, toleranceMargin); + const PxReal toleranceLength = params.mToleranceLength; + const FloatV minMargin = Gu::CalculatePCMBoxMargin(boxExtents, toleranceLength, GU_PCM_MESH_MANIFOLD_EPSILON); BoxV boxV(V3Zero(), boxExtents); diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGen.h b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGen.h index 0d7e3c73..31c34142 100644 --- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGen.h +++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGen.h @@ -46,15 +46,15 @@ namespace Gu //full contact gen code for box/convexhull vs convexhull bool generateFullContactManifold(Gu::PolygonalData& polyData0, Gu::PolygonalData& polyData1, Gu::SupportLocal* map0, Gu::SupportLocal* map1, Gu::PersistentContact* manifoldContacts, PxU32& numContacts, const Ps::aos::FloatVArg contactDist, const Ps::aos::Vec3VArg normal, const Ps::aos::Vec3VArg closestA, const Ps::aos::Vec3VArg closestB, - const Ps::aos::FloatVArg toleranceA, const Ps::aos::FloatVArg toleranceB, bool doOverlapTest, Cm::RenderOutput* renderOutput, const Ps::aos::FloatVArg toleranceScale); + const PxReal toleranceA, const PxReal toleranceB, bool doOverlapTest, Cm::RenderOutput* renderOutput, const PxReal toleranceScale); //full contact gen code for capsule vs convexhulll bool generateFullContactManifold(const Gu::CapsuleV& capsule, Gu::PolygonalData& polyData, Gu::SupportLocal* map, const Ps::aos::PsMatTransformV& aToB, Gu::PersistentContact* manifoldContacts, - PxU32& numContacts, const Ps::aos::FloatVArg contactDist, Ps::aos::Vec3V& normal, const Ps::aos::Vec3VArg closest, const Ps::aos::FloatVArg tolerance, bool doOverlapTest, const Ps::aos::FloatVArg toleranceScale); + PxU32& numContacts, const Ps::aos::FloatVArg contactDist, Ps::aos::Vec3V& normal, const Ps::aos::Vec3VArg closest, const PxReal tolerance, bool doOverlapTest, const PxReal toleranceScale); //full contact gen code for capsule vs box bool generateCapsuleBoxFullContactManifold(const Gu::CapsuleV& capsule, Gu::PolygonalData& polyData, Gu::SupportLocal* map, const Ps::aos::PsMatTransformV& aToB, Gu::PersistentContact* manifoldContacts, PxU32& numContacts, - const Ps::aos::FloatVArg contactDist, Ps::aos::Vec3V& normal, const Ps::aos::Vec3VArg closest, const Ps::aos::FloatVArg boxMargin, const bool doOverlapTest, const Ps::aos::FloatVArg toeranceScale); + const Ps::aos::FloatVArg contactDist, Ps::aos::Vec3V& normal, const Ps::aos::Vec3VArg closest, const PxReal boxMargin, const bool doOverlapTest, const PxReal toeranceScale); //MTD code for box/convexhull vs box/convexhull bool computeMTD(Gu::PolygonalData& polyData0, Gu::PolygonalData& polyData1, SupportLocal* map0, SupportLocal* map1, Ps::aos::FloatV& penDepth, Ps::aos::Vec3V& normal); diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenBoxConvex.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenBoxConvex.cpp index de5d3b1b..5b593869 100644 --- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenBoxConvex.cpp +++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenBoxConvex.cpp @@ -566,8 +566,8 @@ namespace Gu } bool generateFullContactManifold(PolygonalData& polyData0, PolygonalData& polyData1, SupportLocal* map0, SupportLocal* map1, PersistentContact* manifoldContacts, PxU32& numContacts, - const FloatVArg contactDist, const Vec3VArg normal, const Vec3VArg closestA, const Vec3VArg closestB, const FloatVArg marginA, const FloatVArg marginB, const bool doOverlapTest, - Cm::RenderOutput* renderOutput, const Ps::aos::FloatVArg toleranceScale) + const FloatVArg contactDist, const Vec3VArg normal, const Vec3VArg closestA, const Vec3VArg closestB, const PxReal marginA, const PxReal marginB, const bool doOverlapTest, + Cm::RenderOutput* renderOutput, const PxReal toleranceLength) { const PsMatTransformV transform1To0V = map0->transform.transformInv(map1->transform); @@ -657,19 +657,50 @@ EdgeTest: } else { - const FloatV eps = FLoad(PCM_WITNESS_POINT_SCALE); - const FloatV lowerEps = FMul(toleranceScale, FLoad(PCM_WITNESS_POINT_LOWER_EPS)); - const FloatV toleranceA = FMax(FMul(marginA, eps), lowerEps); - - const FloatV toleranceB = FMax(FMul(marginB, eps), lowerEps); + const PxReal lowerEps = toleranceLength * PCM_WITNESS_POINT_LOWER_EPS; + const PxReal upperEps = toleranceLength * PCM_WITNESS_POINT_UPPER_EPS; + const PxReal toleranceA = PxClamp(marginA, lowerEps, upperEps); + const PxReal toleranceB = PxClamp(marginB, lowerEps, upperEps); + + const Vec3V negNormal = V3Neg(normal); + const Vec3V normalIn0 = transform0To1V.rotateInv(normal); - const PxU32 faceIndex1 = getWitnessPolygonIndex(polyData1, map1, V3Neg(normal), closestB, toleranceB); - const PxU32 faceIndex0 = getWitnessPolygonIndex(polyData0, map0, transform0To1V.rotateInv(normal), transform0To1V.transformInv(closestA), toleranceA); + const PxU32 faceIndex1 = getWitnessPolygonIndex(polyData1, map1, negNormal, closestB, toleranceB); + const PxU32 faceIndex0 = getWitnessPolygonIndex(polyData0, map0, normalIn0, transform0To1V.transformInv(closestA), + toleranceA); const HullPolygonData& referencePolygon = polyData1.mPolygons[faceIndex1]; const HullPolygonData& incidentPolygon = polyData0.mPolygons[faceIndex0]; - generatedContacts(polyData1, polyData0, referencePolygon, incidentPolygon, map1, map0, transform1To0V, manifoldContacts, numContacts, contactDist, renderOutput); - + + const Vec3V referenceNormal = V3Normalize(M33TrnspsMulV3(map1->shape2Vertex, V3LoadU(referencePolygon.mPlane.n))); + const Vec3V incidentNormal = V3Normalize(M33TrnspsMulV3(map0->shape2Vertex, V3LoadU(incidentPolygon.mPlane.n))); + + const FloatV referenceProject = FAbs(V3Dot(referenceNormal, negNormal)); + const FloatV incidentProject = FAbs(V3Dot(incidentNormal, normalIn0)); + + if (FAllGrtrOrEq(referenceProject, incidentProject) ) + { + generatedContacts(polyData1, polyData0, referencePolygon, incidentPolygon, map1, map0, transform1To0V, manifoldContacts, numContacts, contactDist, renderOutput); + } + else + { + generatedContacts(polyData0, polyData1, incidentPolygon, referencePolygon, map0, map1, transform0To1V, manifoldContacts, numContacts, contactDist, renderOutput); + + if (numContacts > 0) + { + const Vec3V n = transform0To1V.rotate(incidentNormal); + + const Vec3V nn = V3Neg(n); + //flip the contacts + for (PxU32 i = 0; i<numContacts; ++i) + { + const Vec3V localPointB = manifoldContacts[i].mLocalPointB; + manifoldContacts[i].mLocalPointB = manifoldContacts[i].mLocalPointA; + manifoldContacts[i].mLocalPointA = localPointB; + manifoldContacts[i].mLocalNormalPen = V4SetW(nn, V4GetW(manifoldContacts[i].mLocalNormalPen)); + } + } + } } return true; diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenSphereCapsule.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenSphereCapsule.cpp index 188ea3eb..ae732263 100644 --- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenSphereCapsule.cpp +++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenSphereCapsule.cpp @@ -306,7 +306,7 @@ namespace Gu bool generateCapsuleBoxFullContactManifold(const CapsuleV& capsule, PolygonalData& polyData, SupportLocal* map, const PsMatTransformV& aToB, PersistentContact* manifoldContacts, PxU32& numContacts, - const FloatVArg contactDist, Vec3V& normal, const Vec3VArg closest, const FloatVArg margin, const bool doOverlapTest, const FloatVArg toleranceScale) + const FloatVArg contactDist, Vec3V& normal, const Vec3VArg closest, const PxReal margin, const bool doOverlapTest, const PxReal toleranceScale) { const PxU32 originalContacts = numContacts; @@ -324,11 +324,12 @@ namespace Gu } else { - const FloatV eps = FLoad(PCM_WITNESS_POINT_SCALE); - const FloatV lowerEps = FMul(toleranceScale, FLoad(PCM_WITNESS_POINT_LOWER_EPS)); - const FloatV tolerance = FMax(FMul(margin, eps), lowerEps); + const PxReal lowerEps = toleranceScale * PCM_WITNESS_POINT_LOWER_EPS; + const PxReal upperEps = toleranceScale * PCM_WITNESS_POINT_UPPER_EPS; + const PxReal tolerance = PxClamp(margin, lowerEps, upperEps); - referencePolygon = &polyData.mPolygons[getWitnessPolygonIndex(polyData, map, V3Neg(normal), closest, tolerance)]; + const PxU32 featureIndex = getWitnessPolygonIndex(polyData, map, V3Neg(normal), closest, tolerance); + referencePolygon = &polyData.mPolygons[featureIndex]; } @@ -344,7 +345,7 @@ namespace Gu //capsule is in the local space of polyData bool generateFullContactManifold(const CapsuleV& capsule, PolygonalData& polyData, SupportLocal* map, const PsMatTransformV& aToB, PersistentContact* manifoldContacts, PxU32& numContacts, - const FloatVArg contactDist, Vec3V& normal, const Vec3VArg closest, const FloatVArg margin, const bool doOverlapTest, const Ps::aos::FloatVArg toleranceScale) + const FloatVArg contactDist, Vec3V& normal, const Vec3VArg closest, const PxReal margin, const bool doOverlapTest, const PxReal toleranceLength) { const PxU32 originalContacts = numContacts; @@ -374,11 +375,13 @@ namespace Gu const PxU32 faceContacts = numContacts - originalContacts; if(faceContacts < 2) { - const FloatV eps = FLoad(PCM_WITNESS_POINT_SCALE); - const FloatV lowerEps = FMul(toleranceScale, FLoad(PCM_WITNESS_POINT_LOWER_EPS)); - const FloatV toleranceA = FMax(FMul(margin, eps), lowerEps); + + const PxReal lowerEps = toleranceLength * PCM_WITNESS_POINT_LOWER_EPS; + const PxReal upperEps = toleranceLength * PCM_WITNESS_POINT_UPPER_EPS; + const PxReal tolerance = PxClamp(margin, lowerEps, upperEps); - const Gu::HullPolygonData& referencePolygon = polyData.mPolygons[getWitnessPolygonIndex(polyData, map, V3Neg(tNormal), closest, toleranceA)]; + const PxU32 featureIndex = getWitnessPolygonIndex(polyData, map, V3Neg(tNormal), closest, tolerance); + const Gu::HullPolygonData& referencePolygon = polyData.mPolygons[featureIndex]; generatedContactsEEContacts(capsule, polyData,referencePolygon, map, aToB, manifoldContacts, numContacts, contactDist, tNormal); } } diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenUtil.h b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenUtil.h index 8469481e..5171a4fc 100644 --- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenUtil.h +++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenUtil.h @@ -36,10 +36,10 @@ #include "GuVecCapsule.h" #include "GuConvexSupportTable.h" -//Percentage of margin -#define PCM_WITNESS_POINT_SCALE 1e-1f //The smallest epsilon we will permit (scaled by PxTolerancesScale.length) -#define PCM_WITNESS_POINT_LOWER_EPS 1e-3f +#define PCM_WITNESS_POINT_LOWER_EPS 1e-2f +//The largest epsilon we will permit (scaled by PxTolerancesScale.length) +#define PCM_WITNESS_POINT_UPPER_EPS 5e-2f namespace physx { @@ -273,39 +273,73 @@ namespace Gu } - PX_FORCE_INLINE PxU32 getWitnessPolygonIndex(const Gu::PolygonalData& polyData, SupportLocal* map, const Ps::aos::Vec3VArg normal, - const Ps::aos::Vec3VArg closest, const Ps::aos::FloatVArg tolerance) + PX_FORCE_INLINE PxU32 getWitnessPolygonIndex(const Gu::PolygonalData& polyData, SupportLocal* map, const Ps::aos::Vec3VArg normal, + const Ps::aos::Vec3VArg closest, const PxReal tolerance) { using namespace Ps::aos; + PxReal pd[256]; + //first pass : calculate the smallest distance from the closest point to the polygon face + //transform the closest p to vertex space const Vec3V p = M33MulV3(map->shape2Vertex, closest); PxU32 closestFaceIndex = 0; - Vec4V plane = V4LoadU(&polyData.mPolygons[0].mPlane.n.x); - Vec3V n = Vec3V_From_Vec4V(plane); - Vec3V bestN = V3Normalize(M33TrnspsMulV3(map->shape2Vertex, n)); - FloatV bestProj = V3Dot(bestN, normal); - FloatV d = V4GetW(plane); - FloatV pd = FAbs(FAdd(d, V3Dot(n, p))); - FloatV minDist = pd; - for(PxU32 i=1; i< polyData.mNbPolygons; ++i) + PxVec3 closestP; + V3StoreU(p, closestP); + + const PxReal eps = -tolerance; + + PxPlane plane = polyData.mPolygons[0].mPlane; + PxReal dist = plane.distance(closestP); + PxReal minDist = dist >= eps ? dist : PX_MAX_F32; + pd[0] = minDist; + PxReal maxDist = dist; + PxU32 maxFaceIndex = 0; + + for (PxU32 i = 1; i < polyData.mNbPolygons; ++i) + { + plane = polyData.mPolygons[i].mPlane; + dist = plane.distance(closestP); + pd[i] = dist >= eps ? dist : PX_MAX_F32; + if (minDist > pd[i]) + { + minDist = pd[i]; + closestFaceIndex = i; + } + if (dist > maxDist) + { + maxDist = dist; + maxFaceIndex = i; + } + } + + if (minDist == PX_MAX_F32) + return maxFaceIndex; + + //second pass : select the face which has the normal most close to the gjk/epa normal + Vec4V plane4 = V4LoadU(&polyData.mPolygons[closestFaceIndex].mPlane.n.x); + Vec3V n = Vec3V_From_Vec4V(plane4); + n = V3Normalize(M33TrnspsMulV3(map->shape2Vertex, n)); + FloatV bestProj = V3Dot(n, normal); + + const PxU32 firstPassIndex = closestFaceIndex; + + for (PxU32 i = 0; i< polyData.mNbPolygons; ++i) { - plane = V4LoadU(&polyData.mPolygons[i].mPlane.n.x); - n = Vec3V_From_Vec4V(plane); - d = V4GetW(plane); - pd = FAbs(FAdd(d, V3Dot(n, p))); - //if the difference between the minimum distance and the distance of p to plane i is within tolerance, we use the normal to chose the best plane - if (FAllGrtrOrEq(tolerance, FSub(pd, minDist))) + if ((tolerance >(pd[i] - minDist)) && (firstPassIndex != i)) { - //rotate the plane normal to shape space + plane4 = V4LoadU(&polyData.mPolygons[i].mPlane.n.x); + n = Vec3V_From_Vec4V(plane4); + //rotate the plane normal to shape space. We can roate normal into the vertex space. The reason for + //that is because it will lose some numerical precision if the object has large scale and this algorithm + //will choose different face n = V3Normalize(M33TrnspsMulV3(map->shape2Vertex, n)); FloatV proj = V3Dot(n, normal); - if(FAllGrtr(bestProj, proj)) + if (FAllGrtr(bestProj, proj)) { - minDist= pd; closestFaceIndex = i; bestProj = proj; } @@ -315,7 +349,6 @@ namespace Gu return closestFaceIndex; } - //ML: this function is shared by the sphere/capsule vs convex hulls full contact gen, capsule in the local space of polyData PX_FORCE_INLINE bool testPolyDataAxis(const Gu::CapsuleV& capsule, const Gu::PolygonalData& polyData, SupportLocal* map, const Ps::aos::FloatVArg contactDist, Ps::aos::FloatV& minOverlap, Ps::aos::Vec3V& separatingAxis) { diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactPlaneBox.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactPlaneBox.cpp index be08d46d..308cc0c8 100644 --- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactPlaneBox.cpp +++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactPlaneBox.cpp @@ -65,7 +65,8 @@ bool pcmContactPlaneBox(GU_CONTACT_METHOD_ARGS) const Vec3V boxExtents = V3LoadU(shapeBox.halfExtents); - const FloatV boxMargin = CalculatePCMBoxMargin(boxExtents); + const PxReal toleranceLength = params.mToleranceLength; + const FloatV boxMargin = CalculatePCMBoxMargin(boxExtents, toleranceLength); const FloatV projectBreakingThreshold = FMul(boxMargin, FLoad(0.2f)); const PxU32 initialContacts = manifold.mNumContacts; @@ -74,8 +75,6 @@ bool pcmContactPlaneBox(GU_CONTACT_METHOD_ARGS) const PxU32 newContacts = manifold.mNumContacts; const bool bLostContacts = (newContacts != initialContacts);//((initialContacts == 0) || (newContacts != initialContacts)); - //PX_UNUSED(bLostContacts); - //if(bLostContacts || manifold.invalidate_BoxConvex(curTransf, boxMargin)) if(bLostContacts || manifold.invalidate_PrimitivesPlane(curTransf, boxMargin, FLoad(0.2f))) { //ML:localNormal is the local space of plane normal, however, because shape1 is box and shape0 is plane, we need to use the reverse of contact normal(which will be the plane normal) to make the refreshContactPoints diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactPlaneConvex.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactPlaneConvex.cpp index d0d3daec..215e43c7 100644 --- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactPlaneConvex.cpp +++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactPlaneConvex.cpp @@ -60,14 +60,14 @@ bool pcmContactPlaneConvex(GU_CONTACT_METHOD_ARGS) const Vec3V vScale = V3LoadU_SafeReadW(shapeConvex.scale.scale); // PT: safe because 'rotation' follows 'scale' in PxMeshScale const Gu::ConvexHullData* hullData = shapeConvex.hullData; - const FloatV convexMargin = Gu::CalculatePCMConvexMargin(hullData, vScale); - + const PxReal toleranceLength = params.mToleranceLength; + const FloatV convexMargin = Gu::CalculatePCMConvexMargin(hullData, vScale, toleranceLength); + //in world space const Vec3V planeNormal = V3Normalize(QuatGetBasisVector0(transf1.q)); const Vec3V negPlaneNormal = V3Neg(planeNormal); - const FloatV contactDist = FLoad(params.mContactDistance); //const FloatV replaceBreakingThreshold = FMul(convexMargin, FLoad(0.001f)); @@ -80,8 +80,6 @@ bool pcmContactPlaneConvex(GU_CONTACT_METHOD_ARGS) const bool bLostContacts = (newContacts != initialContacts);//((initialContacts == 0) || (newContacts != initialContacts)); - PX_UNUSED(bLostContacts); - //if(bLostContacts || manifold.invalidate_BoxConvex(curTransf, convexMargin)) if(bLostContacts || manifold.invalidate_PrimitivesPlane(curTransf, convexMargin, FLoad(0.2f))) { const PsMatTransformV aToB(curTransf); @@ -126,7 +124,7 @@ bool pcmContactPlaneConvex(GU_CONTACT_METHOD_ARGS) if(numContacts >= Gu::ContactBuffer::MAX_CONTACTS) { //ML: number of contacts are more than MAX_CONTACTS, we need to force contact reduction - manifold.reduceBatchContacts(manifoldContacts, numContacts); + manifold.reduceBatchContacts(manifoldContacts, numContacts, toleranceLength); numContacts = GU_MANIFOLD_CACHE_SIZE; for(PxU32 j=0; j<GU_MANIFOLD_CACHE_SIZE; ++j) { @@ -137,7 +135,7 @@ bool pcmContactPlaneConvex(GU_CONTACT_METHOD_ARGS) } //reduce contacts - manifold.addBatchManifoldContacts(manifoldContacts, numContacts); + manifold.addBatchManifoldContacts(manifoldContacts, numContacts, toleranceLength); manifold.addManifoldContactsToContactBuffer(contactBuffer, negPlaneNormal, transf1, contactDist); #if PCM_LOW_LEVEL_DEBUG manifold.drawManifold(*renderOutput, transf0, transf1); diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactSphereConvex.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactSphereConvex.cpp index 10358836..5efd3e98 100644 --- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactSphereConvex.cpp +++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactSphereConvex.cpp @@ -136,8 +136,8 @@ bool pcmContactSphereConvex(GU_CONTACT_METHOD_ARGS) const PsTransformV curRTrans(transf1.transformInv(transf0)); const PsMatTransformV aToB(curRTrans); - - const FloatV convexMargin = Gu::CalculatePCMConvexMargin(hullData, vScale); + const PxReal toleranceLength = params.mToleranceLength; + const FloatV convexMargin = Gu::CalculatePCMConvexMargin(hullData, vScale, toleranceLength); const PxU32 initialContacts = manifold.mNumContacts; const FloatV minMargin = FMin(convexMargin, sphereRadius); @@ -169,7 +169,7 @@ bool pcmContactSphereConvex(GU_CONTACT_METHOD_ARGS) const bool idtScale = shapeConvex.scale.isIdentity(); //use the original shape ConvexHullV convexHull(hullData, zeroV, vScale, vQuat, idtScale); - convexHull.setMargin(zero); + convexHull.setMargin(0.f); //transform capsule into the local space of convexHull CapsuleV capsule(aToB.p, sphereRadius); diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMTriangleContactGen.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMTriangleContactGen.cpp index fde67300..7a0e68a2 100644 --- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMTriangleContactGen.cpp +++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMTriangleContactGen.cpp @@ -689,23 +689,26 @@ namespace physx points1In0[i] = V3SetZ(points1In0[i], d); iPolygonMin = V3Min(iPolygonMin, points1In0[i]); iPolygonMax = V3Max(iPolygonMax, points1In0[i]); - if(FAllGrtr(rd, z)) + + bool penetrated = false; + + if (FAllGrtr(rd, z)) { - points1In0Penetration[i] = true; + penetrated = true; - if(contains(points0In0, 3, points1In0[i], rPolygonMin, rPolygonMax)) + if (contains(points0In0, 3, points1In0[i], rPolygonMin, rPolygonMax)) { inside++; - + //add this contact to the buffer const FloatV t = V3Dot(contactNormal, V3Sub(triangle.verts[0], vert1)); - const Vec3V projectPoint = V3ScaleAdd(contactNormal, t, vert1); + const Vec3V projectPoint = V3ScaleAdd(contactNormal, t, vert1); const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(contactNormal), FNeg(t)); numManifoldContacts = addMeshContacts(manifoldContacts, vert1, projectPoint, localNormalPen, triangleIndex, numManifoldContacts); - + //if the numContacts are more than GU_MESH_CONTACT_REDUCTION_THRESHOLD, we need to do contact reduction const PxU32 numContacts = numManifoldContacts - previousContacts; - if(numContacts >= GU_MESH_CONTACT_REDUCTION_THRESHOLD) + if (numContacts >= GU_MESH_CONTACT_REDUCTION_THRESHOLD) { //a polygon has more than GU_MESH_CONTACT_REDUCTION_THRESHOLD(16) contacts with this triangle, we will reduce //the contacts to GU_SINGLE_MANIFOLD_SINGLE_POLYGONE_CACHE_SIZE(4) points @@ -714,7 +717,8 @@ namespace physx } } } - + + points1In0Penetration[i] = penetrated; } diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPersistentContactManifold.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPersistentContactManifold.cpp index f93c703e..9ce3e9c0 100644 --- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPersistentContactManifold.cpp +++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPersistentContactManifold.cpp @@ -860,7 +860,8 @@ void Gu::PersistentContactManifold::addManifoldContactsToContactBuffer(Gu::Conta GU_MANIFOLD_CACHE_SIZE, we will need to do contact reduction while we are storing the chosen manifold contacts from the manifold contact list to the manifold contact buffer. */ -void Gu::PersistentContactManifold::addBatchManifoldContacts(const PersistentContact* manifoldContacts, const PxU32 numPoints) +void Gu::PersistentContactManifold::addBatchManifoldContacts(const PersistentContact* manifoldContacts, + const PxU32 numPoints, const PxReal toleranceLength) { if(numPoints <= GU_MANIFOLD_CACHE_SIZE) @@ -875,7 +876,7 @@ void Gu::PersistentContactManifold::addBatchManifoldContacts(const PersistentCon } else { - reduceBatchContacts(manifoldContacts, numPoints); + reduceBatchContacts(manifoldContacts, numPoints, toleranceLength); mNumContacts = GU_MANIFOLD_CACHE_SIZE; } } @@ -967,12 +968,16 @@ void Gu::PersistentContactManifold::reduceBatchContactsCluster(const PersistentC maxDist = nmax; - index = PxU32(-1); + index = GU_MANIFOLD_INVALID_INDEX; v = V3Sub(mContactPoints[1].mLocalPointB, mContactPoints[0].mLocalPointB); - Vec3V norm = V3Normalize(V3Cross(v, Vec3V_From_Vec4V(mContactPoints[0].mLocalNormalPen))); + + const Vec3V cn0 = Vec3V_From_Vec4V(mContactPoints[0].mLocalNormalPen); + Vec3V norm = V3Cross(v, cn0); + const FloatV sqLen = V3Dot(norm, norm); + norm = V3Sel(FIsGrtr(sqLen, FZero()), V3ScaleInv(norm, FSqrt(sqLen)), cn0); FloatV minDist = max; - PxU32 index1 = PxU32(-1); + PxU32 index1 = GU_MANIFOLD_INVALID_INDEX; //calculate the min and max point away from the segment @@ -1066,112 +1071,179 @@ void Gu::PersistentContactManifold::reduceBatchContactsCluster(const PersistentC /* This function is for box/convexhull full contact generation. If the numPoints > 4, we will reduce the contact points to 4 */ -void Gu::PersistentContactManifold::reduceBatchContacts(const PersistentContact* manifoldPoints, const PxU32 numPoints) +void Gu::PersistentContactManifold::reduceBatchContacts(const PersistentContact* manifoldPoints, + const PxU32 numPoints, const PxReal tolereanceLength) { using namespace Ps::aos; - - bool chosen[64]; - physx::PxMemZero(chosen, sizeof(bool)*numPoints); + + PxU8 chosenIndices[4]; + PxU8 candidates[64]; + + const FloatV zero = FZero(); const FloatV max = FMax(); const FloatV nmax = FNeg(max); - FloatV maxDist = V4GetW(manifoldPoints[0].mLocalNormalPen); - PxI32 index = 0; - //keep the deepest point - for(PxU32 i=1; i<numPoints; ++i) + FloatV maxPen = V4GetW(manifoldPoints[0].mLocalNormalPen); + FloatV minPen = nmax; + PxU32 index = 0; + candidates[0] = 0; + PxU32 candidateIndex = 0; + + PxU32 nbCandiates = numPoints; + //keep the deepest point, candidateIndex will be the same as index + for (PxU32 i = 1; i<numPoints; ++i) { + //at the begining candidates and indices will be the same + candidates[i] = PxU8(i); + const FloatV pen = V4GetW(manifoldPoints[i].mLocalNormalPen); - if(FAllGrtr(maxDist, pen)) + minPen = FMax(minPen, pen); + if (FAllGrtr(maxPen, pen)) { - maxDist = pen; - index = PxI32(i); + maxPen = pen; + index = i; + candidateIndex = i; } } + //keep the deepest points in the first position - mContactPoints[0] = manifoldPoints[index]; - chosen[index] = true; + chosenIndices[0] = PxU8(index); + //move the chosen indices out of the candidates indices + nbCandiates = nbCandiates - 1; + candidates[candidateIndex] = candidates[nbCandiates]; + //indices[index] = nbCandiates; //calculate the furthest away points - Vec3V v = V3Sub(manifoldPoints[0].mLocalPointB, mContactPoints[0].mLocalPointB); - maxDist = V3Dot(v, v); - index = 0; + Vec3V v = V3Sub(manifoldPoints[candidates[0]].mLocalPointB, manifoldPoints[chosenIndices[0]].mLocalPointB); + FloatV maxDist = V3Dot(v, v); + index = candidates[0]; + candidateIndex = 0; - for(PxU32 i=1; i<numPoints; ++i) + for (PxU32 i = 1; i<nbCandiates; ++i) { - v = V3Sub(manifoldPoints[i].mLocalPointB, mContactPoints[0].mLocalPointB); + v = V3Sub(manifoldPoints[candidates[i]].mLocalPointB, manifoldPoints[chosenIndices[0]].mLocalPointB); const FloatV d = V3Dot(v, v); - if(FAllGrtr(d, maxDist)) + if (FAllGrtr(d, maxDist)) { maxDist = d; - index = PxI32(i); + index = candidates[i]; + candidateIndex = i; } } - //PX_ASSERT(chosen[index] == false); - mContactPoints[1] = manifoldPoints[index]; - chosen[index] = true; + chosenIndices[1] = PxU8(index); + //move the chosen indices out of the candidates indices + nbCandiates = nbCandiates - 1; + candidates[candidateIndex] = candidates[nbCandiates]; + v = V3Sub(manifoldPoints[chosenIndices[1]].mLocalPointB, manifoldPoints[chosenIndices[0]].mLocalPointB); + const Vec3V cn0 = Vec3V_From_Vec4V(manifoldPoints[chosenIndices[0]].mLocalNormalPen); + Vec3V norm = V3Cross(v, cn0); + const FloatV sqLen = V3Dot(norm, norm); + norm = V3Sel(FIsGrtr(sqLen, zero), V3ScaleInv(norm, FSqrt(sqLen)), cn0); + //reset maxDist and index maxDist = nmax; - index = -1; - - v = V3Sub(mContactPoints[1].mLocalPointB, mContactPoints[0].mLocalPointB); - const Vec3V vCross = V3Cross(v, Vec3V_From_Vec4V(mContactPoints[0].mLocalNormalPen)); - const FloatV sqLen = V3Dot(vCross, vCross); - const Vec3V norm = V3Sel(FIsGrtr(sqLen, FZero()), V3ScaleInv(vCross, FSqrt(sqLen)), V3Zero()); - + index = GU_MANIFOLD_INVALID_INDEX; + candidateIndex = GU_MANIFOLD_INVALID_INDEX; FloatV minDist = max; - PxI32 index1 = -1; - + PxU32 index1 = GU_MANIFOLD_INVALID_INDEX; + PxU32 candidateIndex1 = GU_MANIFOLD_INVALID_INDEX; + //calculate the min and max point away from the segment - for(PxU32 i=0; i<numPoints; ++i) + for (PxU32 i = 0; i<nbCandiates; ++i) { - if(!chosen[i]) + v = V3Sub(manifoldPoints[candidates[i]].mLocalPointB, manifoldPoints[chosenIndices[0]].mLocalPointB); + const FloatV d = V3Dot(v, norm); + if (FAllGrtr(d, maxDist)) { - v = V3Sub(manifoldPoints[i].mLocalPointB, mContactPoints[0].mLocalPointB); - const FloatV d = V3Dot(v, norm); - if(FAllGrtr(d, maxDist)) - { - maxDist = d; - index = PxI32(i); - } + maxDist = d; + index = candidates[i]; + candidateIndex = i; + } - if(FAllGrtr(minDist, d)) - { - minDist = d; - index1 = PxI32(i); - } + if (FAllGrtr(minDist, d)) + { + minDist = d; + index1 = candidates[i]; + candidateIndex1 = i; } + } - mContactPoints[2] = manifoldPoints[index]; - chosen[index] = true; + + chosenIndices[2] = PxU8(index); + //move the chosen indices out of the candidates indices + nbCandiates = nbCandiates - 1; + candidates[candidateIndex] = candidates[nbCandiates]; + if (nbCandiates == candidateIndex1) + candidateIndex1 = candidateIndex; //if min and max in the same side, chose again const FloatV temp = FMul(minDist, maxDist); - if(FAllGrtr(temp, FZero())) + if (FAllGrtr(temp, zero)) { //chose again maxDist = nmax; - for(PxU32 i=0; i<numPoints; ++i) + for (PxU32 i = 0; i < nbCandiates; ++i) { - if(!chosen[i]) + v = V3Sub(manifoldPoints[candidates[i]].mLocalPointB, manifoldPoints[chosenIndices[0]].mLocalPointB); + const FloatV d = V3Dot(v, norm); + if (FAllGrtr(d, maxDist)) { - v = V3Sub(manifoldPoints[i].mLocalPointB, mContactPoints[0].mLocalPointB); - const FloatV d = V3Dot(v, norm); - if(FAllGrtr(d, maxDist)) + maxDist = d; + index1 = candidates[i]; + candidateIndex1 = i; + } + } + + } + + chosenIndices[3] = PxU8(index1); + //move the chosen indices out of the candidates indices + nbCandiates = nbCandiates - 1; + candidates[candidateIndex1] = candidates[nbCandiates]; + + const FloatV eps = FLoad(tolereanceLength * 0.02f); + const BoolV con = BAnd(FIsGrtr(eps, maxPen), FIsGrtr(minPen, eps)); + if (BAllEqTTTT(con)) + { + //post process + for (PxU32 i = 0; i < 4; ++i) + { + FloatV pen = V4GetW(manifoldPoints[chosenIndices[i]].mLocalNormalPen); + + if (FAllGrtr(pen, eps)) + { + candidateIndex = GU_MANIFOLD_INVALID_INDEX; + for (PxU32 j = 0; j < nbCandiates; ++j) { - maxDist = d; - index1 = PxI32(i); + const FloatV pen1 = V4GetW(manifoldPoints[candidates[j]].mLocalNormalPen); + if (FAllGrtr(pen, pen1) && FAllGrtr(eps, pen1)) + { + pen = pen1; + candidateIndex = j; + } + } + if (candidateIndex < nbCandiates) + { + const PxU8 originalIndex = chosenIndices[i]; + chosenIndices[i] = candidates[candidateIndex]; + candidates[candidateIndex] = originalIndex; } } + mContactPoints[i] = manifoldPoints[chosenIndices[i]]; + } + } + else + { + for (PxU32 i = 0; i < 4; ++i) + { + mContactPoints[i] = manifoldPoints[chosenIndices[i]]; } } - - mContactPoints[3] = manifoldPoints[index1]; } - /* This function is for capsule full contact generation. If the numPoints > 2, we will reduce the contact points to 2 */ @@ -1540,7 +1612,7 @@ Ps::aos::FloatV Gu::SinglePersistentContactManifold::reduceBatchContactsConvex(c const FloatV max = FMax(); const FloatV nmax = FNeg(max); FloatV maxDis = nmax; - PxI32 index = -1; + PxU32 index = GU_MANIFOLD_INVALID_INDEX; PCMContactPatch* currentPatch = &patch; while(currentPatch) @@ -1552,7 +1624,7 @@ Ps::aos::FloatV Gu::SinglePersistentContactManifold::reduceBatchContactsConvex(c if(FAllGrtr(v, maxDis)) { maxDis = v; - index = PxI32(i); + index = i; } } currentPatch = currentPatch->mNextPatch; @@ -1568,7 +1640,7 @@ Ps::aos::FloatV Gu::SinglePersistentContactManifold::reduceBatchContactsConvex(c //calculate the furthest away points Vec3V v = V3Sub(manifoldContactExt[patch.mStartIndex].mLocalPointB, contact0); maxDis = V3Dot(v, v); - index = PxI32(patch.mStartIndex); + index = patch.mStartIndex; currentPatch = &patch; while(currentPatch) @@ -1580,7 +1652,7 @@ Ps::aos::FloatV Gu::SinglePersistentContactManifold::reduceBatchContactsConvex(c if(FAllGrtr(d, maxDis)) { maxDis = d; - index = PxI32(i); + index = i; } } currentPatch = currentPatch->mNextPatch; @@ -1594,11 +1666,14 @@ Ps::aos::FloatV Gu::SinglePersistentContactManifold::reduceBatchContactsConvex(c maxPen = FMin(maxPen, V4GetW(manifoldContactExt[index].mLocalNormalPen)); maxDis = nmax; - index = -1; + index = GU_MANIFOLD_INVALID_INDEX; v = V3Sub(contact1, contact0); - Vec3V norm = V3Normalize(V3Cross(v, Vec3V_From_Vec4V(mContactPoints[0].mLocalNormalPen))); + const Vec3V cn0 = Vec3V_From_Vec4V(mContactPoints[0].mLocalNormalPen); + Vec3V norm = V3Cross(v, cn0); + const FloatV sqLen = V3Dot(norm, norm); + norm = V3Sel(FIsGrtr(sqLen, FZero()), V3ScaleInv(norm, FSqrt(sqLen)), cn0); FloatV minDis = max; - PxI32 index1 = -1; + PxU32 index1 = GU_MANIFOLD_INVALID_INDEX; //calculate the point furthest way to the segment @@ -1615,13 +1690,13 @@ Ps::aos::FloatV Gu::SinglePersistentContactManifold::reduceBatchContactsConvex(c if(FAllGrtr(d, maxDis)) { maxDis = d; - index = PxI32(i); + index = i; } if(FAllGrtr(minDis, d)) { minDis = d; - index1 = PxI32(i); + index1 = i; } } } @@ -1654,7 +1729,7 @@ Ps::aos::FloatV Gu::SinglePersistentContactManifold::reduceBatchContactsConvex(c if(FAllGrtr(d, maxDis)) { maxDis = d; - index1 = PxI32(i); + index1 = i; } } } @@ -1678,8 +1753,6 @@ Ps::aos::FloatV Gu::SinglePersistentContactManifold::reduceBatchContactsConvex(c inds[a] = 0; } - - { currentPatch = &patch; @@ -1714,10 +1787,6 @@ Ps::aos::FloatV Gu::SinglePersistentContactManifold::reduceBatchContactsConvex(c maxPen = FMin(maxPen, pens[i]); } } - - - - return maxPen; } @@ -1725,125 +1794,163 @@ PxU32 Gu::SinglePersistentContactManifold::reduceContacts(MeshPersistentContact* { using namespace Ps::aos; - bool* chosen = reinterpret_cast<bool*>(PxAlloca(sizeof(bool)*numPoints)); - physx::PxMemZero(chosen, sizeof(bool)*numPoints); - FloatV max = FMax(); - FloatV maxDis = max; - PxU32 index = 0xffffffff; + PxU8 chosenIndices[GU_SINGLE_MANIFOLD_SINGLE_POLYGONE_CACHE_SIZE]; + PxU8* candidates = reinterpret_cast<PxU8*>(PxAlloca(sizeof(PxU8) * numPoints)); + + const FloatV max = FMax(); + const FloatV nmax = FNeg(max); + const FloatV zero = FZero(); + FloatV maxPen = V4GetW(manifoldPoints[0].mLocalNormalPen); + PxU32 index = 0; + candidates[0] = 0; + PxU32 candidateIndex = 0; + PxU32 nbCandiates = numPoints; + MeshPersistentContact newManifold[GU_SINGLE_MANIFOLD_SINGLE_POLYGONE_CACHE_SIZE]; //keep the deepest point - for(PxU32 i=0; i<numPoints; ++i) + for(PxU32 i=1; i<numPoints; ++i) { + //at the begining candidates and indices will be the same + candidates[i] = PxU8(i); + const FloatV pen = V4GetW(manifoldPoints[i].mLocalNormalPen); - if(FAllGrtr(maxDis, pen)) + if(FAllGrtr(maxPen, pen)) { - maxDis = pen; + maxPen = pen; index = i; + candidateIndex = i; } } + //keep the deepest points in the first position - newManifold[0] = manifoldPoints[index]; - chosen[index] = true; + chosenIndices[0] = PxU8(index); + + //move the chosen indices out of the candidates indices + nbCandiates = nbCandiates - 1; + candidates[candidateIndex] = candidates[nbCandiates]; + //keep the deepest points in the first position + newManifold[0] = manifoldPoints[chosenIndices[0]]; //calculate the furthest away points - Vec3V v = V3Sub(manifoldPoints[0].mLocalPointB, newManifold[0].mLocalPointB); - maxDis = V3Dot(v, v); - index = 0; + Vec3V v = V3Sub(manifoldPoints[candidates[0]].mLocalPointB, newManifold[0].mLocalPointB); + maxPen = V3Dot(v, v); + index = candidates[0]; + candidateIndex = 0; - for(PxU32 i=1; i<numPoints; ++i) + for(PxU32 i=1; i<nbCandiates; ++i) { - v = V3Sub(manifoldPoints[i].mLocalPointB, newManifold[0].mLocalPointB); + v = V3Sub(manifoldPoints[candidates[i]].mLocalPointB, manifoldPoints[chosenIndices[0]].mLocalPointB); const FloatV d = V3Dot(v, v); - if(FAllGrtr(d, maxDis)) + if(FAllGrtr(d, maxPen)) { - maxDis = d; - index = i; + maxPen = d; + index = candidates[i]; + candidateIndex = i; } } - //PX_ASSERT(chosen[index] == false); - newManifold[1] = manifoldPoints[index]; - chosen[index] = true; + chosenIndices[1] = PxU8(index); + //move the chosen indices out of the candidates indices + nbCandiates = nbCandiates - 1; + candidates[candidateIndex] = candidates[nbCandiates]; + + newManifold[1] = manifoldPoints[chosenIndices[1]]; - maxDis = FNeg(max); - index = 0xffffffff; - v = V3Sub(newManifold[1].mLocalPointB, newManifold[0].mLocalPointB); - Vec3V norm = V3Normalize(V3Cross(v, Vec3V_From_Vec4V(newManifold[0].mLocalNormalPen))); - FloatV minDis = max; - PxU32 index1 = 0xffffffff; + v = V3Sub(newManifold[1].mLocalPointB, newManifold[0].mLocalPointB); + const Vec3V cn0 = Vec3V_From_Vec4V(newManifold[0].mLocalNormalPen); + Vec3V norm = V3Cross(v, cn0); + const FloatV sqLen = V3Dot(norm, norm); + norm = V3Sel(FIsGrtr(sqLen, zero), V3ScaleInv(norm, FSqrt(sqLen)), cn0); + + FloatV maxDist = nmax; + FloatV minDist = max; + index = GU_MANIFOLD_INVALID_INDEX; + PxU32 index1 = GU_MANIFOLD_INVALID_INDEX; + PxU32 candidateIndex1 = GU_MANIFOLD_INVALID_INDEX; //calculate the point furthest way to the segment - for(PxU32 i=0; i<numPoints; ++i) + for(PxU32 i=0; i<nbCandiates; ++i) { - if(!chosen[i]) + v = V3Sub(manifoldPoints[candidates[i]].mLocalPointB, newManifold[0].mLocalPointB); + const FloatV d = V3Dot(v, norm); + if(FAllGrtr(d, maxDist)) { - v = V3Sub(manifoldPoints[i].mLocalPointB, newManifold[0].mLocalPointB); - const FloatV d = V3Dot(v, norm); - if(FAllGrtr(d, maxDis)) - { - maxDis = d; - index = i; - } + maxDist = d; + index = candidates[i]; + candidateIndex = i; + } - if(FAllGrtr(minDis, d)) - { - minDis = d; - index1 = i; - } + if(FAllGrtr(minDist, d)) + { + minDist = d; + index1 = candidates[i]; + candidateIndex1 = i; } + } - //PX_ASSERT(chosen[index] == false && chosen[index1]== false); + chosenIndices[2] = PxU8(index); + //move the chosen indices out of the candidates indices + nbCandiates = nbCandiates - 1; + candidates[candidateIndex] = candidates[nbCandiates]; - chosen[index] = true; - newManifold[2] = manifoldPoints[index]; + newManifold[2] = manifoldPoints[chosenIndices[2]]; + + if (nbCandiates == candidateIndex1) + candidateIndex1 = candidateIndex; - const FloatV temp = FMul(minDis, maxDis); + const FloatV temp = FMul(minDist, maxDist); if(FAllGrtr(temp, FZero())) { - //chose the something further away from newManifold[2] - maxDis = FNeg(max); - for(PxU32 i=0; i<numPoints; ++i) + //chose again + maxDist = nmax; + for (PxU32 i = 0; i < nbCandiates; ++i) { - if(!chosen[i]) + v = V3Sub(manifoldPoints[candidates[i]].mLocalPointB, newManifold[0].mLocalPointB); + const FloatV d = V3Dot(v, norm); + if(FAllGrtr(d, maxDist)) { - v = V3Sub(manifoldPoints[i].mLocalPointB, newManifold[0].mLocalPointB); - const FloatV d = V3Dot(v, norm); - if(FAllGrtr(d, maxDis)) - { - maxDis = d; - index1 = i; - } + maxDist = d; + index1 = candidates[i]; + candidateIndex1 = i; } } - } - newManifold[3]= manifoldPoints[index1]; - chosen[index1] = true; + chosenIndices[3] = PxU8(index1); + //move the chosen indices out of the candidates indices + nbCandiates = nbCandiates - 1; + candidates[candidateIndex1] = candidates[nbCandiates]; - maxDis = max; - index = 0xffffffff; + newManifold[3]= manifoldPoints[chosenIndices[3]]; + + maxDist = max; + index = GU_MANIFOLD_INVALID_INDEX; + candidateIndex = GU_MANIFOLD_INVALID_INDEX; //choose the 5 point, second deepest in the left overlap point - for (PxU32 i = 0; i < numPoints; ++i) + for (PxU32 i = 0; i < nbCandiates; ++i) { - if (!chosen[i]) + const FloatV pen = V4GetW(manifoldPoints[candidates[i]].mLocalNormalPen); + if (FAllGrtr(maxDist, pen)) { - const FloatV pen = V4GetW(manifoldPoints[i].mLocalNormalPen); - if (FAllGrtr(maxDis, pen)) - { - maxDis = pen; - index = i; - } + maxDist = pen; + index = candidates[i]; + candidateIndex = i; } + } - newManifold[4] = manifoldPoints[index]; - chosen[index] = true; + chosenIndices[4] = PxU8(index); + //move the chosen indices out of the candidates indices + nbCandiates = nbCandiates - 1; + candidates[candidateIndex] = candidates[nbCandiates]; + + newManifold[4] = manifoldPoints[chosenIndices[4]]; + //copy the new manifold back for(PxU32 i=0; i<GU_SINGLE_MANIFOLD_SINGLE_POLYGONE_CACHE_SIZE; ++i) @@ -1854,7 +1961,6 @@ PxU32 Gu::SinglePersistentContactManifold::reduceContacts(MeshPersistentContact* return GU_SINGLE_MANIFOLD_SINGLE_POLYGONE_CACHE_SIZE; } - Ps::aos::FloatV Gu::SinglePersistentContactManifold::refreshContactPoints(const Ps::aos::PsMatTransformV& aToB, const Ps::aos::FloatVArg projectBreakingThreshold, const Ps::aos::FloatVArg /*contactOffset*/) { using namespace Ps::aos; diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPersistentContactManifold.h b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPersistentContactManifold.h index e06622da..b2491174 100644 --- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPersistentContactManifold.h +++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPersistentContactManifold.h @@ -52,6 +52,8 @@ namespace physx #define GU_MAX_MANIFOLD_SIZE 6 #define GU_MESH_CONTACT_REDUCTION_THRESHOLD 16 +#define GU_MANIFOLD_INVALID_INDEX 0xffffffff + //ML: this is used to compared with the shape's margin to decide the final tolerance used in the manifold to validate the existing contacts. //In the case of big shape and relatively speaking small triangles in the mesh, we need to take a smaller margin. This helps because the PCM @@ -317,13 +319,13 @@ public: void addBatchManifoldContacts2( const PersistentContact* manifoldPoints, const PxU32 numPoints);//max two points of contacts //This function is used in the box/convexhull full manifold contact generation(maximum 4 points). - void addBatchManifoldContacts(const PersistentContact* manifoldPoints, const PxU32 numPoints); + void addBatchManifoldContacts(const PersistentContact* manifoldPoints, const PxU32 numPoints, const PxReal toleranceLength); //This function is using the cluster algorithm to reduce contacts void reduceBatchContactsCluster(const PersistentContact* manifoldPoints, const PxU32 numPoints); //This function is called by addBatchManifoldContacts2 to reduce the manifold contacts to 2 points; void reduceBatchContacts2(const PersistentContact* manifoldPoints, const PxU32 numPoints); //This function is called by addBatchManifoldContacts to reduce the manifold contacts to 4 points - void reduceBatchContacts(const PersistentContact* manifoldPoints, const PxU32 numPoints); + void reduceBatchContacts(const PersistentContact* manifoldPoints, const PxU32 numPoints, const PxReal toleranceLength); //This function is used for incremental manifold contact reduction for box/convexhull PxU32 reduceContactsForPCM(const Ps::aos::Vec3VArg localPointA, const Ps::aos::Vec3VArg localPointB, const Ps::aos::Vec4VArg localNormalPen); diff --git a/PhysX_3.4/Source/GeomUtils/src/sweep/GuSweepCapsuleTriangle.cpp b/PhysX_3.4/Source/GeomUtils/src/sweep/GuSweepCapsuleTriangle.cpp index dc4ce4c2..26e0d101 100644 --- a/PhysX_3.4/Source/GeomUtils/src/sweep/GuSweepCapsuleTriangle.cpp +++ b/PhysX_3.4/Source/GeomUtils/src/sweep/GuSweepCapsuleTriangle.cpp @@ -140,7 +140,10 @@ bool Gu::sweepCapsuleTriangles_Precise( PxU32 nbTris, const PxTriangle* PX_RESTR continue; if(testInitialOverlap && intersectCapsuleTriangle(triNormal, currentTri.verts[0], currentTri.verts[1], currentTri.verts[2], capsule, params)) + { + triNormalOut = -unitDir; return setInitialOverlapResults(hit, unitDir, i); + } const PxReal magnitude = triNormal.magnitude(); if(magnitude==0.0f) @@ -211,7 +214,10 @@ bool Gu::sweepCapsuleTriangles_Precise( PxU32 nbTris, const PxTriangle* PX_RESTR } if(testInitialOverlap && intersectCapsuleTriangle(denormalizedNormal, currentSrcTri.verts[0], currentSrcTri.verts[1], currentSrcTri.verts[2], capsule, params)) + { + triNormalOut = -unitDir; return setInitialOverlapResults(hit, unitDir, i); + } // Extrude mesh on the fly PxU32 nbExtrudedTris=0; diff --git a/PhysX_3.4/Source/GeomUtils/src/sweep/GuSweepSphereCapsule.cpp b/PhysX_3.4/Source/GeomUtils/src/sweep/GuSweepSphereCapsule.cpp index aaf56990..02b9c018 100644 --- a/PhysX_3.4/Source/GeomUtils/src/sweep/GuSweepSphereCapsule.cpp +++ b/PhysX_3.4/Source/GeomUtils/src/sweep/GuSweepSphereCapsule.cpp @@ -69,7 +69,7 @@ bool Gu::sweepSphereCapsule(const Sphere& sphere, const Capsule& lss, const PxVe Capsule Inflated(lss.p0, lss.p1, radiusSum); // Raycast against it - PxReal t; + PxReal t = 0.0f; if(intersectRayCapsule(sphere.center, dir, Inflated, t)) { if(t>=0.0f && t<=length) diff --git a/PhysX_3.4/Source/GeomUtils/src/sweep/GuSweepSphereTriangle.cpp b/PhysX_3.4/Source/GeomUtils/src/sweep/GuSweepSphereTriangle.cpp index abbf68b4..4ec6be61 100644 --- a/PhysX_3.4/Source/GeomUtils/src/sweep/GuSweepSphereTriangle.cpp +++ b/PhysX_3.4/Source/GeomUtils/src/sweep/GuSweepSphereTriangle.cpp @@ -326,7 +326,10 @@ bool Gu::sweepSphereTriangles( PxU32 nbTris, const PxTriangle* PX_RESTRICT trian continue; if(currentDistance==0.0f) + { + triNormalOut = -unitDir; return setInitialOverlapResults(h, unitDir, i); + } curT = currentDistance; index = i; |