feat(physics): wire physx sdk into build
This commit is contained in:
724
engine/third_party/physx/source/geomutils/src/ccd/GuCCDSweepConvexMesh.cpp
vendored
Normal file
724
engine/third_party/physx/source/geomutils/src/ccd/GuCCDSweepConvexMesh.cpp
vendored
Normal file
@@ -0,0 +1,724 @@
|
||||
// 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 "GuVecCapsule.h"
|
||||
#include "GuVecBox.h"
|
||||
#include "GuVecConvexHull.h"
|
||||
#include "GuVecTriangle.h"
|
||||
#include "GuGJKRaycast.h"
|
||||
#include "GuCCDSweepConvexMesh.h"
|
||||
#include "GuHeightFieldUtil.h"
|
||||
#include "foundation/PxInlineArray.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 "geometry/PxGeometryQuery.h"
|
||||
|
||||
// PT: this one makes the "behavior after impact" PEEL test "fail" (rocks stop after impact)
|
||||
// It also makes these UTs fail:
|
||||
// [ FAILED ] CCDReportTest.CCD_soakTest_mesh
|
||||
// [ FAILED ] CCDNegativeScalingTest.SLOW_ccdNegScaledMesh
|
||||
static const bool gUseGeometryQuery = false;
|
||||
|
||||
// PT: this one seems to work.
|
||||
// Timings for PEEL's "limits of speculative contacts test2", for 3 runs:
|
||||
// false: true:
|
||||
// Time: 504 220
|
||||
// Time: 89 7
|
||||
// Time: 5 84
|
||||
// Time: 8 11
|
||||
// Time: 423 56
|
||||
// Time: 103 14
|
||||
// Time: 10 11
|
||||
// Time: 10 9
|
||||
// Time: 418 60
|
||||
// Time: 139 17
|
||||
// Time: 9 9
|
||||
// Time: 9 10
|
||||
static const bool gUseGeometryQueryEst = false;
|
||||
|
||||
//#define CCD_BASIC_PROFILING
|
||||
#ifdef CCD_BASIC_PROFILING
|
||||
#include <stdio.h>
|
||||
#endif
|
||||
|
||||
namespace physx
|
||||
{
|
||||
namespace Gu
|
||||
{
|
||||
|
||||
PxReal SweepShapeTriangle(GU_TRIANGLE_SWEEP_METHOD_ARGS);
|
||||
|
||||
using namespace aos;
|
||||
|
||||
namespace
|
||||
{
|
||||
struct AccumCallback: public MeshHitCallback<PxGeomRaycastHit>
|
||||
{
|
||||
PX_NOCOPY(AccumCallback)
|
||||
public:
|
||||
|
||||
PxInlineArray<PxU32, 64>& mResult;
|
||||
AccumCallback(PxInlineArray<PxU32, 64>& result)
|
||||
: MeshHitCallback<PxGeomRaycastHit>(CallbackMode::eMULTIPLE),
|
||||
mResult(result)
|
||||
{
|
||||
}
|
||||
|
||||
virtual PxAgain processHit( // all reported coords are in mesh local space including hit.position
|
||||
const PxGeomRaycastHit& hit, const PxVec3&, const PxVec3&, const PxVec3&, PxReal&, const PxU32*) PX_OVERRIDE PX_FINAL
|
||||
{
|
||||
mResult.pushBack(hit.faceIndex);
|
||||
return true;
|
||||
}
|
||||
};
|
||||
|
||||
// PT: TODO: refactor with MidPhaseQueryLocalReport
|
||||
struct EntityReportContainerCallback : public OverlapReport
|
||||
{
|
||||
PxInlineArray<PxU32, 64>& container;
|
||||
EntityReportContainerCallback(PxInlineArray<PxU32,64>& container_) : container(container_)
|
||||
{
|
||||
container.forceSize_Unsafe(0);
|
||||
}
|
||||
virtual ~EntityReportContainerCallback() {}
|
||||
|
||||
virtual bool reportTouchedTris(PxU32 nb, const PxU32* indices) PX_OVERRIDE PX_FINAL
|
||||
{
|
||||
for(PxU32 i=0; i<nb; i++)
|
||||
container.pushBack(indices[i]);
|
||||
return true;
|
||||
}
|
||||
private:
|
||||
EntityReportContainerCallback& operator=(const EntityReportContainerCallback&);
|
||||
};
|
||||
|
||||
class TriangleHelper
|
||||
{
|
||||
public:
|
||||
TriangleHelper(const PxTriangleMeshGeometry& shapeMesh,
|
||||
const Cm::FastVertex2ShapeScaling& skew, // object is not copied, beware!
|
||||
const PxU32 triangleIndex);
|
||||
|
||||
void getBounds(PxBounds3& bounds, const physx::PxTransform& transform) const;
|
||||
|
||||
//non-virtuals:
|
||||
PX_FORCE_INLINE const TriangleMesh* getMeshData() const { return _getMeshData(mShapeMesh); }
|
||||
|
||||
PxVec3 getPolygonNormal() const;
|
||||
private:
|
||||
TriangleHelper& operator=(const TriangleHelper&);
|
||||
|
||||
const PxTriangleMeshGeometry& mShapeMesh;
|
||||
const Cm::FastVertex2ShapeScaling& mVertex2ShapeSkew;
|
||||
const PxU32 mTriangleIndex;
|
||||
};
|
||||
|
||||
TriangleHelper::TriangleHelper(const PxTriangleMeshGeometry& md, const Cm::FastVertex2ShapeScaling& skew, const PxU32 tg)
|
||||
: mShapeMesh(md), mVertex2ShapeSkew(skew), mTriangleIndex(tg)
|
||||
{
|
||||
}
|
||||
|
||||
void TriangleHelper::getBounds(PxBounds3& bounds, const physx::PxTransform& transform) const
|
||||
{
|
||||
PxTriangle localTri;
|
||||
getMeshData()->getLocalTriangle(localTri, mTriangleIndex, false); // PT: 'false': no need to flip winding to compute bounds
|
||||
|
||||
//gotta take bounds in shape space because building it in vertex space and transforming it out would skew it.
|
||||
bounds = PxBounds3::empty();
|
||||
bounds.include(transform.transform(mVertex2ShapeSkew * localTri.verts[0]));
|
||||
bounds.include(transform.transform(mVertex2ShapeSkew * localTri.verts[1]));
|
||||
bounds.include(transform.transform(mVertex2ShapeSkew * localTri.verts[2]));
|
||||
}
|
||||
|
||||
PxVec3 TriangleHelper::getPolygonNormal() const
|
||||
{
|
||||
PxTriangle localTri;
|
||||
getMeshData()->getLocalTriangle(localTri, mTriangleIndex, mVertex2ShapeSkew.flipsNormal());
|
||||
|
||||
const PxVec3 t0 = mVertex2ShapeSkew * localTri.verts[0];
|
||||
const PxVec3 t1 = mVertex2ShapeSkew * localTri.verts[1];
|
||||
const PxVec3 t2 = mVertex2ShapeSkew * localTri.verts[2];
|
||||
const PxVec3 v0 = t0 - t1;
|
||||
const PxVec3 v1 = t0 - t2;
|
||||
const PxVec3 nor = v0.cross(v1);
|
||||
return nor.getNormalized();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
PxReal SweepAnyShapeHeightfield(GU_SWEEP_METHOD_ARGS)
|
||||
{
|
||||
PX_UNUSED(toiEstimate);
|
||||
|
||||
PX_ASSERT(shape1.mGeometry->getType()==PxGeometryType::eHEIGHTFIELD);
|
||||
const HeightFieldUtil hfUtil(static_cast<const PxHeightFieldGeometry&>(*shape1.mGeometry));
|
||||
|
||||
PxInlineArray<PxU32,64> tempContainer;
|
||||
|
||||
EntityReportContainerCallback callback(tempContainer);
|
||||
|
||||
const PxVec3 trA = transform0.p - lastTm0.p;
|
||||
const PxVec3 trB = transform1.p - lastTm1.p;
|
||||
|
||||
const PxVec3 relTr = trA - trB;
|
||||
const PxVec3 halfRelTr = relTr * 0.5f;
|
||||
|
||||
const PxVec3 ext = shape0.mExtents + halfRelTr.abs() + PxVec3(restDistance);
|
||||
const PxVec3 cent = shape0.mCenter + halfRelTr;
|
||||
|
||||
const PxBounds3 bounds0(cent - ext, cent + ext);
|
||||
|
||||
hfUtil.overlapAABBTriangles(transform1, bounds0, callback);
|
||||
|
||||
PxArray<PxU32> orderedContainer(tempContainer.size());
|
||||
|
||||
PxArray<PxU32> distanceEntries(tempContainer.size());
|
||||
|
||||
PxU32* orderedList = orderedContainer.begin();
|
||||
PxF32* distances = reinterpret_cast<PxF32*>(distanceEntries.begin());
|
||||
|
||||
const PxVec3 origin = shape0.mCenter;
|
||||
const 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;
|
||||
|
||||
const PxVec3 sphereCenter(shape0.mPrevTransform.p);
|
||||
const PxF32 inSphereRadius = shape0.mFastMovingThreshold;
|
||||
const PxF32 inRadSq = inSphereRadius * inSphereRadius;
|
||||
|
||||
const PxVec3 sphereCenterInTr1 = transform1.transformInv(sphereCenter);
|
||||
const 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)
|
||||
{
|
||||
PX_ASSERT(shape1.mGeometry->getType()==PxGeometryType::eHEIGHTFIELD);
|
||||
const HeightFieldUtil hfUtil(static_cast<const PxHeightFieldGeometry&>(*shape1.mGeometry));
|
||||
|
||||
PxInlineArray<PxU32,64> tempContainer;
|
||||
|
||||
EntityReportContainerCallback callback(tempContainer);
|
||||
|
||||
const PxTransform& transform0 = shape0.mCurrentTransform;
|
||||
const PxTransform& lastTr0 = shape0.mPrevTransform;
|
||||
const PxTransform& transform1 = shape1.mCurrentTransform;
|
||||
const PxTransform& lastTr1 = shape1.mPrevTransform;
|
||||
|
||||
const PxVec3 trA = transform0.p - lastTr0.p;
|
||||
const PxVec3 trB = transform1.p - lastTr1.p;
|
||||
|
||||
const PxVec3 relTr = trA - trB;
|
||||
const PxVec3 halfRelTr = relTr * 0.5f;
|
||||
|
||||
const PxVec3 extents = shape0.mExtents + halfRelTr.abs() + PxVec3(restDistance);
|
||||
const PxVec3 center = shape0.mCenter + halfRelTr;
|
||||
|
||||
const PxBounds3 bounds0(center - extents, center + extents);
|
||||
|
||||
hfUtil.overlapAABBTriangles(transform1, bounds0, 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.
|
||||
|
||||
const PxVec3 trA = transform0.p - lastTm0.p;
|
||||
const PxVec3 trB = transform1.p - lastTm1.p;
|
||||
|
||||
const PxVec3 relTr = trA - trB;
|
||||
PxVec3 unitDir = relTr;
|
||||
const PxReal length = unitDir.normalize();
|
||||
|
||||
PX_UNUSED(restDistance);
|
||||
PX_UNUSED(fastMovingThreshold);
|
||||
|
||||
if(gUseGeometryQuery)
|
||||
{
|
||||
PxGeomSweepHit sweepHit;
|
||||
if(!PxGeometryQuery::sweep(unitDir, length, *shape0.mGeometry, lastTm0, *shape1.mGeometry, lastTm1, sweepHit, PxHitFlag::eDEFAULT, 0.0f, PxGeometryQueryFlag::Enum(0), NULL))
|
||||
//if(!PxGeometryQuery::sweep(unitDir, length, *shape0.mGeometry, transform0, *shape1.mGeometry, transform1, sweepHit, PxHitFlag::eDEFAULT, 0.0f, PxGeometryQueryFlag::Enum(0), NULL))
|
||||
return PX_MAX_REAL;
|
||||
|
||||
worldNormal = sweepHit.normal;
|
||||
worldPoint = sweepHit.position;
|
||||
outCCDFaceIndex = sweepHit.faceIndex;
|
||||
|
||||
return sweepHit.distance/length;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Get actual shape data
|
||||
PX_ASSERT(shape1.mGeometry->getType()==PxGeometryType::eTRIANGLEMESH);
|
||||
const PxTriangleMeshGeometry& shapeMesh = static_cast<const PxTriangleMeshGeometry&>(*shape1.mGeometry);
|
||||
|
||||
const Cm::FastVertex2ShapeScaling meshScaling(shapeMesh.scale);
|
||||
|
||||
const PxMat33 matRot(PxIdentity);
|
||||
|
||||
//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);
|
||||
|
||||
PxInlineArray<PxU32, 64> tempContainer;
|
||||
|
||||
AccumCallback callback(tempContainer);
|
||||
|
||||
// AP scaffold: early out opportunities, should probably use fat raycast
|
||||
Midphase::intersectOBB(_getMeshData(shapeMesh), 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);
|
||||
|
||||
PxInlineArray<PxU32, 64> orderedContainer;
|
||||
orderedContainer.resize(tempContainer.size());
|
||||
|
||||
PxInlineArray<PxU32, 64> distanceEntries;
|
||||
distanceEntries.resize(tempContainer.size());
|
||||
|
||||
PxU32* orderedList = orderedContainer.begin();
|
||||
PxF32* distances = reinterpret_cast<PxF32*>(distanceEntries.begin());
|
||||
|
||||
PxReal minTOI = PX_MAX_REAL;
|
||||
|
||||
PxU32 count = 0;
|
||||
for(PxU32 a = 0; a < numTrigs; ++a)
|
||||
{
|
||||
const TriangleHelper convexPartOfMesh1(shapeMesh, meshScaling, triangleIndices[a]);
|
||||
|
||||
const PxVec3 resultNormal = -transform1.rotate(convexPartOfMesh1.getPolygonNormal());
|
||||
|
||||
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;
|
||||
|
||||
const PxVec3 sphereCenter(lastTm1.p);
|
||||
const PxF32 inSphereRadius = shape0.mFastMovingThreshold;
|
||||
//PxF32 inRadSq = inSphereRadius * inSphereRadius;
|
||||
|
||||
const PxVec3 sphereCenterInTransform1 = transform1.transformInv(sphereCenter);
|
||||
|
||||
const PxVec3 sphereCenterInTransform0p = transform1.transformInv(lastTm0.p);
|
||||
|
||||
for (PxU32 ti = 0; ti < count /*&& PxMax(minTOI, 0.f) >= distances[ti]*/; ti++)
|
||||
{
|
||||
const TriangleHelper convexPartOfMesh1(shapeMesh, meshScaling, orderedList[ti]);
|
||||
|
||||
PxVec3 resultNormal, resultPoint;
|
||||
PxTriangle localTri;
|
||||
_getMeshData(shapeMesh)->getLocalTriangle(localTri, orderedList[ti], meshScaling.flipsNormal());
|
||||
|
||||
const PxVec3 v0 = meshScaling * localTri.verts[0];
|
||||
const PxVec3 v1 = meshScaling * localTri.verts[1];
|
||||
const PxVec3 v2 = meshScaling * localTri.verts[2];
|
||||
|
||||
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;
|
||||
|
||||
const PxF32 inRad = inSphereRadius + restDistance;
|
||||
const 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!
|
||||
|
||||
const 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 = transform1.rotate(convexPartOfMesh1.getPolygonNormal());
|
||||
}
|
||||
|
||||
if (res < minTOI)
|
||||
{
|
||||
tempWorldNormal = resultNormal;//convexPartOfMesh1.getPolygonNormal(0);//transform1.rotate(convexPartOfMesh1.getPolygonNormal(0));
|
||||
tempWorldPoint = resultPoint;
|
||||
minTOI = res;
|
||||
ccdFaceIndex = orderedList[ti];
|
||||
}
|
||||
}
|
||||
|
||||
worldNormal = tempWorldNormal;//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
|
||||
PX_ASSERT(shape1.mGeometry->getType()==PxGeometryType::eTRIANGLEMESH);
|
||||
const PxTriangleMeshGeometry& shapeMesh = static_cast<const PxTriangleMeshGeometry&>(*shape1.mGeometry);
|
||||
|
||||
const PxTransform& transform0 = shape0.mCurrentTransform;
|
||||
const PxTransform& lastTr0 = shape0.mPrevTransform;
|
||||
const PxTransform& transform1 = shape1.mCurrentTransform;
|
||||
const PxTransform& lastTr1 = shape1.mPrevTransform;
|
||||
|
||||
const PxVec3 trA = transform0.p - lastTr0.p;
|
||||
const PxVec3 trB = transform1.p - lastTr1.p;
|
||||
|
||||
const PxVec3 relTr = trA - trB;
|
||||
PxVec3 unitDir = relTr;
|
||||
const PxReal length = unitDir.normalize();
|
||||
|
||||
#ifdef CCD_BASIC_PROFILING
|
||||
unsigned long long time = __rdtsc();
|
||||
#endif
|
||||
|
||||
if(gUseGeometryQueryEst)
|
||||
{
|
||||
PX_UNUSED(restDistance);
|
||||
PX_UNUSED(fastMovingThreshold);
|
||||
PX_UNUSED(shapeMesh);
|
||||
{
|
||||
PxGeomSweepHit sweepHit;
|
||||
|
||||
bool status = PxGeometryQuery::sweep(unitDir, length, *shape0.mGeometry, lastTr0, *shape1.mGeometry, lastTr1, sweepHit, PxHitFlag::eDEFAULT, 0.0f, PxGeometryQueryFlag::Enum(0), NULL);
|
||||
#ifdef CCD_BASIC_PROFILING
|
||||
unsigned long long time2 = __rdtsc();
|
||||
printf("Time: %d\n", PxU32(time2 - time)/1024);
|
||||
#endif
|
||||
return status ? sweepHit.distance/length : PX_MAX_REAL;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
const Cm::FastVertex2ShapeScaling meshScaling(shapeMesh.scale);
|
||||
|
||||
const PxMat33 matRot(PxIdentity);
|
||||
|
||||
//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<PxGeomRaycastHit>
|
||||
{
|
||||
PxReal minTOI;
|
||||
PxReal sumFastMovingThresh;
|
||||
const PxTriangleMeshGeometry& 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 PxTriangleMeshGeometry& aShapeMesh, const Cm::FastVertex2ShapeScaling& aMeshScaling,
|
||||
const PxVec3& aRelTr, const PxVec3& atrA, const PxVec3& atrB, const PxTransform& aTransform1, const PxVec3& aOrigin, const PxVec3& aExtent)
|
||||
: MeshHitCallback<PxGeomRaycastHit>(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 PxGeomRaycastHit& hit, const PxVec3&, const PxVec3&, const PxVec3&, PxReal& shrunkMaxT, const PxU32*) PX_OVERRIDE PX_FINAL
|
||||
{
|
||||
const TriangleHelper convexPartOfMesh1(shapeMesh, meshScaling, hit.faceIndex);
|
||||
const PxVec3 resultNormal = -transform1.rotate(convexPartOfMesh1.getPolygonNormal());
|
||||
if(relTr.dot(resultNormal) >= sumFastMovingThresh)
|
||||
{
|
||||
PxBounds3 bounds;
|
||||
convexPartOfMesh1.getBounds(bounds, transform1);
|
||||
//OK, we have all 3 vertices, now calculate bounds...
|
||||
|
||||
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&) {}
|
||||
};
|
||||
|
||||
const PxVec3& origin = shape0.mCenter;
|
||||
const PxVec3 extent = shape0.mExtents + PxVec3(restDistance);
|
||||
|
||||
CB callback(fastMovingThreshold, shapeMesh, meshScaling, relTr, trA, trB, transform1, origin, extent);
|
||||
Midphase::intersectOBB(_getMeshData(shapeMesh), vertexSpaceBox, callback, true);
|
||||
|
||||
#ifdef CCD_BASIC_PROFILING
|
||||
unsigned long long time2 = __rdtsc();
|
||||
printf("Time: %d\n", PxU32(time2 - time)/1024);
|
||||
#endif
|
||||
|
||||
return callback.minTOI;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
165
engine/third_party/physx/source/geomutils/src/ccd/GuCCDSweepConvexMesh.h
vendored
Normal file
165
engine/third_party/physx/source/geomutils/src/ccd/GuCCDSweepConvexMesh.h
vendored
Normal file
@@ -0,0 +1,165 @@
|
||||
// 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.
|
||||
|
||||
#ifndef GU_CCD_SWEEP_CONVEX_MESH_H
|
||||
#define GU_CCD_SWEEP_CONVEX_MESH_H
|
||||
|
||||
#include "common/PxPhysXCommonConfig.h"
|
||||
#include "foundation/PxVecTransform.h"
|
||||
#include "CmScaling.h"
|
||||
|
||||
#define GU_TRIANGLE_SWEEP_METHOD_ARGS \
|
||||
const PxGeometry& shape0, \
|
||||
const PxGeometry& shape1, \
|
||||
const PxTransform32& transform0, \
|
||||
const PxTransform32& transform1, \
|
||||
const PxTransform32& lastTm0, \
|
||||
const PxTransform32& lastTm1, \
|
||||
PxReal restDistance, \
|
||||
PxVec3& worldNormal, \
|
||||
PxVec3& worldPoint, \
|
||||
const Cm::FastVertex2ShapeScaling& meshScaling, \
|
||||
Gu::TriangleV& triangle, \
|
||||
const PxF32 toiEstimate
|
||||
|
||||
#define GU_SWEEP_METHOD_ARGS \
|
||||
const Gu::CCDShape& shape0, \
|
||||
const Gu::CCDShape& shape1, \
|
||||
const PxTransform32& transform0, \
|
||||
const PxTransform32& transform1, \
|
||||
const PxTransform32& lastTm0, \
|
||||
const PxTransform32& lastTm1, \
|
||||
PxReal restDistance, \
|
||||
PxVec3& worldNormal, \
|
||||
PxVec3& worldPoint, \
|
||||
const PxF32 toiEstimate, \
|
||||
PxU32& outCCDFaceIndex, \
|
||||
const PxReal fastMovingThreshold
|
||||
|
||||
#define GU_SWEEP_ESTIMATE_ARGS \
|
||||
const CCDShape& shape0, \
|
||||
const CCDShape& shape1, \
|
||||
const PxReal restDistance, \
|
||||
const PxReal fastMovingThreshold
|
||||
|
||||
#define GU_SWEEP_METHOD_ARGS_UNUSED \
|
||||
const Gu::CCDShape& /*shape0*/, \
|
||||
const Gu::CCDShape& /*shape1*/, \
|
||||
const PxTransform32& /*transform0*/,\
|
||||
const PxTransform32& /*transform1*/,\
|
||||
const PxTransform32& /*lastTm0*/, \
|
||||
const PxTransform32& /*lastTm1*/, \
|
||||
PxReal /*restDistance*/, \
|
||||
PxVec3& /*worldNormal*/, \
|
||||
PxVec3& /*worldPoint*/, \
|
||||
const PxF32 /*toiEstimate*/, \
|
||||
PxU32& /*outCCDFaceIndex*/, \
|
||||
const PxReal /*fastMovingThreshold*/
|
||||
|
||||
namespace physx
|
||||
{
|
||||
namespace Gu
|
||||
{
|
||||
struct CCDShape
|
||||
{
|
||||
const PxGeometry* mGeometry;
|
||||
PxReal mFastMovingThreshold; //The CCD threshold for this shape
|
||||
PxTransform mPrevTransform; //This shape's previous transform
|
||||
PxTransform mCurrentTransform; //This shape's current transform
|
||||
PxVec3 mExtents; //The extents of this shape's AABB
|
||||
PxVec3 mCenter; //The center of this shape's AABB
|
||||
PxU32 mUpdateCount; //How many times this shape has been updated in the CCD. This is correlated with the CCD body's update count.
|
||||
};
|
||||
|
||||
PX_FORCE_INLINE PxF32 sweepAABBAABB(const PxVec3& centerA, const PxVec3& extentsA, const PxVec3& centerB, const PxVec3& extentsB, const PxVec3& trA, const PxVec3& trB)
|
||||
{
|
||||
//Sweep 2 AABBs against each other, return the TOI when they hit else PX_MAX_REAL if they don't hit
|
||||
const PxVec3 cAcB = centerA - centerB;
|
||||
const PxVec3 sumExtents = extentsA + extentsB;
|
||||
|
||||
//Initial hit
|
||||
if(PxAbs(cAcB.x) <= sumExtents.x &&
|
||||
PxAbs(cAcB.y) <= sumExtents.y &&
|
||||
PxAbs(cAcB.z) <= sumExtents.z)
|
||||
return 0.f;
|
||||
|
||||
//No initial hit - perform the sweep
|
||||
const PxVec3 relTr = trB - trA;
|
||||
PxF32 tfirst = 0.f;
|
||||
PxF32 tlast = 1.f;
|
||||
|
||||
const PxVec3 aMax = centerA + extentsA;
|
||||
const PxVec3 aMin = centerA - extentsA;
|
||||
const PxVec3 bMax = centerB + extentsB;
|
||||
const PxVec3 bMin = centerB - extentsB;
|
||||
|
||||
const PxF32 eps = 1e-6f;
|
||||
|
||||
for(PxU32 a = 0; a < 3; ++a)
|
||||
{
|
||||
if(relTr[a] < -eps)
|
||||
{
|
||||
if(bMax[a] < aMin[a])
|
||||
return PX_MAX_REAL;
|
||||
if(aMax[a] < bMin[a])
|
||||
tfirst = PxMax((aMax[a] - bMin[a])/relTr[a], tfirst);
|
||||
if(bMax[a] > aMin[a])
|
||||
tlast = PxMin((aMin[a] - bMax[a])/relTr[a], tlast);
|
||||
}
|
||||
else if(relTr[a] > eps)
|
||||
{
|
||||
if(bMin[a] > aMax[a])
|
||||
return PX_MAX_REAL;
|
||||
if(bMax[a] < aMin[a])
|
||||
tfirst = PxMax((aMin[a] - bMax[a])/relTr[a], tfirst);
|
||||
if(aMax[a] > bMin[a])
|
||||
tlast = PxMin((aMax[a] - bMin[a])/relTr[a], tlast);
|
||||
}
|
||||
else
|
||||
{
|
||||
if(bMax[a] < aMin[a] || bMin[a] > aMax[a])
|
||||
return PX_MAX_REAL;
|
||||
}
|
||||
|
||||
//No hit
|
||||
if(tfirst > tlast)
|
||||
return PX_MAX_REAL;
|
||||
}
|
||||
//There was a hit so return the TOI
|
||||
return tfirst;
|
||||
}
|
||||
|
||||
PX_PHYSX_COMMON_API PxReal SweepShapeShape(GU_SWEEP_METHOD_ARGS);
|
||||
|
||||
PX_PHYSX_COMMON_API PxReal SweepEstimateAnyShapeHeightfield(GU_SWEEP_ESTIMATE_ARGS);
|
||||
|
||||
PX_PHYSX_COMMON_API PxReal SweepEstimateAnyShapeMesh(GU_SWEEP_ESTIMATE_ARGS);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
415
engine/third_party/physx/source/geomutils/src/ccd/GuCCDSweepPrimitives.cpp
vendored
Normal file
415
engine/third_party/physx/source/geomutils/src/ccd/GuCCDSweepPrimitives.cpp
vendored
Normal file
@@ -0,0 +1,415 @@
|
||||
// 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 "GuVecCapsule.h"
|
||||
#include "GuVecBox.h"
|
||||
#include "GuVecConvexHull.h"
|
||||
#include "GuVecTriangle.h"
|
||||
#include "GuGJKRaycast.h"
|
||||
#include "GuCCDSweepConvexMesh.h"
|
||||
#include "GuGJKType.h"
|
||||
#include "geometry/PxSphereGeometry.h"
|
||||
#include "geometry/PxCustomGeometry.h"
|
||||
|
||||
//#define USE_VIRTUAL_GJK
|
||||
|
||||
namespace physx
|
||||
{
|
||||
namespace Gu
|
||||
{
|
||||
|
||||
using namespace aos;
|
||||
|
||||
template<typename Geom> PX_FORCE_INLINE PxReal getRadius(const PxGeometry&)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
template<> PX_FORCE_INLINE PxReal getRadius<CapsuleV>(const PxGeometry& g)
|
||||
{
|
||||
PX_ASSERT(g.getType() == PxGeometryType::eCAPSULE || g.getType() == PxGeometryType::eSPHERE);
|
||||
PX_COMPILE_TIME_ASSERT(PX_OFFSET_OF(PxSphereGeometry, radius) == PX_OFFSET_OF(PxCapsuleGeometry, radius));
|
||||
return static_cast<const PxSphereGeometry&>(g).radius;
|
||||
}
|
||||
|
||||
#ifdef USE_VIRTUAL_GJK
|
||||
static bool virtualGjkRaycastPenetration(const GjkConvex& a, const GjkConvex& b, const aos::Vec3VArg initialDir, const aos::FloatVArg initialLambda, const aos::Vec3VArg s, const aos::Vec3VArg r, aos::FloatV& lambda,
|
||||
aos::Vec3V& normal, aos::Vec3V& closestA, const PxReal _inflation, const bool initialOverlap)
|
||||
{
|
||||
return gjkRaycastPenetration<GjkConvex, GjkConvex >(a, b, initialDir, initialLambda, s, r, lambda, normal, closestA, _inflation, initialOverlap);
|
||||
}
|
||||
#endif
|
||||
|
||||
template<class ConvexA, class ConvexB>
|
||||
static PX_FORCE_INLINE PxReal CCDSweep( ConvexA& a, ConvexB& b, const PxTransform32& transform0, const PxTransform32& transform1, const PxTransform32& lastTm0, const PxTransform32& lastTm1,
|
||||
const aos::FloatV& toiEstimate, PxVec3& worldPoint, PxVec3& worldNormal, PxReal inflation = 0.0f)
|
||||
{
|
||||
PX_UNUSED(toiEstimate); //KS - TODO - can we use this again?
|
||||
using namespace aos;
|
||||
|
||||
const QuatV q0 = QuatVLoadA(&transform0.q.x);
|
||||
const Vec3V p0 = V3LoadA(&lastTm0.p.x);
|
||||
|
||||
const QuatV q1 = QuatVLoadA(&transform1.q.x);
|
||||
const Vec3V p1 = V3LoadA(&lastTm1.p.x);
|
||||
|
||||
const PxTransformV tr0(p0, q0);
|
||||
const PxTransformV tr1(p1, q1);
|
||||
|
||||
const PxMatTransformV aToB(tr1.transformInv(tr0));
|
||||
|
||||
const Vec3V trans0p = V3LoadA(transform0.p);
|
||||
const Vec3V trans1p = V3LoadA(transform1.p);
|
||||
const Vec3V trA = V3Sub(trans0p, p0);
|
||||
const Vec3V trB = V3Sub(trans1p, p1);
|
||||
const Vec3V relTr = tr1.rotateInv(V3Sub(trB, trA));
|
||||
|
||||
FloatV lambda;
|
||||
Vec3V closestA, normal;
|
||||
const FloatV initialLambda = FZero();
|
||||
const RelativeConvex<ConvexA> convexA(a, aToB);
|
||||
const LocalConvex<ConvexB> convexB(b);
|
||||
#ifdef USE_VIRTUAL_GJK
|
||||
if(virtualGjkRaycastPenetration(convexA, convexB, aToB.p, initialLambda, V3Zero(), relTr, lambda, normal, closestA, inflation, true))
|
||||
#else
|
||||
if(gjkRaycastPenetration<RelativeConvex<ConvexA>, LocalConvex<ConvexB> >(convexA, convexB, aToB.p, initialLambda, V3Zero(), relTr, lambda, normal, closestA, inflation, true))
|
||||
#endif
|
||||
{
|
||||
//Adjust closestA because it will be on the surface of convex a in its initial position (s). If the TOI > 0, we need to move
|
||||
//the point along the sweep direction to get the world-space hit position.
|
||||
PxF32 res;
|
||||
FStore(lambda, &res);
|
||||
closestA = V3ScaleAdd(trA, FMax(lambda, FZero()), tr1.transform(closestA));
|
||||
normal = tr1.rotate(normal);
|
||||
|
||||
V3StoreU(normal, worldNormal);
|
||||
V3StoreU(closestA, worldPoint);
|
||||
return res;
|
||||
}
|
||||
return PX_MAX_REAL;
|
||||
}
|
||||
|
||||
//
|
||||
// lookup table for geometry-vs-geometry sweeps
|
||||
//
|
||||
|
||||
PxReal UnimplementedSweep (GU_SWEEP_METHOD_ARGS_UNUSED)
|
||||
{
|
||||
return PX_MAX_REAL; //no impact
|
||||
}
|
||||
|
||||
template<typename Geom0, typename Geom1>
|
||||
static PxReal SweepGeomGeom(GU_SWEEP_METHOD_ARGS)
|
||||
{
|
||||
PX_UNUSED(outCCDFaceIndex);
|
||||
PX_UNUSED(fastMovingThreshold);
|
||||
|
||||
const PxGeometry& g0 = *shape0.mGeometry;
|
||||
const PxGeometry& g1 = *shape1.mGeometry;
|
||||
|
||||
typename ConvexGeom<Geom0>::Type geom0(g0);
|
||||
typename ConvexGeom<Geom1>::Type geom1(g1);
|
||||
|
||||
return CCDSweep(geom0, geom1, transform0, transform1, lastTm0, lastTm1, FLoad(toiEstimate), worldPoint, worldNormal, restDistance+getRadius<Geom0>(g0)+getRadius<Geom1>(g1) );
|
||||
}
|
||||
|
||||
static PxReal SweepAnyShapeCustom(GU_SWEEP_METHOD_ARGS)
|
||||
{
|
||||
PX_UNUSED(fastMovingThreshold);
|
||||
PX_UNUSED(toiEstimate);
|
||||
PX_UNUSED(restDistance);
|
||||
|
||||
const PxGeometry& g0 = *shape0.mGeometry;
|
||||
const PxGeometry& g1 = *shape1.mGeometry;
|
||||
|
||||
PX_ASSERT(g1.getType() == PxGeometryType::eCUSTOM);
|
||||
|
||||
const PxVec3 trA = transform0.p - lastTm0.p;
|
||||
const PxVec3 trB = transform1.p - lastTm1.p;
|
||||
|
||||
const PxVec3 relTr = trA - trB;
|
||||
PxVec3 unitDir = relTr;
|
||||
const PxReal length = unitDir.normalize();
|
||||
|
||||
PxGeomSweepHit sweepHit;
|
||||
|
||||
if (!static_cast<const PxCustomGeometry&>(g1).callbacks->sweep(unitDir, length, g1, lastTm1, g0, lastTm0, sweepHit, PxHitFlag::eDEFAULT, 0.0f, NULL))
|
||||
return PX_MAX_REAL;
|
||||
|
||||
worldNormal = sweepHit.normal;
|
||||
worldPoint = sweepHit.position;
|
||||
outCCDFaceIndex = sweepHit.faceIndex;
|
||||
|
||||
return sweepHit.distance / length;
|
||||
}
|
||||
|
||||
typedef PxReal (*SweepMethod) (GU_SWEEP_METHOD_ARGS);
|
||||
|
||||
PxReal SweepAnyShapeHeightfield(GU_SWEEP_METHOD_ARGS);
|
||||
PxReal SweepAnyShapeMesh(GU_SWEEP_METHOD_ARGS);
|
||||
|
||||
SweepMethod g_SweepMethodTable[][PxGeometryType::eGEOMETRY_COUNT] =
|
||||
{
|
||||
//PxGeometryType::eSPHERE
|
||||
{
|
||||
SweepGeomGeom<CapsuleV, CapsuleV>, //PxGeometryType::eSPHERE
|
||||
UnimplementedSweep, //PxGeometryType::ePLANE
|
||||
SweepGeomGeom<CapsuleV, CapsuleV>, //PxGeometryType::eCAPSULE
|
||||
SweepGeomGeom<CapsuleV, BoxV>, //PxGeometryType::eBOX
|
||||
UnimplementedSweep, //PxGeometryType::eCONVEXCORE
|
||||
SweepGeomGeom<CapsuleV, ConvexHullV>, //PxGeometryType::eCONVEXMESH
|
||||
UnimplementedSweep, //PxGeometryType::ePARTICLESYSTEM
|
||||
UnimplementedSweep, //PxGeometryType::eTETRAHEDRONMESH
|
||||
SweepAnyShapeMesh, //PxGeometryType::eTRIANGLEMESH
|
||||
SweepAnyShapeHeightfield, //PxGeometryType::eHEIGHTFIELD
|
||||
SweepAnyShapeCustom, //PxGeometryType::eCUSTOM
|
||||
},
|
||||
|
||||
//PxGeometryType::ePLANE
|
||||
{
|
||||
0, //PxGeometryType::eSPHERE
|
||||
UnimplementedSweep, //PxGeometryType::ePLANE
|
||||
UnimplementedSweep, //PxGeometryType::eCAPSULE
|
||||
UnimplementedSweep, //PxGeometryType::eBOX
|
||||
UnimplementedSweep, //PxGeometryType::eCONVEXCORE
|
||||
UnimplementedSweep, //PxGeometryType::eCONVEXMESH
|
||||
UnimplementedSweep, //PxGeometryType::ePARTICLESYSTEM
|
||||
UnimplementedSweep, //PxGeometryType::eTETRAHEDRONMESH
|
||||
UnimplementedSweep, //PxGeometryType::eTRIANGLEMESH
|
||||
UnimplementedSweep, //PxGeometryType::eHEIGHTFIELD
|
||||
UnimplementedSweep, //PxGeometryType::eCUSTOM
|
||||
},
|
||||
|
||||
//PxGeometryType::eCAPSULE
|
||||
{
|
||||
0, //PxGeometryType::eSPHERE
|
||||
0, //PxGeometryType::ePLANE
|
||||
SweepGeomGeom<CapsuleV, CapsuleV>, //PxGeometryType::eCAPSULE
|
||||
SweepGeomGeom<CapsuleV, BoxV>, //PxGeometryType::eBOX
|
||||
UnimplementedSweep, //PxGeometryType::eCONVEXCORE
|
||||
SweepGeomGeom<CapsuleV, ConvexHullV>, //PxGeometryType::eCONVEXMESH
|
||||
UnimplementedSweep, //PxGeometryType::ePARTICLESYSTEM
|
||||
UnimplementedSweep, //PxGeometryType::eTETRAHEDRONMESH
|
||||
SweepAnyShapeMesh, //PxGeometryType::eTRIANGLEMESH
|
||||
SweepAnyShapeHeightfield, //PxGeometryType::eHEIGHTFIELD
|
||||
SweepAnyShapeCustom, //PxGeometryType::eCUSTOM
|
||||
},
|
||||
|
||||
//PxGeometryType::eBOX
|
||||
{
|
||||
0, //PxGeometryType::eSPHERE
|
||||
0, //PxGeometryType::ePLANE
|
||||
0, //PxGeometryType::eCAPSULE
|
||||
SweepGeomGeom<BoxV, BoxV>, //PxGeometryType::eBOX
|
||||
UnimplementedSweep, //PxGeometryType::eCONVEXCORE
|
||||
SweepGeomGeom<BoxV, ConvexHullV>, //PxGeometryType::eCONVEXMESH
|
||||
UnimplementedSweep, //PxGeometryType::ePARTICLESYSTEM
|
||||
UnimplementedSweep, //PxGeometryType::eTETRAHEDRONMESH
|
||||
SweepAnyShapeMesh, //PxGeometryType::eTRIANGLEMESH
|
||||
SweepAnyShapeHeightfield, //PxGeometryType::eHEIGHTFIELD
|
||||
SweepAnyShapeCustom, //PxGeometryType::eCUSTOM
|
||||
},
|
||||
|
||||
//PxGeometryType::eCONVEXCORE
|
||||
{
|
||||
0, //PxGeometryType::eSPHERE
|
||||
0, //PxGeometryType::ePLANE
|
||||
0, //PxGeometryType::eCAPSULE
|
||||
0, //PxGeometryType::eBOX
|
||||
UnimplementedSweep, //PxGeometryType::eCONVEXCORE
|
||||
UnimplementedSweep, //PxGeometryType::eCONVEXMESH
|
||||
UnimplementedSweep, //PxGeometryType::ePARTICLESYSTEM
|
||||
UnimplementedSweep, //PxGeometryType::eTETRAHEDRONMESH
|
||||
UnimplementedSweep, //PxGeometryType::eTRIANGLEMESH
|
||||
UnimplementedSweep, //PxGeometryType::eHEIGHTFIELD
|
||||
UnimplementedSweep, //PxGeometryType::eCUSTOM
|
||||
},
|
||||
|
||||
//PxGeometryType::eCONVEXMESH
|
||||
{
|
||||
0, //PxGeometryType::eSPHERE
|
||||
0, //PxGeometryType::ePLANE
|
||||
0, //PxGeometryType::eCAPSULE
|
||||
0, //PxGeometryType::eBOX
|
||||
0, //PxGeometryType::eCONVEXCORE
|
||||
SweepGeomGeom<ConvexHullV, ConvexHullV>, //PxGeometryType::eCONVEXMESH
|
||||
UnimplementedSweep, //PxGeometryType::ePARTICLESYSTEM
|
||||
UnimplementedSweep, //PxGeometryType::eTETRAHEDRONMESH
|
||||
SweepAnyShapeMesh, //PxGeometryType::eTRIANGLEMESH
|
||||
SweepAnyShapeHeightfield, //PxGeometryType::eHEIGHTFIELD
|
||||
SweepAnyShapeCustom, //PxGeometryType::eCUSTOM
|
||||
},
|
||||
|
||||
//PxGeometryType::ePARTICLESYSTEM
|
||||
{
|
||||
0, //PxGeometryType::eSPHERE
|
||||
0, //PxGeometryType::ePLANE
|
||||
0, //PxGeometryType::eCAPSULE
|
||||
0, //PxGeometryType::eBOX
|
||||
0, //PxGeometryType::eCONVEXCORE
|
||||
0, //PxGeometryType::eCONVEXMESH
|
||||
UnimplementedSweep, //PxGeometryType::ePARTICLESYSTEM
|
||||
UnimplementedSweep, //PxGeometryType::eTETRAHEDRONMESH
|
||||
UnimplementedSweep, //PxGeometryType::eTRIANGLEMESH
|
||||
UnimplementedSweep, //PxGeometryType::eHEIGHTFIELD
|
||||
UnimplementedSweep, //PxGeometryType::eCUSTOM
|
||||
},
|
||||
|
||||
//PxGeometryType::eTETRAHEDRONMESH
|
||||
{
|
||||
0, //PxGeometryType::eSPHERE
|
||||
0, //PxGeometryType::ePLANE
|
||||
0, //PxGeometryType::eCAPSULE
|
||||
0, //PxGeometryType::eBOX
|
||||
0, //PxGeometryType::eCONVEXCORE
|
||||
0, //PxGeometryType::eCONVEXMESH
|
||||
0, //PxGeometryType::ePARTICLESYSTEM
|
||||
UnimplementedSweep, //PxGeometryType::eTETRAHEDRONMESH
|
||||
UnimplementedSweep, //PxGeometryType::eTRIANGLEMESH
|
||||
UnimplementedSweep, //PxGeometryType::eHEIGHTFIELD
|
||||
UnimplementedSweep, //PxGeometryType::eCUSTOM
|
||||
},
|
||||
|
||||
//PxGeometryType::eTRIANGLEMESH
|
||||
{
|
||||
0, //PxGeometryType::eSPHERE
|
||||
0, //PxGeometryType::ePLANE
|
||||
0, //PxGeometryType::eCAPSULE
|
||||
0, //PxGeometryType::eBOX
|
||||
0, //PxGeometryType::eCONVEXCORE
|
||||
0, //PxGeometryType::eCONVEXMESH
|
||||
0, //PxGeometryType::ePARTICLESYSTEM
|
||||
0, //PxGeometryType::eTETRAHEDRONMESH
|
||||
UnimplementedSweep, //PxGeometryType::eTRIANGLEMESH
|
||||
UnimplementedSweep, //PxGeometryType::eHEIGHTFIELD
|
||||
SweepAnyShapeCustom, //PxGeometryType::eCUSTOM
|
||||
},
|
||||
|
||||
//PxGeometryType::eHEIGHTFIELD
|
||||
{
|
||||
0, //PxGeometryType::eSPHERE
|
||||
0, //PxGeometryType::ePLANE
|
||||
0, //PxGeometryType::eCAPSULE
|
||||
0, //PxGeometryType::eBOX
|
||||
0, //PxGeometryType::eCONVEXCORE
|
||||
0, //PxGeometryType::eCONVEXMESH
|
||||
0, //PxGeometryType::ePARTICLESYSTEM
|
||||
0, //PxGeometryType::eTETRAHEDRONMESH
|
||||
0, //PxGeometryType::eTRIANGLEMESH
|
||||
UnimplementedSweep, //PxGeometryType::eHEIGHTFIELD
|
||||
SweepAnyShapeCustom, //PxGeometryType::eCUSTOM
|
||||
},
|
||||
|
||||
//PxGeometryType::eCUSTOM
|
||||
{
|
||||
0, //PxGeometryType::eSPHERE
|
||||
0, //PxGeometryType::ePLANE
|
||||
0, //PxGeometryType::eCAPSULE
|
||||
0, //PxGeometryType::eBOX
|
||||
0, //PxGeometryType::eCONVEXCORE
|
||||
0, //PxGeometryType::eCONVEXMESH
|
||||
0, //PxGeometryType::ePARTICLESYSTEM
|
||||
0, //PxGeometryType::eTETRAHEDRONMESH
|
||||
0, //PxGeometryType::eTRIANGLEMESH
|
||||
0, //PxGeometryType::eHEIGHTFIELD
|
||||
SweepAnyShapeCustom, //PxGeometryType::eCUSTOM
|
||||
},
|
||||
};
|
||||
PX_COMPILE_TIME_ASSERT(sizeof(g_SweepMethodTable) / sizeof(g_SweepMethodTable[0]) == PxGeometryType::eGEOMETRY_COUNT);
|
||||
|
||||
PxReal SweepShapeShape(GU_SWEEP_METHOD_ARGS)
|
||||
{
|
||||
const PxGeometryType::Enum type0 = shape0.mGeometry->getType();
|
||||
const PxGeometryType::Enum type1 = shape1.mGeometry->getType();
|
||||
|
||||
return g_SweepMethodTable[type0][type1](shape0, shape1, transform0, transform1, lastTm0, lastTm1,
|
||||
restDistance, worldNormal, worldPoint, toiEstimate, outCCDFaceIndex, fastMovingThreshold);
|
||||
}
|
||||
|
||||
//
|
||||
// lookup table for sweeps agains triangles
|
||||
//
|
||||
|
||||
PxReal UnimplementedTriangleSweep(GU_TRIANGLE_SWEEP_METHOD_ARGS)
|
||||
{
|
||||
PX_UNUSED(shape0);
|
||||
PX_UNUSED(shape1);
|
||||
PX_UNUSED(transform0);
|
||||
PX_UNUSED(transform1);
|
||||
PX_UNUSED(lastTm0);
|
||||
PX_UNUSED(lastTm1);
|
||||
PX_UNUSED(restDistance);
|
||||
PX_UNUSED(worldNormal);
|
||||
PX_UNUSED(worldPoint);
|
||||
PX_UNUSED(meshScaling);
|
||||
PX_UNUSED(triangle);
|
||||
PX_UNUSED(toiEstimate);
|
||||
|
||||
return 1e10f; //no impact
|
||||
}
|
||||
|
||||
template<typename Geom>
|
||||
PxReal SweepGeomTriangles(GU_TRIANGLE_SWEEP_METHOD_ARGS)
|
||||
{
|
||||
PX_UNUSED(meshScaling);
|
||||
PX_UNUSED(shape1);
|
||||
|
||||
const PxGeometry& g = shape0;
|
||||
//Geom geom(g);
|
||||
typename ConvexGeom<Geom>::Type geom(g);
|
||||
|
||||
return CCDSweep<TriangleV, Geom>(triangle, geom, transform1, transform0, lastTm1, lastTm0, FLoad(toiEstimate), worldPoint, worldNormal, restDistance+getRadius<Geom>(g) );
|
||||
}
|
||||
|
||||
typedef PxReal (*TriangleSweepMethod) (GU_TRIANGLE_SWEEP_METHOD_ARGS);
|
||||
TriangleSweepMethod g_TriangleSweepMethodTable[] =
|
||||
{
|
||||
SweepGeomTriangles<CapsuleV>, //PxGeometryType::eSPHERE
|
||||
UnimplementedTriangleSweep, //PxGeometryType::ePLANE
|
||||
SweepGeomTriangles<CapsuleV>, //PxGeometryType::eCAPSULE
|
||||
SweepGeomTriangles<BoxV>, //PxGeometryType::eBOX
|
||||
UnimplementedTriangleSweep, //PxGeometryType::eCONVEXCORE
|
||||
SweepGeomTriangles<ConvexHullV>, //PxGeometryType::eCONVEXMESH
|
||||
UnimplementedTriangleSweep, //PxGeometryType::ePARTICLESYSTEM
|
||||
UnimplementedTriangleSweep, //PxGeometryType::eTETRAHEDRONMESH
|
||||
UnimplementedTriangleSweep, //PxGeometryType::eTRIANGLEMESH
|
||||
UnimplementedTriangleSweep, //PxGeometryType::eHEIGHTFIELD
|
||||
UnimplementedTriangleSweep, //PxGeometryType::eCUSTOM
|
||||
};
|
||||
PX_COMPILE_TIME_ASSERT(sizeof(g_TriangleSweepMethodTable) / sizeof(g_TriangleSweepMethodTable[0]) == PxGeometryType::eGEOMETRY_COUNT);
|
||||
|
||||
PxReal SweepShapeTriangle(GU_TRIANGLE_SWEEP_METHOD_ARGS)
|
||||
{
|
||||
const PxGeometryType::Enum type0 = shape0.getType();
|
||||
const TriangleSweepMethod method = g_TriangleSweepMethodTable[type0];
|
||||
return method(shape0, shape1, transform0, transform1, lastTm0, lastTm1, restDistance, worldNormal, worldPoint, meshScaling, triangle, toiEstimate);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user