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] --- PhysX_3.4/Source/GeomUtils/src/GuSweepMTD.cpp | 1183 +++++++++++++++++++++++++ 1 file changed, 1183 insertions(+) create mode 100644 PhysX_3.4/Source/GeomUtils/src/GuSweepMTD.cpp (limited to 'PhysX_3.4/Source/GeomUtils/src/GuSweepMTD.cpp') diff --git a/PhysX_3.4/Source/GeomUtils/src/GuSweepMTD.cpp b/PhysX_3.4/Source/GeomUtils/src/GuSweepMTD.cpp new file mode 100644 index 00000000..d94198b3 --- /dev/null +++ b/PhysX_3.4/Source/GeomUtils/src/GuSweepMTD.cpp @@ -0,0 +1,1183 @@ +// 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 "GuHeightFieldUtil.h" +#include "GuEntityReport.h" +#include "PxConvexMeshGeometry.h" +#include "GuConvexMesh.h" +#include "GuSweepSharedTests.h" +#include "GuConvexUtilsInternal.h" +#include "GuTriangleMesh.h" +#include "GuVecBox.h" +#include "GuVecTriangle.h" +#include "GuVecConvexHullNoScale.h" +#include "GuMidphaseInterface.h" +#include "GuPCMContactConvexCommon.h" +#include "GuSweepMTD.h" +#include "GuPCMShapeConvex.h" +#include "GuDistanceSegmentSegment.h" +#include "GuDistancePointSegment.h" +#include "GuInternal.h" +#include "GuConvexEdgeFlags.h" + +using namespace physx; +using namespace Gu; + +static PX_FORCE_INLINE void getScaledTriangle(const PxTriangleMeshGeometry& triGeom, const Cm::Matrix34& vertex2worldSkew, bool flipsNormal, PxTriangle& triangle, PxTriangleID triangleIndex) +{ + TriangleMesh* tm = static_cast(triGeom.triangleMesh); + tm->computeWorldTriangle(triangle, triangleIndex, vertex2worldSkew, flipsNormal); +} + +#define BATCH_TRIANGLE_NUMBER 32u + +struct MTDTriangle : public PxTriangle +{ +public: + PxU8 extraTriData;//active edge flag data +}; + +struct MeshMTDGenerationCallback : MeshHitCallback +{ +public: + + Ps::Array& container; + + MeshMTDGenerationCallback(Ps::Array& tempContainer) + : MeshHitCallback(CallbackMode::eMULTIPLE), container(tempContainer) + { + } + + virtual PxAgain processHit( + const PxRaycastHit& hit, const PxVec3&, const PxVec3&, const PxVec3&, PxReal&, const PxU32*) + { + container.pushBack(hit.faceIndex); + + return true; + } + + void operator=(const MeshMTDGenerationCallback&) {} +}; + +static bool getMTDPerTriangle(const MeshPersistentContact* manifoldContacts, const PxU32 numContacts, const PxU32 triangleIndex, Ps::aos::Vec3V& normal, Ps::aos::Vec3V& closestA, Ps::aos::Vec3V& closestB, PxU32& faceIndex, Ps::aos::FloatV& deepestPen) +{ + using namespace Ps::aos; + + FloatV deepest = V4GetW(manifoldContacts[0].mLocalNormalPen); + PxU32 index = 0; + for(PxU32 k=1; k& tempContainer) +{ + TriangleMesh* meshData = static_cast(meshGeom.triangleMesh); + + Box vertexSpaceBox; + computeVertexSpaceOBB(vertexSpaceBox, bound, pose, meshGeom.scale); + + MeshMTDGenerationCallback callback(tempContainer); + Midphase::intersectOBB(meshData, vertexSpaceBox, callback, true); +} + +// PT: TODO: refactor with EntityReportContainerCallback +struct MidPhaseQueryLocalReport : EntityReport +{ + MidPhaseQueryLocalReport(Ps::Array& _container) : container(_container) + { + + } + virtual bool onEvent(PxU32 nb, PxU32* indices) + { + for(PxU32 i=0; i& container; + +private: + MidPhaseQueryLocalReport operator=(MidPhaseQueryLocalReport& report); +} ; + +static void midPhaseQuery(const HeightFieldUtil& hfUtil, const PxTransform& pose, const PxBounds3& bounds, Ps::Array& tempContainer, PxU32 flags) +{ + MidPhaseQueryLocalReport localReport(tempContainer); + hfUtil.overlapAABBTriangles(pose, bounds, flags, &localReport); +} + +static bool calculateMTD( const CapsuleV& capsuleV, const Ps::aos::FloatVArg inflatedRadiusV, const bool isDoubleSide, const MTDTriangle* triangles, const PxU32 nbTriangles, const PxU32 startIndex, MeshPersistentContact* manifoldContacts, + PxU32& numContacts, Ps::aos::Vec3V& normal, Ps::aos::Vec3V& closestA, Ps::aos::Vec3V& closestB, PxU32& faceIndex, Ps::aos::FloatV& mtd) +{ + using namespace Ps::aos; + const FloatV zero = FZero(); + bool hadContacts = false; + FloatV deepestPen = mtd; + + for(PxU32 j=0; j(triMeshGeom.triangleMesh); + const PxU8* extraTrigData = triMesh->getExtraTrigData(); + const bool flipsNormal = triMeshGeom.scale.hasNegativeDeterminant(); + + MeshPersistentContact manifoldContacts[64]; + PxU32 numContacts = 0; + //inflated the capsule by 15% in case of some disagreement between sweep and mtd calculation. If sweep said initial overlap, but mtd has a positive separation, + //we are still be able to return a valid normal but we should zero the distance. + const FloatV inflatedRadiusV = FLoad(inflatedRadius*1.15f); + + bool foundInitial = false; + const PxU32 iterations = 4; + + const Cm::Matrix34 vertexToWorldSkew = pose * triMeshGeom.scale; + + Ps::Array tempContainer; + tempContainer.reserve(128); + + Vec3V closestA = zeroV, closestB = zeroV, normal = zeroV; + PxU32 triangleIndex = 0xfffffff; + + Vec3V translation = zeroV; + FloatV mtd; + FloatV distV; + for(PxU32 i=0; i tempContainer; + tempContainer.reserve(128); + + const HeightFieldUtil hfUtil(heightFieldGeom); + + Vec3V closestA = zeroV, closestB = zeroV, normal = zeroV; + PxU32 triangleIndex = 0xfffffff; + + Vec3V translation = zeroV; + FloatV mtd; + FloatV distV; + for(PxU32 i=0; i(triMeshGeom.triangleMesh); + const PxU8* extraTrigData = triMesh->getExtraTrigData(); + + const Vec3V zeroV = V3Zero(); + + MeshPersistentContact manifoldContacts[64]; + PxU32 numContacts = 0; + + bool foundInitial = false; + const PxU32 iterations = 4; + + Ps::Array tempContainer; + tempContainer.reserve(128); + + Vec3V closestA = zeroV, closestB = zeroV, normal = zeroV; + Vec3V worldNormal = zeroV, worldContactA = zeroV;//, worldContactB = zeroV; + PxU32 triangleIndex = 0xfffffff; + + Vec3V translation = zeroV; + + Box box = _box; + + const QuatV q0 = QuatVLoadU(&boxTransform.q.x); + const Vec3V p0 = V3LoadU(&boxTransform.p.x); + + const Vec3V boxExtents = V3LoadU(box.extents); + const FloatV minMargin = CalculatePCMBoxMargin(boxExtents); + const FloatV inflationV = FAdd(FLoad(inflation), minMargin); + PxReal boundInflation; + FStore(inflationV, &boundInflation); + + box.extents += PxVec3(boundInflation); + const BoxV boxV(zeroV, boxExtents); + + Vec3V boxCenter = V3LoadU(box.center); + + //create the polyData based on the original data + PolygonalData polyData; + const PCMPolygonalBox polyBox(_box.extents); + polyBox.getPolygonalData(&polyData); + + const Mat33V identity = M33Identity(); + + const Cm::Matrix34 meshToWorldSkew = pose * triMeshGeom.scale; + + PsTransformV boxTransformV(p0, q0);//box + + FloatV mtd; + FloatV distV; + for(PxU32 i=0; i boxMap(boxV, boxTransformV, identity, identity, true); + + boxMap.setShapeSpaceCenterofMass(zeroV); + // Move to AABB space + Cm::Matrix34 WorldToBox; + computeWorldToBoxMatrix(WorldToBox, box); + const Cm::Matrix34 meshToBox = WorldToBox*meshToWorldSkew; + + const Ps::aos::Mat33V rot(V3LoadU(meshToBox.m.column0), V3LoadU(meshToBox.m.column1), V3LoadU(meshToBox.m.column2)); + const Ps::aos::PsMatTransformV meshToConvex(V3LoadU(meshToBox.p), rot); + + bool hadContacts = false; + + const PxU32 nbBatches = (nbTriangles + BATCH_TRIANGLE_NUMBER - 1)/BATCH_TRIANGLE_NUMBER; + mtd = FMax(); + MTDTriangle triangles[BATCH_TRIANGLE_NUMBER]; + for(PxU32 a = 0; a < nbBatches; ++a) + { + const PxU32 startIndex = a * BATCH_TRIANGLE_NUMBER; + const PxU32 nbTrigs = PxMin(nbTriangles - startIndex, BATCH_TRIANGLE_NUMBER); + for(PxU32 k=0; kgetLocalTriangle(triangles[k], triangleIndex1, triMeshGeom.scale.hasNegativeDeterminant()); + triangles[k].extraTriData = getConvexEdgeFlags(extraTrigData, triangleIndex1); + } + + //ML: mtd has back face culling, so if the capsule's center is below the triangle, we won't generate any contacts + hadContacts = calculateMTD(polyData, &boxMap, boxTransformV, meshToConvex, isDoubleSided, inflationV, triangles, nbTrigs, startIndex, manifoldContacts, numContacts, normal, closestA, closestB, triangleIndex, mtd) || hadContacts; + } + + if(!hadContacts) + break; + + triangleIndex = tempContainer[triangleIndex]; + + foundInitial = true; + + distV = mtd; + worldNormal = boxTransformV.rotate(normal); + worldContactA = boxTransformV.transform(closestA); + if(FAllGrtrOrEq(FZero(), distV)) + { + const Vec3V t = V3Scale(worldNormal, mtd); + translation = V3Sub(translation, t); + boxCenter = V3Sub(boxCenter, t); + V3StoreU(boxCenter, box.center); + } + else + { + if(i == 0) + { + //First iteration so keep this normal + hit.distance = 0.f; + V3StoreU(worldContactA, hit.position); + V3StoreU(worldNormal, hit.normal); + hit.faceIndex = triangleIndex; + return true; + } + break; + } + } + + worldNormal = V3Normalize(translation); + distV = FNeg(V3Length(translation)); + if(foundInitial) + { + //transform closestA to world space + FStore(distV, &hit.distance); + V3StoreU(worldContactA, hit.position); + V3StoreU(worldNormal, hit.normal); + hit.faceIndex = triangleIndex; + } + return foundInitial; +} + +bool physx::Gu::computeBox_HeightFieldMTD( const PxHeightFieldGeometry& heightFieldGeom, const PxTransform& pose, const Box& _box, const PxTransform& boxTransform, PxReal inflation, + bool isDoubleSided, const PxU32 flags, PxSweepHit& hit) +{ + using namespace Ps::aos; + + const Vec3V zeroV = V3Zero(); + MeshPersistentContact manifoldContacts[64]; + PxU32 numContacts = 0; + bool foundInitial = false; + const PxU32 iterations = 4; + + Ps::Array tempContainer; + tempContainer.reserve(128); + const HeightFieldUtil hfUtil(heightFieldGeom); + + Vec3V closestA = zeroV, closestB = zeroV, normal = zeroV; + Vec3V worldNormal = zeroV, worldContactA = zeroV;//, worldContactB = zeroV; + PxU32 triangleIndex = 0xfffffff; + + Vec3V translation = zeroV; + + Box box = _box; + + const QuatV q0 = QuatVLoadU(&boxTransform.q.x); + const Vec3V p0 = V3LoadU(&boxTransform.p.x); + + const Vec3V boxExtents = V3LoadU(box.extents); + const FloatV minMargin = CalculatePCMBoxMargin(boxExtents); + const FloatV inflationV = FAdd(FLoad(inflation), minMargin); + //const FloatV inflationV = FLoad(inflation); + + PxReal boundInflation; + FStore(inflationV, &boundInflation); + box.extents += PxVec3(boundInflation); + + const BoxV boxV(zeroV, boxExtents); + + Vec3V boxCenter = V3LoadU(box.center); + + //create the polyData based on the original box + PolygonalData polyData; + const PCMPolygonalBox polyBox(_box.extents); + polyBox.getPolygonalData(&polyData); + + const Mat33V identity = M33Identity(); + + const Cm::Matrix34 meshToWorldSkew(pose); + + PsTransformV boxTransformV(p0, q0);//box + + FloatV mtd; + FloatV distV; + for(PxU32 i=0; i boxMap(boxV, boxTransformV, identity, identity, true); + boxMap.setShapeSpaceCenterofMass(zeroV); + // Move to AABB space + Cm::Matrix34 WorldToBox; + computeWorldToBoxMatrix(WorldToBox, box); + const Cm::Matrix34 meshToBox = WorldToBox*meshToWorldSkew; + + const Ps::aos::Mat33V rot(V3LoadU(meshToBox.m.column0), V3LoadU(meshToBox.m.column1), V3LoadU(meshToBox.m.column2)); + const Ps::aos::PsMatTransformV meshToConvex(V3LoadU(meshToBox.p), rot); + + bool hadContacts = false; + + const PxU32 nbBatches = (nbTriangles + BATCH_TRIANGLE_NUMBER-1)/BATCH_TRIANGLE_NUMBER; + mtd = FMax(); + MTDTriangle triangles[BATCH_TRIANGLE_NUMBER]; + for(PxU32 a = 0; a < nbBatches; ++a) + { + const PxU32 startIndex = a * BATCH_TRIANGLE_NUMBER; + const PxU32 nbTrigs = PxMin(nbTriangles - startIndex, BATCH_TRIANGLE_NUMBER); + for(PxU32 k=0; k(triMeshGeom.triangleMesh); + ConvexMesh* cm = static_cast(convexGeom.convexMesh); + const PxU8* extraTrigData = triMesh->getExtraTrigData(); + + MeshPersistentContact manifoldContacts[64]; + PxU32 numContacts = 0; + + bool foundInitial = false; + const PxU32 iterations = 2; + + ConvexHullData* hullData = &cm->getHull(); + + const bool idtScaleConvex = convexGeom.scale.isIdentity(); + + Cm::FastVertex2ShapeScaling convexScaling; + if(!idtScaleConvex) + convexScaling.init(convexGeom.scale); + + const PxVec3 _shapeSpaceCenterOfMass = convexScaling * hullData->mCenterOfMass; + const Vec3V shapeSpaceCenterOfMass = V3LoadU(_shapeSpaceCenterOfMass); + + const QuatV q0 = QuatVLoadU(&convexPose.q.x); + const Vec3V p0 = V3LoadU(&convexPose.p.x); + PsTransformV convexTransformV(p0, q0); + + const Vec3V vScale = V3LoadU_SafeReadW(convexGeom.scale.scale); // PT: safe because 'rotation' follows 'scale' in PxMeshScale + const QuatV vQuat = QuatVLoadU(&convexGeom.scale.rotation.x); + ConvexHullV convexHull(hullData, V3Zero(), vScale, vQuat, idtScaleConvex); + PX_ALIGN(16, PxU8 convexBuff[sizeof(SupportLocalImpl)]); + + const FloatV convexMargin = CalculatePCMConvexMargin(hullData, vScale); + const FloatV inflationV = FAdd(FLoad(inflation), convexMargin); + PxReal boundInflation; + FStore(inflationV, &boundInflation); + + Ps::Array tempContainer; + tempContainer.reserve(128); + + Vec3V closestA = zeroV, closestB = zeroV, normal = zeroV; + PxU32 triangleIndex = 0xfffffff; + + Vec3V translation = zeroV; + + const Cm::Matrix34 meshToWorldSkew = pose * triMeshGeom.scale; + + PolygonalData polyData; + getPCMConvexData(convexHull, idtScaleConvex, polyData); + + FloatV mtd; + FloatV distV; + Vec3V center = p0; + PxTransform tempConvexPose = convexPose; + Vec3V worldNormal = zeroV, worldContactA = zeroV;//, worldContactB = zeroV; + + for(PxU32 i=0; i(PX_PLACEMENT_NEW(convexBuff, SupportLocalImpl)(static_cast(convexHull), convexTransformV, convexHull.vertex2Shape, convexHull.shape2Vertex, idtScaleConvex)) : + static_cast(PX_PLACEMENT_NEW(convexBuff, SupportLocalImpl)(convexHull, convexTransformV, convexHull.vertex2Shape, convexHull.shape2Vertex, idtScaleConvex))); + + convexMap->setShapeSpaceCenterofMass(shapeSpaceCenterOfMass); + + Box hullOBB; + computeOBBAroundConvex(hullOBB, convexGeom, cm, tempConvexPose); + + hullOBB.extents += PxVec3(boundInflation); + + midPhaseQuery(triMeshGeom, pose, hullOBB, tempContainer); + + // Get results + const PxU32 nbTriangles = tempContainer.size(); + if(!nbTriangles) + break; + + // Move to AABB space + const Cm::Matrix34 worldToConvex(tempConvexPose.getInverse()); + const Cm::Matrix34 meshToConvex = worldToConvex*meshToWorldSkew; + + const Ps::aos::Mat33V rot(V3LoadU(meshToConvex.m.column0), V3LoadU(meshToConvex.m.column1), V3LoadU(meshToConvex.m.column2)); + const Ps::aos::PsMatTransformV meshToConvexV(V3LoadU(meshToConvex.p), rot); + + bool hadContacts = false; + + const PxU32 nbBatches = (nbTriangles + BATCH_TRIANGLE_NUMBER-1)/BATCH_TRIANGLE_NUMBER; + mtd = FMax(); + MTDTriangle triangles[BATCH_TRIANGLE_NUMBER]; + for(PxU32 a = 0; a < nbBatches; ++a) + { + const PxU32 startIndex = a * BATCH_TRIANGLE_NUMBER; + const PxU32 nbTrigs = PxMin(nbTriangles - startIndex, BATCH_TRIANGLE_NUMBER); + for(PxU32 k=0; kgetLocalTriangle(triangles[k], triangleIndex1, triMeshGeom.scale.hasNegativeDeterminant()); + triangles[k].extraTriData = getConvexEdgeFlags(extraTrigData, triangleIndex1); + } + + //ML: mtd has back face culling, so if the capsule's center is below the triangle, we won't generate any contacts + hadContacts = calculateMTD(polyData, convexMap, convexTransformV, meshToConvexV, isDoubleSided, inflationV, triangles, nbTrigs, startIndex, manifoldContacts, numContacts, normal, closestA, closestB, triangleIndex, mtd) || hadContacts; + } + + if(!hadContacts) + break; + + triangleIndex = tempContainer[triangleIndex]; + + foundInitial = true; + + distV = mtd; + worldNormal = convexTransformV.rotate(normal); + worldContactA = convexTransformV.transform(closestA); + if(FAllGrtrOrEq(FZero(), distV)) + { + const Vec3V t = V3Scale(worldNormal, mtd); + translation = V3Sub(translation, t); + center = V3Sub(center, t); + } + else + { + if(i == 0) + { + //First iteration so keep this normal + hit.distance = 0.f; + V3StoreU(worldContactA, hit.position); + V3StoreU(worldNormal, hit.normal); + hit.faceIndex = triangleIndex; + return true; + } + break; + } + } + + worldNormal = V3Normalize(translation); + distV = FNeg(V3Length(translation)); + if(foundInitial) + { + //transform closestA to world space + FStore(distV, &hit.distance); + V3StoreU(worldContactA, hit.position); + V3StoreU(worldNormal, hit.normal); + hit.faceIndex = triangleIndex; + } + return foundInitial; +} + +bool physx::Gu::computeConvex_HeightFieldMTD( const PxHeightFieldGeometry& heightFieldGeom, const PxTransform& pose, const PxConvexMeshGeometry& convexGeom, const PxTransform& convexPose, PxReal inflation, + bool isDoubleSided, const PxU32 flags, PxSweepHit& hit) +{ + using namespace Ps::aos; + + const HeightFieldUtil hfUtil(heightFieldGeom); + + const Vec3V zeroV = V3Zero(); + MeshPersistentContact manifoldContacts[64]; + PxU32 numContacts = 0; + + bool foundInitial = false; + const PxU32 iterations = 2; + + ConvexMesh* cm = static_cast(convexGeom.convexMesh); + + ConvexHullData* hullData = &cm->getHull(); + + const bool idtScaleConvex = convexGeom.scale.isIdentity(); + + Cm::FastVertex2ShapeScaling convexScaling; + if(!idtScaleConvex) + convexScaling.init(convexGeom.scale); + + const PxVec3 _shapeSpaceCenterOfMass = convexScaling * hullData->mCenterOfMass; + const Vec3V shapeSpaceCenterOfMass = V3LoadU(_shapeSpaceCenterOfMass); + + const QuatV q0 = QuatVLoadU(&convexPose.q.x); + const Vec3V p0 = V3LoadU(&convexPose.p.x); + PsTransformV convexTransformV(p0, q0); + + const Vec3V vScale = V3LoadU_SafeReadW(convexGeom.scale.scale); // PT: safe because 'rotation' follows 'scale' in PxMeshScale + const QuatV vQuat = QuatVLoadU(&convexGeom.scale.rotation.x); + ConvexHullV convexHull(hullData, zeroV, vScale, vQuat, idtScaleConvex); + PX_ALIGN(16, PxU8 convexBuff[sizeof(SupportLocalImpl)]); + + const FloatV convexMargin = CalculatePCMConvexMargin(hullData, vScale); + const FloatV inflationV = FAdd(FLoad(inflation), convexMargin); + PxReal boundInflation; + FStore(inflationV, &boundInflation); + + Ps::Array tempContainer; + tempContainer.reserve(128); + + Vec3V closestA = zeroV, closestB = zeroV, normal = zeroV; + Vec3V worldNormal = zeroV, worldContactA = zeroV;//, worldContactB = zeroV; + PxU32 triangleIndex = 0xfffffff; + + Vec3V translation = zeroV; + + PolygonalData polyData; + getPCMConvexData(convexHull, idtScaleConvex, polyData); + + FloatV mtd; + FloatV distV; + Vec3V center = p0; + PxTransform tempConvexPose = convexPose; + const Cm::Matrix34 meshToWorldSkew(pose); + + for(PxU32 i=0; i(PX_PLACEMENT_NEW(convexBuff, SupportLocalImpl)(static_cast(convexHull), convexTransformV, convexHull.vertex2Shape, convexHull.shape2Vertex, idtScaleConvex)) : + static_cast(PX_PLACEMENT_NEW(convexBuff, SupportLocalImpl)(convexHull, convexTransformV, convexHull.vertex2Shape, convexHull.shape2Vertex, idtScaleConvex))); + + convexMap->setShapeSpaceCenterofMass(shapeSpaceCenterOfMass); + + Box hullOBB; + computeOBBAroundConvex(hullOBB, convexGeom, cm, tempConvexPose); + hullOBB.extents += PxVec3(boundInflation); + + const PxBounds3 bounds = PxBounds3::basisExtent(hullOBB.center, hullOBB.rot, hullOBB.extents); + + midPhaseQuery(hfUtil, pose, bounds, tempContainer, flags); + + // Get results + const PxU32 nbTriangles = tempContainer.size(); + + if(!nbTriangles) + break; + + // Move to AABB space + const Cm::Matrix34 worldToConvex(tempConvexPose.getInverse()); + const Cm::Matrix34 meshToConvex = worldToConvex*meshToWorldSkew; + + const Ps::aos::Mat33V rot(V3LoadU(meshToConvex.m.column0), V3LoadU(meshToConvex.m.column1), V3LoadU(meshToConvex.m.column2)); + const Ps::aos::PsMatTransformV meshToConvexV(V3LoadU(meshToConvex.p), rot); + + bool hadContacts = false; + + const PxU32 nbBatches = (nbTriangles + BATCH_TRIANGLE_NUMBER-1)/BATCH_TRIANGLE_NUMBER; + mtd = FMax(); + MTDTriangle triangles[BATCH_TRIANGLE_NUMBER]; + for(PxU32 a = 0; a < nbBatches; ++a) + { + const PxU32 startIndex = a * BATCH_TRIANGLE_NUMBER; + const PxU32 nbTrigs = PxMin(nbTriangles - startIndex, BATCH_TRIANGLE_NUMBER); + for(PxU32 k=0; k d) + { + index = i; + dmin = d; + } + } + hit.normal = plane.n; + hit.distance = dmin; + hit.position = pts[index] - plane.n*dmin; + return true; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +bool physx::Gu::computePlane_ConvexMTD(const PxPlane& plane, const PxConvexMeshGeometry& convexGeom, const PxTransform& convexPose, PxSweepHit& hit) +{ + const ConvexMesh* convexMesh = static_cast(convexGeom.convexMesh); + const Cm::FastVertex2ShapeScaling convexScaling(convexGeom.scale); + PxU32 nbVerts = convexMesh->getNbVerts(); + const PxVec3* PX_RESTRICT verts = convexMesh->getVerts(); + + PxVec3 worldPointMin = convexPose.transform(convexScaling * verts[0]); + PxReal dmin = plane.distance(worldPointMin); + for(PxU32 i=1;i d) + { + dmin = d; + worldPointMin = worldPoint; + } + } + + hit.normal = plane.n; + hit.distance = dmin; + hit.position = worldPointMin - plane.n * dmin; + return true; +} -- cgit v1.2.3