355 lines
13 KiB
C++
355 lines
13 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/PxSphereGeometry.h"
|
|
#include "geometry/PxConvexCoreGeometry.h"
|
|
#include "GuMidphaseInterface.h"
|
|
#include "CmScaling.h"
|
|
#include "GuSphere.h"
|
|
#include "GuInternal.h"
|
|
#include "GuConvexUtilsInternal.h"
|
|
#include "GuVecTriangle.h"
|
|
#include "GuVecConvexHull.h"
|
|
#include "GuConvexMesh.h"
|
|
#include "GuGJK.h"
|
|
#include "GuSweepSharedTests.h"
|
|
#include "CmMatrix34.h"
|
|
#include "GuBounds.h"
|
|
#include "GuConvexSupport.h"
|
|
#include "GuConvexGeometry.h"
|
|
|
|
using namespace physx;
|
|
using namespace Cm;
|
|
using namespace Gu;
|
|
using namespace aos;
|
|
|
|
bool GeomOverlapCallback_SphereMesh(GU_OVERLAP_FUNC_PARAMS)
|
|
{
|
|
PX_ASSERT(geom0.getType()==PxGeometryType::eSPHERE);
|
|
PX_ASSERT(geom1.getType()==PxGeometryType::eTRIANGLEMESH);
|
|
PX_UNUSED(cache);
|
|
PX_UNUSED(threadContext);
|
|
|
|
const PxSphereGeometry& sphereGeom = static_cast<const PxSphereGeometry&>(geom0);
|
|
const PxTriangleMeshGeometry& meshGeom = static_cast<const PxTriangleMeshGeometry&>(geom1);
|
|
|
|
const Sphere worldSphere(pose0.p, sphereGeom.radius);
|
|
|
|
TriangleMesh* meshData = static_cast<TriangleMesh*>(meshGeom.triangleMesh);
|
|
return Midphase::intersectSphereVsMesh(worldSphere, *meshData, pose1, meshGeom.scale, NULL);
|
|
}
|
|
|
|
bool GeomOverlapCallback_CapsuleMesh(GU_OVERLAP_FUNC_PARAMS)
|
|
{
|
|
PX_ASSERT(geom0.getType()==PxGeometryType::eCAPSULE);
|
|
PX_ASSERT(geom1.getType()==PxGeometryType::eTRIANGLEMESH);
|
|
PX_UNUSED(cache);
|
|
PX_UNUSED(threadContext);
|
|
|
|
const PxCapsuleGeometry& capsuleGeom = static_cast<const PxCapsuleGeometry&>(geom0);
|
|
const PxTriangleMeshGeometry& meshGeom = static_cast<const PxTriangleMeshGeometry&>(geom1);
|
|
|
|
TriangleMesh* meshData = static_cast<TriangleMesh*>(meshGeom.triangleMesh);
|
|
|
|
Capsule capsule;
|
|
getCapsule(capsule, capsuleGeom, pose0);
|
|
return Midphase::intersectCapsuleVsMesh(capsule, *meshData, pose1, meshGeom.scale, NULL);
|
|
}
|
|
|
|
bool GeomOverlapCallback_BoxMesh(GU_OVERLAP_FUNC_PARAMS)
|
|
{
|
|
PX_ASSERT(geom0.getType()==PxGeometryType::eBOX);
|
|
PX_ASSERT(geom1.getType()==PxGeometryType::eTRIANGLEMESH);
|
|
PX_UNUSED(cache);
|
|
PX_UNUSED(threadContext);
|
|
|
|
const PxBoxGeometry& boxGeom = static_cast<const PxBoxGeometry&>(geom0);
|
|
const PxTriangleMeshGeometry& meshGeom = static_cast<const PxTriangleMeshGeometry&>(geom1);
|
|
|
|
TriangleMesh* meshData = static_cast<TriangleMesh*>(meshGeom.triangleMesh);
|
|
|
|
Box box;
|
|
buildFrom(box, pose0.p, boxGeom.halfExtents, pose0.q);
|
|
return Midphase::intersectBoxVsMesh(box, *meshData, pose1, meshGeom.scale, NULL);
|
|
}
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
|
|
namespace
|
|
{
|
|
struct ConvexVsMeshOverlapCallback : MeshHitCallback<PxGeomRaycastHit>
|
|
{
|
|
PxMatTransformV MeshToBoxV;
|
|
Vec3V boxExtents;
|
|
|
|
ConvexVsMeshOverlapCallback(
|
|
const ConvexMesh& cm, const PxMeshScale& convexScale, const FastVertex2ShapeScaling& meshScale,
|
|
const PxTransform& tr0, const PxTransform& tr1, bool identityScale, const Box& meshSpaceOBB)
|
|
:
|
|
MeshHitCallback<PxGeomRaycastHit>(CallbackMode::eMULTIPLE),
|
|
mAnyHit (false),
|
|
mIdentityScale (identityScale)
|
|
{
|
|
if (!identityScale) // not done in initializer list for performance
|
|
mMeshScale = aos::Mat33V(
|
|
V3LoadU(meshScale.getVertex2ShapeSkew().column0),
|
|
V3LoadU(meshScale.getVertex2ShapeSkew().column1),
|
|
V3LoadU(meshScale.getVertex2ShapeSkew().column2) );
|
|
using namespace aos;
|
|
|
|
const ConvexHullData* hullData = &cm.getHull();
|
|
|
|
const Vec3V vScale0 = V3LoadU_SafeReadW(convexScale.scale); // PT: safe because 'rotation' follows 'scale' in PxMeshScale
|
|
const QuatV vQuat0 = QuatVLoadU(&convexScale.rotation.x);
|
|
|
|
mConvex = ConvexHullV(hullData, V3Zero(), vScale0, vQuat0, convexScale.isIdentity());
|
|
aToB = PxMatTransformV(tr0.transformInv(tr1));
|
|
|
|
{
|
|
// Move to AABB space
|
|
PxMat34 MeshToBox;
|
|
computeWorldToBoxMatrix(MeshToBox, meshSpaceOBB);
|
|
|
|
const Vec3V base0 = V3LoadU(MeshToBox.m.column0);
|
|
const Vec3V base1 = V3LoadU(MeshToBox.m.column1);
|
|
const Vec3V base2 = V3LoadU(MeshToBox.m.column2);
|
|
const Mat33V matV(base0, base1, base2);
|
|
const Vec3V p = V3LoadU(MeshToBox.p);
|
|
MeshToBoxV = PxMatTransformV(p, matV);
|
|
boxExtents = V3LoadU(meshSpaceOBB.extents+PxVec3(0.001f));
|
|
}
|
|
}
|
|
virtual ~ConvexVsMeshOverlapCallback() {}
|
|
|
|
virtual PxAgain processHit( // all reported coords are in mesh local space including hit.position
|
|
const PxGeomRaycastHit&, const PxVec3& v0a, const PxVec3& v1a, const PxVec3& v2a, PxReal&, const PxU32*)
|
|
{
|
|
using namespace aos;
|
|
Vec3V v0 = V3LoadU(v0a);
|
|
Vec3V v1 = V3LoadU(v1a);
|
|
Vec3V v2 = V3LoadU(v2a);
|
|
|
|
// test triangle AABB in box space vs box AABB in box local space
|
|
{
|
|
const Vec3V triV0 = MeshToBoxV.transform(v0); // AP: MeshToBoxV already includes mesh scale so we have to use unscaled verts here
|
|
const Vec3V triV1 = MeshToBoxV.transform(v1);
|
|
const Vec3V triV2 = MeshToBoxV.transform(v2);
|
|
const Vec3V triMn = V3Min(V3Min(triV0, triV1), triV2);
|
|
const Vec3V triMx = V3Max(V3Max(triV0, triV1), triV2);
|
|
const Vec3V negExtents = V3Neg(boxExtents);
|
|
const BoolV minSeparated = V3IsGrtr(triMn, boxExtents), maxSeparated = V3IsGrtr(negExtents, triMx);
|
|
const BoolV bSeparated = BAnyTrue3(BOr(minSeparated, maxSeparated));
|
|
if(BAllEqTTTT(bSeparated))
|
|
return true; // continue traversal
|
|
}
|
|
|
|
if(!mIdentityScale)
|
|
{
|
|
v0 = M33MulV3(mMeshScale, v0);
|
|
v1 = M33MulV3(mMeshScale, v1);
|
|
v2 = M33MulV3(mMeshScale, v2);
|
|
}
|
|
|
|
TriangleV triangle(v0, v1, v2);
|
|
Vec3V contactA, contactB, normal;
|
|
FloatV dist;
|
|
const RelativeConvex<TriangleV> convexA(triangle, aToB);
|
|
const LocalConvex<ConvexHullV> convexB(mConvex);
|
|
const GjkStatus status = gjk(convexA, convexB, aToB.p, FZero(), contactA, contactB, normal, dist);
|
|
if(status == GJK_CONTACT || status == GJK_CLOSE)// || FAllGrtrOrEq(mSqTolerance, sqDist))
|
|
{
|
|
mAnyHit = true;
|
|
return false; // abort traversal
|
|
}
|
|
return true; // continue traversal
|
|
}
|
|
|
|
ConvexHullV mConvex;
|
|
PxMatTransformV aToB;
|
|
aos::Mat33V mMeshScale;
|
|
bool mAnyHit;
|
|
const bool mIdentityScale;
|
|
|
|
private:
|
|
ConvexVsMeshOverlapCallback& operator=(const ConvexVsMeshOverlapCallback&);
|
|
};
|
|
}
|
|
|
|
// PT: TODO: refactor bits of this with convex-vs-mesh code
|
|
bool GeomOverlapCallback_ConvexMesh(GU_OVERLAP_FUNC_PARAMS)
|
|
{
|
|
PX_ASSERT(geom0.getType()==PxGeometryType::eCONVEXMESH);
|
|
PX_ASSERT(geom1.getType()==PxGeometryType::eTRIANGLEMESH);
|
|
PX_UNUSED(cache);
|
|
PX_UNUSED(threadContext);
|
|
|
|
const PxConvexMeshGeometry& convexGeom = static_cast<const PxConvexMeshGeometry&>(geom0);
|
|
const PxTriangleMeshGeometry& meshGeom = static_cast<const PxTriangleMeshGeometry&>(geom1);
|
|
|
|
ConvexMesh* cm = static_cast<ConvexMesh*>(convexGeom.convexMesh);
|
|
TriangleMesh* meshData = static_cast<TriangleMesh*>(meshGeom.triangleMesh);
|
|
|
|
const bool idtScaleConvex = convexGeom.scale.isIdentity();
|
|
const bool idtScaleMesh = meshGeom.scale.isIdentity();
|
|
|
|
FastVertex2ShapeScaling convexScaling;
|
|
if (!idtScaleConvex)
|
|
convexScaling.init(convexGeom.scale);
|
|
|
|
FastVertex2ShapeScaling meshScaling;
|
|
if (!idtScaleMesh)
|
|
meshScaling.init(meshGeom.scale);
|
|
|
|
PX_ASSERT(!cm->getLocalBoundsFast().isEmpty());
|
|
const PxBounds3 hullAABB = cm->getLocalBoundsFast().transformFast(convexScaling.getVertex2ShapeSkew());
|
|
|
|
Box hullOBB;
|
|
{
|
|
const Matrix34FromTransform world0(pose0);
|
|
const Matrix34FromTransform world1(pose1);
|
|
computeHullOBB(hullOBB, hullAABB, 0.0f, world0, world1, meshScaling, idtScaleMesh);
|
|
}
|
|
|
|
ConvexVsMeshOverlapCallback cb(*cm, convexGeom.scale, meshScaling, pose0, pose1, idtScaleMesh, hullOBB);
|
|
Midphase::intersectOBB(meshData, hullOBB, cb, true, false);
|
|
|
|
return cb.mAnyHit;
|
|
}
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
|
|
bool GeomOverlapCallback_MeshMesh(GU_OVERLAP_FUNC_PARAMS)
|
|
{
|
|
PX_ASSERT(geom0.getType()==PxGeometryType::eTRIANGLEMESH);
|
|
PX_ASSERT(geom1.getType()==PxGeometryType::eTRIANGLEMESH);
|
|
PX_UNUSED(cache);
|
|
PX_UNUSED(threadContext);
|
|
|
|
const PxTriangleMeshGeometry& meshGeom0 = static_cast<const PxTriangleMeshGeometry&>(geom0);
|
|
const PxTriangleMeshGeometry& meshGeom1 = static_cast<const PxTriangleMeshGeometry&>(geom1);
|
|
|
|
const TriangleMesh* tm0 = static_cast<const TriangleMesh*>(meshGeom0.triangleMesh);
|
|
const TriangleMesh* tm1 = static_cast<const TriangleMesh*>(meshGeom1.triangleMesh);
|
|
|
|
// PT: only implemented for BV4
|
|
if(!tm0 || !tm1 || tm0->getConcreteType()!=PxConcreteType::eTRIANGLE_MESH_BVH34 || tm1->getConcreteType()!=PxConcreteType::eTRIANGLE_MESH_BVH34)
|
|
return PxGetFoundation().error(PxErrorCode::eINVALID_OPERATION, PX_FL, "PxGeometryQuery::overlap(): only available between two BVH34 triangles meshes.");
|
|
|
|
class AnyHitReportCallback : public PxReportCallback<PxGeomIndexPair>
|
|
{
|
|
public:
|
|
AnyHitReportCallback()
|
|
{
|
|
mCapacity = 1;
|
|
}
|
|
|
|
virtual bool flushResults(PxU32, const PxGeomIndexPair*)
|
|
{
|
|
return false;
|
|
}
|
|
};
|
|
|
|
AnyHitReportCallback callback;
|
|
|
|
// PT: ...so we don't need a table like for the other ops, just go straight to BV4
|
|
return intersectMeshVsMesh_BV4(callback, *tm0, pose0, meshGeom0.scale, *tm1, pose1, meshGeom1.scale, PxMeshMeshQueryFlag::eDEFAULT, 0.0f);
|
|
}
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
|
|
bool GeomOverlapCallback_ConvexCoreMesh(GU_OVERLAP_FUNC_PARAMS)
|
|
{
|
|
PX_UNUSED(geom0);
|
|
PX_UNUSED(pose0);
|
|
PX_UNUSED(geom1);
|
|
PX_UNUSED(pose1);
|
|
PX_UNUSED(cache);
|
|
PX_UNUSED(threadContext);
|
|
|
|
struct Callback : MeshHitCallback<PxGeomRaycastHit>
|
|
{
|
|
Gu::ConvexShape convex;
|
|
PxMeshScale scale;
|
|
bool hit;
|
|
|
|
Callback(const PxConvexCoreGeometry& geom, const PxTransform& pose, const PxMeshScale& s)
|
|
:
|
|
MeshHitCallback<PxGeomRaycastHit>(CallbackMode::eMULTIPLE),
|
|
scale(s), hit(false)
|
|
{
|
|
Gu::makeConvexShape(geom, pose, convex);
|
|
}
|
|
|
|
virtual PxAgain processHit(const PxGeomRaycastHit& /*hit*/, const PxVec3& v0, const PxVec3& v1, const PxVec3& v2, PxReal&, const PxU32*)
|
|
{
|
|
const PxVec3 verts[] = { v0, v1, v2 };
|
|
|
|
Gu::ConvexShape tri;
|
|
tri.coreType = Gu::ConvexCore::Type::ePOINTS;
|
|
tri.pose = PxTransform(PxIdentity);
|
|
Gu::ConvexCore::PointsCore& core = *reinterpret_cast<Gu::ConvexCore::PointsCore*>(tri.coreData);
|
|
core.points = verts;
|
|
core.numPoints = 3;
|
|
core.stride = sizeof(PxVec3);
|
|
core.S = scale.scale;
|
|
core.R = scale.rotation;
|
|
tri.margin = 0;
|
|
|
|
PxVec3 point0, point1, axis;
|
|
PxReal dist = Gu::RefGjkEpa::computeGjkDistance(convex, tri, convex.pose, tri.pose, convex.margin + tri.margin, point0, point1, axis);
|
|
hit = (dist <= convex.margin + tri.margin);
|
|
|
|
return !hit;
|
|
}
|
|
};
|
|
|
|
PX_ASSERT(geom0.getType() == PxGeometryType::eCONVEXCORE);
|
|
PX_ASSERT(geom1.getType() == PxGeometryType::eTRIANGLEMESH);
|
|
|
|
const PxConvexCoreGeometry& shapeConvex = static_cast<const PxConvexCoreGeometry&>(geom0);
|
|
const PxTriangleMeshGeometry& shapeMesh = static_cast<const PxTriangleMeshGeometry&>(geom1);
|
|
const TriangleMesh* meshData = _getMeshData(shapeMesh);
|
|
|
|
const PxTransform pose0in1 = pose1.transformInv(pose0);
|
|
const PxBounds3 bounds = Gu::computeBounds(geom0, PxTransform(PxIdentity));
|
|
|
|
Box queryBox;
|
|
queryBox.extents = bounds.getExtents();
|
|
queryBox.center = pose0in1.transform(bounds.getCenter());
|
|
queryBox.rot = PxMat33(pose0in1.q);
|
|
|
|
const FastVertex2ShapeScaling meshScaling(shapeMesh.scale);
|
|
meshScaling.transformQueryBounds(queryBox.center, queryBox.extents, queryBox.rot);
|
|
|
|
Callback callback(shapeConvex, pose0in1, shapeMesh.scale);
|
|
|
|
Midphase::intersectOBB(meshData, queryBox, callback, false);
|
|
|
|
return callback.hit;
|
|
}
|