From 3dfe2108cfab31ba3ee5527e217d0d8e99a51162 Mon Sep 17 00:00:00 2001 From: git perforce import user Date: Tue, 25 Oct 2016 12:29:14 -0600 Subject: Initial commit: PhysX 3.4.0 Update @ 21294896 APEX 1.4.0 Update @ 21275617 [CL 21300167] --- .../GeomUtils/src/ccd/GuCCDSweepConvexMesh.cpp | 733 +++++++++++++++++++++ 1 file changed, 733 insertions(+) create mode 100644 PhysX_3.4/Source/GeomUtils/src/ccd/GuCCDSweepConvexMesh.cpp (limited to 'PhysX_3.4/Source/GeomUtils/src/ccd/GuCCDSweepConvexMesh.cpp') diff --git a/PhysX_3.4/Source/GeomUtils/src/ccd/GuCCDSweepConvexMesh.cpp b/PhysX_3.4/Source/GeomUtils/src/ccd/GuCCDSweepConvexMesh.cpp new file mode 100644 index 00000000..3d3d9e36 --- /dev/null +++ b/PhysX_3.4/Source/GeomUtils/src/ccd/GuCCDSweepConvexMesh.cpp @@ -0,0 +1,733 @@ +// This code contains NVIDIA Confidential Information and is disclosed to you +// under a form of NVIDIA software license agreement provided separately to you. +// +// Notice +// NVIDIA Corporation and its licensors retain all intellectual property and +// proprietary rights in and to this software and related documentation and +// any modifications thereto. Any use, reproduction, disclosure, or +// distribution of this software and related documentation without an express +// license agreement from NVIDIA Corporation is strictly prohibited. +// +// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES +// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO +// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT, +// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE. +// +// Information and code furnished is believed to be accurate and reliable. +// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such +// information or for any infringement of patents or other rights of third parties that may +// result from its use. No license is granted by implication or otherwise under any patent +// or patent rights of NVIDIA Corporation. Details are subject to change without notice. +// This code supersedes and replaces all information previously supplied. +// NVIDIA Corporation products are not authorized for use as critical +// components in life support devices or systems without express written approval of +// NVIDIA Corporation. +// +// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved. +// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved. +// Copyright (c) 2001-2004 NovodeX AG. All rights reserved. + +#include "Ps.h" +#include "GuVecCapsule.h" +#include "GuVecBox.h" +#include "GuVecConvexHull.h" +#include "GuVecTriangle.h" +#include "GuVecShrunkConvexHull.h" +#include "GuVecShrunkBox.h" +#include "GuGJKRaycast.h" +#include "GuCCDSweepConvexMesh.h" +#include "GuHeightFieldUtil.h" +#include "PsInlineArray.h" +#include "GuEntityReport.h" +#include "PxContact.h" +#include "GuDistancePointTriangle.h" +#include "GuBox.h" +#include "GuInternal.h" +#include "GuBoxConversion.h" +#include "GuConvexUtilsInternal.h" +#include "GuMidphaseInterface.h" +#include "GuTriangleVertexPointers.h" + + + +namespace physx +{ +namespace Gu +{ + +PxReal SweepShapeTriangle(GU_TRIANGLE_SWEEP_METHOD_ARGS); + +using namespace Ps::aos; + +namespace +{ +struct AccumCallback: public MeshHitCallback +{ + PX_NOCOPY(AccumCallback) + public: + + Ps::InlineArray& mResult; + AccumCallback(Ps::InlineArray& result) + : MeshHitCallback(CallbackMode::eMULTIPLE), + mResult(result) + { + } + + virtual PxAgain processHit( // all reported coords are in mesh local space including hit.position + const PxRaycastHit& hit, const PxVec3&, const PxVec3&, const PxVec3&, PxReal&, const PxU32*) + { + mResult.pushBack(hit.faceIndex); + return true; + } +}; + +// PT: TODO: refactor with MidPhaseQueryLocalReport +struct EntityReportContainerCallback : public EntityReport +{ + Ps::InlineArray& container; + EntityReportContainerCallback(Ps::InlineArray& container_) : container(container_) + { + container.forceSize_Unsafe(0); + } + virtual ~EntityReportContainerCallback() {} + + virtual bool onEvent(PxU32 nb, PxU32* indices) + { + for(PxU32 i=0; iget()); + + Ps::InlineArray tempContainer; + + EntityReportContainerCallback callback(tempContainer); + + PxVec3 trA = transform0.p - lastTm0.p; + PxVec3 trB = transform1.p - lastTm1.p; + + PxVec3 relTr = trA - trB; + PxVec3 halfRelTr = relTr * 0.5f; + + const PxVec3 ext = shape0.mExtents + halfRelTr.abs() + PxVec3(restDistance); + const PxVec3 cent = shape0.mCenter + halfRelTr; + + PxBounds3 bounds0(cent - ext, cent + ext); + + hfUtil.overlapAABBTriangles(transform1, bounds0, GuHfQueryFlags::eWORLD_SPACE, &callback); + + Ps::Array orderedContainer(tempContainer.size()); + + Ps::Array distanceEntries(tempContainer.size()); + + PxU32* orderedList = orderedContainer.begin(); + PxF32* distances = reinterpret_cast(distanceEntries.begin()); + + PxVec3 origin = shape0.mCenter; + PxVec3 extent = shape0.mExtents + PxVec3(restDistance); + + PxReal minTOI = PX_MAX_REAL; + + PxU32 numTrigs = tempContainer.size(); + PxU32* trianglesIndices = tempContainer.begin(); + + PxU32 count = 0; + for(PxU32 a = 0; a < numTrigs; ++a) + { + PxTriangle tri; + hfUtil.getTriangle(shape1.mPrevTransform, tri, 0, 0, trianglesIndices[a], true, true); + + PxVec3 resultNormal = -(tri.verts[1]-tri.verts[0]).cross(tri.verts[2]-tri.verts[0]); + resultNormal.normalize(); + + if(relTr.dot(resultNormal) >= fastMovingThreshold) + { + + PxBounds3 bounds; + bounds.setEmpty(); + bounds.include(tri.verts[0]); + bounds.include(tri.verts[1]); + bounds.include(tri.verts[2]); + + PxF32 toi = sweepAABBAABB(origin, extent * 1.1f, bounds.getCenter(), (bounds.getExtents() + PxVec3(0.01f, 0.01f, 0.01f)) * 1.1f, trA, trB); + + PxU32 index = 0; + if(toi <= 1.f) + { + for(PxU32 b = count; b > 0; --b) + { + if(distances[b-1] <= toi) + { + //shuffle down and swap + index = b; + break; + } + PX_ASSERT(b > 0); + PX_ASSERT(b < numTrigs); + distances[b] = distances[b-1]; + orderedList[b] = orderedList[b-1]; + } + PX_ASSERT(index < numTrigs); + orderedList[index] = trianglesIndices[a]; + distances[index] = toi; + count++; + } + } + } + + + + worldNormal = PxVec3(PxReal(0)); + worldPoint = PxVec3(PxReal(0)); + Cm::FastVertex2ShapeScaling idScale; + PxU32 ccdFaceIndex = PXC_CONTACT_NO_FACE_INDEX; + + PxVec3 sphereCenter(shape0.mPrevTransform.p); + PxF32 inSphereRadius = shape0.mFastMovingThreshold; + PxF32 inRadSq = inSphereRadius * inSphereRadius; + + + PxVec3 sphereCenterInTr1 = transform1.transformInv(sphereCenter); + PxVec3 sphereCenterInTr1T0 = transform1.transformInv(lastTm0.p); + + PxVec3 tempWorldNormal(0.f), tempWorldPoint(0.f); + + for (PxU32 ti = 0; ti < count; ti++) + { + PxTriangle tri; + hfUtil.getTriangle(lastTm1, tri, 0, 0, orderedList[ti], false, false); + + PxVec3 resultNormal, resultPoint; + + TriangleV triangle(V3LoadU(tri.verts[0]), V3LoadU(tri.verts[1]), V3LoadU(tri.verts[2])); + + //do sweep + + PxReal res = SweepShapeTriangle( + *shape0.mGeometry, *shape1.mGeometry, transform0, transform1, lastTm0, lastTm1, restDistance, + resultNormal, resultPoint, Cm::FastVertex2ShapeScaling(), triangle, + 0.f); + + if(res <= 0.f) + { + res = 0.f; + + const PxVec3 v0 = tri.verts[1] - tri.verts[0] ; + const PxVec3 v1 = tri.verts[2] - tri.verts[0]; + + //Now we have a 0 TOI, lets see if the in-sphere hit it! + + PxF32 distanceSq = distancePointTriangleSquared( sphereCenterInTr1, tri.verts[0], v0, v1); + + if(distanceSq < inRadSq) + { + const PxVec3 nor = v0.cross(v1); + const PxF32 distance = PxSqrt(distanceSq); + res = distance - inSphereRadius; + const PxF32 d = nor.dot(tri.verts[0]); + const PxF32 dd = nor.dot(sphereCenterInTr1T0); + if((dd - d) > 0.f) + { + //back side, penetration + res = -(2.f * inSphereRadius - distance); + } + } + } + + if (res < minTOI) + { + const PxVec3 v0 = tri.verts[1] - tri.verts[0] ; + const PxVec3 v1 = tri.verts[2] - tri.verts[0]; + + PxVec3 resultNormal1 = v0.cross(v1); + resultNormal1.normalize(); + //if(norDotRel > 1e-6f) + { + tempWorldNormal = resultNormal1; + tempWorldPoint = resultPoint; + minTOI = res; + ccdFaceIndex = orderedList[ti]; + } + } + + } + + worldNormal = transform1.rotate(tempWorldNormal); + worldPoint = tempWorldPoint; + + outCCDFaceIndex = ccdFaceIndex; + + return minTOI; +} + + +PxReal SweepEstimateAnyShapeHeightfield(GU_SWEEP_ESTIMATE_ARGS) +{ + HeightFieldUtil hfUtil(shape1.mGeometry->get()); + + Ps::InlineArray tempContainer; + + EntityReportContainerCallback callback(tempContainer); + + PxVec3 trA = transform0.p - lastTr0.p; + PxVec3 trB = transform1.p - lastTr1.p; + + PxVec3 relTr = trA - trB; + PxVec3 halfRelTr = relTr * 0.5f; + + const PxVec3 extents = shape0.mExtents + halfRelTr.abs() + PxVec3(restDistance); + const PxVec3 center = shape0.mCenter + halfRelTr; + + + PxBounds3 bounds0(center - extents, center + extents); + + + hfUtil.overlapAABBTriangles(transform1, bounds0, GuHfQueryFlags::eWORLD_SPACE, &callback); + + PxVec3 origin = shape0.mCenter; + PxVec3 extent = shape0.mExtents; + + PxReal minTOI = PX_MAX_REAL; + + PxU32 numTrigs = tempContainer.size(); + PxU32* trianglesIndices = tempContainer.begin(); + + for(PxU32 a = 0; a < numTrigs; ++a) + { + + PxTriangle tri; + hfUtil.getTriangle(shape1.mPrevTransform, tri, 0, 0, trianglesIndices[a], true, true); + + + + PxVec3 resultNormal = -(tri.verts[1]-tri.verts[0]).cross(tri.verts[2]-tri.verts[0]); + resultNormal.normalize(); + + if(relTr.dot(resultNormal) >= fastMovingThreshold) + { + + PxBounds3 bounds; + bounds.setEmpty(); + bounds.include(tri.verts[0]); + bounds.include(tri.verts[1]); + bounds.include(tri.verts[2]); + + PxF32 toi = sweepAABBAABB(origin, extent * 1.1f, bounds.getCenter(), (bounds.getExtents() + PxVec3(0.01f, 0.01f, 0.01f)) * 1.1f, trA, trB); + + minTOI = PxMin(minTOI, toi); + } + } + + return minTOI; +} + + + +PxReal SweepAnyShapeMesh(GU_SWEEP_METHOD_ARGS) +{ + PX_UNUSED(toiEstimate); + // this is the trimesh midphase for convex vs mesh sweep. shape0 is the convex shape. + + // Get actual shape data + const PxTriangleMeshGeometryLL& shapeMesh = shape1.mGeometry->get(); + + const Cm::FastVertex2ShapeScaling meshScaling(shapeMesh.scale); + + /*---------------------------------------------------*\ + | + | STEP1: OPCODE Geometry collection + | + \*---------------------------------------------------*/ + + PxVec3 trA = transform0.p - lastTm0.p; + PxVec3 trB = transform1.p - lastTm1.p; + + PxVec3 relTr = trA - trB; + PxVec3 unitDir = relTr; + PxReal length = unitDir.normalize(); + + PxMat33 matRot(lastTm0.q); + + + //1) Compute the swept bounds + Box sweptBox; + computeSweptBox(sweptBox, shape0.mExtents, shape0.mCenter, matRot, unitDir, length); + + Box vertexSpaceBox; + if (shapeMesh.scale.isIdentity()) + vertexSpaceBox = transformBoxOrthonormal(sweptBox, transform1.getInverse()); + else + computeVertexSpaceOBB(vertexSpaceBox, sweptBox, transform1, shapeMesh.scale); + + + vertexSpaceBox.extents += PxVec3(restDistance); + + Ps::InlineArray tempContainer; + + AccumCallback callback(tempContainer); + + // AP scaffold: early out opportunities, should probably use fat raycast + Midphase::intersectOBB(shapeMesh.meshData, vertexSpaceBox, callback, true); + + if (tempContainer.size() == 0) + return PX_MAX_REAL; + + // Intersection found, fetch triangles + PxU32 numTrigs = tempContainer.size(); + const PxU32* triangleIndices = tempContainer.begin(); + + PxVec3 origin = shape0.mCenter; + PxVec3 extent = shape0.mExtents + PxVec3(restDistance); + + Ps::InlineArray orderedContainer; + orderedContainer.resize(tempContainer.size()); + + Ps::InlineArray distanceEntries; + distanceEntries.resize(tempContainer.size()); + + PxU32* orderedList = orderedContainer.begin(); + PxF32* distances = reinterpret_cast(distanceEntries.begin()); + + PxReal minTOI = PX_MAX_REAL; + + + PxU32 count = 0; + for(PxU32 a = 0; a < numTrigs; ++a) + { + PxU32 unused; + ConvexTriangles convexPartOfMesh1(shapeMesh, meshScaling, &triangleIndices[a], 1, &unused); + + PxVec3 resultNormal = -transform1.rotate(convexPartOfMesh1.getPolygonNormal(0)); + + if(relTr.dot(resultNormal) >= fastMovingThreshold) + { + PxBounds3 bounds; + convexPartOfMesh1.getBounds(bounds, lastTm1); + //OK, we have all 3 vertices, now calculate bounds... + + PxF32 toi = sweepAABBAABB(origin, extent, bounds.getCenter(), bounds.getExtents() + PxVec3(0.02f, 0.02f, 0.02f), trA, trB); + + PxU32 index = 0; + if(toi <= 1.f) + { + for(PxU32 b = count; b > 0; --b) + { + if(distances[b-1] <= toi) + { + //shuffle down and swap + index = b; + break; + } + PX_ASSERT(b > 0); + PX_ASSERT(b < numTrigs); + distances[b] = distances[b-1]; + orderedList[b] = orderedList[b-1]; + } + PX_ASSERT(index < numTrigs); + orderedList[index] = triangleIndices[a]; + distances[index] = toi; + count++; + } + } + } + + + + PxVec3 tempWorldNormal(0.f), tempWorldPoint(0.f); + + Cm::FastVertex2ShapeScaling idScale; + PxU32 ccdFaceIndex = PXC_CONTACT_NO_FACE_INDEX; + + PxVec3 sphereCenter(lastTm1.p); + PxF32 inSphereRadius = shape0.mFastMovingThreshold; + //PxF32 inRadSq = inSphereRadius * inSphereRadius; + + PxVec3 sphereCenterInTransform1 = transform1.transformInv(sphereCenter); + + PxVec3 sphereCenterInTransform0p = transform1.transformInv(lastTm0.p); + + + for (PxU32 ti = 0; ti < count /*&& PxMax(minTOI, 0.f) >= distances[ti]*/; ti++) + { + PxU32 unused; + ConvexTriangles convexPartOfMesh1(shapeMesh, meshScaling, &orderedList[ti], 1, &unused); + + PxVec3 resultNormal, resultPoint, v0l, v1l, v2l; + TriangleVertexPointers::getTriangleVerts(shapeMesh.meshData, orderedList[ti], v0l, v1l, v2l); + const bool flipNormal = meshScaling.flipsNormal(); + + const PxVec3 v0 = meshScaling * v0l; + const PxVec3 v1 = meshScaling * (flipNormal ? v2l : v1l); + const PxVec3 v2 = meshScaling * (flipNormal ? v1l : v2l); + + TriangleV triangle(V3LoadU(v0), V3LoadU(v1), V3LoadU(v2)); + + //do sweep + PxReal res = SweepShapeTriangle( + *shape0.mGeometry, *shape1.mGeometry, transform0, transform1, lastTm0, lastTm1, restDistance, + resultNormal, resultPoint, Cm::FastVertex2ShapeScaling(), triangle, + 0.f); + + resultNormal = -resultNormal; + + if(res <= 0.f) + { + res = 0.f; + + PxF32 inRad = inSphereRadius + restDistance; + PxF32 inRadSq = inRad*inRad; + + const PxVec3 vv0 = v1 - v0 ; + const PxVec3 vv1 = v2 - v0; + const PxVec3 nor = vv0.cross(vv1); + + //Now we have a 0 TOI, lets see if the in-sphere hit it! + + PxF32 distanceSq = distancePointTriangleSquared( sphereCenterInTransform1, v0, vv0, vv1); + + if(distanceSq < inRadSq) + { + const PxF32 distance = PxSqrt(distanceSq); + res = distance - inRad; + const PxF32 d = nor.dot(v0); + const PxF32 dd = nor.dot(sphereCenterInTransform0p); + if((dd - d) < 0.f) + { + //back side, penetration + res = -(2.f * inRad - distance); + } + } + PX_ASSERT(PxIsFinite(res)); + resultNormal = convexPartOfMesh1.getPolygonNormal(0); + } + + if (res < minTOI) + { + tempWorldNormal = resultNormal;//convexPartOfMesh1.getPolygonNormal(0);//transform1.rotate(convexPartOfMesh1.getPolygonNormal(0)); + tempWorldPoint = resultPoint; + minTOI = res; + ccdFaceIndex = orderedList[ti]; + } + + } + + worldNormal = transform1.rotate(tempWorldNormal); + worldPoint = tempWorldPoint; + outCCDFaceIndex = ccdFaceIndex; + return minTOI; +} + + +/** +\brief This code performs a conservative estimate of the TOI of a shape v mesh. +*/ +PxReal SweepEstimateAnyShapeMesh(GU_SWEEP_ESTIMATE_ARGS) +{ + // this is the trimesh midphase for convex vs mesh sweep. shape0 is the convex shape. + // Get actual shape data + const PxTriangleMeshGeometryLL& shapeMesh = shape1.mGeometry->get(); + + const Cm::FastVertex2ShapeScaling meshScaling(shapeMesh.scale); + + + /*---------------------------------------------------*\ + | + | STEP1: OPCODE Geometry collection + | + \*---------------------------------------------------*/ + + PxVec3 trA = transform0.p - lastTr0.p; + PxVec3 trB = transform1.p - lastTr1.p; + + PxVec3 relTr = trA - trB; + PxVec3 unitDir = relTr; + PxReal length = unitDir.normalize(); + + PxMat33 matRot(lastTr0.q); + + //1) Compute the swept bounds + Box sweptBox; + computeSweptBox(sweptBox, shape0.mExtents, shape0.mCenter, matRot, unitDir, length); + + Box vertexSpaceBox; + computeVertexSpaceOBB(vertexSpaceBox, sweptBox, transform1, shapeMesh.scale); + + vertexSpaceBox.extents += PxVec3(restDistance); + + // TODO: implement a cached mode that fetches the trigs from a cache rather than per opcode if there is little motion. + + struct CB : MeshHitCallback + { + PxReal minTOI; + PxReal sumFastMovingThresh; + const PxTriangleMeshGeometryLL& shapeMesh; + const Cm::FastVertex2ShapeScaling& meshScaling; + const PxVec3& relTr; + const PxVec3& trA; + const PxVec3& trB; + const PxTransform& transform1; + const PxVec3& origin; + const PxVec3& extent; + + CB(PxReal aSumFast, const PxTriangleMeshGeometryLL& aShapeMesh, const Cm::FastVertex2ShapeScaling& aMeshScaling, + const PxVec3& aRelTr, const PxVec3& atrA, const PxVec3& atrB, const PxTransform& aTransform1, const PxVec3& aOrigin, const PxVec3& aExtent) + : MeshHitCallback(CallbackMode::eMULTIPLE), + sumFastMovingThresh(aSumFast), shapeMesh(aShapeMesh), meshScaling(aMeshScaling), relTr(aRelTr), trA(atrA), trB(atrB), + transform1(aTransform1), origin(aOrigin), extent(aExtent) + { + minTOI = PX_MAX_REAL; + } + + virtual PxAgain processHit( // all reported coords are in mesh local space including hit.position + const PxRaycastHit& hit, const PxVec3&, const PxVec3&, const PxVec3&, PxReal& shrunkMaxT, const PxU32*) + { + PxU32 unused; + ConvexTriangles convexPartOfMesh1(shapeMesh, meshScaling, &hit.faceIndex, 1, &unused); + PxVec3 resultNormal = -transform1.rotate(convexPartOfMesh1.getPolygonNormal(0)); + if(relTr.dot(resultNormal) >= sumFastMovingThresh) + { + PxBounds3 bounds; + convexPartOfMesh1.getBounds(bounds, transform1); + //OK, we have all 3 vertices, now calculate bounds... + + PX_ASSERT(trB.x == 0.f); + PX_ASSERT(trB.y == 0.f); + PX_ASSERT(trB.z == 0.f); + PxF32 toi = sweepAABBAABB( + origin, extent * 1.1f, bounds.getCenter(), (bounds.getExtents() + PxVec3(0.01f, 0.01f, 0.01f)) * 1.1f, trA, trB); + + minTOI = PxMin(minTOI, toi); + shrunkMaxT = minTOI; + } + + return (minTOI > 0.0f); // stop traversal if minTOI == 0.0f + } + + void operator=(const CB&) {} + }; + + PxVec3 origin = shape0.mCenter; + PxVec3 extent = shape0.mExtents + PxVec3(restDistance); + + CB callback(fastMovingThreshold, shapeMesh, meshScaling, relTr, trA, trB, transform1, origin, extent); + Midphase::intersectOBB(shapeMesh.meshData, vertexSpaceBox, callback, true); + + return callback.minTOI; +} + + +} +} + -- cgit v1.2.3