416 lines
16 KiB
C++
416 lines
16 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 "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);
|
|
}
|
|
|
|
}
|
|
}
|
|
|