1152 lines
38 KiB
C++
1152 lines
38 KiB
C++
// Redistribution and use in source and binary forms, with or without
|
|
// modification, are permitted provided that the following conditions
|
|
// are met:
|
|
// * Redistributions of source code must retain the above copyright
|
|
// notice, this list of conditions and the following disclaimer.
|
|
// * Redistributions in binary form must reproduce the above copyright
|
|
// notice, this list of conditions and the following disclaimer in the
|
|
// documentation and/or other materials provided with the distribution.
|
|
// * Neither the name of NVIDIA CORPORATION nor the names of its
|
|
// contributors may be used to endorse or promote products derived
|
|
// from this software without specific prior written permission.
|
|
//
|
|
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
|
|
// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
|
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
|
|
// PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
|
|
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
|
|
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
|
|
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
|
|
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
|
|
// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
|
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
|
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|
//
|
|
// Copyright (c) 2008-2025 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 "geometry/PxConvexMeshGeometry.h"
|
|
#include "GuHeightFieldUtil.h"
|
|
#include "GuEntityReport.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"
|
|
#include "GuMTD.h"
|
|
#include "CmMatrix34.h"
|
|
|
|
using namespace physx;
|
|
using namespace Gu;
|
|
using namespace Cm;
|
|
using namespace aos;
|
|
|
|
#define BATCH_TRIANGLE_NUMBER 32u
|
|
|
|
struct MTDTriangle : public PxTriangle
|
|
{
|
|
public:
|
|
PxU8 extraTriData;//active edge flag data
|
|
};
|
|
|
|
struct MeshMTDGenerationCallback : MeshHitCallback<PxGeomRaycastHit>
|
|
{
|
|
public:
|
|
|
|
PxArray<PxU32>& container;
|
|
|
|
MeshMTDGenerationCallback(PxArray<PxU32>& tempContainer)
|
|
: MeshHitCallback<PxGeomRaycastHit>(CallbackMode::eMULTIPLE), container(tempContainer)
|
|
{
|
|
}
|
|
|
|
virtual PxAgain processHit(
|
|
const PxGeomRaycastHit& 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, Vec3V& normal, Vec3V& closestA, Vec3V& closestB, PxU32& faceIndex, FloatV& deepestPen)
|
|
{
|
|
FloatV deepest = V4GetW(manifoldContacts[0].mLocalNormalPen);
|
|
PxU32 index = 0;
|
|
for(PxU32 k=1; k<numContacts; ++k)
|
|
{
|
|
const FloatV pen = V4GetW(manifoldContacts[k].mLocalNormalPen);
|
|
if(FAllGrtr(deepest, pen))
|
|
{
|
|
deepest = pen;
|
|
index = k;
|
|
}
|
|
}
|
|
|
|
if(FAllGrtr(deepestPen, deepest))
|
|
{
|
|
PX_ASSERT(triangleIndex == manifoldContacts[index].mFaceIndex);
|
|
faceIndex = triangleIndex;
|
|
deepestPen = deepest;
|
|
normal = Vec3V_From_Vec4V(manifoldContacts[index].mLocalNormalPen);
|
|
closestA = manifoldContacts[index].mLocalPointB;
|
|
closestB = manifoldContacts[index].mLocalPointA;
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
static void midPhaseQuery(const PxTriangleMeshGeometry& meshGeom, const PxTransform& pose, const Box& bound, PxArray<PxU32>& tempContainer)
|
|
{
|
|
TriangleMesh* meshData = static_cast<TriangleMesh*>(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 : OverlapReport
|
|
{
|
|
MidPhaseQueryLocalReport(PxArray<PxU32>& _container) : container(_container)
|
|
{
|
|
|
|
}
|
|
virtual bool reportTouchedTris(PxU32 nb, const PxU32* indices)
|
|
{
|
|
for(PxU32 i=0; i<nb; i++)
|
|
container.pushBack(indices[i]);
|
|
return true;
|
|
}
|
|
|
|
PxArray<PxU32>& container;
|
|
|
|
private:
|
|
MidPhaseQueryLocalReport operator=(MidPhaseQueryLocalReport& report);
|
|
};
|
|
|
|
static void midPhaseQuery(const HeightFieldUtil& hfUtil, const PxTransform& pose, const PxBounds3& bounds, PxArray<PxU32>& tempContainer)
|
|
{
|
|
MidPhaseQueryLocalReport localReport(tempContainer);
|
|
hfUtil.overlapAABBTriangles(pose, bounds, localReport);
|
|
}
|
|
|
|
static bool calculateMTD( const CapsuleV& capsuleV, const FloatVArg inflatedRadiusV, const bool isDoubleSide, const MTDTriangle* triangles, const PxU32 nbTriangles, const PxU32 startIndex, MeshPersistentContact* manifoldContacts,
|
|
PxU32& numContacts, Vec3V& normal, Vec3V& closestA, Vec3V& closestB, PxU32& faceIndex, FloatV& mtd)
|
|
{
|
|
const FloatV zero = FZero();
|
|
bool hadContacts = false;
|
|
FloatV deepestPen = mtd;
|
|
|
|
for(PxU32 j=0; j<nbTriangles; ++j)
|
|
{
|
|
numContacts = 0;
|
|
|
|
const MTDTriangle& curTri = triangles[j];
|
|
TriangleV triangleV;
|
|
triangleV.verts[0] = V3LoadU(curTri.verts[0]);
|
|
triangleV.verts[1] = V3LoadU(curTri.verts[1]);
|
|
triangleV.verts[2] = V3LoadU(curTri.verts[2]);
|
|
const PxU8 triFlag = curTri.extraTriData;
|
|
|
|
const Vec3V triangleNormal = triangleV.normal();
|
|
const Vec3V v = V3Sub(capsuleV.getCenter(), triangleV.verts[0]);
|
|
const FloatV dotV = V3Dot(triangleNormal, v);
|
|
|
|
// Backface culling
|
|
const bool culled = !isDoubleSide && (FAllGrtr(zero, dotV));
|
|
if(culled)
|
|
continue;
|
|
|
|
PCMCapsuleVsMeshContactGeneration::processTriangle(triangleV, j+startIndex, capsuleV, inflatedRadiusV, triFlag, manifoldContacts, numContacts);
|
|
|
|
if(numContacts ==0)
|
|
continue;
|
|
|
|
hadContacts = true;
|
|
|
|
getMTDPerTriangle(manifoldContacts, numContacts, j + startIndex, normal, closestA, closestB, faceIndex, deepestPen);
|
|
}
|
|
|
|
mtd = deepestPen;
|
|
return hadContacts;
|
|
}
|
|
|
|
static PX_FORCE_INLINE bool finalizeMTD(PxGeomSweepHit& hit, const Vec3VArg translationV, const Vec3VArg posV, PxU32 triangleIndex, bool foundInitial)
|
|
{
|
|
if(foundInitial)
|
|
{
|
|
const FloatV translationF = V3Length(translationV);
|
|
const FloatV distV = FNeg(translationF);
|
|
|
|
const BoolV con = FIsGrtr(translationF, FZero());
|
|
const Vec3V nrm = V3Sel(con, V3ScaleInv(translationV, translationF), V3Zero());
|
|
|
|
FStore(distV, &hit.distance);
|
|
V3StoreU(posV, hit.position);
|
|
V3StoreU(nrm, hit.normal);
|
|
hit.faceIndex = triangleIndex;
|
|
}
|
|
return foundInitial;
|
|
}
|
|
|
|
bool physx::Gu::computeCapsule_TriangleMeshMTD( const PxTriangleMeshGeometry& triMeshGeom, const PxTransform& pose, CapsuleV& capsuleV, PxReal inflatedRadius,
|
|
bool isDoubleSided, PxGeomSweepHit& hit)
|
|
{
|
|
TriangleMesh* triMesh = static_cast<TriangleMesh*>(triMeshGeom.triangleMesh);
|
|
const PxU8* extraTrigData = triMesh->getExtraTrigData();
|
|
const bool flipsNormal = triMeshGeom.scale.hasNegativeDeterminant();
|
|
|
|
//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);
|
|
|
|
const PxMat34 vertexToWorldSkew = pose * triMeshGeom.scale;
|
|
|
|
const Vec3V zeroV = V3Zero();
|
|
Vec3V closestA = zeroV, closestB = zeroV, normal = zeroV;
|
|
|
|
/////
|
|
|
|
MeshPersistentContact manifoldContacts[64];
|
|
PxU32 numContacts = 0;
|
|
|
|
PxArray<PxU32> tempContainer;
|
|
tempContainer.reserve(128);
|
|
|
|
PxU32 triangleIndex = 0xfffffff;
|
|
Vec3V translation = zeroV;
|
|
bool foundInitial = false;
|
|
const PxU32 iterations = 4;
|
|
|
|
/////
|
|
|
|
for(PxU32 i=0; i<iterations; ++i)
|
|
{
|
|
tempContainer.forceSize_Unsafe(0);
|
|
{
|
|
Capsule inflatedCapsule;
|
|
V3StoreU(capsuleV.p0, inflatedCapsule.p0);
|
|
V3StoreU(capsuleV.p1, inflatedCapsule.p1);
|
|
inflatedCapsule.radius = inflatedRadius;
|
|
|
|
Box capsuleBox;
|
|
computeBoxAroundCapsule(inflatedCapsule, capsuleBox);
|
|
|
|
midPhaseQuery(triMeshGeom, pose, capsuleBox, tempContainer);
|
|
}
|
|
|
|
// Get results
|
|
const PxU32 nbTriangles = tempContainer.size();
|
|
if(!nbTriangles)
|
|
break;
|
|
|
|
FloatV mtd;
|
|
{
|
|
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<nbTrigs; k++)
|
|
{
|
|
//triangle world space
|
|
const PxU32 currentTriangleIndex = tempContainer[startIndex+k];
|
|
triMesh->computeWorldTriangle(triangles[k], currentTriangleIndex, vertexToWorldSkew, flipsNormal);
|
|
triangles[k].extraTriData = getConvexEdgeFlags(extraTrigData, currentTriangleIndex);
|
|
}
|
|
|
|
//ML: mtd has back face culling, so if the capsule's center is below the triangle, we won't generate any contacts
|
|
hadContacts = calculateMTD(capsuleV, inflatedRadiusV, isDoubleSided, triangles, nbTrigs, startIndex, manifoldContacts, numContacts, normal, closestA, closestB, triangleIndex, mtd) || hadContacts;
|
|
}
|
|
|
|
if(!hadContacts)
|
|
break;
|
|
|
|
triangleIndex = tempContainer[triangleIndex];
|
|
foundInitial = true;
|
|
}
|
|
|
|
//move the capsule to depenetrate it
|
|
|
|
const FloatV distV = FSub(mtd, capsuleV.radius);
|
|
if(FAllGrtr(FZero(), distV))
|
|
{
|
|
Vec3V center = capsuleV.getCenter();
|
|
const Vec3V t = V3Scale(normal, distV);
|
|
translation = V3Sub(translation, t);
|
|
center = V3Sub(center, t);
|
|
capsuleV.setCenter(center);
|
|
}
|
|
else
|
|
{
|
|
if(i == 0)
|
|
{
|
|
//First iteration so keep this normal
|
|
hit.distance = 0.0f;
|
|
V3StoreU(closestA, hit.position);
|
|
V3StoreU(normal, hit.normal);
|
|
hit.faceIndex = triangleIndex;
|
|
return true;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|
|
return finalizeMTD(hit, translation, closestA, triangleIndex, foundInitial);
|
|
}
|
|
|
|
|
|
bool physx::Gu::computeCapsule_HeightFieldMTD(const PxHeightFieldGeometry& heightFieldGeom, const PxTransform& pose, CapsuleV& capsuleV, PxReal inflatedRadius, bool isDoubleSided, PxGeomSweepHit& hit)
|
|
{
|
|
//inflated the capsule by 1% 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.01f);
|
|
|
|
const HeightFieldUtil hfUtil(heightFieldGeom);
|
|
|
|
const Vec3V zeroV = V3Zero();
|
|
Vec3V closestA = zeroV, closestB = zeroV, normal = zeroV;
|
|
|
|
/////
|
|
|
|
MeshPersistentContact manifoldContacts[64];
|
|
PxU32 numContacts = 0;
|
|
|
|
PxArray<PxU32> tempContainer;
|
|
tempContainer.reserve(128);
|
|
|
|
PxU32 triangleIndex = 0xfffffff;
|
|
Vec3V translation = zeroV;
|
|
bool foundInitial = false;
|
|
const PxU32 iterations = 4;
|
|
|
|
/////
|
|
|
|
for(PxU32 i=0; i<iterations; ++i)
|
|
{
|
|
tempContainer.forceSize_Unsafe(0);
|
|
{
|
|
Capsule inflatedCapsule;
|
|
V3StoreU(capsuleV.p0, inflatedCapsule.p0);
|
|
V3StoreU(capsuleV.p1, inflatedCapsule.p1);
|
|
inflatedCapsule.radius = inflatedRadius;
|
|
|
|
Box capsuleBox;
|
|
computeBoxAroundCapsule(inflatedCapsule, capsuleBox);
|
|
|
|
const PxTransform capsuleBoxTransform = capsuleBox.getTransform();
|
|
const PxBounds3 bounds = PxBounds3::poseExtent(capsuleBoxTransform, capsuleBox.extents);
|
|
|
|
midPhaseQuery(hfUtil, pose, bounds, tempContainer);
|
|
}
|
|
|
|
// Get results
|
|
const PxU32 nbTriangles = tempContainer.size();
|
|
if(!nbTriangles)
|
|
break;
|
|
|
|
FloatV mtd;
|
|
{
|
|
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<nbTrigs; k++)
|
|
{
|
|
//triangle vertex space
|
|
const PxU32 currentTriangleIndex = tempContainer[startIndex+k];
|
|
hfUtil.getTriangle(pose, triangles[k], NULL, NULL, currentTriangleIndex, true);
|
|
triangles[k].extraTriData = ETD_CONVEX_EDGE_ALL;
|
|
}
|
|
|
|
//ML: mtd has back face culling, so if the capsule's center is below the triangle, we won't generate any contacts
|
|
hadContacts = calculateMTD(capsuleV, inflatedRadiusV, isDoubleSided, triangles, nbTrigs, startIndex, manifoldContacts, numContacts, normal, closestA, closestB, triangleIndex, mtd) || hadContacts;
|
|
}
|
|
|
|
if(!hadContacts)
|
|
break;
|
|
|
|
triangleIndex = tempContainer[triangleIndex];
|
|
foundInitial = true;
|
|
}
|
|
|
|
const FloatV distV = FSub(mtd, capsuleV.radius);
|
|
if(FAllGrtr(FZero(), distV))
|
|
{
|
|
//move the capsule to depenetrate it
|
|
Vec3V center = capsuleV.getCenter();
|
|
const Vec3V t = V3Scale(normal, distV);
|
|
translation = V3Sub(translation, t);
|
|
center = V3Sub(center, t);
|
|
capsuleV.setCenter(center);
|
|
}
|
|
else
|
|
{
|
|
if(i == 0)
|
|
{
|
|
//First iteration so keep this normal
|
|
hit.distance = 0.0f;
|
|
V3StoreU(closestA, hit.position);
|
|
V3StoreU(normal, hit.normal);
|
|
hit.faceIndex = triangleIndex;
|
|
return true;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|
|
return finalizeMTD(hit, translation, closestA, triangleIndex, foundInitial);
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
|
|
static bool calculateMTD( const PolygonalData& polyData, const SupportLocal* polyMap, const PxTransformV& convexTransform, const PxMatTransformV& meshToConvex, bool isDoubleSided, const FloatVArg inflation, const MTDTriangle* triangles, PxU32 nbTriangles, PxU32 startIndex,
|
|
MeshPersistentContact* manifoldContacts, PxU32& numContacts, Vec3V& normal, Vec3V& closestA, Vec3V& closestB, PxU32& faceIndex, FloatV& mtd)
|
|
{
|
|
bool hadContacts = false;
|
|
FloatV deepestPen = mtd;
|
|
|
|
for(PxU32 j=0; j<nbTriangles; ++j)
|
|
{
|
|
numContacts = 0;
|
|
const MTDTriangle& curTri = triangles[j];
|
|
const PxU8 triFlag = curTri.extraTriData;
|
|
|
|
PCMConvexVsMeshContactGeneration::processTriangle(polyData, polyMap, curTri.verts, j+startIndex, triFlag, inflation, isDoubleSided, convexTransform, meshToConvex, manifoldContacts, numContacts);
|
|
|
|
if(numContacts ==0)
|
|
continue;
|
|
|
|
hadContacts = true;
|
|
getMTDPerTriangle(manifoldContacts, numContacts, j+startIndex, normal, closestA, closestB, faceIndex, deepestPen);
|
|
}
|
|
|
|
mtd = deepestPen;
|
|
|
|
return hadContacts;
|
|
}
|
|
|
|
bool physx::Gu::computeBox_TriangleMeshMTD(const PxTriangleMeshGeometry& triMeshGeom, const PxTransform& pose, const Box& _box, const PxTransform& boxTransform, PxReal inflation, bool isDoubleSided, PxGeomSweepHit& hit)
|
|
{
|
|
TriangleMesh* triMesh = static_cast<TriangleMesh*>(triMeshGeom.triangleMesh);
|
|
const PxU8* extraTrigData = triMesh->getExtraTrigData();
|
|
const bool flipsNormal = triMeshGeom.scale.hasNegativeDeterminant();
|
|
|
|
const Vec3V zeroV = V3Zero();
|
|
Vec3V closestA = zeroV, closestB = zeroV, normal = zeroV;
|
|
Vec3V worldNormal = zeroV, worldContactA = zeroV;//, worldContactB = 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 = CalculateMTDBoxMargin(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 PxMat34 meshToWorldSkew = pose * triMeshGeom.scale;
|
|
|
|
PxTransformV boxTransformV(p0, q0);//box
|
|
|
|
/////
|
|
|
|
MeshPersistentContact manifoldContacts[64];
|
|
PxU32 numContacts = 0;
|
|
|
|
PxArray<PxU32> tempContainer;
|
|
tempContainer.reserve(128);
|
|
|
|
PxU32 triangleIndex = 0xfffffff;
|
|
Vec3V translation = zeroV;
|
|
bool foundInitial = false;
|
|
const PxU32 iterations = 4;
|
|
|
|
/////
|
|
|
|
for(PxU32 i=0; i<iterations; ++i)
|
|
{
|
|
tempContainer.forceSize_Unsafe(0);
|
|
{
|
|
midPhaseQuery(triMeshGeom, pose, box, tempContainer);
|
|
}
|
|
|
|
// Get results
|
|
const PxU32 nbTriangles = tempContainer.size();
|
|
if(!nbTriangles)
|
|
break;
|
|
|
|
boxTransformV.p = boxCenter;
|
|
SupportLocalImpl<BoxV> boxMap(boxV, boxTransformV, identity, identity, true);
|
|
|
|
boxMap.setShapeSpaceCenterofMass(zeroV);
|
|
// Move to AABB space
|
|
PxMat34 WorldToBox;
|
|
computeWorldToBoxMatrix(WorldToBox, box);
|
|
const PxMat34 meshToBox = WorldToBox*meshToWorldSkew;
|
|
|
|
const Mat33V rot(V3LoadU(meshToBox.m.column0), V3LoadU(meshToBox.m.column1), V3LoadU(meshToBox.m.column2));
|
|
const PxMatTransformV meshToConvex(V3LoadU(meshToBox.p), rot);
|
|
|
|
FloatV mtd;
|
|
{
|
|
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<nbTrigs; k++)
|
|
{
|
|
//triangle vertex space
|
|
const PxU32 currentTriangleIndex = tempContainer[startIndex+k];
|
|
triMesh->getLocalTriangle(triangles[k], currentTriangleIndex, flipsNormal);
|
|
triangles[k].extraTriData = getConvexEdgeFlags(extraTrigData, currentTriangleIndex);
|
|
}
|
|
|
|
//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;
|
|
}
|
|
|
|
const FloatV distV = mtd;
|
|
worldNormal = boxTransformV.rotate(normal);
|
|
worldContactA = boxTransformV.transform(closestA);
|
|
if(FAllGrtr(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.0f;
|
|
V3StoreU(worldContactA, hit.position);
|
|
V3StoreU(worldNormal, hit.normal);
|
|
hit.faceIndex = triangleIndex;
|
|
return true;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|
|
return finalizeMTD(hit, translation, worldContactA, triangleIndex, foundInitial);
|
|
}
|
|
|
|
bool physx::Gu::computeBox_HeightFieldMTD(const PxHeightFieldGeometry& heightFieldGeom, const PxTransform& pose, const Box& _box, const PxTransform& boxTransform, PxReal inflation, bool isDoubleSided, PxGeomSweepHit& hit)
|
|
{
|
|
const HeightFieldUtil hfUtil(heightFieldGeom);
|
|
|
|
const Vec3V zeroV = V3Zero();
|
|
Vec3V closestA = zeroV, closestB = zeroV, normal = zeroV;
|
|
Vec3V worldNormal = zeroV, worldContactA = zeroV;//, worldContactB = 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 = CalculateMTDBoxMargin(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 Matrix34FromTransform meshToWorldSkew(pose);
|
|
|
|
PxTransformV boxTransformV(p0, q0);//box
|
|
|
|
/////
|
|
|
|
MeshPersistentContact manifoldContacts[64];
|
|
PxU32 numContacts = 0;
|
|
|
|
PxArray<PxU32> tempContainer;
|
|
tempContainer.reserve(128);
|
|
|
|
PxU32 triangleIndex = 0xfffffff;
|
|
Vec3V translation = zeroV;
|
|
bool foundInitial = false;
|
|
const PxU32 iterations = 4;
|
|
|
|
/////
|
|
|
|
for(PxU32 i=0; i<iterations; ++i)
|
|
{
|
|
tempContainer.forceSize_Unsafe(0);
|
|
{
|
|
const PxBounds3 bounds = PxBounds3::poseExtent(box.getTransform(), box.extents);
|
|
|
|
midPhaseQuery(hfUtil, pose, bounds, tempContainer);
|
|
}
|
|
|
|
// Get results
|
|
const PxU32 nbTriangles = tempContainer.size();
|
|
if(!nbTriangles)
|
|
break;
|
|
|
|
boxTransformV.p = boxCenter;
|
|
SupportLocalImpl<BoxV> boxMap(boxV, boxTransformV, identity, identity, true);
|
|
boxMap.setShapeSpaceCenterofMass(zeroV);
|
|
// Move to AABB space
|
|
PxMat34 WorldToBox;
|
|
computeWorldToBoxMatrix(WorldToBox, box);
|
|
const PxMat34 meshToBox = WorldToBox*meshToWorldSkew;
|
|
|
|
const Mat33V rot(V3LoadU(meshToBox.m.column0), V3LoadU(meshToBox.m.column1), V3LoadU(meshToBox.m.column2));
|
|
const PxMatTransformV meshToConvex(V3LoadU(meshToBox.p), rot);
|
|
|
|
FloatV mtd;
|
|
{
|
|
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<nbTrigs; k++)
|
|
{
|
|
//triangle vertex space
|
|
const PxU32 currentTriangleIndex = tempContainer[startIndex+k];
|
|
hfUtil.getTriangle(pose, triangles[k], NULL, NULL, currentTriangleIndex, false, false);
|
|
triangles[k].extraTriData = ETD_CONVEX_EDGE_ALL;
|
|
}
|
|
|
|
//ML: mtd has back face culling, so if the box'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;
|
|
}
|
|
|
|
const FloatV distV = mtd;
|
|
worldNormal = boxTransformV.rotate(normal);
|
|
worldContactA = boxTransformV.transform(closestA);
|
|
if(FAllGrtr(FZero(), distV))
|
|
{
|
|
//worldContactB = boxTransformV.transform(closestB);
|
|
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.0f;
|
|
V3StoreU(worldContactA, hit.position);
|
|
V3StoreU(worldNormal, hit.normal);
|
|
hit.faceIndex = triangleIndex;
|
|
return true;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|
|
return finalizeMTD(hit, translation, worldContactA, triangleIndex, foundInitial);
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
|
|
bool physx::Gu::computeConvex_TriangleMeshMTD( const PxTriangleMeshGeometry& triMeshGeom, const PxTransform& pose, const PxConvexMeshGeometry& convexGeom, const PxTransform& convexPose, PxReal inflation,
|
|
bool isDoubleSided, PxGeomSweepHit& hit)
|
|
{
|
|
const Vec3V zeroV = V3Zero();
|
|
|
|
TriangleMesh* triMesh = static_cast<TriangleMesh*>(triMeshGeom.triangleMesh);
|
|
ConvexMesh* cm = static_cast<ConvexMesh*>(convexGeom.convexMesh);
|
|
const PxU8* extraTrigData = triMesh->getExtraTrigData();
|
|
const bool flipsNormal = triMeshGeom.scale.hasNegativeDeterminant();
|
|
|
|
ConvexHullData* hullData = &cm->getHull();
|
|
|
|
const bool idtScaleConvex = convexGeom.scale.isIdentity();
|
|
|
|
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);
|
|
PxTransformV 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);
|
|
const ConvexHullV convexHull(hullData, V3Zero(), vScale, vQuat, idtScaleConvex);
|
|
PX_ALIGN(16, PxU8 convexBuff[sizeof(SupportLocalImpl<ConvexHullV>)]);
|
|
|
|
const FloatV convexMargin = CalculateMTDConvexMargin(hullData, vScale);
|
|
const FloatV inflationV = FAdd(FLoad(inflation), convexMargin);
|
|
PxReal boundInflation;
|
|
FStore(inflationV, &boundInflation);
|
|
|
|
Vec3V closestA = zeroV, closestB = zeroV, normal = zeroV;
|
|
|
|
const PxMat34 meshToWorldSkew = pose * triMeshGeom.scale;
|
|
|
|
PolygonalData polyData;
|
|
getPCMConvexData(convexHull, idtScaleConvex, polyData);
|
|
|
|
Vec3V center = p0;
|
|
PxTransform tempConvexPose = convexPose;
|
|
Vec3V worldNormal = zeroV, worldContactA = zeroV;//, worldContactB = zeroV;
|
|
|
|
/////
|
|
|
|
MeshPersistentContact manifoldContacts[64];
|
|
PxU32 numContacts = 0;
|
|
|
|
PxArray<PxU32> tempContainer;
|
|
tempContainer.reserve(128);
|
|
|
|
PxU32 triangleIndex = 0xfffffff;
|
|
Vec3V translation = zeroV;
|
|
bool foundInitial = false;
|
|
const PxU32 iterations = 2; // PT: TODO: why 2 here instead of 4?
|
|
|
|
/////
|
|
|
|
for(PxU32 i=0; i<iterations; ++i)
|
|
{
|
|
tempContainer.forceSize_Unsafe(0);
|
|
SupportLocal* convexMap;
|
|
{
|
|
//ML:: construct convex hull data
|
|
V3StoreU(center, tempConvexPose.p);
|
|
convexTransformV.p = center;
|
|
|
|
convexMap = idtScaleConvex ? static_cast<SupportLocal*>(PX_PLACEMENT_NEW(convexBuff, SupportLocalImpl<ConvexHullNoScaleV>)(static_cast<const ConvexHullNoScaleV&>(convexHull), convexTransformV, convexHull.vertex2Shape, convexHull.shape2Vertex, idtScaleConvex)) :
|
|
static_cast<SupportLocal*>(PX_PLACEMENT_NEW(convexBuff, SupportLocalImpl<ConvexHullV>)(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 Matrix34FromTransform worldToConvex(tempConvexPose.getInverse());
|
|
const PxMat34 meshToConvex = worldToConvex*meshToWorldSkew;
|
|
|
|
const Mat33V rot(V3LoadU(meshToConvex.m.column0), V3LoadU(meshToConvex.m.column1), V3LoadU(meshToConvex.m.column2));
|
|
const PxMatTransformV meshToConvexV(V3LoadU(meshToConvex.p), rot);
|
|
|
|
FloatV mtd;
|
|
{
|
|
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<nbTrigs; k++)
|
|
{
|
|
//triangle vertex space
|
|
const PxU32 currentTriangleIndex = tempContainer[startIndex+k];
|
|
triMesh->getLocalTriangle(triangles[k], currentTriangleIndex, flipsNormal);
|
|
triangles[k].extraTriData = getConvexEdgeFlags(extraTrigData, currentTriangleIndex);
|
|
}
|
|
|
|
//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;
|
|
}
|
|
|
|
const FloatV distV = mtd;
|
|
worldNormal = convexTransformV.rotate(normal);
|
|
worldContactA = convexTransformV.transform(closestA);
|
|
if(FAllGrtr(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.0f;
|
|
V3StoreU(worldContactA, hit.position);
|
|
V3StoreU(worldNormal, hit.normal);
|
|
hit.faceIndex = triangleIndex;
|
|
return true;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|
|
return finalizeMTD(hit, translation, worldContactA, triangleIndex, foundInitial);
|
|
}
|
|
|
|
bool physx::Gu::computeConvex_HeightFieldMTD(const PxHeightFieldGeometry& heightFieldGeom, const PxTransform& pose, const PxConvexMeshGeometry& convexGeom, const PxTransform& convexPose, PxReal inflation, bool isDoubleSided, PxGeomSweepHit& hit)
|
|
{
|
|
const HeightFieldUtil hfUtil(heightFieldGeom);
|
|
|
|
const Vec3V zeroV = V3Zero();
|
|
|
|
ConvexMesh* cm = static_cast<ConvexMesh*>(convexGeom.convexMesh);
|
|
|
|
ConvexHullData* hullData = &cm->getHull();
|
|
|
|
const bool idtScaleConvex = convexGeom.scale.isIdentity();
|
|
|
|
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);
|
|
PxTransformV 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);
|
|
const ConvexHullV convexHull(hullData, zeroV, vScale, vQuat, idtScaleConvex);
|
|
PX_ALIGN(16, PxU8 convexBuff[sizeof(SupportLocalImpl<ConvexHullV>)]);
|
|
|
|
const FloatV convexMargin = CalculateMTDConvexMargin(hullData, vScale);
|
|
const FloatV inflationV = FAdd(FLoad(inflation), convexMargin);
|
|
PxReal boundInflation;
|
|
FStore(inflationV, &boundInflation);
|
|
|
|
Vec3V closestA = zeroV, closestB = zeroV, normal = zeroV;
|
|
Vec3V worldNormal = zeroV, worldContactA = zeroV;//, worldContactB = zeroV;
|
|
|
|
PolygonalData polyData;
|
|
getPCMConvexData(convexHull, idtScaleConvex, polyData);
|
|
|
|
Vec3V center = p0;
|
|
PxTransform tempConvexPose = convexPose;
|
|
const Matrix34FromTransform meshToWorldSkew(pose);
|
|
|
|
/////
|
|
|
|
MeshPersistentContact manifoldContacts[64];
|
|
PxU32 numContacts = 0;
|
|
|
|
PxArray<PxU32> tempContainer;
|
|
tempContainer.reserve(128);
|
|
|
|
PxU32 triangleIndex = 0xfffffff;
|
|
Vec3V translation = zeroV;
|
|
bool foundInitial = false;
|
|
const PxU32 iterations = 2; // PT: TODO: why 2 here instead of 4?
|
|
|
|
/////
|
|
|
|
for(PxU32 i=0; i<iterations; ++i)
|
|
{
|
|
tempContainer.forceSize_Unsafe(0);
|
|
SupportLocal* convexMap;
|
|
{
|
|
//ML:: construct convex hull data
|
|
V3StoreU(center, tempConvexPose.p);
|
|
convexTransformV.p = center;
|
|
|
|
convexMap = idtScaleConvex ? static_cast<SupportLocal*>(PX_PLACEMENT_NEW(convexBuff, SupportLocalImpl<ConvexHullNoScaleV>)(static_cast<const ConvexHullNoScaleV&>(convexHull), convexTransformV, convexHull.vertex2Shape, convexHull.shape2Vertex, idtScaleConvex)) :
|
|
static_cast<SupportLocal*>(PX_PLACEMENT_NEW(convexBuff, SupportLocalImpl<ConvexHullV>)(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);
|
|
}
|
|
|
|
// Get results
|
|
const PxU32 nbTriangles = tempContainer.size();
|
|
if(!nbTriangles)
|
|
break;
|
|
|
|
// Move to AABB space
|
|
const Matrix34FromTransform worldToConvex(tempConvexPose.getInverse());
|
|
const PxMat34 meshToConvex = worldToConvex*meshToWorldSkew;
|
|
|
|
const Mat33V rot(V3LoadU(meshToConvex.m.column0), V3LoadU(meshToConvex.m.column1), V3LoadU(meshToConvex.m.column2));
|
|
const PxMatTransformV meshToConvexV(V3LoadU(meshToConvex.p), rot);
|
|
|
|
FloatV mtd;
|
|
{
|
|
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<nbTrigs; k++)
|
|
{
|
|
//triangle vertex space
|
|
const PxU32 currentTriangleIndex = tempContainer[startIndex+k];
|
|
hfUtil.getTriangle(pose, triangles[k], NULL, NULL, currentTriangleIndex, false, false);
|
|
triangles[k].extraTriData = ETD_CONVEX_EDGE_ALL;
|
|
}
|
|
|
|
//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;
|
|
}
|
|
|
|
const FloatV distV = mtd;
|
|
worldNormal = convexTransformV.rotate(normal);
|
|
worldContactA = convexTransformV.transform(closestA);
|
|
if(FAllGrtr(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.0f;
|
|
V3StoreU(worldContactA, hit.position);
|
|
V3StoreU(worldNormal, hit.normal);
|
|
hit.faceIndex = triangleIndex;
|
|
return true;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|
|
return finalizeMTD(hit, translation, worldContactA, triangleIndex, foundInitial);
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
bool physx::Gu::computeSphere_SphereMTD(const Sphere& sphere0, const Sphere& sphere1, PxGeomSweepHit& hit)
|
|
{
|
|
const PxVec3 delta = sphere1.center - sphere0.center;
|
|
const PxReal d2 = delta.magnitudeSquared();
|
|
const PxReal radiusSum = sphere0.radius + sphere1.radius;
|
|
|
|
const PxReal d = manualNormalize(hit.normal, delta, d2);
|
|
hit.distance = d - radiusSum;
|
|
hit.position = sphere0.center + hit.normal * sphere0.radius;
|
|
return true;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
bool physx::Gu::computeSphere_CapsuleMTD( const Sphere& sphere, const Capsule& capsule, PxGeomSweepHit& hit)
|
|
{
|
|
const PxReal radiusSum = sphere.radius + capsule.radius;
|
|
|
|
PxReal u;
|
|
distancePointSegmentSquared(capsule, sphere.center, &u);
|
|
|
|
const PxVec3 normal = capsule.getPointAt(u) - sphere.center;
|
|
|
|
const PxReal lenSq = normal.magnitudeSquared();
|
|
const PxF32 d = manualNormalize(hit.normal, normal, lenSq);
|
|
hit.distance = d - radiusSum;
|
|
hit.position = sphere.center + hit.normal * sphere.radius;
|
|
return true;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
bool physx::Gu::computeCapsule_CapsuleMTD(const Capsule& capsule0, const Capsule& capsule1, PxGeomSweepHit& hit)
|
|
{
|
|
PxReal s,t;
|
|
distanceSegmentSegmentSquared(capsule0, capsule1, &s, &t);
|
|
|
|
const PxReal radiusSum = capsule0.radius + capsule1.radius;
|
|
|
|
const PxVec3 pointAtCapsule0 = capsule0.getPointAt(s);
|
|
const PxVec3 pointAtCapsule1 = capsule1.getPointAt(t);
|
|
|
|
const PxVec3 normal = pointAtCapsule0 - pointAtCapsule1;
|
|
const PxReal lenSq = normal.magnitudeSquared();
|
|
const PxF32 len = manualNormalize(hit.normal, normal, lenSq);
|
|
hit.distance = len - radiusSum;
|
|
hit.position = pointAtCapsule1 + hit.normal * capsule1.radius;
|
|
return true;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
bool physx::Gu::computePlane_CapsuleMTD(const PxPlane& plane, const Capsule& capsule, PxGeomSweepHit& hit)
|
|
{
|
|
const PxReal d0 = plane.distance(capsule.p0);
|
|
const PxReal d1 = plane.distance(capsule.p1);
|
|
PxReal dmin;
|
|
PxVec3 point;
|
|
if(d0 < d1)
|
|
{
|
|
dmin = d0;
|
|
point = capsule.p0;
|
|
}
|
|
else
|
|
{
|
|
dmin = d1;
|
|
point = capsule.p1;
|
|
}
|
|
|
|
hit.normal = plane.n;
|
|
hit.distance = dmin - capsule.radius;
|
|
hit.position = point - hit.normal * dmin;
|
|
return true;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
bool physx::Gu::computePlane_BoxMTD(const PxPlane& plane, const Box& box, PxGeomSweepHit& hit)
|
|
{
|
|
PxVec3 pts[8];
|
|
box.computeBoxPoints(pts);
|
|
|
|
PxReal dmin = plane.distance(pts[0]);
|
|
PxU32 index = 0;
|
|
for(PxU32 i=1;i<8;i++)
|
|
{
|
|
const PxReal d = plane.distance(pts[i]);
|
|
if(dmin > 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, PxGeomSweepHit& hit)
|
|
{
|
|
const ConvexMesh* convexMesh = static_cast<const ConvexMesh*>(convexGeom.convexMesh);
|
|
const 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<nbVerts;i++)
|
|
{
|
|
const PxVec3 worldPoint = convexPose.transform(convexScaling * verts[i]);
|
|
const PxReal d = plane.distance(worldPoint);
|
|
if(dmin > d)
|
|
{
|
|
dmin = d;
|
|
worldPointMin = worldPoint;
|
|
}
|
|
}
|
|
|
|
hit.normal = plane.n;
|
|
hit.distance = dmin;
|
|
hit.position = worldPointMin - plane.n * dmin;
|
|
return true;
|
|
}
|