feat(physics): wire physx sdk into build
This commit is contained in:
971
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactBoxBox.cpp
vendored
Normal file
971
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactBoxBox.cpp
vendored
Normal file
@@ -0,0 +1,971 @@
|
||||
// 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 "geomutils/PxContactBuffer.h"
|
||||
#include "GuGJKPenetration.h"
|
||||
#include "GuEPA.h"
|
||||
#include "GuVecBox.h"
|
||||
#include "GuConvexHelper.h"
|
||||
#include "GuPCMShapeConvex.h"
|
||||
#include "GuContactMethodImpl.h"
|
||||
#include "GuPCMContactGenUtil.h"
|
||||
|
||||
using namespace physx;
|
||||
using namespace Gu;
|
||||
using namespace aos;
|
||||
|
||||
static void getIncidentPolygon(Vec3V* pts, Vec3V& faceNormal, const Vec3VArg axis, const PxMatTransformV& transf1To0, const Vec3VArg extents)
|
||||
{
|
||||
const FloatV zero = FZero();
|
||||
|
||||
FloatV ex = V3GetX(extents);
|
||||
FloatV ey = V3GetY(extents);
|
||||
FloatV ez = V3GetZ(extents);
|
||||
|
||||
const Vec3V u0 = transf1To0.getCol0();
|
||||
const Vec3V u1 = transf1To0.getCol1();
|
||||
const Vec3V u2 = transf1To0.getCol2();
|
||||
|
||||
//calculate the insident face for b
|
||||
const FloatV d0 = V3Dot(u0, axis);
|
||||
const FloatV d1 = V3Dot(u1, axis);
|
||||
const FloatV d2 = V3Dot(u2, axis);
|
||||
|
||||
const FloatV absd0 = FAbs(d0);
|
||||
const FloatV absd1 = FAbs(d1);
|
||||
const FloatV absd2 = FAbs(d2);
|
||||
|
||||
if(FAllGrtrOrEq(absd0, absd1) && FAllGrtrOrEq(absd0, absd2))
|
||||
{
|
||||
//the incident face is on u0
|
||||
const BoolV con = FIsGrtr(d0, zero);
|
||||
faceNormal = V3Sel(con, V3Neg(u0), u0);
|
||||
ex = FSel(con, FNeg(ex), ex);
|
||||
const Vec3V r0 = V3Scale(u0, ex);
|
||||
const Vec3V r1 = V3Scale(u1, ey);
|
||||
const Vec3V r2 = V3Scale(u2, ez);
|
||||
|
||||
const Vec3V temp0 = V3Add(transf1To0.p, r0);
|
||||
const Vec3V temp1 = V3Add(r1, r2);
|
||||
const Vec3V temp2 = V3Sub(r1, r2);
|
||||
|
||||
pts[0] = V3Add(temp0, temp1); // (-x/x, y, z)
|
||||
pts[1] = V3Add(temp0, temp2); // (-x/x, y, -z)
|
||||
pts[2] = V3Sub(temp0, temp1); // (-x/x, -y, -z)
|
||||
pts[3] = V3Sub(temp0, temp2); // (-x/x, -y, z)
|
||||
}
|
||||
else if(FAllGrtrOrEq(absd1, absd2))
|
||||
{
|
||||
//the incident face is on u1
|
||||
const BoolV con = FIsGrtr(d1, zero);
|
||||
faceNormal = V3Sel(con, V3Neg(u1), u1);
|
||||
ey = FSel(con, FNeg(ey), ey);
|
||||
const Vec3V r0 = V3Scale(u0, ex);
|
||||
const Vec3V r1 = V3Scale(u1, ey);
|
||||
const Vec3V r2 = V3Scale(u2, ez);
|
||||
|
||||
const Vec3V temp0 = V3Add(transf1To0.p, r1);
|
||||
const Vec3V temp1 = V3Add(r0, r2);
|
||||
const Vec3V temp2 = V3Sub(r0, r2);
|
||||
|
||||
pts[0] = V3Add(temp0, temp1); // (x, -y/y, z)
|
||||
pts[1] = V3Add(temp0, temp2); // (x, -y/y, -z)
|
||||
pts[2] = V3Sub(temp0, temp1); // (-x, -y/y, -z)
|
||||
pts[3] = V3Sub(temp0, temp2); // (-x, -y/y, z)
|
||||
}
|
||||
else
|
||||
{
|
||||
//the incident face is on u2
|
||||
const BoolV con = FIsGrtr(d2, zero);
|
||||
faceNormal = V3Sel(con, V3Neg(u2), u2);
|
||||
ez = FSel(con, FNeg(ez), ez);
|
||||
const Vec3V r0 = V3Scale(u0, ex);
|
||||
const Vec3V r1 = V3Scale(u1, ey);
|
||||
const Vec3V r2 = V3Scale(u2, ez);
|
||||
|
||||
const Vec3V temp0 = V3Add(transf1To0.p, r2);
|
||||
const Vec3V temp1 = V3Add(r0, r1);
|
||||
const Vec3V temp2 = V3Sub(r0, r1);
|
||||
|
||||
pts[0] = V3Add(temp0, temp1); // ( x, y, z)
|
||||
pts[1] = V3Add(temp0, temp2); // ( x, -y, z)
|
||||
pts[2] = V3Sub(temp0, temp1); // (-x, -y, z)
|
||||
pts[3] = V3Sub(temp0, temp2); // (-x, y, z)
|
||||
}
|
||||
}
|
||||
|
||||
//p0 and p1 is in the local space of AABB
|
||||
static bool intersectSegmentAABB(const Vec3VArg p0, const Vec3VArg d, const Vec3VArg max, const Vec3VArg min, FloatV& tmin, FloatV& tmax)
|
||||
{
|
||||
const Vec3V eps = V3Load(1e-6f);
|
||||
const Vec3V absV = V3Abs(d);
|
||||
const FloatV one = FOne();
|
||||
const Vec3V zero = V3Zero();
|
||||
const Vec3V fMax = Vec3V_From_FloatV(FMax());
|
||||
|
||||
FloatV tminf = FZero();
|
||||
FloatV tmaxf = one;
|
||||
const BoolV isParallel = V3IsGrtr(eps, absV);
|
||||
const BoolV isOutsideOfRange = BOr(V3IsGrtr(p0, max), V3IsGrtr(min, p0));
|
||||
//const BoolV isParallelAndOutOfRange = BAnd(isParallel, isOutsideOfRange);
|
||||
|
||||
if(!BAllEqFFFF(BAnd(isParallel, isOutsideOfRange)))
|
||||
return false;
|
||||
|
||||
const Vec3V odd = V3RecipFast(d);
|
||||
const Vec3V t1 = V3Sel(isParallel, zero, V3Mul(V3Sub(min, p0), odd));
|
||||
const Vec3V t2 = V3Sel(isParallel, fMax, V3Mul(V3Sub(max, p0), odd));
|
||||
|
||||
const Vec3V tt1 = V3Min(t1, t2);
|
||||
const Vec3V tt2 = V3Max(t1, t2);
|
||||
|
||||
const FloatV ft1 = V3ExtractMax(tt1);
|
||||
const FloatV ft2 = V3ExtractMin(tt2);
|
||||
|
||||
tminf = FMax(ft1, tminf);
|
||||
tmaxf = FMin(tmaxf, ft2);
|
||||
|
||||
tmin = tminf;
|
||||
tmax = tmaxf;
|
||||
|
||||
const BoolV con0 = FIsGrtr(tminf, tmaxf);
|
||||
const BoolV con1 = FIsGrtr(tminf, one);
|
||||
const BoolV isNotIntersect = BOr(con0, con1);
|
||||
return BAllEqFFFF(isNotIntersect) == 1;
|
||||
}
|
||||
|
||||
//pts, faceNormal and contact normal are in the local space of new space
|
||||
static void calculateContacts(const FloatVArg extentX_, const FloatVArg extentY_, Vec3V* pts, const Vec3VArg incidentFaceNormalInNew, const Vec3VArg localNormal, PersistentContact* manifoldContacts, PxU32& numContacts, const FloatVArg contactDist)
|
||||
{
|
||||
const FloatV zero = FZero();
|
||||
const FloatV max = FMax();
|
||||
const FloatV eps = FLoad(1.0001f);
|
||||
|
||||
const FloatV extentX = FMul(extentX_, eps);
|
||||
const FloatV extentY = FMul(extentY_, eps);
|
||||
|
||||
const FloatV nExtentX = FNeg(extentX);
|
||||
const FloatV nExtentY = FNeg(extentY);
|
||||
|
||||
bool pPenetration[4];
|
||||
bool pArea[4];
|
||||
|
||||
Vec3V bmin = V3Splat(max);
|
||||
Vec3V bmax = V3Neg(bmin);
|
||||
|
||||
const Vec3V bound = V3Merge(extentX, extentY, max);
|
||||
|
||||
//get the projection point of pts
|
||||
for(PxU32 i=0; i< 4; ++i)
|
||||
{
|
||||
bmin = V3Min(bmin, pts[i]);
|
||||
bmax = V3Max(bmax, pts[i]);
|
||||
const FloatV z = FNeg(V3GetZ(pts[i]));
|
||||
if(FAllGrtr(contactDist, z))
|
||||
{
|
||||
pPenetration[i] = true;
|
||||
|
||||
const Vec3V absPt = V3Abs(pts[i]);
|
||||
const BoolV con = V3IsGrtrOrEq(bound, absPt);
|
||||
if(BAllEqTTTT(con))
|
||||
{
|
||||
pArea[i] = true;
|
||||
|
||||
//Add the point to the manifold
|
||||
manifoldContacts[numContacts].mLocalPointA = V3SetZ(pts[i], zero); //transformNewTo0.transform(localPointA);
|
||||
manifoldContacts[numContacts].mLocalPointB = pts[i];//transform1ToNew.transformInv(pts[i]);
|
||||
manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(localNormal), z);
|
||||
}
|
||||
else
|
||||
{
|
||||
pArea[i] = false;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
pPenetration[i] = false;
|
||||
pArea[i] = false;
|
||||
}
|
||||
}
|
||||
|
||||
if(numContacts == 4)
|
||||
return;
|
||||
|
||||
//if(pPenetration[0] && pPenetration[1] && pPenetration[2] && pPenetration[3])
|
||||
{
|
||||
//if(!pArea[0] || !pArea[1] || !pArea[2] || !pArea[3])
|
||||
{
|
||||
const FloatV denom = V3GetZ(incidentFaceNormalInNew);
|
||||
{
|
||||
const Vec3V q0 = V3Merge(extentX, extentY, zero);
|
||||
|
||||
if(contains(pts, 4, q0, bmin, bmax))
|
||||
{
|
||||
const FloatV nom = V3Dot(incidentFaceNormalInNew, V3Sub(pts[0], q0));
|
||||
const FloatV t = FDiv(nom, denom);
|
||||
const FloatV pen = FNeg(t);
|
||||
if(FAllGrtr(contactDist, pen))
|
||||
{
|
||||
manifoldContacts[numContacts].mLocalPointA = q0;
|
||||
manifoldContacts[numContacts].mLocalPointB = V3SetZ(q0, t);
|
||||
manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(localNormal), pen);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
const Vec3V q0 = V3Merge(extentX, nExtentY, zero);
|
||||
if(contains(pts, 4, q0, bmin, bmax))
|
||||
{
|
||||
const FloatV nom = V3Dot(incidentFaceNormalInNew, V3Sub(pts[0], q0));
|
||||
const FloatV t = FDiv(nom, denom);
|
||||
const FloatV pen = FNeg(t);
|
||||
if(FAllGrtr(contactDist, pen))
|
||||
{
|
||||
manifoldContacts[numContacts].mLocalPointA = q0;
|
||||
manifoldContacts[numContacts].mLocalPointB = V3SetZ(q0, t);
|
||||
manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(localNormal), pen);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
const Vec3V q0 = V3Merge( nExtentX, extentY, zero);
|
||||
if(contains(pts, 4, q0, bmin, bmax))
|
||||
{
|
||||
const FloatV nom = V3Dot(incidentFaceNormalInNew, V3Sub(pts[0], q0));
|
||||
const FloatV t = FDiv(nom, denom);
|
||||
const FloatV pen = FNeg(t);
|
||||
if(FAllGrtr(contactDist, pen))
|
||||
{
|
||||
manifoldContacts[numContacts].mLocalPointA = q0;
|
||||
manifoldContacts[numContacts].mLocalPointB = V3SetZ(q0, t);
|
||||
manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(localNormal), pen);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
const Vec3V q0 = V3Merge(nExtentX, nExtentY, zero);
|
||||
|
||||
if(contains(pts, 4, q0, bmin, bmax))
|
||||
{
|
||||
const FloatV nom = V3Dot(incidentFaceNormalInNew, V3Sub(pts[0], q0));
|
||||
const FloatV t = FDiv(nom, denom);
|
||||
const FloatV pen = FNeg(t);
|
||||
if(FAllGrtr(contactDist, pen))
|
||||
{
|
||||
manifoldContacts[numContacts].mLocalPointA = q0;
|
||||
manifoldContacts[numContacts].mLocalPointB = V3SetZ(q0, t);
|
||||
manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(localNormal), pen);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
const Vec3V ext = V3Merge(extentX, extentY, max);
|
||||
const Vec3V negExt = V3Merge(nExtentX, nExtentY, FNeg(FAdd(contactDist, FEps())));
|
||||
|
||||
for (PxU32 rStart = 0, rEnd = 3; rStart < 4; rEnd = rStart++)
|
||||
{
|
||||
const Vec3V p0 = pts[rStart];
|
||||
const Vec3V p1 = pts[rEnd];
|
||||
|
||||
if(!pPenetration[rStart] && !pPenetration[rEnd])
|
||||
continue;
|
||||
|
||||
const bool con0 = pPenetration[rStart] && pArea[rStart];
|
||||
const bool con1 = pPenetration[rEnd] && pArea[rEnd];
|
||||
if(con0 && con1)
|
||||
continue;
|
||||
|
||||
//intersect t value with x plane
|
||||
const Vec3V p0p1 = V3Sub(p1, p0);
|
||||
|
||||
FloatV tmin, tmax;
|
||||
if(::intersectSegmentAABB(p0, p0p1, ext, negExt, tmin, tmax))
|
||||
{
|
||||
if(!con0)
|
||||
{
|
||||
const Vec3V intersectP = V3ScaleAdd(p0p1, tmin, p0);
|
||||
manifoldContacts[numContacts].mLocalPointA = V3SetZ(intersectP, zero);
|
||||
manifoldContacts[numContacts].mLocalPointB = intersectP;
|
||||
manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(localNormal), FNeg(V3GetZ(intersectP)));
|
||||
}
|
||||
if(!con1)
|
||||
{
|
||||
const Vec3V intersectP = V3ScaleAdd(p0p1, tmax, p0);
|
||||
manifoldContacts[numContacts].mLocalPointA = V3SetZ(intersectP, zero);
|
||||
manifoldContacts[numContacts].mLocalPointB = intersectP;
|
||||
manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(localNormal), FNeg(V3GetZ(intersectP)));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static PxU32 doBoxBoxGenerateContacts(const Vec3VArg box0Extent, const Vec3VArg box1Extent, const PxMatTransformV& transform0, const PxMatTransformV& transform1, const FloatVArg contactDist, PersistentContact* manifoldContacts, PxU32& numContacts)
|
||||
{
|
||||
const FloatV ea0 = V3GetX(box0Extent);
|
||||
const FloatV ea1 = V3GetY(box0Extent);
|
||||
const FloatV ea2 = V3GetZ(box0Extent);
|
||||
|
||||
const FloatV eb0 = V3GetX(box1Extent);
|
||||
const FloatV eb1 = V3GetY(box1Extent);
|
||||
const FloatV eb2 = V3GetZ(box1Extent);
|
||||
|
||||
const PxMatTransformV transform1To0 = transform0.transformInv(transform1);
|
||||
const Mat33V rot0To1 = M33Trnsps(transform1To0.rot);
|
||||
|
||||
const Vec3V uEps = V3Load(1e-6f);
|
||||
|
||||
const FloatV zero = FZero();
|
||||
|
||||
const FloatV tx = V3GetX(transform1To0.p);
|
||||
const FloatV ty = V3GetY(transform1To0.p);
|
||||
const FloatV tz = V3GetZ(transform1To0.p);
|
||||
const Vec3V col0 = transform1To0.getCol0();
|
||||
const Vec3V col1 = transform1To0.getCol1();
|
||||
const Vec3V col2 = transform1To0.getCol2();
|
||||
|
||||
const Vec3V abs1To0Col0 = V3Add(V3Abs(col0), uEps);
|
||||
const Vec3V abs1To0Col1 = V3Add(V3Abs(col1), uEps);
|
||||
const Vec3V abs1To0Col2 = V3Add(V3Abs(col2), uEps);
|
||||
|
||||
const Vec3V abs0To1Col0 = V3Add(V3Abs(rot0To1.col0), uEps);
|
||||
const Vec3V abs0To1Col1 = V3Add(V3Abs(rot0To1.col1), uEps);
|
||||
const Vec3V abs0To1Col2 = V3Add(V3Abs(rot0To1.col2), uEps);
|
||||
|
||||
FloatV sign[6];
|
||||
FloatV overlap[6];
|
||||
|
||||
FloatV ra, rb, radiusSum;
|
||||
//ua0
|
||||
{
|
||||
sign[0] = tx;
|
||||
|
||||
const Vec3V vtemp3 = V3Mul(abs0To1Col0, box1Extent);
|
||||
rb = FAdd(V3GetX(vtemp3), FAdd(V3GetY(vtemp3), V3GetZ(vtemp3)));
|
||||
|
||||
radiusSum = FAdd(ea0, rb);
|
||||
overlap[0] = FAdd(FSub(radiusSum, FAbs(sign[0])), contactDist);
|
||||
if(FAllGrtr(zero, overlap[0]))
|
||||
return false;
|
||||
}
|
||||
|
||||
//ua1
|
||||
{
|
||||
sign[1] = ty;
|
||||
|
||||
const Vec3V vtemp3 = V3Mul(abs0To1Col1, box1Extent);
|
||||
rb = FAdd(V3GetX(vtemp3), FAdd(V3GetY(vtemp3), V3GetZ(vtemp3)));
|
||||
|
||||
radiusSum = FAdd(ea1, rb);
|
||||
overlap[1] = FAdd(FSub(radiusSum, FAbs(sign[1])), contactDist);
|
||||
if(FAllGrtr(zero, overlap[1]))
|
||||
return false;
|
||||
}
|
||||
|
||||
//ua2
|
||||
{
|
||||
sign[2] = tz;
|
||||
ra = ea2;
|
||||
|
||||
const Vec3V vtemp3 = V3Mul(abs0To1Col2, box1Extent);
|
||||
rb = FAdd(V3GetX(vtemp3), FAdd(V3GetY(vtemp3), V3GetZ(vtemp3)));
|
||||
|
||||
radiusSum = FAdd(ea2, rb);
|
||||
overlap[2] = FAdd(FSub(radiusSum, FAbs(sign[2])), contactDist);
|
||||
if(FAllGrtr(zero, overlap[2]))
|
||||
return false;
|
||||
}
|
||||
|
||||
//ub0
|
||||
{
|
||||
sign[3] = V3Dot(transform1To0.p, col0);
|
||||
|
||||
const Vec3V vtemp3 = V3Mul(abs1To0Col0, box0Extent);
|
||||
ra = FAdd(V3GetX(vtemp3), FAdd(V3GetY(vtemp3), V3GetZ(vtemp3)));
|
||||
|
||||
radiusSum = FAdd(ra, eb0);
|
||||
overlap[3] = FAdd(FSub(radiusSum, FAbs(sign[3])), contactDist);
|
||||
if(FAllGrtr(zero, overlap[3]))
|
||||
return false;
|
||||
}
|
||||
|
||||
//ub1
|
||||
{
|
||||
sign[4] = V3Dot(transform1To0.p, col1);
|
||||
|
||||
const Vec3V vtemp3 = V3Mul(abs1To0Col1, box0Extent);
|
||||
ra = FAdd(V3GetX(vtemp3), FAdd(V3GetY(vtemp3), V3GetZ(vtemp3)));
|
||||
|
||||
radiusSum = FAdd(ra, eb1);
|
||||
overlap[4] = FAdd(FSub(radiusSum, FAbs(sign[4])), contactDist);
|
||||
if(FAllGrtr(zero, overlap[4]))
|
||||
return false;
|
||||
}
|
||||
|
||||
//ub2
|
||||
{
|
||||
sign[5] = V3Dot(transform1To0.p, col2);
|
||||
|
||||
const Vec3V vtemp3 = V3Mul(abs1To0Col2, box0Extent);
|
||||
ra = FAdd(V3GetX(vtemp3), FAdd(V3GetY(vtemp3), V3GetZ(vtemp3)));
|
||||
|
||||
radiusSum = FAdd(ra, eb2);
|
||||
overlap[5] = FAdd(FSub(radiusSum, FAbs(sign[5])), contactDist);
|
||||
if(FAllGrtr(zero, overlap[5]))
|
||||
return false;
|
||||
}
|
||||
|
||||
//ua0 X ub0
|
||||
{
|
||||
//B into A's space, ua0Xub0[0,-b3, b2]
|
||||
const FloatV absSign = FAbs(FSub(FMul(V3GetY(col0), tz), FMul(V3GetZ(col0), ty)));
|
||||
|
||||
//B into A's space, ua0Xub0[0,-b3, b2]
|
||||
const FloatV vtemp0 = FMul(V3GetZ(abs1To0Col0), ea1);
|
||||
const FloatV vtemp1 = FMul(V3GetY(abs1To0Col0), ea2);
|
||||
ra = FAdd(vtemp0, vtemp1);
|
||||
|
||||
//A into B's space, ua0Xub0[0, a3, -a2]
|
||||
const FloatV vtemp01 = FMul(V3GetZ(abs0To1Col0), eb1);
|
||||
const FloatV vtemp02 = FMul(V3GetY(abs0To1Col0), eb2);
|
||||
rb = FAdd(vtemp01, vtemp02);
|
||||
|
||||
radiusSum = FAdd(FAdd(ra, rb), contactDist);
|
||||
if(FAllGrtr(absSign, radiusSum)) return false;
|
||||
}
|
||||
|
||||
//ua0 X ub1
|
||||
{
|
||||
//B into A's space, ua0Xub0[0, -b3, b2]
|
||||
const FloatV absSign = FAbs(FSub(FMul(V3GetY(col1), tz), FMul(V3GetZ(col1), ty)));
|
||||
|
||||
//B into A's space, ua0Xub0[0, -b3, b2]
|
||||
const FloatV vtemp0 = FMul(V3GetZ(abs1To0Col1), ea1);
|
||||
const FloatV vtemp1 = FMul(V3GetY(abs1To0Col1), ea2);
|
||||
ra = FAdd(vtemp0, vtemp1);
|
||||
|
||||
//A into B's space, ua0Xub1[-a3, 0, a1]
|
||||
const FloatV vtemp01 = FMul(V3GetZ(abs0To1Col0), eb0);
|
||||
const FloatV vtemp02 = FMul(V3GetX(abs0To1Col0), eb2);
|
||||
rb = FAdd(vtemp01, vtemp02);
|
||||
|
||||
radiusSum = FAdd(FAdd(ra, rb), contactDist);
|
||||
|
||||
if(FAllGrtr(absSign, radiusSum)) return false;
|
||||
}
|
||||
|
||||
//ua0 X ub2
|
||||
{
|
||||
//B into A's space, ua0Xub0[0, -b3, b2]
|
||||
const FloatV absSign = FAbs(FSub(FMul(V3GetY(col2), tz), FMul(V3GetZ(col2), ty)));
|
||||
|
||||
//B into A's space, ua0Xub0[0, -b3, b2]
|
||||
const FloatV vtemp0 = FMul(V3GetZ(abs1To0Col2), ea1);
|
||||
const FloatV vtemp1 = FMul(V3GetY(abs1To0Col2), ea2);
|
||||
ra = FAdd(vtemp0, vtemp1);
|
||||
|
||||
//A into B's space, ua0Xub1[a2, -a1, 0]
|
||||
const FloatV vtemp01 = FMul(V3GetY(abs0To1Col0), eb0);
|
||||
const FloatV vtemp02 = FMul(V3GetX(abs0To1Col0), eb1);
|
||||
rb = FAdd(vtemp01, vtemp02);
|
||||
|
||||
radiusSum = FAdd(FAdd(ra, rb), contactDist);
|
||||
if(FAllGrtr(absSign, radiusSum)) return false;
|
||||
}
|
||||
|
||||
//ua1 X ub0
|
||||
{
|
||||
//B into A's space, ua0Xub0[b3, 0, -b1]
|
||||
const FloatV absSign = FAbs(FSub(FMul(V3GetZ(col0), tx), FMul(V3GetX(col0), tz)));
|
||||
|
||||
//B into A's space, ua0Xub0[b3, 0, -b1]
|
||||
const FloatV vtemp0 = FMul(V3GetZ(abs1To0Col0), ea0);
|
||||
const FloatV vtemp1 = FMul(V3GetX(abs1To0Col0), ea2);
|
||||
ra = FAdd(vtemp0, vtemp1);
|
||||
|
||||
//A into B's space, ua0Xub1[0, a3, -a2]
|
||||
const FloatV vtemp01 = FMul(V3GetZ(abs0To1Col1), eb1);
|
||||
const FloatV vtemp02 = FMul(V3GetY(abs0To1Col1), eb2);
|
||||
rb = FAdd(vtemp01, vtemp02);
|
||||
|
||||
radiusSum = FAdd(FAdd(ra, rb), contactDist);
|
||||
if(FAllGrtr(absSign, radiusSum))return false;
|
||||
}
|
||||
|
||||
//ua1 X ub1
|
||||
{
|
||||
//B into A's space, ua0Xub0[b3, 0, -b1]
|
||||
const FloatV absSign = FAbs(FSub(FMul(V3GetZ(col1), tx), FMul(V3GetX(col1), tz)));
|
||||
|
||||
//B into A's space, ua0Xub0[b3, 0, -b1]
|
||||
const FloatV vtemp0 = FMul(V3GetZ(abs1To0Col1), ea0);
|
||||
const FloatV vtemp1 = FMul(V3GetX(abs1To0Col1), ea2);
|
||||
ra = FAdd(vtemp0, vtemp1);
|
||||
|
||||
//A into B's space, ua0Xub1[-a3, 0, -a1]
|
||||
const FloatV vtemp01 = FMul(V3GetZ(abs0To1Col1), eb0);
|
||||
const FloatV vtemp02 = FMul(V3GetX(abs0To1Col1), eb2);
|
||||
rb = FAdd(vtemp01, vtemp02);
|
||||
|
||||
radiusSum = FAdd(FAdd(ra, rb), contactDist);
|
||||
if(FAllGrtr(absSign, radiusSum))return false;
|
||||
}
|
||||
|
||||
//ua1 X ub2
|
||||
{
|
||||
//B into A's space, ua0Xub0[b3, 0, -b1]
|
||||
const FloatV absSign=FAbs(FSub(FMul(V3GetZ(col2), tx), FMul(V3GetX(col2), tz)));
|
||||
|
||||
//B into A's space, ua0Xub0[b3, 0, -b1]
|
||||
const FloatV vtemp0 = FMul(V3GetZ(abs1To0Col2), ea0);
|
||||
const FloatV vtemp1 = FMul(V3GetX(abs1To0Col2), ea2);
|
||||
ra = FAdd(vtemp0, vtemp1);
|
||||
|
||||
//A into B's space, ua0Xub1[a2, -a1, 0]
|
||||
const FloatV vtemp01 = FMul(V3GetY(abs0To1Col1), eb0);
|
||||
const FloatV vtemp02 = FMul(V3GetX(abs0To1Col1), eb1);
|
||||
rb = FAdd(vtemp01, vtemp02);
|
||||
|
||||
radiusSum = FAdd(FAdd(ra, rb), contactDist);
|
||||
if(FAllGrtr(absSign, radiusSum))return false;
|
||||
}
|
||||
|
||||
//ua2 X ub0
|
||||
{
|
||||
//B into A's space, ua2Xub0[-b2, b1, 0]
|
||||
const FloatV absSign = FAbs(FSub(FMul(V3GetX(col0), ty), FMul(V3GetY(col0), tx)));
|
||||
|
||||
|
||||
//B into A's space, ua2Xub0[-b2, b1, 0]
|
||||
const FloatV vtemp0 = FMul(V3GetY(abs1To0Col0), ea0);
|
||||
const FloatV vtemp1 = FMul(V3GetX(abs1To0Col0), ea1);
|
||||
ra = FAdd(vtemp0, vtemp1);
|
||||
|
||||
//A into B's space, ua0Xub1[0, a3, -a2]
|
||||
const FloatV vtemp01 = FMul(V3GetZ(abs0To1Col2), eb1);
|
||||
const FloatV vtemp02 = FMul(V3GetY(abs0To1Col2), eb2);
|
||||
rb = FAdd(vtemp01, vtemp02);
|
||||
|
||||
radiusSum = FAdd(FAdd(ra, rb), contactDist);
|
||||
if(FAllGrtr(absSign, radiusSum))return false;
|
||||
}
|
||||
|
||||
//ua2 X ub1
|
||||
{
|
||||
//B into A's space, ua2Xub0[-b2, b1, 0]
|
||||
const FloatV absSign = FAbs(FSub(FMul(V3GetX(col1), ty), FMul(V3GetY(col1), tx)));
|
||||
|
||||
//B into A's space, ua2Xub0[-b2, b1, 0]
|
||||
const FloatV vtemp0 = FMul(V3GetY(abs1To0Col1), ea0);
|
||||
const FloatV vtemp1 = FMul(V3GetX(abs1To0Col1), ea1);
|
||||
ra = FAdd(vtemp0, vtemp1);
|
||||
|
||||
//A into B's space, ua0Xub1[-a3, 0, a1]
|
||||
const FloatV vtemp01 = FMul(V3GetZ(abs0To1Col2), eb0);
|
||||
const FloatV vtemp02 = FMul(V3GetX(abs0To1Col2), eb2);
|
||||
rb = FAdd(vtemp01, vtemp02);
|
||||
|
||||
radiusSum = FAdd(FAdd(ra, rb), contactDist);
|
||||
if(FAllGrtr(absSign, radiusSum))return false;
|
||||
}
|
||||
|
||||
//ua2 X ub2
|
||||
{
|
||||
//B into A's space, ua2Xub0[-b2, b1, 0]
|
||||
const FloatV absSign=FAbs(FSub(FMul(V3GetX(col2), ty), FMul(V3GetY(col2), tx)));
|
||||
|
||||
//B into A's space, ua2Xub0[-b2, b1, 0]
|
||||
const FloatV vtemp0 = FMul(V3GetY(abs1To0Col2), ea0);
|
||||
const FloatV vtemp1 = FMul(V3GetX(abs1To0Col2), ea1);
|
||||
ra = FAdd(vtemp0, vtemp1);
|
||||
|
||||
//A into B's space, ua0Xub1[a2, -a1, 0]
|
||||
const FloatV vtemp01 = FMul(V3GetY(abs0To1Col2), eb0);
|
||||
const FloatV vtemp02 = FMul(V3GetX(abs0To1Col2), eb1);
|
||||
rb = FAdd(vtemp01, vtemp02);
|
||||
|
||||
radiusSum = FAdd(FAdd(ra, rb), contactDist);
|
||||
if(FAllGrtr(absSign, radiusSum))return false;
|
||||
}
|
||||
|
||||
Vec3V mtd;
|
||||
|
||||
PxU32 feature = 0;
|
||||
FloatV minOverlap = overlap[0];
|
||||
|
||||
for(PxU32 i=1; i<6; ++i)
|
||||
{
|
||||
if(FAllGrtr(minOverlap, overlap[i]))
|
||||
{
|
||||
minOverlap = overlap[i];
|
||||
feature = i;
|
||||
}
|
||||
}
|
||||
|
||||
PxMatTransformV newTransformV;
|
||||
const Vec3V axis00 = transform0.getCol0();
|
||||
const Vec3V axis01 = transform0.getCol1();
|
||||
const Vec3V axis02 = transform0.getCol2();
|
||||
const Vec3V axis10 = transform1.getCol0();
|
||||
const Vec3V axis11 = transform1.getCol1();
|
||||
const Vec3V axis12 = transform1.getCol2();
|
||||
|
||||
Vec3V incidentFaceNormalInNew;
|
||||
Vec3V pts[4];
|
||||
bool flip = false;
|
||||
switch(feature)
|
||||
{
|
||||
case 0: //ua0
|
||||
{
|
||||
if(FAllGrtrOrEq(zero, sign[0]))
|
||||
{
|
||||
mtd = axis00;
|
||||
newTransformV.rot.col0 = V3Neg(axis02);
|
||||
newTransformV.rot.col1 = axis01;
|
||||
newTransformV.rot.col2 = axis00;
|
||||
newTransformV.p = V3NegScaleSub(axis00, ea0, transform0.p);
|
||||
}
|
||||
else
|
||||
{
|
||||
const Vec3V nAxis00 = V3Neg(axis00);
|
||||
mtd = nAxis00;
|
||||
newTransformV.rot.col0 = axis02;
|
||||
newTransformV.rot.col1 = axis01;
|
||||
newTransformV.rot.col2 = nAxis00;
|
||||
newTransformV.p = V3ScaleAdd(axis00, ea0, transform0.p);
|
||||
}
|
||||
|
||||
const PxMatTransformV transform1ToNew = newTransformV.transformInv(transform1);
|
||||
const Vec3V localNormal = newTransformV.rotateInv(mtd);
|
||||
getIncidentPolygon(pts, incidentFaceNormalInNew, V3Neg(localNormal), transform1ToNew, box1Extent);
|
||||
|
||||
calculateContacts(ea2, ea1, pts, incidentFaceNormalInNew, localNormal, manifoldContacts, numContacts, contactDist);
|
||||
|
||||
break;
|
||||
};
|
||||
case 1: //ua1
|
||||
{
|
||||
if(FAllGrtrOrEq(zero, sign[1]))
|
||||
{
|
||||
mtd = axis01;
|
||||
newTransformV.rot.col0 = axis00;
|
||||
newTransformV.rot.col1 = V3Neg(axis02);
|
||||
newTransformV.rot.col2 = axis01;
|
||||
newTransformV.p = V3NegScaleSub(axis01, ea1, transform0.p);
|
||||
}
|
||||
else
|
||||
{
|
||||
const Vec3V nAxis01 = V3Neg(axis01);
|
||||
mtd = nAxis01;
|
||||
newTransformV.rot.col0 = axis00;
|
||||
newTransformV.rot.col1 = axis02;
|
||||
newTransformV.rot.col2 = nAxis01;
|
||||
newTransformV.p = V3ScaleAdd(axis01, ea1, transform0.p);
|
||||
}
|
||||
|
||||
const PxMatTransformV transform1ToNew = newTransformV.transformInv(transform1);
|
||||
const Vec3V localNormal = newTransformV.rotateInv(mtd);
|
||||
getIncidentPolygon(pts, incidentFaceNormalInNew, V3Neg(localNormal), transform1ToNew, box1Extent);
|
||||
|
||||
calculateContacts(ea0, ea2, pts, incidentFaceNormalInNew, localNormal, manifoldContacts, numContacts, contactDist);
|
||||
|
||||
break;
|
||||
};
|
||||
case 2: //ua2
|
||||
{
|
||||
if(FAllGrtrOrEq(zero, sign[2]))
|
||||
{
|
||||
mtd = axis02;
|
||||
newTransformV.rot.col0 = axis00;
|
||||
newTransformV.rot.col1 = axis01;
|
||||
newTransformV.rot.col2 = axis02;
|
||||
|
||||
newTransformV.p = V3NegScaleSub(axis02, ea2, transform0.p);
|
||||
}
|
||||
else
|
||||
{
|
||||
const Vec3V nAxis02 = V3Neg(axis02);
|
||||
mtd = nAxis02;
|
||||
newTransformV.rot.col0 = axis00;
|
||||
newTransformV.rot.col1 = V3Neg(axis01);
|
||||
newTransformV.rot.col2 = nAxis02;
|
||||
newTransformV.p = V3ScaleAdd(axis02, ea2, transform0.p);
|
||||
}
|
||||
|
||||
const PxMatTransformV transform1ToNew = newTransformV.transformInv(transform1);
|
||||
const Vec3V localNormal = newTransformV.rotateInv(mtd);
|
||||
getIncidentPolygon(pts, incidentFaceNormalInNew, V3Neg(localNormal), transform1ToNew, box1Extent);
|
||||
|
||||
calculateContacts(ea0, ea1, pts, incidentFaceNormalInNew, localNormal, manifoldContacts, numContacts, contactDist);
|
||||
|
||||
break;
|
||||
};
|
||||
case 3: //ub0
|
||||
{
|
||||
flip = true;
|
||||
if(FAllGrtrOrEq(zero, sign[3]))
|
||||
{
|
||||
mtd = axis10;
|
||||
newTransformV.rot.col0 = axis12;
|
||||
newTransformV.rot.col1 = axis11;
|
||||
newTransformV.rot.col2 = V3Neg(axis10);
|
||||
newTransformV.p = V3ScaleAdd(axis10, eb0, transform1.p); //transform0.p - extents0.x*axis00;
|
||||
}
|
||||
else
|
||||
{
|
||||
mtd = V3Neg(axis10);
|
||||
newTransformV.rot.col0 = V3Neg(axis12);
|
||||
newTransformV.rot.col1 = axis11;
|
||||
newTransformV.rot.col2 = axis10;
|
||||
newTransformV.p = V3NegScaleSub(axis10, eb0, transform1.p);//transform0.p + extents0.x*axis00;
|
||||
}
|
||||
|
||||
const PxMatTransformV transform1ToNew = newTransformV.transformInv(transform0);
|
||||
const Vec3V localNormal = newTransformV.rotateInv(mtd);
|
||||
getIncidentPolygon(pts, incidentFaceNormalInNew, localNormal, transform1ToNew, box0Extent);
|
||||
|
||||
calculateContacts(eb2, eb1, pts, incidentFaceNormalInNew, localNormal, manifoldContacts, numContacts, contactDist);
|
||||
|
||||
break;
|
||||
};
|
||||
case 4: //ub1;
|
||||
{
|
||||
flip = true;
|
||||
if(FAllGrtrOrEq(zero, sign[4]))
|
||||
{
|
||||
mtd = axis11;
|
||||
newTransformV.rot.col0 = axis10;
|
||||
newTransformV.rot.col1 = axis12;
|
||||
newTransformV.rot.col2 = V3Neg(axis11);
|
||||
|
||||
newTransformV.p = V3ScaleAdd(axis11, eb1, transform1.p);
|
||||
}
|
||||
else
|
||||
{
|
||||
mtd = V3Neg(axis11);
|
||||
|
||||
newTransformV.rot.col0 = axis10;
|
||||
newTransformV.rot.col1 = V3Neg(axis12);
|
||||
newTransformV.rot.col2 = axis11;
|
||||
newTransformV.p = V3NegScaleSub(axis11, eb1, transform1.p); //transform0.p + extents0.x*axis00;
|
||||
}
|
||||
|
||||
const PxMatTransformV transform1ToNew = newTransformV.transformInv(transform0);
|
||||
const Vec3V localNormal = newTransformV.rotateInv(mtd);
|
||||
getIncidentPolygon(pts, incidentFaceNormalInNew, localNormal, transform1ToNew, box0Extent);
|
||||
calculateContacts(eb0, eb2, pts, incidentFaceNormalInNew, localNormal, manifoldContacts, numContacts, contactDist);
|
||||
break;
|
||||
}
|
||||
case 5: //ub2;
|
||||
{
|
||||
|
||||
flip = true;
|
||||
if(FAllGrtrOrEq(zero, sign[5]))
|
||||
{
|
||||
mtd = axis12;
|
||||
newTransformV.rot.col0 = axis10;
|
||||
newTransformV.rot.col1 = V3Neg(axis11);
|
||||
newTransformV.rot.col2 = V3Neg(axis12);
|
||||
newTransformV.p = V3ScaleAdd(axis12, eb2, transform1.p);
|
||||
}
|
||||
else
|
||||
{
|
||||
mtd = V3Neg(axis12);
|
||||
|
||||
newTransformV.rot.col0 = axis10;
|
||||
newTransformV.rot.col1 = axis11;
|
||||
newTransformV.rot.col2 = axis12;
|
||||
newTransformV.p = V3NegScaleSub(axis12, eb2, transform1.p);
|
||||
}
|
||||
|
||||
const PxMatTransformV transform1ToNew = newTransformV.transformInv(transform0);
|
||||
const Vec3V localNormal = newTransformV.rotateInv(mtd);
|
||||
getIncidentPolygon(pts, incidentFaceNormalInNew, localNormal, transform1ToNew, box0Extent);
|
||||
|
||||
calculateContacts(eb0, eb1, pts, incidentFaceNormalInNew, localNormal, manifoldContacts, numContacts, contactDist);
|
||||
break;
|
||||
};
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
|
||||
if(numContacts != 0)
|
||||
{
|
||||
if(flip)
|
||||
{
|
||||
for(PxU32 i=0; i<numContacts; ++i)
|
||||
{
|
||||
const Vec3V localB = manifoldContacts[i].mLocalPointB;
|
||||
manifoldContacts[i].mLocalPointB = manifoldContacts[i].mLocalPointA;
|
||||
manifoldContacts[i].mLocalPointA = localB;
|
||||
}
|
||||
}
|
||||
const PxMatTransformV transformNewTo1 = transform1.transformInv(newTransformV);
|
||||
const PxMatTransformV transformNewTo0 = transform0.transformInv(newTransformV);
|
||||
//transform points to the local space of transform0 and transform1
|
||||
const Vec3V localNormalInB = transformNewTo1.rotate(Vec3V_From_Vec4V(manifoldContacts[0].mLocalNormalPen));
|
||||
|
||||
for(PxU32 i=0; i<numContacts; ++i)
|
||||
{
|
||||
manifoldContacts[i].mLocalPointA = transformNewTo0.transform(manifoldContacts[i].mLocalPointA);
|
||||
manifoldContacts[i].mLocalPointB = transformNewTo1.transform(manifoldContacts[i].mLocalPointB);
|
||||
manifoldContacts[i].mLocalNormalPen = V4SetW(localNormalInB, V4GetW(manifoldContacts[i].mLocalNormalPen));
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Gu::pcmContactBoxBox(GU_CONTACT_METHOD_ARGS)
|
||||
{
|
||||
PX_UNUSED(renderOutput);
|
||||
PX_ASSERT(transform1.q.isSane());
|
||||
PX_ASSERT(transform0.q.isSane());
|
||||
|
||||
const PxBoxGeometry& shapeBox0 = checkedCast<PxBoxGeometry>(shape0);
|
||||
const PxBoxGeometry& shapeBox1 = checkedCast<PxBoxGeometry>(shape1);
|
||||
|
||||
// Get actual shape data
|
||||
PersistentContactManifold& manifold = cache.getManifold();
|
||||
PxPrefetchLine(&manifold, 256);
|
||||
|
||||
const FloatV contactDist = FLoad(params.mContactDistance);
|
||||
const Vec3V boxExtents0 = V3LoadU(shapeBox0.halfExtents);
|
||||
const Vec3V boxExtents1 = V3LoadU(shapeBox1.halfExtents);
|
||||
|
||||
//Transfer A into the local space of B
|
||||
const PxTransformV transf0 = loadTransformA(transform0);
|
||||
const PxTransformV transf1 = loadTransformA(transform1);
|
||||
const PxTransformV curRTrans(transf1.transformInv(transf0));
|
||||
const PxMatTransformV aToB(curRTrans);
|
||||
|
||||
const PxReal toleranceLength = params.mToleranceLength;
|
||||
const FloatV boxMargin0 = CalculatePCMBoxMargin(boxExtents0, toleranceLength);
|
||||
const FloatV boxMargin1 = CalculatePCMBoxMargin(boxExtents1, toleranceLength);
|
||||
const FloatV minMargin = FMin(boxMargin0, boxMargin1);
|
||||
|
||||
const PxU32 initialContacts = manifold.mNumContacts;
|
||||
const FloatV projectBreakingThreshold = FMul(minMargin, FLoad(0.8f));
|
||||
|
||||
manifold.refreshContactPoints(aToB, projectBreakingThreshold, contactDist);
|
||||
|
||||
const PxU32 newContacts = manifold.mNumContacts;
|
||||
|
||||
const bool bLostContacts = (newContacts != initialContacts);
|
||||
|
||||
const FloatV radiusA = V3Length(boxExtents0);
|
||||
const FloatV radiusB = V3Length(boxExtents1);
|
||||
if(bLostContacts || manifold.invalidate_BoxConvex(curRTrans, transf0.q, transf1.q, minMargin, radiusA, radiusB))
|
||||
{
|
||||
manifold.setRelativeTransform(curRTrans, transf0.q, transf1.q);
|
||||
|
||||
PxMatTransformV transfV0(transf0);
|
||||
PxMatTransformV transfV1(transf1);
|
||||
|
||||
transfV0.rot.col0 = V3Normalize(transfV0.rot.col0);
|
||||
transfV0.rot.col1 = V3Normalize(transfV0.rot.col1);
|
||||
transfV0.rot.col2 = V3Normalize(transfV0.rot.col2);
|
||||
|
||||
transfV1.rot.col0 = V3Normalize(transfV1.rot.col0);
|
||||
transfV1.rot.col1 = V3Normalize(transfV1.rot.col1);
|
||||
transfV1.rot.col2 = V3Normalize(transfV1.rot.col2);
|
||||
|
||||
PersistentContact* manifoldContacts = PX_CP_TO_PCP(contactBuffer.contacts);
|
||||
PxU32 numContacts = 0;
|
||||
|
||||
if(doBoxBoxGenerateContacts(boxExtents0, boxExtents1, transfV0, transfV1, contactDist, manifoldContacts, numContacts))
|
||||
{
|
||||
if(numContacts > 0)
|
||||
{
|
||||
manifold.addBatchManifoldContacts(manifoldContacts, numContacts, toleranceLength);
|
||||
const Vec3V worldNormal = V3Normalize(transfV1.rotate(Vec3V_From_Vec4V(manifold.mContactPoints[0].mLocalNormalPen)));
|
||||
manifold.addManifoldContactsToContactBuffer(contactBuffer, worldNormal, transfV1);
|
||||
#if PCM_LOW_LEVEL_DEBUG
|
||||
manifold.drawManifold(*renderOutput, transf0, transf1);
|
||||
#endif
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
const Vec3V zeroV = V3Zero();
|
||||
const BoxV box0(zeroV, boxExtents0);
|
||||
const BoxV box1(zeroV, boxExtents1);
|
||||
|
||||
manifold.mNumWarmStartPoints = 0;
|
||||
const RelativeConvex<BoxV> convexA(box0, aToB);
|
||||
const LocalConvex<BoxV> convexB(box1);
|
||||
GjkOutput output;
|
||||
|
||||
GjkStatus status = gjkPenetration<RelativeConvex<BoxV>, LocalConvex<BoxV> >(convexA, convexB, aToB.p, contactDist, true,
|
||||
manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, output);
|
||||
|
||||
if(status == EPA_CONTACT)
|
||||
{
|
||||
const RelativeConvex<BoxV> convexA1(box0, aToB);
|
||||
const LocalConvex<BoxV> convexB1(box1);
|
||||
|
||||
status = epaPenetration(convexA1, convexB1, manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints,
|
||||
true, FLoad(toleranceLength), output);
|
||||
}
|
||||
|
||||
if(status == GJK_CONTACT || status == EPA_CONTACT)
|
||||
{
|
||||
const FloatV replaceBreakingThreshold = FMul(minMargin, FLoad(0.05f));
|
||||
const Vec3V localPointA = aToB.transformInv(output.closestA);//curRTrans.transformInv(closestA);
|
||||
const Vec3V localPointB = output.closestB;
|
||||
const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(output.normal), output.penDep);
|
||||
numContacts += manifold.addManifoldPoint(localPointA, localPointB, localNormalPen, replaceBreakingThreshold);
|
||||
|
||||
//transform the normal back to world space
|
||||
const Vec3V worldNormal = V3Normalize(transf1.rotate(output.normal));
|
||||
|
||||
manifold.addManifoldContactsToContactBuffer(contactBuffer, worldNormal, transf1, contactDist);
|
||||
|
||||
#if PCM_LOW_LEVEL_DEBUG
|
||||
manifold.drawManifold(*renderOutput, transf0, transf1);
|
||||
#endif
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else if(manifold.getNumContacts() > 0)
|
||||
{
|
||||
const Vec3V worldNormal = manifold.getWorldNormal(transf1);
|
||||
manifold.addManifoldContactsToContactBuffer(contactBuffer, worldNormal, transf1, contactDist);
|
||||
#if PCM_LOW_LEVEL_DEBUG
|
||||
manifold.drawManifold(*renderOutput, transf0, transf1);
|
||||
#endif
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
256
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactBoxConvex.cpp
vendored
Normal file
256
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactBoxConvex.cpp
vendored
Normal file
@@ -0,0 +1,256 @@
|
||||
// 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 "geomutils/PxContactBuffer.h"
|
||||
#include "GuGJKPenetration.h"
|
||||
#include "GuEPA.h"
|
||||
#include "GuVecBox.h"
|
||||
#include "GuVecConvexHull.h"
|
||||
#include "GuVecConvexHullNoScale.h"
|
||||
#include "GuContactMethodImpl.h"
|
||||
#include "GuPCMContactGen.h"
|
||||
#include "GuPCMShapeConvex.h"
|
||||
|
||||
#define PCM_BOX_HULL_DEBUG 0
|
||||
|
||||
using namespace physx;
|
||||
using namespace Gu;
|
||||
using namespace aos;
|
||||
|
||||
static bool fullContactsGenerationBoxConvex(const GjkConvex* relativeConvex, const GjkConvex* localConvex, const PxTransformV& transf0, const PxTransformV& transf1,
|
||||
PersistentContact* manifoldContacts, PxContactBuffer& contactBuffer, PersistentContactManifold& manifold, Vec3VArg normal,
|
||||
const Vec3VArg closestA, const Vec3VArg closestB, const FloatVArg contactDist, bool idtScale, bool doOverlapTest, PxRenderOutput* renderOutput,
|
||||
PxReal toleranceLength)
|
||||
{
|
||||
PolygonalData polyData0;
|
||||
|
||||
const BoxV& box = relativeConvex->getConvex<BoxV>();
|
||||
const ConvexHullV& convexHull = localConvex->getConvex<ConvexHullV>();
|
||||
|
||||
PxVec3 halfExtents;
|
||||
V3StoreU(box.extents, halfExtents);
|
||||
PCMPolygonalBox polyBox0(halfExtents);
|
||||
polyBox0.getPolygonalData(&polyData0);
|
||||
polyData0.mPolygonVertexRefs = gPCMBoxPolygonData;
|
||||
|
||||
PolygonalData polyData1;
|
||||
getPCMConvexData(convexHull, idtScale, polyData1);
|
||||
|
||||
const Mat33V identity = M33Identity();
|
||||
SupportLocalImpl<BoxV> map0(box, transf0, identity, identity, true);
|
||||
|
||||
PX_ALIGN(16, PxU8 buff1[sizeof(SupportLocalImpl<ConvexHullV>)]);
|
||||
|
||||
SupportLocal* map1 = (idtScale ? static_cast<SupportLocal*>(PX_PLACEMENT_NEW(buff1, SupportLocalImpl<ConvexHullNoScaleV>)(static_cast<const ConvexHullNoScaleV&>(convexHull), transf1, convexHull.vertex2Shape, convexHull.shape2Vertex, idtScale)) :
|
||||
static_cast<SupportLocal*>(PX_PLACEMENT_NEW(buff1, SupportLocalImpl<ConvexHullV>)(convexHull, transf1, convexHull.vertex2Shape, convexHull.shape2Vertex, idtScale)));
|
||||
|
||||
PxU32 numContacts = 0;
|
||||
if(generateFullContactManifold(polyData0, polyData1, &map0, map1, manifoldContacts, numContacts, contactDist, normal, closestA, closestB, box.getMarginF(), convexHull.getMarginF(),
|
||||
doOverlapTest, renderOutput, toleranceLength))
|
||||
{
|
||||
if (numContacts > 0)
|
||||
{
|
||||
//reduce contacts
|
||||
manifold.addBatchManifoldContacts(manifoldContacts, numContacts, toleranceLength);
|
||||
|
||||
#if PCM_LOW_LEVEL_DEBUG
|
||||
manifold.drawManifold(*renderOutput, transf0, transf1);
|
||||
#endif
|
||||
const Vec3V worldNormal = manifold.getWorldNormal(transf1);
|
||||
|
||||
manifold.addManifoldContactsToContactBuffer(contactBuffer, worldNormal, transf1, contactDist);
|
||||
}
|
||||
else
|
||||
{
|
||||
//if doOverlapTest is true, which means GJK/EPA degenerate so we won't have any contact in the manifoldContacts array
|
||||
if (!doOverlapTest)
|
||||
{
|
||||
const Vec3V worldNormal = manifold.getWorldNormal(transf1);
|
||||
|
||||
manifold.addManifoldContactsToContactBuffer(contactBuffer, worldNormal, transf1, contactDist);
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
static bool generateOrProcessContactsBoxConvex( const GjkConvex* relativeConvex, const GjkConvex* localConvex, const PxTransformV& transf0, const PxTransformV& transf1,
|
||||
const PxMatTransformV& aToB, GjkStatus status, GjkOutput& output, PersistentContactManifold& manifold, PxContactBuffer& contactBuffer,
|
||||
PxU32 initialContacts, const FloatV minMargin, const FloatV contactDist,
|
||||
bool idtScale, PxReal toleranceLength, PxRenderOutput* renderOutput)
|
||||
{
|
||||
if (status == GJK_NON_INTERSECT)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
else
|
||||
{
|
||||
PersistentContact* manifoldContacts = PX_CP_TO_PCP(contactBuffer.contacts);
|
||||
|
||||
const Vec3V localNor = manifold.mNumContacts ? manifold.getLocalNormal() : V3Zero();
|
||||
|
||||
const FloatV replaceBreakingThreshold = FMul(minMargin, FLoad(0.05f));
|
||||
|
||||
//addGJKEPAContacts will increase the number of contacts in manifold. If status == EPA_CONTACT, we need to run epa algorithm and generate closest points, normal and
|
||||
//pentration. If epa doesn't degenerate, we will store the contacts information in the manifold. Otherwise, we will return true to do the fallback test
|
||||
const bool doOverlapTest = addGJKEPAContacts(relativeConvex, localConvex, aToB, status, manifoldContacts, replaceBreakingThreshold, FLoad(toleranceLength), output, manifold);
|
||||
|
||||
#if PCM_LOW_LEVEL_DEBUG
|
||||
manifold.drawManifold(*renderOutput, transf0, transf1);
|
||||
#endif
|
||||
//ML: after we refresh the contacts(newContacts) and generate a GJK/EPA contacts(we will store that in the manifold), if the number of contacts is still less than the original contacts,
|
||||
//which means we lose too mang contacts and we should regenerate all the contacts in the current configuration
|
||||
//Also, we need to look at the existing contacts, if the existing contacts has very different normal than the GJK/EPA contacts,
|
||||
//which means we should throw away the existing contacts and do full contact gen
|
||||
const bool fullContactGen = FAllGrtr(FLoad(0.707106781f), V3Dot(localNor, output.normal)) || (manifold.mNumContacts < initialContacts);
|
||||
|
||||
if (fullContactGen || doOverlapTest)
|
||||
{
|
||||
return fullContactsGenerationBoxConvex(relativeConvex, localConvex, transf0, transf1, manifoldContacts, contactBuffer,
|
||||
manifold, output.normal, output.closestA, output.closestB, contactDist, idtScale, doOverlapTest, renderOutput, toleranceLength);
|
||||
}
|
||||
else
|
||||
{
|
||||
const Vec3V newLocalNor = V3Add(localNor, output.normal);
|
||||
const Vec3V worldNormal = V3Normalize(transf1.rotate(newLocalNor));
|
||||
//const Vec3V worldNormal = transf1.rotate(normal);
|
||||
manifold.addManifoldContactsToContactBuffer(contactBuffer, worldNormal, transf1, contactDist);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool Gu::pcmContactBoxConvex(GU_CONTACT_METHOD_ARGS)
|
||||
{
|
||||
PX_ASSERT(transform1.q.isSane());
|
||||
PX_ASSERT(transform0.q.isSane());
|
||||
|
||||
const PxConvexMeshGeometry& shapeConvex = checkedCast<PxConvexMeshGeometry>(shape1);
|
||||
const PxBoxGeometry& shapeBox = checkedCast<PxBoxGeometry>(shape0);
|
||||
|
||||
PersistentContactManifold& manifold = cache.getManifold();
|
||||
const ConvexHullData* hullData = _getHullData(shapeConvex);
|
||||
PxPrefetchLine(hullData);
|
||||
|
||||
const FloatV contactDist = FLoad(params.mContactDistance);
|
||||
const Vec3V boxExtents = V3LoadU(shapeBox.halfExtents);
|
||||
|
||||
const Vec3V vScale = V3LoadU_SafeReadW(shapeConvex.scale.scale); // PT: safe because 'rotation' follows 'scale' in PxMeshScale
|
||||
|
||||
const PxTransformV transf0 = loadTransformA(transform0);
|
||||
const PxTransformV transf1 = loadTransformA(transform1);
|
||||
const PxTransformV curRTrans(transf1.transformInv(transf0));
|
||||
const PxMatTransformV aToB(curRTrans);
|
||||
|
||||
const PxReal toleranceLength = params.mToleranceLength;
|
||||
const FloatV convexMargin = CalculatePCMConvexMargin(hullData, vScale, toleranceLength);
|
||||
const FloatV boxMargin = CalculatePCMBoxMargin(boxExtents, toleranceLength);
|
||||
|
||||
#if PCM_BOX_HULL_DEBUG
|
||||
const PxVec3* verts = hullData->getHullVertices();
|
||||
|
||||
for (PxU32 i = 0; i < hullData->mNbPolygons; ++i)
|
||||
{
|
||||
const HullPolygonData& polygon = hullData->mPolygons[i];
|
||||
const PxU8* inds = hullData->getVertexData8() + polygon.mVRef8;
|
||||
Vec3V* points = reinterpret_cast<Vec3V*>(PxAllocaAligned(sizeof(Vec3V)*polygon.mNbVerts, 16));
|
||||
|
||||
for (PxU32 j = 0; j < polygon.mNbVerts; ++j)
|
||||
{
|
||||
points[j] = V3LoadU_SafeReadW(verts[inds[j]]);
|
||||
}
|
||||
|
||||
PersistentContactManifold::drawPolygon(*renderOutput, transf1, points, (PxU32)polygon.mNbVerts, 0x00ff0000);
|
||||
}
|
||||
#endif
|
||||
|
||||
const FloatV minMargin = FMin(convexMargin, boxMargin);//FMin(boxMargin, convexMargin);
|
||||
const FloatV projectBreakingThreshold = FMul(minMargin, FLoad(0.8f));
|
||||
const PxU32 initialContacts = manifold.mNumContacts;
|
||||
|
||||
manifold.refreshContactPoints(aToB, projectBreakingThreshold, contactDist);
|
||||
|
||||
const Vec3V extents = V3Mul(V3LoadU_SafeReadW(hullData->mInternal.mInternalExtents), vScale);
|
||||
const FloatV radiusA = V3Length(boxExtents);
|
||||
const FloatV radiusB = V3Length(extents);
|
||||
|
||||
//After the refresh contact points, the numcontacts in the manifold will be changed
|
||||
const bool bLostContacts = (manifold.mNumContacts != initialContacts);
|
||||
|
||||
if(bLostContacts || manifold.invalidate_BoxConvex(curRTrans, transf0.q, transf1.q, minMargin, radiusA, radiusB))
|
||||
{
|
||||
manifold.setRelativeTransform(curRTrans, transf0.q, transf1.q);
|
||||
|
||||
GjkStatus status = manifold.mNumContacts > 0 ? GJK_UNDEFINED : GJK_NON_INTERSECT;
|
||||
|
||||
const QuatV vQuat = QuatVLoadU(&shapeConvex.scale.rotation.x);
|
||||
const bool idtScale = shapeConvex.scale.isIdentity();
|
||||
const ConvexHullV convexHull(hullData, V3LoadU(hullData->mCenterOfMass), vScale, vQuat, idtScale);
|
||||
const BoxV box(V3Zero(), boxExtents);
|
||||
GjkOutput output;
|
||||
|
||||
const RelativeConvex<BoxV> relativeConvex(box, aToB);
|
||||
|
||||
if(idtScale)
|
||||
{
|
||||
const LocalConvex<ConvexHullNoScaleV> localConvex(static_cast<const ConvexHullNoScaleV&>(convexHull));
|
||||
|
||||
status = gjkPenetration<RelativeConvex<BoxV>, LocalConvex<ConvexHullNoScaleV> >(relativeConvex, localConvex, aToB.p, contactDist, true,
|
||||
manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, output);
|
||||
|
||||
return generateOrProcessContactsBoxConvex(&relativeConvex, &localConvex, transf0, transf1, aToB,
|
||||
status, output, manifold, contactBuffer, initialContacts,
|
||||
minMargin, contactDist, idtScale, toleranceLength, renderOutput);
|
||||
}
|
||||
else
|
||||
{
|
||||
const LocalConvex<ConvexHullV> localConvex(convexHull);
|
||||
|
||||
status = gjkPenetration<RelativeConvex<BoxV>, LocalConvex<ConvexHullV> >(relativeConvex, localConvex, aToB.p, contactDist, true,
|
||||
manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, output);
|
||||
|
||||
return generateOrProcessContactsBoxConvex(&relativeConvex, &localConvex, transf0, transf1, aToB,
|
||||
status, output, manifold, contactBuffer, initialContacts,
|
||||
minMargin, contactDist, idtScale, toleranceLength, renderOutput);
|
||||
}
|
||||
}
|
||||
else if(manifold.getNumContacts()>0)
|
||||
{
|
||||
const Vec3V worldNormal = manifold.getWorldNormal(transf1);
|
||||
manifold.addManifoldContactsToContactBuffer(contactBuffer, worldNormal, transf1, contactDist);
|
||||
#if PCM_LOW_LEVEL_DEBUG
|
||||
manifold.drawManifold(*renderOutput, transf0, transf1);
|
||||
#endif
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
202
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactCapsuleBox.cpp
vendored
Normal file
202
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactCapsuleBox.cpp
vendored
Normal file
@@ -0,0 +1,202 @@
|
||||
// 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 "geomutils/PxContactBuffer.h"
|
||||
#include "GuVecBox.h"
|
||||
#include "GuVecCapsule.h"
|
||||
#include "GuContactMethodImpl.h"
|
||||
#include "GuPCMContactGen.h"
|
||||
#include "GuPCMShapeConvex.h"
|
||||
#include "GuGJKPenetration.h"
|
||||
#include "GuEPA.h"
|
||||
|
||||
using namespace physx;
|
||||
using namespace Gu;
|
||||
using namespace aos;
|
||||
|
||||
static bool fullContactsGenerationCapsuleBox(const CapsuleV& capsule, const BoxV& box, const PxVec3& halfExtents, const PxMatTransformV& aToB, const PxTransformV& transf0, const PxTransformV& transf1,
|
||||
PersistentContact* manifoldContacts, PxU32& numContacts, PxContactBuffer& contactBuffer, PersistentContactManifold& manifold, Vec3V& normal, const Vec3VArg closest,
|
||||
PxReal boxMargin, const FloatVArg contactDist, bool doOverlapTest, PxReal toleranceScale, PxRenderOutput* renderOutput)
|
||||
{
|
||||
PolygonalData polyData;
|
||||
PCMPolygonalBox polyBox(halfExtents);
|
||||
polyBox.getPolygonalData(&polyData);
|
||||
|
||||
const Mat33V identity = M33Identity();
|
||||
SupportLocalImpl<BoxV> map(box, transf1, identity, identity);
|
||||
|
||||
PxU32 origContacts = numContacts;
|
||||
if(!generateCapsuleBoxFullContactManifold(capsule, polyData, &map, aToB, manifoldContacts, numContacts, contactDist, normal, closest, boxMargin, doOverlapTest, toleranceScale, renderOutput))
|
||||
return false;
|
||||
|
||||
//EPA has contacts and we have new contacts, we discard the EPA contacts
|
||||
if(origContacts != 0 && numContacts != origContacts)
|
||||
{
|
||||
numContacts--;
|
||||
manifoldContacts++;
|
||||
}
|
||||
|
||||
manifold.addBatchManifoldContacts2(manifoldContacts, numContacts);
|
||||
|
||||
normal = transf1.rotate(normal);
|
||||
|
||||
manifold.addManifoldContactsToContactBuffer(contactBuffer, normal, normal, transf0, capsule.radius, contactDist);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Gu::pcmContactCapsuleBox(GU_CONTACT_METHOD_ARGS)
|
||||
{
|
||||
PX_UNUSED(renderOutput);
|
||||
PX_ASSERT(transform1.q.isSane());
|
||||
PX_ASSERT(transform0.q.isSane());
|
||||
|
||||
const PxCapsuleGeometry& shapeCapsule = checkedCast<PxCapsuleGeometry>(shape0);
|
||||
const PxBoxGeometry& shapeBox = checkedCast<PxBoxGeometry>(shape1);
|
||||
|
||||
PersistentContactManifold& manifold = cache.getManifold();
|
||||
PxPrefetchLine(&manifold, 256);
|
||||
|
||||
const Vec3V boxExtents = V3LoadU(shapeBox.halfExtents);
|
||||
|
||||
const FloatV contactDist = FLoad(params.mContactDistance);
|
||||
|
||||
const PxTransformV transf0 = loadTransformA(transform0);
|
||||
const PxTransformV transf1 = loadTransformA(transform1);
|
||||
|
||||
const PxTransformV curRTrans = transf1.transformInv(transf0);
|
||||
const PxMatTransformV aToB_(curRTrans);
|
||||
|
||||
const FloatV capsuleRadius = FLoad(shapeCapsule.radius);
|
||||
const FloatV capsuleHalfHeight = FLoad(shapeCapsule.halfHeight);
|
||||
|
||||
const PxU32 initialContacts = manifold.mNumContacts;
|
||||
|
||||
const PxReal toleranceLength = params.mToleranceLength;
|
||||
const FloatV boxMargin = CalculatePCMBoxMargin(boxExtents, toleranceLength);
|
||||
|
||||
const FloatV minMargin = FMin(boxMargin, capsuleRadius);
|
||||
|
||||
const FloatV projectBreakingThreshold = FMul(minMargin, FLoad(0.8f));
|
||||
|
||||
const FloatV refreshDist = FAdd(contactDist, capsuleRadius);
|
||||
//refreshContactPoints remove invalid contacts from the manifold and update the number correspondingly
|
||||
manifold.refreshContactPoints(aToB_, projectBreakingThreshold, refreshDist);
|
||||
|
||||
const bool bLostContacts = (manifold.mNumContacts != initialContacts);
|
||||
|
||||
if(bLostContacts || manifold.invalidate_SphereCapsule(curRTrans, minMargin))
|
||||
{
|
||||
GjkStatus status = manifold.mNumContacts > 0 ? GJK_UNDEFINED : GJK_NON_INTERSECT;
|
||||
|
||||
manifold.setRelativeTransform(curRTrans);
|
||||
const PxMatTransformV aToB(curRTrans);
|
||||
|
||||
const BoxV box(transf1.p, boxExtents);
|
||||
//transform capsule into the local space of box
|
||||
const CapsuleV capsule(aToB.p, aToB.rotate(V3Scale(V3UnitX(), capsuleHalfHeight)), capsuleRadius);
|
||||
const LocalConvex<CapsuleV> convexA(capsule);
|
||||
const LocalConvex<BoxV> convexB(box);
|
||||
GjkOutput output;
|
||||
|
||||
const Vec3V initialSearchDir = V3Sub(capsule.getCenter(), box.getCenter());
|
||||
status = gjkPenetration<LocalConvex<CapsuleV>, LocalConvex<BoxV> >(convexA, convexB, initialSearchDir, contactDist, true,
|
||||
manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, output);
|
||||
|
||||
PersistentContact* manifoldContacts = PX_CP_TO_PCP(contactBuffer.contacts);
|
||||
PxU32 numContacts = 0;
|
||||
bool doOverlapTest = false;
|
||||
if(status == GJK_NON_INTERSECT)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
else if(status == GJK_DEGENERATE)
|
||||
{
|
||||
return fullContactsGenerationCapsuleBox(capsule, box, shapeBox.halfExtents, aToB, transf0, transf1, manifoldContacts, numContacts, contactBuffer,
|
||||
manifold, output.normal, output.closestB, box.getMarginF(), contactDist, true, params.mToleranceLength, renderOutput);
|
||||
}
|
||||
else
|
||||
{
|
||||
if(status == GJK_CONTACT)
|
||||
{
|
||||
const Vec3V localPointA = aToB.transformInv(output.closestA);//curRTrans.transformInv(closestA);
|
||||
const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(output.normal), output.penDep);
|
||||
//Add contact to contact stream
|
||||
manifoldContacts[numContacts].mLocalPointA = localPointA;
|
||||
manifoldContacts[numContacts].mLocalPointB = output.closestB;
|
||||
manifoldContacts[numContacts++].mLocalNormalPen = localNormalPen;
|
||||
}
|
||||
else
|
||||
{
|
||||
PX_ASSERT(status == EPA_CONTACT);
|
||||
|
||||
status = epaPenetration(convexA, convexB, manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints,
|
||||
true, FLoad(toleranceLength), output);
|
||||
if(status == EPA_CONTACT)
|
||||
{
|
||||
const Vec3V localPointA = aToB.transformInv(output.closestA);//curRTrans.transformInv(closestA);
|
||||
const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(output.normal), output.penDep);
|
||||
//Add contact to contact stream
|
||||
manifoldContacts[numContacts].mLocalPointA = localPointA;
|
||||
manifoldContacts[numContacts].mLocalPointB = output.closestB;
|
||||
manifoldContacts[numContacts++].mLocalNormalPen = localNormalPen;
|
||||
}
|
||||
else
|
||||
{
|
||||
doOverlapTest = true;
|
||||
}
|
||||
}
|
||||
|
||||
if(initialContacts == 0 || bLostContacts || doOverlapTest)
|
||||
{
|
||||
return fullContactsGenerationCapsuleBox(capsule, box, shapeBox.halfExtents, aToB, transf0, transf1, manifoldContacts, numContacts, contactBuffer, manifold, output.normal,
|
||||
output.closestB, box.getMarginF(), contactDist, doOverlapTest, params.mToleranceLength, renderOutput);
|
||||
}
|
||||
else
|
||||
{
|
||||
//The contacts is either come from GJK or EPA
|
||||
const FloatV replaceBreakingThreshold = FMul(minMargin, FLoad(0.1f));
|
||||
const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(output.normal), output.penDep);
|
||||
manifold.addManifoldPoint2(curRTrans.transformInv(output.closestA), output.closestB, localNormalPen, replaceBreakingThreshold);
|
||||
|
||||
const Vec3V normal = transf1.rotate(output.normal);
|
||||
manifold.addManifoldContactsToContactBuffer(contactBuffer, normal, normal, transf0, capsuleRadius, contactDist);
|
||||
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
else if(manifold.getNumContacts() > 0)
|
||||
{
|
||||
const Vec3V worldNormal = manifold.getWorldNormal(transf1);
|
||||
manifold.addManifoldContactsToContactBuffer(contactBuffer, worldNormal, worldNormal, transf0, capsuleRadius, contactDist);
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
275
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactCapsuleCapsule.cpp
vendored
Normal file
275
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactCapsuleCapsule.cpp
vendored
Normal file
@@ -0,0 +1,275 @@
|
||||
// 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 "geomutils/PxContactBuffer.h"
|
||||
#include "GuVecCapsule.h"
|
||||
#include "GuContactMethodImpl.h"
|
||||
#include "GuDistanceSegmentSegment.h"
|
||||
#include "GuPCMContactGenUtil.h"
|
||||
|
||||
using namespace physx;
|
||||
using namespace Gu;
|
||||
using namespace aos;
|
||||
|
||||
static Vec4V pcmDistancePointSegmentTValue22( const Vec3VArg a0, const Vec3VArg b0,
|
||||
const Vec3VArg a1, const Vec3VArg b1,
|
||||
const Vec3VArg p0, const Vec3VArg p1,
|
||||
const Vec3VArg p2, const Vec3VArg p3)
|
||||
{
|
||||
const Vec4V zero = V4Zero();
|
||||
const Vec3V ap00 = V3Sub(p0, a0);
|
||||
const Vec3V ap10 = V3Sub(p1, a0);
|
||||
const Vec3V ap01 = V3Sub(p2, a1);
|
||||
const Vec3V ap11 = V3Sub(p3, a1);
|
||||
|
||||
const Vec3V ab0 = V3Sub(b0, a0);
|
||||
const Vec3V ab1 = V3Sub(b1, a1);
|
||||
|
||||
/* const FloatV nom00 = V3Dot(ap00, ab0);
|
||||
const FloatV nom10 = V3Dot(ap10, ab0);
|
||||
const FloatV nom01 = V3Dot(ap01, ab1);
|
||||
const FloatV nom11 = V3Dot(ap11, ab1);*/
|
||||
|
||||
const Vec4V combinedDot = V3Dot4(ap00, ab0, ap10, ab0, ap01, ab1, ap11, ab1);
|
||||
const FloatV nom00 = V4GetX(combinedDot);
|
||||
const FloatV nom10 = V4GetY(combinedDot);
|
||||
const FloatV nom01 = V4GetZ(combinedDot);
|
||||
const FloatV nom11 = V4GetW(combinedDot);
|
||||
|
||||
const FloatV denom0 = V3Dot(ab0, ab0);
|
||||
const FloatV denom1 = V3Dot(ab1, ab1);
|
||||
|
||||
const Vec4V nom = V4Merge(nom00, nom10, nom01, nom11);
|
||||
const Vec4V denom = V4Merge(denom0, denom0, denom1, denom1);
|
||||
|
||||
const Vec4V tValue = V4Div(nom, denom);
|
||||
return V4Sel(V4IsEq(denom, zero), zero, tValue);
|
||||
}
|
||||
|
||||
static void storeContact(const Vec3VArg contact, const Vec3VArg normal, const FloatVArg separation, PxContactBuffer& buffer)
|
||||
{
|
||||
outputSimplePCMContact(buffer, contact, normal, separation);
|
||||
}
|
||||
|
||||
bool Gu::pcmContactCapsuleCapsule(GU_CONTACT_METHOD_ARGS)
|
||||
{
|
||||
PX_UNUSED(renderOutput);
|
||||
PX_UNUSED(cache);
|
||||
PX_ASSERT(transform1.q.isSane());
|
||||
PX_ASSERT(transform0.q.isSane());
|
||||
|
||||
// Get actual shape data
|
||||
const PxCapsuleGeometry& shapeCapsule0 = checkedCast<PxCapsuleGeometry>(shape0);
|
||||
const PxCapsuleGeometry& shapeCapsule1 = checkedCast<PxCapsuleGeometry>(shape1);
|
||||
|
||||
const Vec3V _p0 = V3LoadA(&transform0.p.x);
|
||||
const QuatV q0 = QuatVLoadA(&transform0.q.x);
|
||||
|
||||
const Vec3V _p1 = V3LoadA(&transform1.p.x);
|
||||
const QuatV q1 = QuatVLoadA(&transform1.q.x);
|
||||
|
||||
/*PsTransformV transf0(p0, q0);
|
||||
PsTransformV transf1(p1, q1);*/
|
||||
|
||||
const FloatV r0 = FLoad(shapeCapsule0.radius);
|
||||
const FloatV halfHeight0 = FLoad(shapeCapsule0.halfHeight);
|
||||
|
||||
const FloatV r1 = FLoad(shapeCapsule1.radius);
|
||||
const FloatV halfHeight1 = FLoad(shapeCapsule1.halfHeight);
|
||||
|
||||
const FloatV cDist = FLoad(params.mContactDistance);
|
||||
|
||||
const Vec3V positionOffset = V3Scale(V3Add(_p0, _p1), FHalf());
|
||||
const Vec3V p0 = V3Sub(_p0, positionOffset);
|
||||
const Vec3V p1 = V3Sub(_p1, positionOffset);
|
||||
|
||||
const FloatV zero = FZero();
|
||||
//const FloatV one = FOne();
|
||||
const Vec3V zeroV = V3Zero();
|
||||
|
||||
/*const Vec3V positionOffset = V3Scale(V3Add(transf0.p, transf1.p), FloatV_From_F32(0.5f));
|
||||
transf0.p = V3Sub(transf0.p, positionOffset);
|
||||
transf1.p = V3Sub(transf1.p, positionOffset);*/
|
||||
|
||||
const Vec3V basisVector0 = QuatGetBasisVector0(q0);
|
||||
const Vec3V tmp0 = V3Scale(basisVector0, halfHeight0);
|
||||
const Vec3V s0 = V3Add(p0, tmp0);
|
||||
const Vec3V e0 = V3Sub(p0, tmp0);
|
||||
const Vec3V d0 = V3Sub(e0, s0);
|
||||
|
||||
const Vec3V basisVector1 = QuatGetBasisVector0(q1);
|
||||
const Vec3V tmp1 = V3Scale(basisVector1, halfHeight1);
|
||||
const Vec3V s1 = V3Add(p1, tmp1);
|
||||
const Vec3V e1 = V3Sub(p1, tmp1);
|
||||
|
||||
const Vec3V d1 = V3Sub(e1, s1);
|
||||
|
||||
const FloatV sumRadius = FAdd(r0, r1);
|
||||
const FloatV inflatedSum = FAdd(sumRadius, cDist);
|
||||
const FloatV inflatedSumSquared = FMul(inflatedSum, inflatedSum);
|
||||
const FloatV a = V3Dot(d0, d0);//squared length of segment1
|
||||
const FloatV e = V3Dot(d1, d1);//squared length of segment2
|
||||
const FloatV eps = FLoad(1e-6f);//FEps();
|
||||
|
||||
FloatV t0, t1;
|
||||
const FloatV sqDist0 = distanceSegmentSegmentSquared(s0, d0, s1, d1, t0, t1);
|
||||
|
||||
if(FAllGrtrOrEq(inflatedSumSquared, sqDist0))
|
||||
{
|
||||
const Vec4V zeroV4 = V4Zero();
|
||||
const Vec4V oneV4 = V4One();
|
||||
//check to see whether these two capsule are paralle
|
||||
const FloatV parallelTolerance = FLoad(0.9998f);
|
||||
|
||||
const BoolV con0 = FIsGrtr(eps, a);
|
||||
const BoolV con1 = FIsGrtr(eps, e);
|
||||
const Vec3V dir0 = V3Sel(con0, zeroV, V3ScaleInv(d0, FSqrt(a)));
|
||||
const Vec3V dir1 = V3Sel(con1, zeroV, V3ScaleInv(d1, FSqrt(e)));
|
||||
|
||||
const FloatV cos = FAbs(V3Dot(dir0, dir1));
|
||||
if(FAllGrtr(cos, parallelTolerance))//paralle
|
||||
{
|
||||
//project s, e into s1e1
|
||||
const Vec4V t = pcmDistancePointSegmentTValue22(s0, e0, s1, e1,
|
||||
s1, e1, s0, e0);
|
||||
|
||||
const BoolV con = BAnd(V4IsGrtrOrEq(t, zeroV4), V4IsGrtrOrEq(oneV4, t));
|
||||
const BoolV con00 = BGetX(con);
|
||||
const BoolV con01 = BGetY(con);
|
||||
const BoolV con10 = BGetZ(con);
|
||||
const BoolV con11 = BGetW(con);
|
||||
|
||||
/* PX_ALIGN(16, PxU32 conditions[4]);
|
||||
F32Array_Aligned_From_Vec4V(con, (PxF32*)conditions);*/
|
||||
|
||||
PxU32 numContact=0;
|
||||
|
||||
if(BAllEqTTTT(con00))
|
||||
{
|
||||
const Vec3V projS1 = V3ScaleAdd(d0, V4GetX(t), s0);
|
||||
const Vec3V v = V3Sub(projS1, s1);
|
||||
const FloatV sqDist = V3Dot(v, v);
|
||||
const BoolV bCon = BAnd(FIsGrtr(sqDist, eps), FIsGrtr(inflatedSumSquared, sqDist));
|
||||
|
||||
if(BAllEqTTTT(bCon))
|
||||
{
|
||||
const FloatV dist = FSqrt(sqDist);
|
||||
const FloatV pen = FSub(dist, sumRadius);
|
||||
const Vec3V normal = V3ScaleInv(v, dist);
|
||||
PX_ASSERT(isFiniteVec3V(normal));
|
||||
const Vec3V _p = V3NegScaleSub(normal, r0, projS1);
|
||||
const Vec3V p = V3Add(_p, positionOffset);
|
||||
|
||||
storeContact(p, normal, pen, contactBuffer);
|
||||
numContact++;
|
||||
}
|
||||
}
|
||||
if(BAllEqTTTT(con01))
|
||||
{
|
||||
const Vec3V projE1 = V3ScaleAdd(d0, V4GetY(t), s0);
|
||||
const Vec3V v = V3Sub(projE1, e1);
|
||||
const FloatV sqDist = V3Dot(v, v);
|
||||
const BoolV bCon = BAnd(FIsGrtr(sqDist, eps), FIsGrtr(inflatedSumSquared, sqDist));
|
||||
|
||||
if(BAllEqTTTT(bCon))
|
||||
{
|
||||
const FloatV dist = FSqrt(sqDist);
|
||||
const FloatV pen = FSub(dist, sumRadius);
|
||||
const Vec3V normal = V3ScaleInv(v, dist);
|
||||
PX_ASSERT(isFiniteVec3V(normal));
|
||||
const Vec3V _p = V3NegScaleSub(normal, r0, projE1);
|
||||
const Vec3V p = V3Add(_p, positionOffset);
|
||||
storeContact(p, normal, pen, contactBuffer);
|
||||
numContact++;
|
||||
}
|
||||
}
|
||||
|
||||
if(BAllEqTTTT(con10))
|
||||
{
|
||||
const Vec3V projS0 = V3ScaleAdd(d1, V4GetZ(t), s1);
|
||||
const Vec3V v = V3Sub(s0, projS0);
|
||||
const FloatV sqDist = V3Dot(v, v);
|
||||
const BoolV bCon = BAnd(FIsGrtr(sqDist, eps), FIsGrtr(inflatedSumSquared, sqDist));
|
||||
|
||||
if(BAllEqTTTT(bCon))
|
||||
{
|
||||
const FloatV dist = FSqrt(sqDist);
|
||||
const FloatV pen = FSub(dist, sumRadius);
|
||||
const Vec3V normal = V3ScaleInv(v, dist);
|
||||
PX_ASSERT(isFiniteVec3V(normal));
|
||||
const Vec3V _p = V3NegScaleSub(normal, r0, s0);
|
||||
const Vec3V p = V3Add(_p, positionOffset);
|
||||
//const Vec3V p = V3ScaleAdd(normal, r0, s0);
|
||||
storeContact(p, normal, pen, contactBuffer);
|
||||
numContact++;
|
||||
}
|
||||
}
|
||||
|
||||
if(BAllEqTTTT(con11))
|
||||
{
|
||||
const Vec3V projE0 = V3ScaleAdd(d1, V4GetW(t), s1);
|
||||
const Vec3V v = V3Sub(e0, projE0);
|
||||
const FloatV sqDist = V3Dot(v, v);
|
||||
const BoolV bCon = BAnd(FIsGrtr(sqDist, eps), FIsGrtr(inflatedSumSquared, sqDist));
|
||||
|
||||
if(BAllEqTTTT(bCon))
|
||||
{
|
||||
const FloatV dist = FSqrt(sqDist);
|
||||
const FloatV pen = FSub(dist, sumRadius);
|
||||
const Vec3V normal = V3ScaleInv(v, dist);
|
||||
PX_ASSERT(isFiniteVec3V(normal));
|
||||
const Vec3V _p = V3NegScaleSub(normal, r0, e0);
|
||||
const Vec3V p = V3Add(_p, positionOffset);
|
||||
//const Vec3V p = V3ScaleAdd(normal, r0, e0);
|
||||
storeContact(p, normal, pen, contactBuffer);
|
||||
numContact++;
|
||||
}
|
||||
}
|
||||
|
||||
if(numContact)
|
||||
return true;
|
||||
}
|
||||
|
||||
const Vec3V closestA = V3ScaleAdd(d0, t0, s0);
|
||||
const Vec3V closestB = V3ScaleAdd(d1, t1, s1);
|
||||
|
||||
const BoolV con = FIsGrtr(eps, sqDist0);
|
||||
//const Vec3V normal = V3Sel(FIsEq(dist, zero), V3Sel(FIsGrtr(a, eps), V3Normalise(d0), V3Scale(V3Sub(closestA, closestB), FRecip(dist)));
|
||||
const Vec3V _normal = V3Sel(con, V3Sel(FIsGrtr(a, eps), d0, V3UnitX()), V3Sub(closestA, closestB));
|
||||
const Vec3V normal = V3Normalize(_normal);
|
||||
PX_ASSERT(isFiniteVec3V(normal));
|
||||
const Vec3V _point = V3NegScaleSub(normal, r0, closestA);
|
||||
const Vec3V p = V3Add(_point, positionOffset);
|
||||
const FloatV dist = FSel(con, zero, FSqrt(sqDist0));
|
||||
const FloatV pen = FSub(dist, sumRadius);
|
||||
//PX_ASSERT(FAllGrtrOrEq(zero, pen));
|
||||
storeContact(p, normal, pen, contactBuffer);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
239
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactCapsuleConvex.cpp
vendored
Normal file
239
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactCapsuleConvex.cpp
vendored
Normal file
@@ -0,0 +1,239 @@
|
||||
// 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 "geomutils/PxContactBuffer.h"
|
||||
#include "GuGJKPenetration.h"
|
||||
#include "GuEPA.h"
|
||||
#include "GuVecCapsule.h"
|
||||
#include "GuVecConvexHull.h"
|
||||
#include "GuVecConvexHullNoScale.h"
|
||||
#include "GuContactMethodImpl.h"
|
||||
#include "GuPCMContactGen.h"
|
||||
#include "GuPCMShapeConvex.h"
|
||||
|
||||
using namespace physx;
|
||||
using namespace Gu;
|
||||
using namespace aos;
|
||||
|
||||
static bool fullContactsGenerationCapsuleConvex(const CapsuleV& capsule, const ConvexHullV& convexHull, const PxMatTransformV& aToB, const PxTransformV& transf0,const PxTransformV& transf1,
|
||||
PersistentContact* manifoldContacts, PxContactBuffer& contactBuffer, bool idtScale, PersistentContactManifold& manifold, Vec3VArg normal,
|
||||
const Vec3VArg closest, PxReal tolerance, const FloatVArg contactDist, bool doOverlapTest, PxRenderOutput* renderOutput, PxReal toleranceLength)
|
||||
{
|
||||
PX_UNUSED(renderOutput);
|
||||
PolygonalData polyData;
|
||||
getPCMConvexData(convexHull,idtScale, polyData);
|
||||
|
||||
PX_ALIGN(16, PxU8 buff[sizeof(SupportLocalImpl<ConvexHullV>)]);
|
||||
SupportLocal* map = (idtScale ? static_cast<SupportLocal*>(PX_PLACEMENT_NEW(buff, SupportLocalImpl<ConvexHullNoScaleV>)(static_cast<const ConvexHullNoScaleV&>(convexHull), transf1, convexHull.vertex2Shape, convexHull.shape2Vertex, idtScale)) :
|
||||
static_cast<SupportLocal*>(PX_PLACEMENT_NEW(buff, SupportLocalImpl<ConvexHullV>)(convexHull, transf1, convexHull.vertex2Shape, convexHull.shape2Vertex, idtScale)));
|
||||
|
||||
PxU32 numContacts = 0;
|
||||
if(!generateFullContactManifold(capsule, polyData, map, aToB, manifoldContacts, numContacts, contactDist, normal, closest, tolerance, doOverlapTest, toleranceLength))
|
||||
return false;
|
||||
|
||||
if (numContacts > 0)
|
||||
{
|
||||
manifold.addBatchManifoldContacts2(manifoldContacts, numContacts);
|
||||
//transform normal into the world space
|
||||
normal = transf1.rotate(normal);
|
||||
manifold.addManifoldContactsToContactBuffer(contactBuffer, normal, normal, transf0, capsule.radius, contactDist);
|
||||
}
|
||||
else
|
||||
{
|
||||
if (!doOverlapTest)
|
||||
{
|
||||
normal = transf1.rotate(normal);
|
||||
manifold.addManifoldContactsToContactBuffer(contactBuffer, normal, normal, transf0, capsule.radius, contactDist);
|
||||
}
|
||||
}
|
||||
|
||||
#if PCM_LOW_LEVEL_DEBUG
|
||||
manifold.drawManifold(*renderOutput, transf0, transf1);
|
||||
#endif
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Gu::pcmContactCapsuleConvex(GU_CONTACT_METHOD_ARGS)
|
||||
{
|
||||
PX_UNUSED(renderOutput);
|
||||
PX_ASSERT(transform1.q.isSane());
|
||||
PX_ASSERT(transform0.q.isSane());
|
||||
|
||||
const PxConvexMeshGeometry& shapeConvex = checkedCast<PxConvexMeshGeometry>(shape1);
|
||||
const PxCapsuleGeometry& shapeCapsule = checkedCast<PxCapsuleGeometry>(shape0);
|
||||
|
||||
PersistentContactManifold& manifold = cache.getManifold();
|
||||
|
||||
const ConvexHullData* hullData = _getHullData(shapeConvex);
|
||||
PxPrefetchLine(hullData);
|
||||
|
||||
const Vec3V vScale = V3LoadU_SafeReadW(shapeConvex.scale.scale); // PT: safe because 'rotation' follows 'scale' in PxMeshScale
|
||||
|
||||
const FloatV contactDist = FLoad(params.mContactDistance);
|
||||
const FloatV capsuleHalfHeight = FLoad(shapeCapsule.halfHeight);
|
||||
const FloatV capsuleRadius = FLoad(shapeCapsule.radius);
|
||||
|
||||
//Transfer A into the local space of B
|
||||
const PxTransformV transf0 = loadTransformA(transform0);
|
||||
const PxTransformV transf1 = loadTransformA(transform1);
|
||||
const PxTransformV curRTrans(transf1.transformInv(transf0));
|
||||
const PxMatTransformV aToB(curRTrans);
|
||||
|
||||
const PxReal toleranceLength = params.mToleranceLength;
|
||||
const FloatV convexMargin = CalculatePCMConvexMargin(hullData, vScale, toleranceLength);
|
||||
const FloatV capsuleMinMargin = CalculateCapsuleMinMargin(capsuleRadius);
|
||||
const FloatV minMargin = FMin(convexMargin, capsuleMinMargin);
|
||||
|
||||
const PxU32 initialContacts = manifold.mNumContacts;
|
||||
const FloatV projectBreakingThreshold = FMul(minMargin, FLoad(1.25f));
|
||||
const FloatV refreshDist = FAdd(contactDist, capsuleRadius);
|
||||
|
||||
manifold.refreshContactPoints(aToB, projectBreakingThreshold, refreshDist);
|
||||
|
||||
//ML: after refreshContactPoints, we might lose some contacts
|
||||
const bool bLostContacts = (manifold.mNumContacts != initialContacts);
|
||||
|
||||
GjkStatus status = manifold.mNumContacts > 0 ? GJK_UNDEFINED : GJK_NON_INTERSECT;
|
||||
|
||||
if(bLostContacts || manifold.invalidate_SphereCapsule(curRTrans, minMargin))
|
||||
{
|
||||
const bool idtScale = shapeConvex.scale.isIdentity();
|
||||
|
||||
manifold.setRelativeTransform(curRTrans);
|
||||
const QuatV vQuat = QuatVLoadU(&shapeConvex.scale.rotation.x);
|
||||
const ConvexHullV convexHull(hullData, V3LoadU(hullData->mCenterOfMass), vScale, vQuat, idtScale);
|
||||
|
||||
//transform capsule(a) into the local space of convexHull(b)
|
||||
const CapsuleV capsule(aToB.p, aToB.rotate(V3Scale(V3UnitX(), capsuleHalfHeight)), capsuleRadius);
|
||||
|
||||
GjkOutput output;
|
||||
const LocalConvex<CapsuleV> convexA(capsule);
|
||||
const Vec3V initialSearchDir = V3Sub(capsule.getCenter(), convexHull.getCenter());
|
||||
if(idtScale)
|
||||
{
|
||||
const LocalConvex<ConvexHullNoScaleV> convexB(*PX_CONVEX_TO_NOSCALECONVEX(&convexHull));
|
||||
status = gjkPenetration<LocalConvex<CapsuleV>, LocalConvex<ConvexHullNoScaleV> >(convexA, convexB, initialSearchDir, contactDist, true,
|
||||
manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, output);
|
||||
}
|
||||
else
|
||||
{
|
||||
const LocalConvex<ConvexHullV> convexB(convexHull);
|
||||
status = gjkPenetration<LocalConvex<CapsuleV>, LocalConvex<ConvexHullV> >(convexA, convexB, initialSearchDir, contactDist, true,
|
||||
manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, output);
|
||||
}
|
||||
|
||||
PersistentContact* manifoldContacts = PX_CP_TO_PCP(contactBuffer.contacts);
|
||||
bool doOverlapTest = false;
|
||||
if(status == GJK_NON_INTERSECT)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
else if(status == GJK_DEGENERATE)
|
||||
{
|
||||
return fullContactsGenerationCapsuleConvex(capsule, convexHull, aToB, transf0, transf1, manifoldContacts, contactBuffer, idtScale, manifold, output.normal,
|
||||
output.closestB, convexHull.getMarginF(), contactDist, true, renderOutput, toleranceLength);
|
||||
}
|
||||
else
|
||||
{
|
||||
const FloatV replaceBreakingThreshold = FMul(minMargin, FLoad(0.05f));
|
||||
|
||||
if(status == GJK_CONTACT)
|
||||
{
|
||||
const Vec3V localPointA = aToB.transformInv(output.closestA);//curRTrans.transformInv(closestA);
|
||||
const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(output.normal), output.penDep);
|
||||
//Add contact to contact stream
|
||||
manifoldContacts[0].mLocalPointA = localPointA;
|
||||
manifoldContacts[0].mLocalPointB = output.closestB;
|
||||
manifoldContacts[0].mLocalNormalPen = localNormalPen;
|
||||
|
||||
//Add contact to manifold
|
||||
manifold.addManifoldPoint2(localPointA, output.closestB, localNormalPen, replaceBreakingThreshold);
|
||||
}
|
||||
else
|
||||
{
|
||||
PX_ASSERT(status == EPA_CONTACT);
|
||||
|
||||
if(idtScale)
|
||||
{
|
||||
const LocalConvex<ConvexHullNoScaleV> convexB(*PX_CONVEX_TO_NOSCALECONVEX(&convexHull));
|
||||
status = epaPenetration(convexA, convexB, manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints,
|
||||
true, FLoad(toleranceLength), output);
|
||||
}
|
||||
else
|
||||
{
|
||||
const LocalConvex<ConvexHullV> convexB(convexHull);
|
||||
status = epaPenetration(convexA, convexB, manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints,
|
||||
true, FLoad(toleranceLength), output);
|
||||
}
|
||||
|
||||
if(status == EPA_CONTACT)
|
||||
{
|
||||
const Vec3V localPointA = aToB.transformInv(output.closestA);//curRTrans.transformInv(closestA);
|
||||
const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(output.normal), output.penDep);
|
||||
//Add contact to contact stream
|
||||
manifoldContacts[0].mLocalPointA = localPointA;
|
||||
manifoldContacts[0].mLocalPointB = output.closestB;
|
||||
manifoldContacts[0].mLocalNormalPen = localNormalPen;
|
||||
|
||||
//Add contact to manifold
|
||||
manifold.addManifoldPoint2(localPointA, output.closestB, localNormalPen, replaceBreakingThreshold);
|
||||
}
|
||||
else
|
||||
{
|
||||
doOverlapTest = true;
|
||||
}
|
||||
}
|
||||
|
||||
if(initialContacts == 0 || bLostContacts || doOverlapTest)
|
||||
{
|
||||
return fullContactsGenerationCapsuleConvex(capsule, convexHull, aToB, transf0, transf1, manifoldContacts, contactBuffer, idtScale, manifold, output.normal,
|
||||
output.closestB, convexHull.getMarginF(), contactDist, doOverlapTest, renderOutput, toleranceLength);
|
||||
}
|
||||
else
|
||||
{
|
||||
//This contact is either come from GJK or EPA
|
||||
const Vec3V normal = transf1.rotate(output.normal);
|
||||
manifold.addManifoldContactsToContactBuffer(contactBuffer, normal, normal, transf0, capsuleRadius, contactDist);
|
||||
#if PCM_LOW_LEVEL_DEBUG
|
||||
manifold.drawManifold(*renderOutput, transf0, transf1);
|
||||
#endif
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (manifold.getNumContacts() > 0)
|
||||
{
|
||||
const Vec3V normal = manifold.getWorldNormal(transf1);
|
||||
manifold.addManifoldContactsToContactBuffer(contactBuffer, normal, normal, transf0, capsuleRadius, contactDist);
|
||||
#if PCM_LOW_LEVEL_DEBUG
|
||||
manifold.drawManifold(*renderOutput, transf0, transf1);
|
||||
#endif
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
138
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactCapsuleHeightField.cpp
vendored
Normal file
138
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactCapsuleHeightField.cpp
vendored
Normal file
@@ -0,0 +1,138 @@
|
||||
// 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/PxTriangleMesh.h"
|
||||
#include "geomutils/PxContactBuffer.h"
|
||||
#include "foundation/PxVecMath.h"
|
||||
#include "foundation/PxVecTransform.h"
|
||||
#include "GuVecTriangle.h"
|
||||
#include "GuContactMethodImpl.h"
|
||||
#include "GuHeightField.h"
|
||||
#include "GuPCMContactConvexCommon.h"
|
||||
#include "GuSegment.h"
|
||||
#include "GuInternal.h"
|
||||
#include "GuPCMContactMeshCallback.h"
|
||||
|
||||
using namespace physx;
|
||||
using namespace Gu;
|
||||
using namespace aos;
|
||||
|
||||
struct PCMCapsuleVsHeightfieldContactGenerationCallback : PCMHeightfieldContactGenerationCallback<PCMCapsuleVsHeightfieldContactGenerationCallback>
|
||||
{
|
||||
PCMCapsuleVsHeightfieldContactGenerationCallback& operator=(const PCMCapsuleVsHeightfieldContactGenerationCallback&);
|
||||
|
||||
PCMCapsuleVsMeshContactGeneration mGeneration;
|
||||
|
||||
PCMCapsuleVsHeightfieldContactGenerationCallback(
|
||||
const CapsuleV& capsule,
|
||||
const FloatVArg contactDistance,
|
||||
const FloatVArg replaceBreakingThreshold,
|
||||
const PxTransformV& capsuleTransform,
|
||||
const PxTransformV& heightfieldTransform,
|
||||
const PxTransform& heightfieldTransform1,
|
||||
MultiplePersistentContactManifold& multiManifold,
|
||||
PxContactBuffer& contactBuffer,
|
||||
PxInlineArray<PxU32, LOCAL_PCM_CONTACTS_SIZE>* deferredContacts,
|
||||
HeightFieldUtil& hfUtil
|
||||
) :
|
||||
PCMHeightfieldContactGenerationCallback<PCMCapsuleVsHeightfieldContactGenerationCallback>(hfUtil, heightfieldTransform1),
|
||||
mGeneration(capsule, contactDistance, replaceBreakingThreshold, capsuleTransform, heightfieldTransform, multiManifold,
|
||||
contactBuffer, deferredContacts)
|
||||
{
|
||||
}
|
||||
|
||||
template<PxU32 CacheSize>
|
||||
void processTriangleCache(TriangleCache<CacheSize>& cache)
|
||||
{
|
||||
mGeneration.processTriangleCache<CacheSize, PCMCapsuleVsMeshContactGeneration>(cache);
|
||||
}
|
||||
};
|
||||
|
||||
bool Gu::pcmContactCapsuleHeightField(GU_CONTACT_METHOD_ARGS)
|
||||
{
|
||||
PX_UNUSED(renderOutput);
|
||||
|
||||
const PxCapsuleGeometry& shapeCapsule = checkedCast<PxCapsuleGeometry>(shape0);
|
||||
const PxHeightFieldGeometry& shapeHeight = checkedCast<PxHeightFieldGeometry>(shape1);
|
||||
|
||||
MultiplePersistentContactManifold& multiManifold = cache.getMultipleManifold();
|
||||
|
||||
const FloatV capsuleRadius = FLoad(shapeCapsule.radius);
|
||||
const FloatV contactDist = FLoad(params.mContactDistance);
|
||||
|
||||
const PxTransformV capsuleTransform = loadTransformA(transform0);//capsule transform
|
||||
const PxTransformV heightfieldTransform = loadTransformA(transform1);//height feild
|
||||
|
||||
const PxTransformV curTransform = heightfieldTransform.transformInv(capsuleTransform);
|
||||
|
||||
const FloatV replaceBreakingThreshold = FMul(capsuleRadius, FLoad(0.001f));
|
||||
|
||||
if(multiManifold.invalidate(curTransform, capsuleRadius, FLoad(0.02f)))
|
||||
{
|
||||
multiManifold.mNumManifolds = 0;
|
||||
multiManifold.setRelativeTransform(curTransform);
|
||||
|
||||
HeightFieldUtil hfUtil(shapeHeight);
|
||||
|
||||
const PxVec3 tmp = getCapsuleHalfHeightVector(transform0, shapeCapsule);
|
||||
|
||||
const PxReal inflatedRadius = shapeCapsule.radius + params.mContactDistance;
|
||||
|
||||
const PxVec3 capsuleCenterInMesh = transform1.transformInv(transform0.p);
|
||||
const PxVec3 capsuleDirInMesh = transform1.rotateInv(tmp);
|
||||
const CapsuleV capsule(V3LoadU(capsuleCenterInMesh), V3LoadU(capsuleDirInMesh), capsuleRadius);
|
||||
|
||||
PCMCapsuleVsHeightfieldContactGenerationCallback callback(
|
||||
capsule,
|
||||
contactDist,
|
||||
replaceBreakingThreshold,
|
||||
capsuleTransform,
|
||||
heightfieldTransform,
|
||||
transform1,
|
||||
multiManifold,
|
||||
contactBuffer,
|
||||
NULL,
|
||||
hfUtil
|
||||
);
|
||||
|
||||
// PT: TODO: improve these bounds - see computeCapsuleBounds
|
||||
hfUtil.overlapAABBTriangles(transform0, transform1, getLocalCapsuleBounds(inflatedRadius, shapeCapsule.halfHeight), callback);
|
||||
|
||||
callback.mGeneration.processContacts(GU_CAPSULE_MANIFOLD_CACHE_SIZE, false);
|
||||
}
|
||||
else
|
||||
{
|
||||
const PxMatTransformV aToB(curTransform);
|
||||
// We must be in local space to use the cache
|
||||
const FloatV projectBreakingThreshold = FMul(capsuleRadius, FLoad(0.05f));
|
||||
const FloatV refereshDistance = FAdd(capsuleRadius, contactDist);
|
||||
multiManifold.refreshManifold(aToB, projectBreakingThreshold, refereshDistance);
|
||||
}
|
||||
|
||||
return multiManifold.addManifoldContactsToContactBuffer(contactBuffer, capsuleTransform, heightfieldTransform, capsuleRadius);
|
||||
}
|
||||
851
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactCapsuleMesh.cpp
vendored
Normal file
851
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactCapsuleMesh.cpp
vendored
Normal file
@@ -0,0 +1,851 @@
|
||||
// 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 "GuVecTriangle.h"
|
||||
#include "GuContactMethodImpl.h"
|
||||
#include "GuPCMContactConvexCommon.h"
|
||||
#include "GuInternal.h"
|
||||
#include "GuPCMContactMeshCallback.h"
|
||||
#include "GuBarycentricCoordinates.h"
|
||||
#include "GuBox.h"
|
||||
|
||||
using namespace physx;
|
||||
using namespace Gu;
|
||||
using namespace aos;
|
||||
|
||||
namespace
|
||||
{
|
||||
struct PCMCapsuleVsMeshContactGenerationCallback : PCMMeshContactGenerationCallback< PCMCapsuleVsMeshContactGenerationCallback >
|
||||
{
|
||||
PCMCapsuleVsMeshContactGenerationCallback& operator=(const PCMCapsuleVsMeshContactGenerationCallback&);
|
||||
|
||||
PCMCapsuleVsMeshContactGeneration mGeneration;
|
||||
|
||||
PCMCapsuleVsMeshContactGenerationCallback(
|
||||
const CapsuleV& capsule,
|
||||
const FloatVArg contactDist,
|
||||
const FloatVArg replaceBreakingThreshold,
|
||||
const PxTransformV& sphereTransform,
|
||||
const PxTransformV& meshTransform,
|
||||
MultiplePersistentContactManifold& multiManifold,
|
||||
PxContactBuffer& contactBuffer,
|
||||
const PxU8* extraTriData,
|
||||
const Cm::FastVertex2ShapeScaling& meshScaling,
|
||||
bool idtMeshScale,
|
||||
PxInlineArray<PxU32, LOCAL_PCM_CONTACTS_SIZE>* deferredContacts,
|
||||
PxRenderOutput* renderOutput = NULL
|
||||
) :
|
||||
PCMMeshContactGenerationCallback<PCMCapsuleVsMeshContactGenerationCallback>(meshScaling, extraTriData, idtMeshScale),
|
||||
mGeneration(capsule, contactDist, replaceBreakingThreshold, sphereTransform, meshTransform, multiManifold, contactBuffer,
|
||||
deferredContacts, renderOutput)
|
||||
{
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE bool doTest(const PxVec3&, const PxVec3&, const PxVec3&) { return true; }
|
||||
|
||||
template<PxU32 CacheSize>
|
||||
void processTriangleCache(TriangleCache<CacheSize>& cache)
|
||||
{
|
||||
mGeneration.processTriangleCache<CacheSize, PCMCapsuleVsMeshContactGeneration>(cache);
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
bool Gu::pcmContactCapsuleMesh(GU_CONTACT_METHOD_ARGS)
|
||||
{
|
||||
MultiplePersistentContactManifold& multiManifold = cache.getMultipleManifold();
|
||||
const PxCapsuleGeometry& shapeCapsule = checkedCast<PxCapsuleGeometry>(shape0);
|
||||
const PxTriangleMeshGeometry& shapeMesh = checkedCast<PxTriangleMeshGeometry>(shape1);
|
||||
|
||||
//gRenderOutPut = cache.mRenderOutput;
|
||||
const FloatV capsuleRadius = FLoad(shapeCapsule.radius);
|
||||
const FloatV contactDist = FLoad(params.mContactDistance);
|
||||
|
||||
const PxTransformV capsuleTransform = loadTransformA(transform0);//capsule transform
|
||||
const PxTransformV meshTransform = loadTransformA(transform1);//triangleMesh
|
||||
|
||||
const PxTransformV curTransform = meshTransform.transformInv(capsuleTransform);
|
||||
|
||||
// We must be in local space to use the cache
|
||||
if(multiManifold.invalidate(curTransform, capsuleRadius, FLoad(0.02f)))
|
||||
{
|
||||
const FloatV replaceBreakingThreshold = FMul(capsuleRadius, FLoad(0.001f));
|
||||
//const FloatV capsuleHalfHeight = FloatV_From_F32(shapeCapsule.halfHeight);
|
||||
Cm::FastVertex2ShapeScaling meshScaling;
|
||||
const bool idtMeshScale = shapeMesh.scale.isIdentity();
|
||||
if(!idtMeshScale)
|
||||
meshScaling.init(shapeMesh.scale);
|
||||
|
||||
// Capsule data
|
||||
const PxVec3 tmp = getCapsuleHalfHeightVector(transform0, shapeCapsule);
|
||||
const Segment worldCapsule(transform0.p + tmp, transform0.p - tmp);
|
||||
|
||||
const Segment meshCapsule( // Capsule in mesh space
|
||||
transform1.transformInv(worldCapsule.p0),
|
||||
transform1.transformInv(worldCapsule.p1));
|
||||
|
||||
const PxReal inflatedRadius = shapeCapsule.radius + params.mContactDistance;
|
||||
|
||||
const PxVec3 capsuleCenterInMesh = transform1.transformInv(transform0.p);
|
||||
const PxVec3 capsuleDirInMesh = transform1.rotateInv(tmp);
|
||||
const CapsuleV capsule(V3LoadU(capsuleCenterInMesh), V3LoadU(capsuleDirInMesh), capsuleRadius);
|
||||
|
||||
// We must be in local space to use the cache
|
||||
const Capsule queryCapsule(meshCapsule, inflatedRadius);
|
||||
|
||||
const TriangleMesh* meshData = _getMeshData(shapeMesh);
|
||||
|
||||
multiManifold.mNumManifolds = 0;
|
||||
multiManifold.setRelativeTransform(curTransform);
|
||||
|
||||
const PxU8* PX_RESTRICT extraData = meshData->getExtraTrigData();
|
||||
// mesh scale is not baked into cached verts
|
||||
PCMCapsuleVsMeshContactGenerationCallback callback(
|
||||
capsule,
|
||||
contactDist,
|
||||
replaceBreakingThreshold,
|
||||
capsuleTransform,
|
||||
meshTransform,
|
||||
multiManifold,
|
||||
contactBuffer,
|
||||
extraData,
|
||||
meshScaling,
|
||||
idtMeshScale,
|
||||
NULL,
|
||||
renderOutput);
|
||||
|
||||
//bound the capsule in shape space by an OBB:
|
||||
Box queryBox;
|
||||
queryBox.create(queryCapsule);
|
||||
|
||||
//apply the skew transform to the box:
|
||||
if(!idtMeshScale)
|
||||
meshScaling.transformQueryBounds(queryBox.center, queryBox.extents, queryBox.rot);
|
||||
|
||||
Midphase::intersectOBB(meshData, queryBox, callback, true);
|
||||
|
||||
callback.flushCache();
|
||||
|
||||
callback.mGeneration.processContacts(GU_CAPSULE_MANIFOLD_CACHE_SIZE, false);
|
||||
}
|
||||
else
|
||||
{
|
||||
const PxMatTransformV aToB(curTransform);
|
||||
const FloatV projectBreakingThreshold = FMul(capsuleRadius, FLoad(0.05f));
|
||||
const FloatV refereshDistance = FAdd(capsuleRadius, contactDist);
|
||||
//multiManifold.refreshManifold(aToB, projectBreakingThreshold, contactDist);
|
||||
multiManifold.refreshManifold(aToB, projectBreakingThreshold, refereshDistance);
|
||||
}
|
||||
|
||||
//multiManifold.drawManifold(*gRenderOutPut, capsuleTransform, meshTransform);
|
||||
return multiManifold.addManifoldContactsToContactBuffer(contactBuffer, capsuleTransform, meshTransform, capsuleRadius);
|
||||
}
|
||||
|
||||
void Gu::PCMCapsuleVsMeshContactGeneration::generateEE(const Vec3VArg p, const Vec3VArg q, const FloatVArg sqInflatedRadius,
|
||||
const Vec3VArg normal, PxU32 triangleIndex, const Vec3VArg a, const Vec3VArg b,
|
||||
MeshPersistentContact* manifoldContacts, PxU32& numContacts)
|
||||
{
|
||||
const FloatV zero = FZero();
|
||||
const Vec3V ab = V3Sub(b, a);
|
||||
const Vec3V n = V3Cross(ab, normal);
|
||||
const FloatV d = V3Dot(n, a);
|
||||
const FloatV np = V3Dot(n, p);
|
||||
const FloatV nq = V3Dot(n,q);
|
||||
const FloatV signP = FSub(np, d);
|
||||
const FloatV signQ = FSub(nq, d);
|
||||
const FloatV temp = FMul(signP, signQ);
|
||||
if(FAllGrtr(temp, zero))
|
||||
return;//If both points in the same side as the plane, no intersect points
|
||||
|
||||
// if colliding edge (p3,p4) and plane are parallel return no collision
|
||||
const Vec3V pq = V3Sub(q, p);
|
||||
const FloatV npq = V3Dot(n, pq);
|
||||
if(FAllEq(npq, zero))
|
||||
return;
|
||||
|
||||
const FloatV one = FOne();
|
||||
//calculate the intersect point in the segment pq
|
||||
const FloatV segTValue = FDiv(FSub(d, np), npq);
|
||||
const Vec3V localPointA = V3ScaleAdd(pq, segTValue, p);
|
||||
|
||||
//calculate a normal perpendicular to ray localPointA + normal, 2D segment segment intersection
|
||||
const Vec3V perNormal = V3Cross(normal, pq);
|
||||
const Vec3V ap = V3Sub(localPointA, a);
|
||||
const FloatV nom = V3Dot(perNormal, ap);
|
||||
const FloatV denom = V3Dot(perNormal, ab);
|
||||
|
||||
//const FloatV tValue = FClamp(FDiv(nom, denom), zero, FOne());
|
||||
const FloatV tValue = FDiv(nom, denom);
|
||||
const BoolV con = BAnd(FIsGrtrOrEq(one, tValue), FIsGrtrOrEq(tValue, zero));
|
||||
if(BAllEqFFFF(con))
|
||||
return;
|
||||
|
||||
//const Vec3V localPointB = V3ScaleAdd(ab, tValue, a); v = V3Sub(localPointA, localPointB); v = V3NegScaleSub(ab, tValue, tap)
|
||||
const Vec3V v = V3NegScaleSub(ab, tValue, ap);
|
||||
const FloatV sqDist = V3Dot(v, v);
|
||||
|
||||
if(FAllGrtr(sqInflatedRadius, sqDist))
|
||||
{
|
||||
const Vec3V localPointB = V3Sub(localPointA, v);
|
||||
const FloatV signedDist = V3Dot(v, normal);
|
||||
|
||||
manifoldContacts[numContacts].mLocalPointA = localPointA;
|
||||
manifoldContacts[numContacts].mLocalPointB = localPointB;
|
||||
manifoldContacts[numContacts].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(normal), signedDist);
|
||||
manifoldContacts[numContacts++].mFaceIndex = triangleIndex;
|
||||
}
|
||||
}
|
||||
|
||||
void Gu::PCMCapsuleVsMeshContactGeneration::generateEEContacts(const Vec3VArg a, const Vec3VArg b,
|
||||
const Vec3VArg c, const Vec3VArg normal,
|
||||
PxU32 triangleIndex, const Vec3VArg p,
|
||||
const Vec3VArg q, const FloatVArg sqInflatedRadius,
|
||||
PxU32 previousNumContacts, MeshPersistentContact* manifoldContacts,
|
||||
PxU32& numContacts)
|
||||
{
|
||||
if((numContacts - previousNumContacts) < 2)
|
||||
generateEE(p, q, sqInflatedRadius, normal, triangleIndex, a, b, manifoldContacts, numContacts);
|
||||
if((numContacts - previousNumContacts) < 2)
|
||||
generateEE(p, q, sqInflatedRadius, normal, triangleIndex, b, c, manifoldContacts, numContacts);
|
||||
if((numContacts - previousNumContacts) < 2)
|
||||
generateEE(p, q, sqInflatedRadius, normal, triangleIndex, a, c, manifoldContacts, numContacts);
|
||||
}
|
||||
|
||||
void Gu::PCMCapsuleVsMeshContactGeneration::generateEEMTD( const Vec3VArg p, const Vec3VArg q, const FloatVArg inflatedRadius,
|
||||
const Vec3VArg normal, PxU32 triangleIndex, const Vec3VArg a, const Vec3VArg b,
|
||||
MeshPersistentContact* manifoldContacts, PxU32& numContacts)
|
||||
{
|
||||
const FloatV zero = FZero();
|
||||
const Vec3V ab = V3Sub(b, a);
|
||||
const Vec3V n = V3Cross(ab, normal);
|
||||
const FloatV d = V3Dot(n, a);
|
||||
const FloatV np = V3Dot(n, p);
|
||||
const FloatV nq = V3Dot(n,q);
|
||||
const FloatV signP = FSub(np, d);
|
||||
const FloatV signQ = FSub(nq, d);
|
||||
const FloatV temp = FMul(signP, signQ);
|
||||
if(FAllGrtr(temp, zero))
|
||||
return;//If both points in the same side as the plane, no intersect points
|
||||
|
||||
// if colliding edge (p3,p4) and plane are parallel return no collision
|
||||
const Vec3V pq = V3Sub(q, p);
|
||||
const FloatV npq = V3Dot(n, pq);
|
||||
if(FAllEq(npq, zero))
|
||||
return;
|
||||
|
||||
//calculate the intersect point in the segment pq
|
||||
const FloatV segTValue = FDiv(FSub(d, np), npq);
|
||||
const Vec3V localPointA = V3ScaleAdd(pq, segTValue, p);
|
||||
|
||||
//calculate a normal perpendicular to ray localPointA + normal, 2D segment segment intersection
|
||||
const Vec3V perNormal = V3Cross(normal, pq);
|
||||
const Vec3V ap = V3Sub(localPointA, a);
|
||||
const FloatV nom = V3Dot(perNormal, ap);
|
||||
const FloatV denom = V3Dot(perNormal, ab);
|
||||
|
||||
const FloatV tValue = FClamp(FDiv(nom, denom), zero, FOne());
|
||||
|
||||
const Vec3V v = V3NegScaleSub(ab, tValue, ap);
|
||||
const FloatV signedDist = V3Dot(v, normal);
|
||||
|
||||
if(FAllGrtr(inflatedRadius, signedDist))
|
||||
{
|
||||
const Vec3V localPointB = V3Sub(localPointA, v);
|
||||
manifoldContacts[numContacts].mLocalPointA = localPointA;
|
||||
manifoldContacts[numContacts].mLocalPointB = localPointB;
|
||||
manifoldContacts[numContacts].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(normal), signedDist);
|
||||
manifoldContacts[numContacts++].mFaceIndex = triangleIndex;
|
||||
}
|
||||
}
|
||||
|
||||
void Gu::PCMCapsuleVsMeshContactGeneration::generateEEContactsMTD( const Vec3VArg a, const Vec3VArg b,
|
||||
const Vec3VArg c, const Vec3VArg normal,
|
||||
PxU32 triangleIndex, const Vec3VArg p,
|
||||
const Vec3VArg q, const FloatVArg inflatedRadius,
|
||||
MeshPersistentContact* manifoldContacts, PxU32& numContacts)
|
||||
{
|
||||
generateEEMTD(p, q, inflatedRadius, normal, triangleIndex, a, b, manifoldContacts, numContacts);
|
||||
generateEEMTD(p, q, inflatedRadius, normal, triangleIndex, b, c, manifoldContacts, numContacts);
|
||||
generateEEMTD(p, q, inflatedRadius, normal, triangleIndex, a, c, manifoldContacts, numContacts);
|
||||
}
|
||||
|
||||
void Gu::PCMCapsuleVsMeshContactGeneration::generateContacts(const Vec3VArg a, const Vec3VArg b,
|
||||
const Vec3VArg c, const Vec3VArg planeNormal,
|
||||
const Vec3VArg normal, PxU32 triangleIndex,
|
||||
const Vec3VArg p, const Vec3VArg q,
|
||||
const FloatVArg inflatedRadius,
|
||||
MeshPersistentContact* manifoldContacts, PxU32& numContacts)
|
||||
{
|
||||
const Vec3V ab = V3Sub(b, a);
|
||||
const Vec3V ac = V3Sub(c, a);
|
||||
const Vec3V ap = V3Sub(p, a);
|
||||
const Vec3V aq = V3Sub(q, a);
|
||||
|
||||
//This is used to calculate the barycentric coordinate
|
||||
const FloatV d00 = V3Dot(ab, ab);
|
||||
const FloatV d01 = V3Dot(ab, ac);
|
||||
const FloatV d11 = V3Dot(ac, ac);
|
||||
|
||||
const FloatV zero = FZero();
|
||||
const FloatV largeValue = FLoad(1e+20f);
|
||||
const FloatV tDenom = FSub(FMul(d00, d11), FMul(d01, d01));
|
||||
const FloatV bdenom = FSel(FIsGrtr(tDenom, zero), FRecip(tDenom), largeValue);
|
||||
|
||||
//compute the intersect point of p and triangle plane abc
|
||||
const FloatV inomp = V3Dot(planeNormal, V3Neg(ap));
|
||||
const FloatV ideom = V3Dot(planeNormal, normal);
|
||||
|
||||
const FloatV ipt = FSel(FIsGrtr(ideom, zero), FNeg(FDiv(inomp, ideom)), largeValue);
|
||||
|
||||
const Vec3V closestP31 = V3NegScaleSub(normal, ipt, p);
|
||||
const Vec3V closestP30 = p;
|
||||
|
||||
//Compute the barycentric coordinate for project point of q
|
||||
const Vec3V pV20 = V3Sub(closestP31, a);
|
||||
const FloatV pD20 = V3Dot(pV20, ab);
|
||||
const FloatV pD21 = V3Dot(pV20, ac);
|
||||
const FloatV v0 = FMul(FSub(FMul(d11, pD20), FMul(d01, pD21)), bdenom);
|
||||
const FloatV w0 = FMul(FSub(FMul(d00, pD21), FMul(d01, pD20)), bdenom);
|
||||
|
||||
//check closestP3 is inside the triangle
|
||||
const BoolV con0 = isValidTriangleBarycentricCoord(v0, w0);
|
||||
|
||||
const BoolV tempCon0 = BAnd(con0, FIsGrtr(inflatedRadius, ipt));
|
||||
if(BAllEqTTTT(tempCon0))
|
||||
{
|
||||
manifoldContacts[numContacts].mLocalPointA = closestP30;//transform to B space, it will get convert to A space later
|
||||
manifoldContacts[numContacts].mLocalPointB = closestP31;
|
||||
manifoldContacts[numContacts].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(normal), ipt);
|
||||
manifoldContacts[numContacts++].mFaceIndex = triangleIndex;
|
||||
}
|
||||
|
||||
const FloatV inomq = V3Dot(planeNormal, V3Neg(aq));
|
||||
|
||||
const FloatV iqt = FSel(FIsGrtr(ideom, FZero()), FNeg(FDiv(inomq, ideom)), FLoad(1e+20f));
|
||||
|
||||
const Vec3V closestP41 = V3NegScaleSub(normal, iqt, q);
|
||||
const Vec3V closestP40 = q;
|
||||
|
||||
//Compute the barycentric coordinate for project point of q
|
||||
const Vec3V qV20 = V3Sub(closestP41, a);
|
||||
const FloatV qD20 = V3Dot(qV20, ab);
|
||||
const FloatV qD21 = V3Dot(qV20, ac);
|
||||
const FloatV v1 = FMul(FSub(FMul(d11, qD20), FMul(d01, qD21)), bdenom);
|
||||
const FloatV w1 = FMul(FSub(FMul(d00, qD21), FMul(d01, qD20)), bdenom);
|
||||
|
||||
const BoolV con1 = isValidTriangleBarycentricCoord(v1, w1);
|
||||
|
||||
const BoolV tempCon1 = BAnd(con1, FIsGrtr(inflatedRadius, iqt));
|
||||
if(BAllEqTTTT(tempCon1))
|
||||
{
|
||||
manifoldContacts[numContacts].mLocalPointA = closestP40;
|
||||
manifoldContacts[numContacts].mLocalPointB = closestP41;
|
||||
manifoldContacts[numContacts].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(normal), iqt);
|
||||
manifoldContacts[numContacts++].mFaceIndex = triangleIndex;
|
||||
}
|
||||
}
|
||||
|
||||
static PX_FORCE_INLINE Vec4V pcmDistanceSegmentSegmentSquared4(const Vec3VArg p, const Vec3VArg d0,
|
||||
const Vec3VArg p02, const Vec3VArg d02,
|
||||
const Vec3VArg p12, const Vec3VArg d12,
|
||||
const Vec3VArg p22, const Vec3VArg d22,
|
||||
const Vec3VArg p32, const Vec3VArg d32,
|
||||
Vec4V& s, Vec4V& t)
|
||||
{
|
||||
const Vec4V zero = V4Zero();
|
||||
const Vec4V one = V4One();
|
||||
const Vec4V eps = V4Eps();
|
||||
const Vec4V half = V4Splat(FHalf());
|
||||
|
||||
const Vec4V d0X = V4Splat(V3GetX(d0));
|
||||
const Vec4V d0Y = V4Splat(V3GetY(d0));
|
||||
const Vec4V d0Z = V4Splat(V3GetZ(d0));
|
||||
const Vec4V pX = V4Splat(V3GetX(p));
|
||||
const Vec4V pY = V4Splat(V3GetY(p));
|
||||
const Vec4V pZ = V4Splat(V3GetZ(p));
|
||||
|
||||
Vec4V d024 = Vec4V_From_Vec3V(d02);
|
||||
Vec4V d124 = Vec4V_From_Vec3V(d12);
|
||||
Vec4V d224 = Vec4V_From_Vec3V(d22);
|
||||
Vec4V d324 = Vec4V_From_Vec3V(d32);
|
||||
|
||||
Vec4V p024 = Vec4V_From_Vec3V(p02);
|
||||
Vec4V p124 = Vec4V_From_Vec3V(p12);
|
||||
Vec4V p224 = Vec4V_From_Vec3V(p22);
|
||||
Vec4V p324 = Vec4V_From_Vec3V(p32);
|
||||
|
||||
Vec4V d0123X, d0123Y, d0123Z;
|
||||
Vec4V p0123X, p0123Y, p0123Z;
|
||||
|
||||
PX_TRANSPOSE_44_34(d024, d124, d224, d324, d0123X, d0123Y, d0123Z);
|
||||
PX_TRANSPOSE_44_34(p024, p124, p224, p324, p0123X, p0123Y, p0123Z);
|
||||
|
||||
const Vec4V rX = V4Sub(pX, p0123X);
|
||||
const Vec4V rY = V4Sub(pY, p0123Y);
|
||||
const Vec4V rZ = V4Sub(pZ, p0123Z);
|
||||
|
||||
//TODO - store this in a transposed state and avoid so many dot products?
|
||||
|
||||
const FloatV dd = V3Dot(d0, d0);
|
||||
|
||||
const Vec4V e = V4MulAdd(d0123Z, d0123Z, V4MulAdd(d0123X, d0123X, V4Mul(d0123Y, d0123Y)));
|
||||
const Vec4V b = V4MulAdd(d0Z, d0123Z, V4MulAdd(d0X, d0123X, V4Mul(d0Y, d0123Y)));
|
||||
const Vec4V c = V4MulAdd(d0Z, rZ, V4MulAdd(d0X, rX, V4Mul(d0Y, rY)));
|
||||
const Vec4V f = V4MulAdd(d0123Z, rZ, V4MulAdd(d0123X, rX, V4Mul(d0123Y, rY)));
|
||||
|
||||
const Vec4V a(V4Splat(dd));
|
||||
|
||||
const Vec4V aRecip(V4Recip(a));
|
||||
const Vec4V eRecip(V4Recip(e));
|
||||
|
||||
//if segments not parallell, compute closest point on two segments and clamp to segment1
|
||||
const Vec4V denom = V4Sub(V4Mul(a, e), V4Mul(b, b));
|
||||
const Vec4V temp = V4Sub(V4Mul(b, f), V4Mul(c, e));
|
||||
//const Vec4V s0 = V4Clamp(V4Div(temp, denom), zero, one);
|
||||
//In PS3, 0(temp)/0(denom) will produce QNaN and V4Clamp can't clamp the value to zero and one. In PC, 0/0 will produce inf and V4Clamp clamp the value to be one.
|
||||
//Therefore, we need to add the select code to protect against this case
|
||||
const Vec4V value = V4Sel(V4IsEq(denom, zero), one, V4Div(temp, denom));
|
||||
const Vec4V s0 = V4Clamp(value, zero, one);
|
||||
|
||||
//test whether segments are parallel
|
||||
const BoolV con2 = V4IsGrtrOrEq(eps, denom);
|
||||
const Vec4V sTmp = V4Sel(con2, half, s0);
|
||||
|
||||
//compute point on segment2 closest to segment1
|
||||
//const Vec4V tTmp = V4Mul(V4Add(V4Mul(b, sTmp), f), eRecip);
|
||||
const Vec4V tTmp = V4Sel(V4IsEq(e, zero), one, V4Mul(V4Add(V4Mul(b, sTmp), f), eRecip));
|
||||
|
||||
//if t is in [zero, one], done. otherwise clamp t
|
||||
const Vec4V t2 = V4Clamp(tTmp, zero, one);
|
||||
|
||||
//recompute s for the new value
|
||||
//const Vec4V comp = V4Mul(V4Sub(V4Mul(b,t2), c), aRecip);
|
||||
const Vec4V comp = V4Sel(V4IsEq(a, zero), one, V4Mul(V4Sub(V4Mul(b,t2), c), aRecip));
|
||||
const Vec4V s2 = V4Clamp(comp, zero, one);
|
||||
|
||||
s = s2;
|
||||
t = t2;
|
||||
|
||||
const Vec4V closest1X = V4MulAdd(d0X, s2, pX);
|
||||
const Vec4V closest1Y = V4MulAdd(d0Y, s2, pY);
|
||||
const Vec4V closest1Z = V4MulAdd(d0Z, s2, pZ);
|
||||
|
||||
const Vec4V closest2X = V4MulAdd(d0123X, t2, p0123X);
|
||||
const Vec4V closest2Y = V4MulAdd(d0123Y, t2, p0123Y);
|
||||
const Vec4V closest2Z = V4MulAdd(d0123Z, t2, p0123Z);
|
||||
|
||||
const Vec4V vvX = V4Sub(closest1X, closest2X);
|
||||
const Vec4V vvY = V4Sub(closest1Y, closest2Y);
|
||||
const Vec4V vvZ = V4Sub(closest1Z, closest2Z);
|
||||
|
||||
const Vec4V vd = V4MulAdd(vvX, vvX, V4MulAdd(vvY, vvY, V4Mul(vvZ, vvZ)));
|
||||
|
||||
return vd;
|
||||
}
|
||||
|
||||
// t is the barycenteric coordinate of a segment
|
||||
// u is the barycenteric coordinate of a triangle
|
||||
// v is the barycenteric coordinate of a triangle
|
||||
static FloatV pcmDistanceSegmentTriangleSquared(const Vec3VArg p, const Vec3VArg q,
|
||||
const Vec3VArg a, const Vec3VArg b, const Vec3VArg c,
|
||||
FloatV& t, FloatV& u, FloatV& v)
|
||||
{
|
||||
const FloatV one = FOne();
|
||||
const FloatV zero = FZero();
|
||||
|
||||
const Vec3V pq = V3Sub(q, p);
|
||||
const Vec3V ab = V3Sub(b, a);
|
||||
const Vec3V ac = V3Sub(c, a);
|
||||
const Vec3V bc = V3Sub(c, b);
|
||||
const Vec3V ap = V3Sub(p, a);
|
||||
const Vec3V aq = V3Sub(q, a);
|
||||
|
||||
const Vec3V n = V3Normalize(V3Cross(ab, ac)); // normalize vector
|
||||
|
||||
const Vec4V combinedDot = V3Dot4(ab, ab, ab, ac, ac, ac, ap, n);
|
||||
const FloatV d00 = V4GetX(combinedDot);
|
||||
const FloatV d01 = V4GetY(combinedDot);
|
||||
const FloatV d11 = V4GetZ(combinedDot);
|
||||
const FloatV dist3 = V4GetW(combinedDot);
|
||||
|
||||
// PT: the new version is copied from Gu::distanceSegmentTriangleSquared
|
||||
const FloatV tDenom = FSub(FMul(d00, d11), FMul(d01, d01));
|
||||
const FloatV bdenom = FSel(FIsGrtr(tDenom, zero), FRecip(tDenom), zero);
|
||||
|
||||
const FloatV sqDist3 = FMul(dist3, dist3);
|
||||
|
||||
//compute the closest point of q and triangle plane abc
|
||||
const FloatV dist4 = V3Dot(aq, n);
|
||||
const FloatV sqDist4 = FMul(dist4, dist4);
|
||||
const FloatV dMul = FMul(dist3, dist4);
|
||||
const BoolV con = FIsGrtr(zero, dMul);
|
||||
|
||||
if(BAllEqTTTT(con))
|
||||
{
|
||||
//compute the intersect point
|
||||
const FloatV nom = FNeg(V3Dot(n, ap));
|
||||
const FloatV denom = FRecip(V3Dot(n, pq));
|
||||
const FloatV t0 = FMul(nom, denom);
|
||||
const Vec3V ip = V3ScaleAdd(pq, t0, p);//V3Add(p, V3Scale(pq, t));
|
||||
const Vec3V v2 = V3Sub(ip, a);
|
||||
const FloatV d20 = V3Dot(v2, ab);
|
||||
const FloatV d21 = V3Dot(v2, ac);
|
||||
|
||||
const FloatV v0 = FMul(FNegScaleSub(d01, d21, FMul(d11, d20)), bdenom);
|
||||
const FloatV w0 = FMul(FNegScaleSub(d01, d20, FMul(d00, d21)), bdenom);
|
||||
|
||||
const BoolV con0 = isValidTriangleBarycentricCoord(v0, w0);
|
||||
if(BAllEqTTTT(con0))
|
||||
{
|
||||
t = t0;
|
||||
u = v0;
|
||||
v = w0;
|
||||
return zero;
|
||||
}
|
||||
}
|
||||
|
||||
const Vec3V closestP31 = V3NegScaleSub(n, dist3, p);//V3Sub(p, V3Scale(n, dist3));
|
||||
const Vec3V closestP41 = V3NegScaleSub(n, dist4, q);// V3Sub(q, V3Scale(n, dist4));
|
||||
|
||||
//Compute the barycentric coordinate for project point of q
|
||||
const Vec3V pV20 = V3Sub(closestP31, a);
|
||||
const Vec3V qV20 = V3Sub(closestP41, a);
|
||||
|
||||
const Vec4V pD2 = V3Dot4(pV20, ab, pV20, ac, qV20, ab, qV20, ac);
|
||||
|
||||
const Vec4V pD2Swizzle = V4PermYXWZ(pD2);
|
||||
|
||||
const Vec4V d11d00 = V4UnpackXY(V4Splat(d11), V4Splat(d00));
|
||||
|
||||
const Vec4V v0w0v1w1 = V4Scale(V4NegMulSub(V4Splat(d01), pD2Swizzle, V4Mul(d11d00, pD2)), bdenom);
|
||||
|
||||
const FloatV v0 = V4GetX(v0w0v1w1);
|
||||
const FloatV w0 = V4GetY(v0w0v1w1);
|
||||
const FloatV v1 = V4GetZ(v0w0v1w1);
|
||||
const FloatV w1 = V4GetW(v0w0v1w1);
|
||||
|
||||
const BoolV _con = isValidTriangleBarycentricCoord2(v0w0v1w1);
|
||||
|
||||
const BoolV con0 = BGetX(_con);
|
||||
const BoolV con1 = BGetY(_con);
|
||||
|
||||
const BoolV cond2 = BAnd(con0, con1);
|
||||
|
||||
if(BAllEqTTTT(cond2))
|
||||
{
|
||||
// both p and q project points are interior point
|
||||
const BoolV d2 = FIsGrtr(sqDist4, sqDist3);
|
||||
t = FSel(d2, zero, one);
|
||||
u = FSel(d2, v0, v1);
|
||||
v = FSel(d2, w0, w1);
|
||||
return FSel(d2, sqDist3, sqDist4);
|
||||
}
|
||||
else
|
||||
{
|
||||
Vec4V t40, t41;
|
||||
const Vec4V sqDist44 = pcmDistanceSegmentSegmentSquared4(p,pq,a,ab, b,bc, a,ac, a,ab, t40, t41);
|
||||
|
||||
const FloatV t00 = V4GetX(t40);
|
||||
const FloatV t10 = V4GetY(t40);
|
||||
const FloatV t20 = V4GetZ(t40);
|
||||
|
||||
const FloatV t01 = V4GetX(t41);
|
||||
const FloatV t11 = V4GetY(t41);
|
||||
const FloatV t21 = V4GetZ(t41);
|
||||
|
||||
//edge ab
|
||||
const FloatV u01 = t01;
|
||||
const FloatV v01 = zero;
|
||||
|
||||
//edge bc
|
||||
const FloatV u11 = FSub(one, t11);
|
||||
const FloatV v11 = t11;
|
||||
|
||||
//edge ac
|
||||
const FloatV u21 = zero;
|
||||
const FloatV v21 = t21;
|
||||
|
||||
const FloatV sqDist0(V4GetX(sqDist44));
|
||||
const FloatV sqDist1(V4GetY(sqDist44));
|
||||
const FloatV sqDist2(V4GetZ(sqDist44));
|
||||
|
||||
const BoolV con2 = BAnd(FIsGrtr(sqDist1, sqDist0), FIsGrtr(sqDist2, sqDist0));
|
||||
const BoolV con3 = FIsGrtr(sqDist2, sqDist1);
|
||||
const FloatV sqDistPE = FSel(con2, sqDist0, FSel(con3, sqDist1, sqDist2));
|
||||
const FloatV uEdge = FSel(con2, u01, FSel(con3, u11, u21));
|
||||
const FloatV vEdge = FSel(con2, v01, FSel(con3, v11, v21));
|
||||
const FloatV tSeg = FSel(con2, t00, FSel(con3, t10, t20));
|
||||
|
||||
if(BAllEqTTTT(con0))
|
||||
{
|
||||
//p's project point is an interior point
|
||||
const BoolV d2 = FIsGrtr(sqDistPE, sqDist3);
|
||||
t = FSel(d2, zero, tSeg);
|
||||
u = FSel(d2, v0, uEdge);
|
||||
v = FSel(d2, w0, vEdge);
|
||||
return FSel(d2, sqDist3, sqDistPE);
|
||||
}
|
||||
else if(BAllEqTTTT(con1))
|
||||
{
|
||||
//q's project point is an interior point
|
||||
const BoolV d2 = FIsGrtr(sqDistPE, sqDist4);
|
||||
t = FSel(d2, one, tSeg);
|
||||
u = FSel(d2, v1, uEdge);
|
||||
v = FSel(d2, w1, vEdge);
|
||||
return FSel(d2, sqDist4, sqDistPE);
|
||||
}
|
||||
else
|
||||
{
|
||||
t = tSeg;
|
||||
u = uEdge;
|
||||
v = vEdge;
|
||||
return sqDistPE;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static bool selectNormal(const FloatVArg u, FloatVArg v, PxU8 data)
|
||||
{
|
||||
const FloatV zero = FLoad(1e-6f);
|
||||
const FloatV one = FLoad(0.999999f);
|
||||
// Analysis
|
||||
if(FAllGrtr(zero, u))
|
||||
{
|
||||
if(FAllGrtr(zero, v))
|
||||
{
|
||||
// Vertex 0
|
||||
if(!(data & (ETD_CONVEX_EDGE_01|ETD_CONVEX_EDGE_20)))
|
||||
return true;
|
||||
}
|
||||
else if(FAllGrtr(v, one))
|
||||
{
|
||||
// Vertex 2
|
||||
if(!(data & (ETD_CONVEX_EDGE_12|ETD_CONVEX_EDGE_20)))
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Edge 0-2
|
||||
if(!(data & ETD_CONVEX_EDGE_20))
|
||||
return true;
|
||||
}
|
||||
}
|
||||
else if(FAllGrtr(u,one))
|
||||
{
|
||||
if(FAllGrtr(zero, v))
|
||||
{
|
||||
// Vertex 1
|
||||
if(!(data & (ETD_CONVEX_EDGE_01|ETD_CONVEX_EDGE_12)))
|
||||
return true;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if(FAllGrtr(zero, v))
|
||||
{
|
||||
// Edge 0-1
|
||||
if(!(data & ETD_CONVEX_EDGE_01))
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
const FloatV threshold = FLoad(0.9999f);
|
||||
const FloatV temp = FAdd(u, v);
|
||||
if(FAllGrtrOrEq(temp, threshold))
|
||||
{
|
||||
// Edge 1-2
|
||||
if(!(data & ETD_CONVEX_EDGE_12))
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Face
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool Gu::PCMCapsuleVsMeshContactGeneration::processTriangle(const PxVec3* verts, PxU32 triangleIndex, PxU8 triFlags, const PxU32* vertInds)
|
||||
{
|
||||
PX_UNUSED(vertInds);
|
||||
|
||||
const FloatV zero = FZero();
|
||||
|
||||
const Vec3V p0 = V3LoadU(verts[0]);
|
||||
const Vec3V p1 = V3LoadU(verts[1]);
|
||||
const Vec3V p2 = V3LoadU(verts[2]);
|
||||
|
||||
const Vec3V p10 = V3Sub(p1, p0);
|
||||
const Vec3V p20 = V3Sub(p2, p0);
|
||||
|
||||
const Vec3V n = V3Normalize(V3Cross(p10, p20));//(p1 - p0).cross(p2 - p0).getNormalized();
|
||||
const FloatV d = V3Dot(p0, n);//d = -p0.dot(n);
|
||||
|
||||
const FloatV dist = FSub(V3Dot(mCapsule.getCenter(), n), d);//p.dot(n) + d;
|
||||
|
||||
// Backface culling
|
||||
if(FAllGrtr(zero, dist))
|
||||
return false;
|
||||
|
||||
FloatV t, u, v;
|
||||
const FloatV sqDist = pcmDistanceSegmentTriangleSquared(mCapsule.p0, mCapsule.p1, p0, p1, p2, t, u, v);
|
||||
|
||||
if(FAllGrtr(mSqInflatedRadius, sqDist))
|
||||
{
|
||||
Vec3V patchNormalInTriangle;
|
||||
if(selectNormal(u, v, triFlags))
|
||||
{
|
||||
patchNormalInTriangle = n;
|
||||
}
|
||||
else
|
||||
{
|
||||
if(FAllEq(sqDist, zero))
|
||||
{
|
||||
//segment intersect with the triangle
|
||||
patchNormalInTriangle = n;
|
||||
}
|
||||
else
|
||||
{
|
||||
const Vec3V pq = V3Sub(mCapsule.p1, mCapsule.p0);
|
||||
const Vec3V pointOnSegment = V3ScaleAdd(pq, t, mCapsule.p0);
|
||||
const FloatV w = FSub(FOne(), FAdd(u, v));
|
||||
const Vec3V pointOnTriangle = V3ScaleAdd(p0, w, V3ScaleAdd(p1, u, V3Scale(p2, v)));
|
||||
patchNormalInTriangle = V3Normalize(V3Sub(pointOnSegment, pointOnTriangle));
|
||||
}
|
||||
}
|
||||
|
||||
const PxU32 previousNumContacts = mNumContacts;
|
||||
|
||||
generateContacts(p0, p1, p2, n, patchNormalInTriangle, triangleIndex, mCapsule.p0, mCapsule.p1, mInflatedRadius, mManifoldContacts, mNumContacts);
|
||||
|
||||
//ML: this need to use the sqInflatedRadius to avoid some bad contacts
|
||||
generateEEContacts(p0, p1, p2, patchNormalInTriangle, triangleIndex, mCapsule.p0, mCapsule.p1, mSqInflatedRadius, previousNumContacts, mManifoldContacts, mNumContacts);
|
||||
|
||||
PxU32 numContacts = mNumContacts - previousNumContacts;
|
||||
|
||||
PX_ASSERT(numContacts <= 2);
|
||||
|
||||
if(numContacts > 0)
|
||||
{
|
||||
FloatV maxPen = FMax();
|
||||
for(PxU32 i = previousNumContacts; i<mNumContacts; ++i)
|
||||
{
|
||||
const FloatV pen = V4GetW(mManifoldContacts[i].mLocalNormalPen);
|
||||
mManifoldContacts[i].mLocalPointA = mMeshToConvex.transform(mManifoldContacts[i].mLocalPointA);
|
||||
maxPen = FMin(maxPen, pen);
|
||||
}
|
||||
|
||||
for(PxU32 i = previousNumContacts; i<mNumContacts; ++i)
|
||||
{
|
||||
Vec3V contact0 = mManifoldContacts[i].mLocalPointB;
|
||||
for(PxU32 j=i+1; j<mNumContacts; ++j)
|
||||
{
|
||||
Vec3V contact1 = mManifoldContacts[j].mLocalPointB;
|
||||
Vec3V dif = V3Sub(contact1, contact0);
|
||||
FloatV d1 = V3Dot(dif, dif);
|
||||
if(FAllGrtr(mSqReplaceBreakingThreshold, d1))
|
||||
{
|
||||
mManifoldContacts[j] = mManifoldContacts[mNumContacts-1];
|
||||
mNumContacts--;
|
||||
j--;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
PX_ASSERT(mNumContactPatch <PCM_MAX_CONTACTPATCH_SIZE);
|
||||
|
||||
addManifoldPointToPatch(patchNormalInTriangle, maxPen, previousNumContacts);
|
||||
|
||||
PX_ASSERT(mNumContactPatch <PCM_MAX_CONTACTPATCH_SIZE);
|
||||
if(mNumContacts >= 16)
|
||||
{
|
||||
PX_ASSERT(mNumContacts <= 64);
|
||||
processContacts(GU_CAPSULE_MANIFOLD_CACHE_SIZE);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Gu::PCMCapsuleVsMeshContactGeneration::processTriangle(const TriangleV& triangleV, PxU32 triangleIndex, const CapsuleV& capsule, const FloatVArg inflatedRadius, const PxU8 trigFlag,
|
||||
MeshPersistentContact* manifoldContacts, PxU32& numContacts)
|
||||
{
|
||||
const FloatV zero = FZero();
|
||||
|
||||
const Vec3V p0 = triangleV.verts[0];
|
||||
const Vec3V p1 = triangleV.verts[1];
|
||||
const Vec3V p2 = triangleV.verts[2];
|
||||
|
||||
const Vec3V n = triangleV.normal();
|
||||
|
||||
const FloatV sqInflatedRadius = FMul(inflatedRadius, inflatedRadius);
|
||||
|
||||
FloatV t, u, v;
|
||||
const FloatV sqDist = pcmDistanceSegmentTriangleSquared(capsule.p0, capsule.p1, p0, p1, p2, t, u, v);
|
||||
|
||||
if(FAllGrtr(sqInflatedRadius, sqDist))
|
||||
{
|
||||
Vec3V patchNormalInTriangle;
|
||||
if(selectNormal(u, v, trigFlag))
|
||||
{
|
||||
patchNormalInTriangle = n;
|
||||
}
|
||||
else
|
||||
{
|
||||
if(FAllEq(sqDist, zero))
|
||||
{
|
||||
//segment intersect with the triangle
|
||||
patchNormalInTriangle = n;
|
||||
}
|
||||
else
|
||||
{
|
||||
const Vec3V pq = V3Sub(capsule.p1, capsule.p0);
|
||||
const Vec3V pointOnSegment = V3ScaleAdd(pq, t, capsule.p0);
|
||||
const FloatV w = FSub(FOne(), FAdd(u, v));
|
||||
const Vec3V pointOnTriangle = V3ScaleAdd(p0, w, V3ScaleAdd(p1, u, V3Scale(p2, v)));
|
||||
patchNormalInTriangle = V3Normalize(V3Sub(pointOnSegment, pointOnTriangle));
|
||||
}
|
||||
}
|
||||
|
||||
generateContacts(p0, p1, p2, n, patchNormalInTriangle, triangleIndex, capsule.p0, capsule.p1, inflatedRadius, manifoldContacts, numContacts);
|
||||
|
||||
generateEEContactsMTD(p0, p1, p2, patchNormalInTriangle, triangleIndex, capsule.p0, capsule.p1, inflatedRadius, manifoldContacts, numContacts);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
// PT: below is just for internal testing
|
||||
PX_PHYSX_COMMON_API FloatV pcmDistanceSegmentTriangleSquaredExported( const Vec3VArg p, const Vec3VArg q,
|
||||
const Vec3VArg a, const Vec3VArg b, const Vec3VArg c,
|
||||
FloatV& t, FloatV& u, FloatV& v)
|
||||
{
|
||||
return pcmDistanceSegmentTriangleSquared(p, q, a, b, c, t, u, v);
|
||||
}
|
||||
276
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactConvexCommon.cpp
vendored
Normal file
276
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactConvexCommon.cpp
vendored
Normal file
@@ -0,0 +1,276 @@
|
||||
// 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 "GuVecTriangle.h"
|
||||
#include "GuPCMContactConvexCommon.h"
|
||||
#include "GuConvexEdgeFlags.h"
|
||||
#include "GuBarycentricCoordinates.h"
|
||||
|
||||
using namespace physx;
|
||||
using namespace Gu;
|
||||
using namespace aos;
|
||||
|
||||
// This function adds the newly created manifold contacts to a new patch or existing patches
|
||||
void PCMConvexVsMeshContactGeneration::addContactsToPatch(const Vec3VArg patchNormal, PxU32 previousNumContacts)
|
||||
{
|
||||
const Vec3V patchNormalInTriangle = mMeshToConvex.rotateInv(patchNormal);
|
||||
|
||||
const PxU32 newContacts = mNumContacts - previousNumContacts;
|
||||
|
||||
if(newContacts > GU_SINGLE_MANIFOLD_SINGLE_POLYGONE_CACHE_SIZE)
|
||||
{
|
||||
//if the current created manifold contacts are more than GU_SINGLE_MANIFOLD_SINGLE_POLYGONE_CACHE_SIZE(4) points, we will reduce the total numContacts
|
||||
//to GU_SINGLE_MANIFOLD_SINGLE_POLYGONE_CACHE_SIZE. However, after we add these points into a patch, the patch contacts will be variable. Then we will
|
||||
//do contact reduction for that patch in the processContacts. After the contact reduction, there will be no more than GU_SINGLE_MANIFOLD_CACHE_SIZE(6)
|
||||
//contacts inside a signlePersistentContactManifold
|
||||
SinglePersistentContactManifold::reduceContacts(&mManifoldContacts[previousNumContacts], newContacts);
|
||||
mNumContacts = previousNumContacts + GU_SINGLE_MANIFOLD_SINGLE_POLYGONE_CACHE_SIZE;
|
||||
}
|
||||
|
||||
//get rid of duplicate manifold contacts for the newly created contacts
|
||||
for(PxU32 i = previousNumContacts; i<mNumContacts; ++i)
|
||||
{
|
||||
for(PxU32 j=i+1; j<mNumContacts; ++j)
|
||||
{
|
||||
Vec3V dif = V3Sub(mManifoldContacts[j].mLocalPointB, mManifoldContacts[i].mLocalPointB);
|
||||
FloatV d = V3Dot(dif, dif);
|
||||
if(FAllGrtr(mSqReplaceBreakingThreshold, d))
|
||||
{
|
||||
mManifoldContacts[j] = mManifoldContacts[mNumContacts-1];
|
||||
mNumContacts--;
|
||||
j--;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//calculate the maxPen and transform the patch normal and localPointB into mesh's local space
|
||||
FloatV maxPen = FMax();
|
||||
for(PxU32 i = previousNumContacts; i<mNumContacts; ++i)
|
||||
{
|
||||
const FloatV pen = V4GetW(mManifoldContacts[i].mLocalNormalPen);
|
||||
mManifoldContacts[i].mLocalNormalPen = V4SetW(patchNormalInTriangle, pen);
|
||||
mManifoldContacts[i].mLocalPointB = mMeshToConvex.transformInv(mManifoldContacts[i].mLocalPointB);
|
||||
maxPen = FMin(maxPen, pen);
|
||||
}
|
||||
|
||||
//Based on the patch normal and add the newly avaiable manifold points to the corresponding patch
|
||||
addManifoldPointToPatch(patchNormalInTriangle, maxPen, previousNumContacts);
|
||||
|
||||
PX_ASSERT(mNumContactPatch <PCM_MAX_CONTACTPATCH_SIZE);
|
||||
if(mNumContacts >= GU_MESH_CONTACT_REDUCTION_THRESHOLD)
|
||||
{
|
||||
PX_ASSERT(mNumContacts <= PxContactBuffer::MAX_CONTACTS);
|
||||
processContacts(GU_SINGLE_MANIFOLD_CACHE_SIZE);
|
||||
}
|
||||
}
|
||||
|
||||
void PCMConvexVsMeshContactGeneration::generateLastContacts()
|
||||
{
|
||||
// Process delayed contacts
|
||||
PxU32 nbEntries = mDeferredContacts->size();
|
||||
|
||||
if(nbEntries)
|
||||
{
|
||||
nbEntries /= sizeof(PCMDeferredPolyData)/sizeof(PxU32);
|
||||
|
||||
const PCMDeferredPolyData* PX_RESTRICT cd = reinterpret_cast<const PCMDeferredPolyData*>(mDeferredContacts->begin());
|
||||
for(PxU32 i=0;i<nbEntries;i++)
|
||||
{
|
||||
const PCMDeferredPolyData& currentContact = cd[i];
|
||||
|
||||
const PxU32 ref0 = currentContact.mInds[0];
|
||||
const PxU32 ref1 = currentContact.mInds[1];
|
||||
const PxU32 ref2 = currentContact.mInds[2];
|
||||
|
||||
const PxU8 triFlags = PxU8(currentContact.triFlags32);
|
||||
|
||||
const bool needsProcessing = (((triFlags & ETD_CONVEX_EDGE_01) != 0 || mEdgeCache.get(CachedEdge(ref0, ref1)) == NULL)) &&
|
||||
(((triFlags & ETD_CONVEX_EDGE_12) != 0 || mEdgeCache.get(CachedEdge(ref1, ref2)) == NULL)) &&
|
||||
(((triFlags & ETD_CONVEX_EDGE_20) != 0 || mEdgeCache.get(CachedEdge(ref2, ref0)) == NULL));
|
||||
|
||||
if(needsProcessing)
|
||||
{
|
||||
const TriangleV localTriangle(currentContact.mVerts);
|
||||
Vec3V patchNormal;
|
||||
const PxU32 previousNumContacts = mNumContacts;
|
||||
//the localTriangle is in the convex space
|
||||
//Generate contacts - we didn't generate contacts with any neighbours
|
||||
generatePolyDataContactManifold(localTriangle, currentContact.mFeatureIndex, currentContact.mTriangleIndex, triFlags, mManifoldContacts, mNumContacts, mContactDist, patchNormal);
|
||||
|
||||
FloatV v, w;
|
||||
const FloatV upperBound = FLoad(0.97f);
|
||||
const FloatV lowerBound = FSub(FOne(), upperBound);
|
||||
PxU32 currentContacts = mNumContacts;
|
||||
for(PxU32 j=currentContacts; j>previousNumContacts; --j)
|
||||
{
|
||||
PxU32 ind = j-1;
|
||||
//calculate the barycentric coordinate of the contacts in localTriangle, p = a + v(b-a) + w(c-a)., p=ua+vb+wc
|
||||
barycentricCoordinates(mManifoldContacts[ind].mLocalPointB, localTriangle.verts[0], localTriangle.verts[1], localTriangle.verts[2], v, w);
|
||||
//const FloatV u = FSub(one, FAdd(v, w));
|
||||
|
||||
bool keepContact = true;
|
||||
if(FAllGrtr(v, upperBound))//v > upperBound
|
||||
{
|
||||
//vertex1
|
||||
keepContact = !mVertexCache.contains(CachedVertex(ref1));
|
||||
}
|
||||
else if(FAllGrtr(w, upperBound))// w > upperBound
|
||||
{
|
||||
//vertex2
|
||||
keepContact = !mVertexCache.contains(CachedVertex(ref2));
|
||||
}
|
||||
else if(FAllGrtrOrEq(lowerBound, FAdd(v, w))) // u(1-(v+w)) > upperBound
|
||||
{
|
||||
//vertex0
|
||||
keepContact = !mVertexCache.contains(CachedVertex(ref0));
|
||||
}
|
||||
|
||||
if(!keepContact)
|
||||
{
|
||||
//ML: if feature code is any of the vertex in this triangle and we have generated contacts with any other triangles which contains this vertex, we should drop it
|
||||
currentContacts--;
|
||||
|
||||
for(PxU32 k = ind; k < currentContacts; ++k)
|
||||
{
|
||||
mManifoldContacts[k] = mManifoldContacts[k+1];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
mNumContacts = currentContacts;
|
||||
|
||||
if(currentContacts > previousNumContacts)
|
||||
{
|
||||
addContactsToPatch(patchNormal, previousNumContacts);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool PCMConvexVsMeshContactGeneration::processTriangle(const PxVec3* verts, PxU32 triangleIndex, PxU8 triFlags, const PxU32* vertInds)
|
||||
{
|
||||
const Mat33V identity = M33Identity();
|
||||
const FloatV zero = FZero();
|
||||
|
||||
const Vec3V v0 = V3LoadU(verts[0]);
|
||||
const Vec3V v1 = V3LoadU(verts[1]);
|
||||
const Vec3V v2 = V3LoadU(verts[2]);
|
||||
|
||||
const Vec3V v10 = V3Sub(v1, v0);
|
||||
const Vec3V v20 = V3Sub(v2, v0);
|
||||
|
||||
const Vec3V n = V3Normalize(V3Cross(v10, v20));//(p1 - p0).cross(p2 - p0).getNormalized();
|
||||
const FloatV d = V3Dot(v0, n);//d = -p0.dot(n);
|
||||
|
||||
const FloatV dist = FSub(V3Dot(mHullCenterMesh, n), d);//p.dot(n) + d;
|
||||
|
||||
// Backface culling
|
||||
if(FAllGrtr(zero, dist))
|
||||
return false;
|
||||
|
||||
//tranform verts into the box local space
|
||||
const Vec3V locV0 = mMeshToConvex.transform(v0);
|
||||
const Vec3V locV1 = mMeshToConvex.transform(v1);
|
||||
const Vec3V locV2 = mMeshToConvex.transform(v2);
|
||||
|
||||
const TriangleV localTriangle(locV0, locV1, locV2);
|
||||
|
||||
{
|
||||
SupportLocalImpl<TriangleV> localTriMap(localTriangle, mConvexTransform, identity, identity, true);
|
||||
|
||||
const PxU32 previousNumContacts = mNumContacts;
|
||||
Vec3V patchNormal;
|
||||
|
||||
generateTriangleFullContactManifold(localTriangle, triangleIndex, vertInds, triFlags, mPolyData, &localTriMap, mPolyMap, mManifoldContacts, mNumContacts, mContactDist, patchNormal);
|
||||
|
||||
if(mNumContacts > previousNumContacts)
|
||||
{
|
||||
#if PCM_LOW_LEVEL_DEBUG
|
||||
PersistentContactManifold::drawTriangle(*mRenderOutput, mMeshTransform.transform(v0), mMeshTransform.transform(v1), mMeshTransform.transform(v2), 0x00ff00);
|
||||
#endif
|
||||
const bool inActiveEdge0 = (triFlags & ETD_CONVEX_EDGE_01) == 0;
|
||||
const bool inActiveEdge1 = (triFlags & ETD_CONVEX_EDGE_12) == 0;
|
||||
const bool inActiveEdge2 = (triFlags & ETD_CONVEX_EDGE_20) == 0;
|
||||
|
||||
if(inActiveEdge0)
|
||||
mEdgeCache.addData(CachedEdge(vertInds[0], vertInds[1]));
|
||||
if(inActiveEdge1)
|
||||
mEdgeCache.addData(CachedEdge(vertInds[1], vertInds[2]));
|
||||
if(inActiveEdge2)
|
||||
mEdgeCache.addData(CachedEdge(vertInds[2], vertInds[0]));
|
||||
|
||||
mVertexCache.addData(CachedVertex(vertInds[0]));
|
||||
mVertexCache.addData(CachedVertex(vertInds[1]));
|
||||
mVertexCache.addData(CachedVertex(vertInds[2]));
|
||||
|
||||
addContactsToPatch(patchNormal, previousNumContacts);
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool PCMConvexVsMeshContactGeneration::processTriangle(const PolygonalData& polyData, const SupportLocal* polyMap, const PxVec3* verts, PxU32 triangleIndex, PxU8 triFlags, const FloatVArg inflation, bool isDoubleSided,
|
||||
const PxTransformV& convexTransform, const PxMatTransformV& meshToConvex, MeshPersistentContact* manifoldContacts, PxU32& numContacts)
|
||||
{
|
||||
const Mat33V identity = M33Identity();
|
||||
const FloatV zero = FZero();
|
||||
|
||||
const Vec3V v0 = V3LoadU(verts[0]);
|
||||
const Vec3V v1 = V3LoadU(verts[1]);
|
||||
const Vec3V v2 = V3LoadU(verts[2]);
|
||||
|
||||
//tranform verts into the box local space
|
||||
const Vec3V locV0 = meshToConvex.transform(v0);
|
||||
const Vec3V locV1 = meshToConvex.transform(v1);
|
||||
const Vec3V locV2 = meshToConvex.transform(v2);
|
||||
|
||||
const Vec3V v10 = V3Sub(locV1, locV0);
|
||||
const Vec3V v20 = V3Sub(locV2, locV0);
|
||||
|
||||
const Vec3V n = V3Normalize(V3Cross(v10, v20));//(p1 - p0).cross(p2 - p0).getNormalized();
|
||||
const FloatV d = V3Dot(locV0, n);//d = -p0.dot(n);
|
||||
|
||||
const FloatV dist = FSub(V3Dot(polyMap->shapeSpaceCenterOfMass, n), d);//p.dot(n) + d;
|
||||
|
||||
// Backface culling
|
||||
const bool culled = !isDoubleSided && (FAllGrtr(zero, dist));
|
||||
if(culled)
|
||||
return false;
|
||||
|
||||
const TriangleV localTriangle(locV0, locV1, locV2);
|
||||
|
||||
SupportLocalImpl<TriangleV> localTriMap(localTriangle, convexTransform, identity, identity, true);
|
||||
|
||||
Vec3V patchNormal;
|
||||
|
||||
generateTriangleFullContactManifold(localTriangle, triangleIndex, triFlags, polyData, &localTriMap, polyMap, manifoldContacts, numContacts, inflation, patchNormal);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
425
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactConvexCommon.h
vendored
Normal file
425
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactConvexCommon.h
vendored
Normal file
@@ -0,0 +1,425 @@
|
||||
// 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_PCM_CONTACT_CONVEX_COMMON_H
|
||||
#define GU_PCM_CONTACT_CONVEX_COMMON_H
|
||||
|
||||
#define PCM_MAX_CONTACTPATCH_SIZE 32
|
||||
|
||||
#include "geomutils/PxContactBuffer.h"
|
||||
#include "GuVecCapsule.h"
|
||||
#include "GuPCMTriangleContactGen.h"
|
||||
#include "GuTriangleCache.h"
|
||||
#include "foundation/PxInlineArray.h"
|
||||
|
||||
namespace physx
|
||||
{
|
||||
namespace Gu
|
||||
{
|
||||
|
||||
#define MAX_CACHE_SIZE 128
|
||||
|
||||
//sizeof(PCMDeferredPolyData)/sizeof(PxU32) = 15, 960/15 = 64 triangles in the local array
|
||||
#define LOCAL_PCM_CONTACTS_SIZE 960
|
||||
|
||||
class PCMMeshContactGeneration
|
||||
{
|
||||
PX_NOCOPY(PCMMeshContactGeneration)
|
||||
public:
|
||||
PCMContactPatch mContactPatch[PCM_MAX_CONTACTPATCH_SIZE];
|
||||
PCMContactPatch* mContactPatchPtr[PCM_MAX_CONTACTPATCH_SIZE];
|
||||
const aos::FloatV mContactDist;
|
||||
const aos::FloatV mReplaceBreakingThreshold;
|
||||
const aos::PxTransformV& mConvexTransform;
|
||||
const aos::PxTransformV& mMeshTransform;
|
||||
Gu::MultiplePersistentContactManifold& mMultiManifold;
|
||||
PxContactBuffer& mContactBuffer;
|
||||
aos::FloatV mAcceptanceEpsilon;
|
||||
aos::FloatV mSqReplaceBreakingThreshold;
|
||||
aos::PxMatTransformV mMeshToConvex;
|
||||
Gu::MeshPersistentContact* mManifoldContacts;
|
||||
PxU32 mNumContacts;
|
||||
PxU32 mNumContactPatch;
|
||||
PxU32 mNumCalls;
|
||||
Gu::CacheMap<Gu::CachedEdge, MAX_CACHE_SIZE> mEdgeCache;
|
||||
Gu::CacheMap<Gu::CachedVertex, MAX_CACHE_SIZE> mVertexCache;
|
||||
PxInlineArray<PxU32, LOCAL_PCM_CONTACTS_SIZE>* mDeferredContacts;
|
||||
PxRenderOutput* mRenderOutput;
|
||||
|
||||
PCMMeshContactGeneration(
|
||||
const aos::FloatVArg contactDist,
|
||||
const aos::FloatVArg replaceBreakingThreshold,
|
||||
const aos::PxTransformV& convexTransform,
|
||||
const aos::PxTransformV& meshTransform,
|
||||
Gu::MultiplePersistentContactManifold& multiManifold,
|
||||
PxContactBuffer& contactBuffer,
|
||||
PxInlineArray<PxU32, LOCAL_PCM_CONTACTS_SIZE>* deferredContacts,
|
||||
PxRenderOutput* renderOutput
|
||||
) :
|
||||
mContactDist(contactDist),
|
||||
mReplaceBreakingThreshold(replaceBreakingThreshold),
|
||||
mConvexTransform(convexTransform),
|
||||
mMeshTransform(meshTransform),
|
||||
mMultiManifold(multiManifold),
|
||||
mContactBuffer(contactBuffer),
|
||||
mDeferredContacts(deferredContacts),
|
||||
mRenderOutput(renderOutput)
|
||||
{
|
||||
using namespace aos;
|
||||
mNumContactPatch = 0;
|
||||
mNumContacts = 0;
|
||||
mNumCalls = 0;
|
||||
|
||||
mMeshToConvex = mConvexTransform.transformInv(mMeshTransform);
|
||||
|
||||
//Assign the PCMContactPatch to the PCMContactPathPtr
|
||||
for(PxU32 i=0; i<PCM_MAX_CONTACTPATCH_SIZE; ++i)
|
||||
{
|
||||
mContactPatchPtr[i] = &mContactPatch[i];
|
||||
}
|
||||
mManifoldContacts = PX_CP_TO_MPCP(contactBuffer.contacts);
|
||||
|
||||
mSqReplaceBreakingThreshold = FMul(replaceBreakingThreshold, replaceBreakingThreshold);
|
||||
|
||||
mAcceptanceEpsilon = FLoad(0.996f);//5 degree
|
||||
//mAcceptanceEpsilon = FloatV_From_F32(0.9999);//5 degree
|
||||
}
|
||||
|
||||
template <PxU32 TriangleCount, typename Derived>
|
||||
bool processTriangleCache(Gu::TriangleCache<TriangleCount>& cache)
|
||||
{
|
||||
PxU32 count = cache.mNumTriangles;
|
||||
PxVec3* verts = cache.mVertices;
|
||||
PxU32* vertInds = cache.mIndices;
|
||||
PxU32* triInds = cache.mTriangleIndex;
|
||||
PxU8* edgeFlags = cache.mEdgeFlags;
|
||||
while(count--)
|
||||
{
|
||||
(static_cast<Derived*>(this))->processTriangle(verts, *triInds, *edgeFlags, vertInds);
|
||||
verts += 3;
|
||||
vertInds += 3;
|
||||
triInds++;
|
||||
edgeFlags++;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
void prioritizeContactPatches();
|
||||
void addManifoldPointToPatch(const aos::Vec3VArg currentPatchNormal, const aos::FloatVArg maxPen, PxU32 previousNumContacts);
|
||||
void processContacts(PxU8 maxContactPerManifold, const bool isNotLastPatch = true);
|
||||
};
|
||||
|
||||
// This function is based on the current patch normal to either create a new patch or merge the manifold contacts in this patch with the manifold contacts in the last existing
|
||||
// patch. This means there might be more than GU_SINGLE_MANIFOLD_CACHE_SIZE in a SinglePersistentContactManifold.
|
||||
PX_FORCE_INLINE void PCMMeshContactGeneration::addManifoldPointToPatch(const aos::Vec3VArg currentPatchNormal, const aos::FloatVArg maxPen, PxU32 previousNumContacts)
|
||||
{
|
||||
using namespace aos;
|
||||
|
||||
bool foundPatch = false;
|
||||
//we have existing patch
|
||||
if(mNumContactPatch > 0)
|
||||
{
|
||||
//if the direction between the last existing patch normal and the current patch normal are within acceptance epsilon, which means we will be
|
||||
//able to merge the last patch's contacts with the current patch's contacts. This is just to avoid to create an extra patch. We have some logic
|
||||
//later to refine the patch again
|
||||
if(FAllGrtr(V3Dot(mContactPatch[mNumContactPatch-1].mPatchNormal, currentPatchNormal), mAcceptanceEpsilon))
|
||||
{
|
||||
//get the last patch
|
||||
PCMContactPatch& patch = mContactPatch[mNumContactPatch-1];
|
||||
|
||||
//remove duplicate contacts
|
||||
for(PxU32 i = patch.mStartIndex; i<patch.mEndIndex; ++i)
|
||||
{
|
||||
for(PxU32 j = previousNumContacts; j<mNumContacts; ++j)
|
||||
{
|
||||
Vec3V dif = V3Sub(mManifoldContacts[j].mLocalPointB, mManifoldContacts[i].mLocalPointB);
|
||||
FloatV d = V3Dot(dif, dif);
|
||||
if(FAllGrtr(mSqReplaceBreakingThreshold, d))
|
||||
{
|
||||
if(FAllGrtr(V4GetW(mManifoldContacts[i].mLocalNormalPen), V4GetW(mManifoldContacts[j].mLocalNormalPen)))
|
||||
{
|
||||
//The new contact is deeper than the old contact so we keep the deeper contact
|
||||
mManifoldContacts[i] = mManifoldContacts[j];
|
||||
}
|
||||
mManifoldContacts[j] = mManifoldContacts[mNumContacts-1];
|
||||
mNumContacts--;
|
||||
j--;
|
||||
}
|
||||
}
|
||||
}
|
||||
patch.mEndIndex = mNumContacts;
|
||||
patch.mPatchMaxPen = FMin(patch.mPatchMaxPen, maxPen);
|
||||
foundPatch = true;
|
||||
}
|
||||
}
|
||||
|
||||
//If there are no existing patch which match the currentPatchNormal, we will create a new patch
|
||||
if(!foundPatch)
|
||||
{
|
||||
mContactPatch[mNumContactPatch].mStartIndex = previousNumContacts;
|
||||
mContactPatch[mNumContactPatch].mEndIndex = mNumContacts;
|
||||
mContactPatch[mNumContactPatch].mPatchMaxPen = maxPen;
|
||||
mContactPatch[mNumContactPatch++].mPatchNormal = currentPatchNormal;
|
||||
}
|
||||
}
|
||||
|
||||
// This function sort the contact patch based on the max penetration so that deepest penetration contact patch will be in front of the less penetration contact
|
||||
// patch
|
||||
PX_FORCE_INLINE void PCMMeshContactGeneration::prioritizeContactPatches()
|
||||
{
|
||||
//we are using insertion sort to prioritize contact patchs
|
||||
using namespace aos;
|
||||
//sort the contact patch based on the max penetration
|
||||
for(PxU32 i=1; i<mNumContactPatch; ++i)
|
||||
{
|
||||
const PxU32 indexi = i-1;
|
||||
if(FAllGrtr(mContactPatchPtr[indexi]->mPatchMaxPen, mContactPatchPtr[i]->mPatchMaxPen))
|
||||
{
|
||||
//swap
|
||||
PCMContactPatch* tmp = mContactPatchPtr[indexi];
|
||||
mContactPatchPtr[indexi] = mContactPatchPtr[i];
|
||||
mContactPatchPtr[i] = tmp;
|
||||
|
||||
for(PxI32 j=PxI32(i-2); j>=0; j--)
|
||||
{
|
||||
const PxU32 indexj = PxU32(j+1);
|
||||
if(FAllGrtrOrEq(mContactPatchPtr[indexj]->mPatchMaxPen, mContactPatchPtr[j]->mPatchMaxPen))
|
||||
break;
|
||||
//swap
|
||||
PCMContactPatch* temp = mContactPatchPtr[indexj];
|
||||
mContactPatchPtr[indexj] = mContactPatchPtr[j];
|
||||
mContactPatchPtr[j] = temp;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE void PCMMeshContactGeneration::processContacts(PxU8 maxContactPerManifold, bool isNotLastPatch)
|
||||
{
|
||||
using namespace aos;
|
||||
|
||||
if(mNumContacts != 0)
|
||||
{
|
||||
//reorder the contact patches based on the max penetration
|
||||
prioritizeContactPatches();
|
||||
//connect the patches which's angle between patch normals are within 5 degree
|
||||
mMultiManifold.refineContactPatchConnective(mContactPatchPtr, mNumContactPatch, mManifoldContacts, mAcceptanceEpsilon);
|
||||
//get rid of duplicate manifold contacts in connected contact patches
|
||||
mMultiManifold.reduceManifoldContactsInDifferentPatches(mContactPatchPtr, mNumContactPatch, mManifoldContacts, mNumContacts, mSqReplaceBreakingThreshold);
|
||||
//add the manifold contact to the corresponding manifold
|
||||
mMultiManifold.addManifoldContactPoints(mManifoldContacts, mNumContacts, mContactPatchPtr, mNumContactPatch, mSqReplaceBreakingThreshold, mAcceptanceEpsilon, maxContactPerManifold);
|
||||
|
||||
mNumContacts = 0;
|
||||
mNumContactPatch = 0;
|
||||
|
||||
if(isNotLastPatch)
|
||||
{
|
||||
//remap the contact patch pointer to contact patch
|
||||
for(PxU32 i=0; i<PCM_MAX_CONTACTPATCH_SIZE; ++i)
|
||||
{
|
||||
mContactPatchPtr[i] = &mContactPatch[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
struct PCMDeferredPolyData
|
||||
{
|
||||
public:
|
||||
PxVec3 mVerts[3]; //36
|
||||
PxU32 mInds[3]; //48
|
||||
PxU32 mTriangleIndex; //52
|
||||
PxU32 mFeatureIndex; //56
|
||||
PxU32 triFlags32; //60
|
||||
};
|
||||
|
||||
#if PX_VC
|
||||
#pragma warning(push)
|
||||
#pragma warning( disable : 4324 ) // Padding was added at the end of a structure because of a __declspec(align) value.
|
||||
#endif
|
||||
|
||||
class PCMConvexVsMeshContactGeneration : public PCMMeshContactGeneration
|
||||
{
|
||||
PCMConvexVsMeshContactGeneration &operator=(PCMConvexVsMeshContactGeneration &);
|
||||
|
||||
public:
|
||||
aos::Vec3V mHullCenterMesh;
|
||||
const Gu::PolygonalData& mPolyData;
|
||||
const SupportLocal* mPolyMap;
|
||||
const Cm::FastVertex2ShapeScaling& mConvexScaling;
|
||||
bool mIdtConvexScale;
|
||||
bool mSilhouetteEdgesAreActive;
|
||||
|
||||
PCMConvexVsMeshContactGeneration(
|
||||
const aos::FloatVArg contactDistance,
|
||||
const aos::FloatVArg replaceBreakingThreshold,
|
||||
const aos::PxTransformV& convexTransform,
|
||||
const aos::PxTransformV& meshTransform,
|
||||
Gu::MultiplePersistentContactManifold& multiManifold,
|
||||
PxContactBuffer& contactBuffer,
|
||||
const Gu::PolygonalData& polyData,
|
||||
const SupportLocal* polyMap,
|
||||
PxInlineArray<PxU32, LOCAL_PCM_CONTACTS_SIZE>* delayedContacts,
|
||||
const Cm::FastVertex2ShapeScaling& convexScaling,
|
||||
bool idtConvexScale,
|
||||
bool silhouetteEdgesAreActive,
|
||||
PxRenderOutput* renderOutput
|
||||
|
||||
) : PCMMeshContactGeneration(contactDistance, replaceBreakingThreshold, convexTransform, meshTransform, multiManifold, contactBuffer,
|
||||
delayedContacts, renderOutput),
|
||||
mPolyData(polyData),
|
||||
mPolyMap(polyMap),
|
||||
mConvexScaling(convexScaling),
|
||||
mIdtConvexScale(idtConvexScale),
|
||||
mSilhouetteEdgesAreActive(silhouetteEdgesAreActive)
|
||||
{
|
||||
using namespace aos;
|
||||
|
||||
// Hull center in local space
|
||||
const Vec3V hullCenterLocal = V3LoadU(mPolyData.mCenter);
|
||||
// Hull center in mesh space
|
||||
mHullCenterMesh = mMeshToConvex.transformInv(hullCenterLocal);
|
||||
}
|
||||
|
||||
bool generateTriangleFullContactManifold(const Gu::TriangleV& localTriangle, PxU32 triangleIndex, const PxU32* triIndices, PxU8 triFlags, const Gu::PolygonalData& polyData, const Gu::SupportLocalImpl<Gu::TriangleV>* localTriMap, const Gu::SupportLocal* polyMap, Gu::MeshPersistentContact* manifoldContacts, PxU32& numContacts,
|
||||
const aos::FloatVArg contactDist, aos::Vec3V& patchNormal);
|
||||
|
||||
bool generatePolyDataContactManifold(const Gu::TriangleV& localTriangle, PxU32 featureIndex, PxU32 triangleIndex, PxU8 triFlags, Gu::MeshPersistentContact* manifoldContacts, PxU32& numContacts, const aos::FloatVArg contactDist, aos::Vec3V& patchNormal);
|
||||
void generateLastContacts();
|
||||
void addContactsToPatch(const aos::Vec3VArg patchNormal, PxU32 previousNumContacts);
|
||||
|
||||
bool processTriangle(const PxVec3* verts, PxU32 triangleIndex, PxU8 triFlags, const PxU32* vertInds);
|
||||
|
||||
static bool generateTriangleFullContactManifold(const Gu::TriangleV& localTriangle, PxU32 triangleIndex, PxU8 triFlags, const Gu::PolygonalData& polyData, const Gu::SupportLocalImpl<Gu::TriangleV>* localTriMap, const Gu::SupportLocal* polyMap, Gu::MeshPersistentContact* manifoldContacts, PxU32& numContacts,
|
||||
const aos::FloatVArg contactDist, aos::Vec3V& patchNormal, PxRenderOutput* renderOutput = NULL);
|
||||
|
||||
static bool processTriangle(const Gu::PolygonalData& polyData, const SupportLocal* polyMap, const PxVec3* verts, PxU32 triangleIndex, PxU8 triFlags, const aos::FloatVArg inflation, bool isDoubleSided,
|
||||
const aos::PxTransformV& convexTransform, const aos::PxMatTransformV& meshToConvex, Gu::MeshPersistentContact* manifoldContact, PxU32& numContacts);
|
||||
};
|
||||
|
||||
#if PX_VC
|
||||
#pragma warning(pop)
|
||||
#endif
|
||||
|
||||
struct SortedTriangle
|
||||
{
|
||||
aos::FloatV mSquareDist;
|
||||
PxU32 mIndex;
|
||||
|
||||
PX_FORCE_INLINE bool operator < (const SortedTriangle& data) const
|
||||
{
|
||||
return aos::FAllGrtrOrEq(mSquareDist, data.mSquareDist) ==0;
|
||||
}
|
||||
};
|
||||
|
||||
class PCMSphereVsMeshContactGeneration : public PCMMeshContactGeneration
|
||||
{
|
||||
public:
|
||||
aos::Vec3V mSphereCenter;
|
||||
aos::FloatV mSphereRadius;
|
||||
aos::FloatV mSqInflatedSphereRadius;
|
||||
PxInlineArray<SortedTriangle, 64> mSortedTriangle;
|
||||
|
||||
PCMSphereVsMeshContactGeneration(
|
||||
const aos::Vec3VArg sphereCenter,
|
||||
const aos::FloatVArg sphereRadius,
|
||||
const aos::FloatVArg contactDist,
|
||||
const aos::FloatVArg replaceBreakingThreshold,
|
||||
const aos::PxTransformV& sphereTransform,
|
||||
const aos::PxTransformV& meshTransform,
|
||||
MultiplePersistentContactManifold& multiManifold,
|
||||
PxContactBuffer& contactBuffer,
|
||||
PxInlineArray<PxU32, LOCAL_PCM_CONTACTS_SIZE>* deferredContacts,
|
||||
PxRenderOutput* renderOutput = NULL
|
||||
|
||||
) : PCMMeshContactGeneration(contactDist, replaceBreakingThreshold, sphereTransform, meshTransform, multiManifold,
|
||||
contactBuffer, deferredContacts, renderOutput),
|
||||
mSphereCenter(sphereCenter),
|
||||
mSphereRadius(sphereRadius)
|
||||
{
|
||||
using namespace aos;
|
||||
const FloatV inflatedSphereRadius = FAdd(sphereRadius, contactDist);
|
||||
mSqInflatedSphereRadius = FMul(inflatedSphereRadius, inflatedSphereRadius);
|
||||
}
|
||||
|
||||
bool processTriangle(const PxVec3* verts, PxU32 triangleIndex, PxU8 triFlags, const PxU32* vertInds);
|
||||
void generateLastContacts();
|
||||
void addToPatch(const aos::Vec3VArg contactP, const aos::Vec3VArg patchNormal, const aos::FloatV pen, PxU32 triangleIndex);
|
||||
};
|
||||
|
||||
class PCMCapsuleVsMeshContactGeneration : public PCMMeshContactGeneration
|
||||
{
|
||||
PCMCapsuleVsMeshContactGeneration &operator=(PCMCapsuleVsMeshContactGeneration &);
|
||||
public:
|
||||
aos::FloatV mInflatedRadius;
|
||||
aos::FloatV mSqInflatedRadius;
|
||||
const CapsuleV& mCapsule;
|
||||
|
||||
PCMCapsuleVsMeshContactGeneration(
|
||||
const CapsuleV& capsule,
|
||||
const aos::FloatVArg contactDist,
|
||||
const aos::FloatVArg replaceBreakingThreshold,
|
||||
const aos::PxTransformV& sphereTransform,
|
||||
const aos::PxTransformV& meshTransform,
|
||||
Gu::MultiplePersistentContactManifold& multiManifold,
|
||||
PxContactBuffer& contactBuffer,
|
||||
PxInlineArray<PxU32, LOCAL_PCM_CONTACTS_SIZE>* deferredContacts,
|
||||
PxRenderOutput* renderOutput = NULL
|
||||
|
||||
) : PCMMeshContactGeneration(contactDist, replaceBreakingThreshold, sphereTransform, meshTransform, multiManifold, contactBuffer,
|
||||
deferredContacts, renderOutput),
|
||||
mCapsule(capsule)
|
||||
{
|
||||
using namespace aos;
|
||||
mInflatedRadius = FAdd(capsule.radius, contactDist);
|
||||
mSqInflatedRadius = FMul(mInflatedRadius, mInflatedRadius);
|
||||
}
|
||||
|
||||
void generateEEContacts(const aos::Vec3VArg a, const aos::Vec3VArg b,const aos::Vec3VArg c, const aos::Vec3VArg normal, PxU32 triangleIndex,
|
||||
const aos::Vec3VArg p, const aos::Vec3VArg q, const aos::FloatVArg sqInflatedRadius, PxU32 previousNumContacts, Gu::MeshPersistentContact* manifoldContacts, PxU32& numContacts);
|
||||
|
||||
void generateEE(const aos::Vec3VArg p, const aos::Vec3VArg q, const aos::FloatVArg sqInflatedRadius, const aos::Vec3VArg normal, PxU32 triangleIndex,
|
||||
const aos::Vec3VArg a, const aos::Vec3VArg b, Gu::MeshPersistentContact* manifoldContacts, PxU32& numContacts);
|
||||
|
||||
static void generateContacts(const aos::Vec3VArg a, const aos::Vec3VArg b,const aos::Vec3VArg c, const aos::Vec3VArg planeNormal, const aos::Vec3VArg normal,
|
||||
PxU32 triangleIndex, const aos::Vec3VArg p, const aos::Vec3VArg q, const aos::FloatVArg inflatedRadius, Gu::MeshPersistentContact* manifoldContacts, PxU32& numContacts);
|
||||
|
||||
static void generateEEContactsMTD(const aos::Vec3VArg a, const aos::Vec3VArg b,const aos::Vec3VArg c, const aos::Vec3VArg normal, PxU32 triangleIndex,
|
||||
const aos::Vec3VArg p, const aos::Vec3VArg q, const aos::FloatVArg inflatedRadius, Gu::MeshPersistentContact* manifoldContacts, PxU32& numContacts);
|
||||
|
||||
static void generateEEMTD(const aos::Vec3VArg p, const aos::Vec3VArg q, const aos::FloatVArg inflatedRadius, const aos::Vec3VArg normal, PxU32 triangleIndex,
|
||||
const aos::Vec3VArg a, const aos::Vec3VArg b, Gu::MeshPersistentContact* manifoldContacts, PxU32& numContacts);
|
||||
|
||||
bool processTriangle(const PxVec3* verts, PxU32 triangleIndex, PxU8 triFlags, const PxU32* vertInds);
|
||||
|
||||
static bool processTriangle(const TriangleV& triangle, PxU32 triangleIndex, const CapsuleV& capsule, const aos::FloatVArg inflatedRadius, const PxU8 triFlag, Gu::MeshPersistentContact* manifoldContacts, PxU32& numContacts);
|
||||
};
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
276
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactConvexConvex.cpp
vendored
Normal file
276
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactConvexConvex.cpp
vendored
Normal file
@@ -0,0 +1,276 @@
|
||||
// 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 "geomutils/PxContactBuffer.h"
|
||||
#include "GuGJKPenetration.h"
|
||||
#include "GuEPA.h"
|
||||
#include "GuVecConvexHull.h"
|
||||
#include "GuVecConvexHullNoScale.h"
|
||||
#include "GuContactMethodImpl.h"
|
||||
#include "GuPCMShapeConvex.h"
|
||||
#include "GuPCMContactGen.h"
|
||||
|
||||
using namespace physx;
|
||||
using namespace Gu;
|
||||
using namespace aos;
|
||||
|
||||
static bool fullContactsGenerationConvexConvex(const GjkConvex* relativeConvex, const GjkConvex* localConvex, const PxTransformV& transf0, const PxTransformV& transf1,
|
||||
bool idtScale0, bool idtScale1, PersistentContact* manifoldContacts, PxContactBuffer& contactBuffer,
|
||||
PersistentContactManifold& manifold, Vec3VArg normal, const Vec3VArg closestA, const Vec3VArg closestB,
|
||||
const FloatVArg contactDist, bool doOverlapTest, PxRenderOutput* renderOutput, PxReal toleranceLength)
|
||||
{
|
||||
PolygonalData polyData0, polyData1;
|
||||
const ConvexHullV& convexHull0 = relativeConvex->getConvex<ConvexHullV>();
|
||||
const ConvexHullV& convexHull1 = localConvex->getConvex<ConvexHullV>();
|
||||
getPCMConvexData(convexHull0, idtScale0, polyData0);
|
||||
getPCMConvexData(convexHull1, idtScale1, polyData1);
|
||||
|
||||
PX_ALIGN(16, PxU8 buff0[sizeof(SupportLocalImpl<ConvexHullV>)]);
|
||||
PX_ALIGN(16, PxU8 buff1[sizeof(SupportLocalImpl<ConvexHullV>)]);
|
||||
|
||||
SupportLocal* map0 = (idtScale0 ? static_cast<SupportLocal*>(PX_PLACEMENT_NEW(buff0, SupportLocalImpl<ConvexHullNoScaleV>)(static_cast<const ConvexHullNoScaleV&>(convexHull0), transf0, convexHull0.vertex2Shape, convexHull0.shape2Vertex, idtScale0)) :
|
||||
static_cast<SupportLocal*>(PX_PLACEMENT_NEW(buff0, SupportLocalImpl<ConvexHullV>)(convexHull0, transf0, convexHull0.vertex2Shape, convexHull0.shape2Vertex, idtScale0)));
|
||||
|
||||
SupportLocal* map1 = (idtScale1 ? static_cast<SupportLocal*>(PX_PLACEMENT_NEW(buff1, SupportLocalImpl<ConvexHullNoScaleV>)(static_cast<const ConvexHullNoScaleV&>(convexHull1), transf1, convexHull1.vertex2Shape, convexHull1.shape2Vertex, idtScale1)) :
|
||||
static_cast<SupportLocal*>(PX_PLACEMENT_NEW(buff1, SupportLocalImpl<ConvexHullV>)(convexHull1, transf1, convexHull1.vertex2Shape, convexHull1.shape2Vertex, idtScale1)));
|
||||
|
||||
PxU32 numContacts = 0;
|
||||
|
||||
if(generateFullContactManifold(polyData0, polyData1, map0, map1, manifoldContacts, numContacts, contactDist, normal, closestA, closestB, convexHull0.getMarginF(),
|
||||
convexHull1.getMarginF(), doOverlapTest, renderOutput, toleranceLength))
|
||||
{
|
||||
if(numContacts > 0)
|
||||
{
|
||||
//reduce contacts
|
||||
manifold.addBatchManifoldContacts(manifoldContacts, numContacts, toleranceLength);
|
||||
|
||||
const Vec3V worldNormal = manifold.getWorldNormal(transf1);
|
||||
|
||||
//add the manifold contacts;
|
||||
manifold.addManifoldContactsToContactBuffer(contactBuffer, worldNormal, transf1, contactDist);
|
||||
|
||||
#if PCM_LOW_LEVEL_DEBUG
|
||||
manifold.drawManifold(*renderOutput, transf0, transf1);
|
||||
#endif
|
||||
}
|
||||
else
|
||||
{
|
||||
//if doOverlapTest is true, which means GJK/EPA degenerate so we won't have any contact in the manifoldContacts array
|
||||
if(!doOverlapTest)
|
||||
{
|
||||
const Vec3V worldNormal = manifold.getWorldNormal(transf1);
|
||||
|
||||
manifold.addManifoldContactsToContactBuffer(contactBuffer, worldNormal, transf1, contactDist);
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
static bool generateOrProcessContactsConvexConvex( const GjkConvex* relativeConvex, const GjkConvex* localConvex, const PxTransformV& transf0, const PxTransformV& transf1,
|
||||
const PxMatTransformV& aToB, GjkStatus status, GjkOutput& output, PersistentContactManifold& manifold, PxContactBuffer& contactBuffer,
|
||||
PxU32 initialContacts, const FloatV minMargin, const FloatV contactDist,
|
||||
bool idtScale0, bool idtScale1, PxReal toleranceLength, PxRenderOutput* renderOutput)
|
||||
{
|
||||
if(status == GJK_NON_INTERSECT)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
else
|
||||
{
|
||||
PersistentContact* manifoldContacts = PX_CP_TO_PCP(contactBuffer.contacts);
|
||||
|
||||
const Vec3V localNor = manifold.mNumContacts ? manifold.getLocalNormal() : V3Zero();
|
||||
|
||||
const FloatV replaceBreakingThreshold = FMul(minMargin, FLoad(0.05f));
|
||||
|
||||
//addGJKEPAContacts will increase the number of contacts in manifold. If status == EPA_CONTACT, we need to run epa algorithm and generate closest points, normal and
|
||||
//pentration. If epa doesn't degenerate, we will store the contacts information in the manifold. Otherwise, we will return true to do the fallback test
|
||||
const bool doOverlapTest = addGJKEPAContacts(relativeConvex, localConvex, aToB, status, manifoldContacts, replaceBreakingThreshold, FLoad(toleranceLength), output, manifold);
|
||||
|
||||
#if PCM_LOW_LEVEL_DEBUG
|
||||
manifold.drawManifold(*renderOutput, transf0, transf1);
|
||||
#endif
|
||||
//ML: after we refresh the contacts(newContacts) and generate a GJK/EPA contacts(we will store that in the manifold), if the number of contacts is still less than the original contacts,
|
||||
//which means we lose too mang contacts and we should regenerate all the contacts in the current configuration
|
||||
//Also, we need to look at the existing contacts, if the existing contacts has very different normal than the GJK/EPA contacts,
|
||||
//which means we should throw away the existing contacts and do full contact gen
|
||||
const bool fullContactGen = FAllGrtr(FLoad(0.707106781f), V3Dot(localNor, output.normal)) || (manifold.mNumContacts < initialContacts);
|
||||
|
||||
if(fullContactGen || doOverlapTest)
|
||||
{
|
||||
return fullContactsGenerationConvexConvex(relativeConvex, localConvex, transf0, transf1, idtScale0, idtScale1, manifoldContacts, contactBuffer,
|
||||
manifold, output.normal, output.closestA, output.closestB, contactDist, doOverlapTest, renderOutput, toleranceLength);
|
||||
}
|
||||
else
|
||||
{
|
||||
const Vec3V newLocalNor = V3Add(localNor, output.normal);
|
||||
const Vec3V worldNormal = V3Normalize(transf1.rotate(newLocalNor));
|
||||
//const Vec3V worldNormal = transf1.rotate(normal);
|
||||
manifold.addManifoldContactsToContactBuffer(contactBuffer, worldNormal, transf1, contactDist);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static bool convexHullNoScale0( const ConvexHullV& convexHull0, const ConvexHullV& convexHull1, const PxTransformV& transf0, const PxTransformV& transf1,
|
||||
const PxMatTransformV& aToB, GjkOutput& output, PersistentContactManifold& manifold, PxContactBuffer& contactBuffer,
|
||||
PxU32 initialContacts, const FloatV minMargin, const FloatV contactDist,
|
||||
bool idtScale1, PxReal toleranceLength, PxRenderOutput* renderOutput)
|
||||
{
|
||||
const RelativeConvex<ConvexHullNoScaleV> convexA(static_cast<const ConvexHullNoScaleV&>(convexHull0), aToB);
|
||||
if(idtScale1)
|
||||
{
|
||||
const LocalConvex<ConvexHullNoScaleV> convexB(static_cast<const ConvexHullNoScaleV&>(convexHull1));
|
||||
GjkStatus status = gjkPenetration<RelativeConvex<ConvexHullNoScaleV>, LocalConvex<ConvexHullNoScaleV> >(convexA, convexB, aToB.p, contactDist, true,
|
||||
manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, output);
|
||||
|
||||
return generateOrProcessContactsConvexConvex(&convexA, &convexB, transf0, transf1, aToB, status, output, manifold,
|
||||
contactBuffer, initialContacts, minMargin, contactDist, true, true, toleranceLength, renderOutput);
|
||||
}
|
||||
else
|
||||
{
|
||||
const LocalConvex<ConvexHullV> convexB(convexHull1);
|
||||
GjkStatus status = gjkPenetration<RelativeConvex<ConvexHullNoScaleV>, LocalConvex<ConvexHullV> >(convexA, convexB, aToB.p, contactDist, true,
|
||||
manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, output);
|
||||
|
||||
return generateOrProcessContactsConvexConvex(&convexA, &convexB, transf0, transf1, aToB, status, output, manifold,
|
||||
contactBuffer, initialContacts, minMargin, contactDist, true, false, toleranceLength, renderOutput);
|
||||
}
|
||||
}
|
||||
|
||||
static bool convexHullHasScale0(const ConvexHullV& convexHull0, const ConvexHullV& convexHull1, const PxTransformV& transf0, const PxTransformV& transf1,
|
||||
const PxMatTransformV& aToB, GjkOutput& output, PersistentContactManifold& manifold, PxContactBuffer& contactBuffer,
|
||||
PxU32 initialContacts, const FloatV minMargin, const FloatV contactDist,
|
||||
bool idtScale1, PxReal toleranceLength, PxRenderOutput* renderOutput)
|
||||
{
|
||||
RelativeConvex<ConvexHullV> convexA(convexHull0, aToB);
|
||||
if(idtScale1)
|
||||
{
|
||||
const LocalConvex<ConvexHullNoScaleV> convexB(static_cast<const ConvexHullNoScaleV&>(convexHull1));
|
||||
GjkStatus status = gjkPenetration<RelativeConvex<ConvexHullV>, LocalConvex<ConvexHullNoScaleV> >(convexA, convexB, aToB.p, contactDist, true,
|
||||
manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints,output);
|
||||
|
||||
return generateOrProcessContactsConvexConvex(&convexA, &convexB, transf0, transf1, aToB, status, output, manifold,
|
||||
contactBuffer, initialContacts, minMargin, contactDist, false, true, toleranceLength, renderOutput);
|
||||
}
|
||||
else
|
||||
{
|
||||
const LocalConvex<ConvexHullV> convexB(convexHull1);
|
||||
GjkStatus status = gjkPenetration<RelativeConvex<ConvexHullV>, LocalConvex<ConvexHullV> >(convexA, convexB, aToB.p, contactDist, true,
|
||||
manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, output);
|
||||
|
||||
return generateOrProcessContactsConvexConvex(&convexA, &convexB, transf0, transf1, aToB, status, output, manifold,
|
||||
contactBuffer, initialContacts, minMargin, contactDist, false, false, toleranceLength, renderOutput);
|
||||
}
|
||||
}
|
||||
|
||||
bool Gu::pcmContactConvexConvex(GU_CONTACT_METHOD_ARGS)
|
||||
{
|
||||
const PxConvexMeshGeometry& shapeConvex0 = checkedCast<PxConvexMeshGeometry>(shape0);
|
||||
const PxConvexMeshGeometry& shapeConvex1 = checkedCast<PxConvexMeshGeometry>(shape1);
|
||||
|
||||
PersistentContactManifold& manifold = cache.getManifold();
|
||||
|
||||
const ConvexHullData* hullData0 = _getHullData(shapeConvex0);
|
||||
const ConvexHullData* hullData1 = _getHullData(shapeConvex1);
|
||||
PxPrefetchLine(hullData0);
|
||||
PxPrefetchLine(hullData1);
|
||||
|
||||
PX_ASSERT(transform1.q.isSane());
|
||||
PX_ASSERT(transform0.q.isSane());
|
||||
|
||||
const Vec3V vScale0 = V3LoadU_SafeReadW(shapeConvex0.scale.scale); // PT: safe because 'rotation' follows 'scale' in PxMeshScale
|
||||
const Vec3V vScale1 = V3LoadU_SafeReadW(shapeConvex1.scale.scale); // PT: safe because 'rotation' follows 'scale' in PxMeshScale
|
||||
const FloatV contactDist = FLoad(params.mContactDistance);
|
||||
|
||||
//Transfer A into the local space of B
|
||||
const PxTransformV transf0 = loadTransformA(transform0);
|
||||
const PxTransformV transf1 = loadTransformA(transform1);
|
||||
const PxTransformV curRTrans(transf1.transformInv(transf0));
|
||||
const PxMatTransformV aToB(curRTrans);
|
||||
|
||||
const PxReal toleranceLength = params.mToleranceLength;
|
||||
const FloatV convexMargin0 = CalculatePCMConvexMargin(hullData0, vScale0, toleranceLength);
|
||||
const FloatV convexMargin1 = CalculatePCMConvexMargin(hullData1, vScale1, toleranceLength);
|
||||
|
||||
const PxU32 initialContacts = manifold.mNumContacts;
|
||||
|
||||
const FloatV minMargin = FMin(convexMargin0, convexMargin1);
|
||||
const FloatV projectBreakingThreshold = FMul(minMargin, FLoad(0.8f));
|
||||
|
||||
manifold.refreshContactPoints(aToB, projectBreakingThreshold, contactDist);
|
||||
|
||||
//ML: after refreshContactPoints, we might lose some contacts
|
||||
const bool bLostContacts = (manifold.mNumContacts != initialContacts);
|
||||
|
||||
const Vec3V extent0 = V3Mul(V3LoadU_SafeReadW(hullData0->mInternal.mInternalExtents), vScale0);
|
||||
const Vec3V extent1 = V3Mul(V3LoadU_SafeReadW(hullData1->mInternal.mInternalExtents), vScale1);
|
||||
|
||||
const FloatV radiusA = V3Length(extent0);
|
||||
const FloatV radiusB = V3Length(extent1);
|
||||
|
||||
if(bLostContacts || manifold.invalidate_BoxConvex(curRTrans, transf0.q, transf1.q, minMargin, radiusA, radiusB))
|
||||
{
|
||||
manifold.setRelativeTransform(curRTrans, transf0.q, transf1.q);
|
||||
|
||||
const bool idtScale0 = shapeConvex0.scale.isIdentity();
|
||||
const bool idtScale1 = shapeConvex1.scale.isIdentity();
|
||||
const QuatV vQuat0 = QuatVLoadU(&shapeConvex0.scale.rotation.x);
|
||||
const QuatV vQuat1 = QuatVLoadU(&shapeConvex1.scale.rotation.x);
|
||||
|
||||
// PT: safe loads because mCenterOfMass isn't the last data in the structure
|
||||
const ConvexHullV convexHull0(hullData0, V3LoadU_SafeReadW(hullData0->mCenterOfMass), vScale0, vQuat0, idtScale0);
|
||||
const ConvexHullV convexHull1(hullData1, V3LoadU_SafeReadW(hullData1->mCenterOfMass), vScale1, vQuat1, idtScale1);
|
||||
|
||||
GjkOutput output;
|
||||
|
||||
if(idtScale0)
|
||||
{
|
||||
return convexHullNoScale0(convexHull0, convexHull1, transf0, transf1, aToB, output, manifold,
|
||||
contactBuffer, initialContacts, minMargin, contactDist, idtScale1, toleranceLength, renderOutput);
|
||||
}
|
||||
else
|
||||
{
|
||||
return convexHullHasScale0(convexHull0, convexHull1, transf0, transf1, aToB, output, manifold,
|
||||
contactBuffer, initialContacts, minMargin, contactDist, idtScale1, toleranceLength, renderOutput);
|
||||
}
|
||||
}
|
||||
else if(manifold.getNumContacts()> 0)
|
||||
{
|
||||
const Vec3V worldNormal = manifold.getWorldNormal(transf1);
|
||||
manifold.addManifoldContactsToContactBuffer(contactBuffer, worldNormal, transf1, contactDist);
|
||||
#if PCM_LOW_LEVEL_DEBUG
|
||||
manifold.drawManifold(*renderOutput, transf0, transf1);
|
||||
#endif
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
245
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactConvexHeightField.cpp
vendored
Normal file
245
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactConvexHeightField.cpp
vendored
Normal file
@@ -0,0 +1,245 @@
|
||||
// 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/PxTriangleMesh.h"
|
||||
#include "geomutils/PxContactBuffer.h"
|
||||
#include "GuVecBox.h"
|
||||
#include "GuVecConvexHull.h"
|
||||
#include "GuVecConvexHullNoScale.h"
|
||||
#include "GuVecTriangle.h"
|
||||
#include "GuContactMethodImpl.h"
|
||||
#include "GuPCMShapeConvex.h"
|
||||
#include "GuHeightField.h"
|
||||
#include "GuHeightFieldUtil.h"
|
||||
#include "GuPCMContactConvexCommon.h"
|
||||
#include "GuPCMContactMeshCallback.h"
|
||||
#include "foundation/PxVecMath.h"
|
||||
|
||||
using namespace physx;
|
||||
using namespace Gu;
|
||||
using namespace aos;
|
||||
|
||||
namespace
|
||||
{
|
||||
struct PCMConvexVsHeightfieldContactGenerationCallback
|
||||
: PCMHeightfieldContactGenerationCallback< PCMConvexVsHeightfieldContactGenerationCallback >
|
||||
{
|
||||
PCMConvexVsHeightfieldContactGenerationCallback& operator=(const PCMConvexVsHeightfieldContactGenerationCallback&);
|
||||
|
||||
PCMConvexVsMeshContactGeneration mGeneration;
|
||||
|
||||
PCMConvexVsHeightfieldContactGenerationCallback(
|
||||
const FloatVArg contactDistance,
|
||||
const FloatVArg replaceBreakingThreshold,
|
||||
const PolygonalData& polyData,
|
||||
const SupportLocal* polyMap,
|
||||
const Cm::FastVertex2ShapeScaling& convexScaling,
|
||||
bool idtConvexScale,
|
||||
const PxTransformV& convexTransform,
|
||||
const PxTransformV& heightfieldTransform,
|
||||
const PxTransform& heightfieldTransform1,
|
||||
MultiplePersistentContactManifold& multiManifold,
|
||||
PxContactBuffer& contactBuffer,
|
||||
HeightFieldUtil& hfUtil,
|
||||
PxInlineArray<PxU32, LOCAL_PCM_CONTACTS_SIZE>* delayedContacts,
|
||||
bool silhouetteEdgesAreActive,
|
||||
PxRenderOutput* renderOutput = NULL
|
||||
) :
|
||||
PCMHeightfieldContactGenerationCallback< PCMConvexVsHeightfieldContactGenerationCallback >(hfUtil, heightfieldTransform1),
|
||||
mGeneration(contactDistance, replaceBreakingThreshold, convexTransform, heightfieldTransform, multiManifold,
|
||||
contactBuffer, polyData, polyMap, delayedContacts, convexScaling, idtConvexScale, silhouetteEdgesAreActive, renderOutput)
|
||||
{
|
||||
}
|
||||
|
||||
template<PxU32 CacheSize>
|
||||
void processTriangleCache(TriangleCache<CacheSize>& cache)
|
||||
{
|
||||
mGeneration.processTriangleCache<CacheSize, PCMConvexVsMeshContactGeneration>(cache);
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
bool Gu::PCMContactConvexHeightfield(
|
||||
const PolygonalData& polyData, const SupportLocal* polyMap, const FloatVArg minMargin,
|
||||
const PxBounds3& hullAABB, const PxHeightFieldGeometry& shapeHeightfield,
|
||||
const PxTransform& transform0, const PxTransform& transform1,
|
||||
PxReal contactDistance, PxContactBuffer& contactBuffer,
|
||||
const Cm::FastVertex2ShapeScaling& convexScaling, bool idtConvexScale,
|
||||
MultiplePersistentContactManifold& multiManifold, PxRenderOutput* renderOutput)
|
||||
{
|
||||
const QuatV q0 = QuatVLoadA(&transform0.q.x);
|
||||
const Vec3V p0 = V3LoadA(&transform0.p.x);
|
||||
|
||||
const QuatV q1 = QuatVLoadA(&transform1.q.x);
|
||||
const Vec3V p1 = V3LoadA(&transform1.p.x);
|
||||
|
||||
const FloatV contactDist = FLoad(contactDistance);
|
||||
//Transfer A into the local space of B
|
||||
const PxTransformV convexTransform(p0, q0);//box
|
||||
const PxTransformV heightfieldTransform(p1, q1);//heightfield
|
||||
const PxTransformV curTransform = heightfieldTransform.transformInv(convexTransform);
|
||||
|
||||
if(multiManifold.invalidate(curTransform, minMargin))
|
||||
{
|
||||
const FloatV replaceBreakingThreshold = FMul(minMargin, FLoad(0.05f));
|
||||
multiManifold.mNumManifolds = 0;
|
||||
multiManifold.setRelativeTransform(curTransform);
|
||||
|
||||
////////////////////
|
||||
|
||||
HeightFieldUtil hfUtil(shapeHeightfield);
|
||||
const HeightField& hf = hfUtil.getHeightField();
|
||||
|
||||
////////////////////
|
||||
|
||||
/*const Cm::Matrix34 world0(transform0);
|
||||
const Cm::Matrix34 world1(transform1);
|
||||
|
||||
const PxU8* PX_RESTRICT extraData = meshData->mExtraTrigData;*/
|
||||
|
||||
PxInlineArray<PxU32, LOCAL_PCM_CONTACTS_SIZE> delayedContacts;
|
||||
|
||||
PCMConvexVsHeightfieldContactGenerationCallback blockCallback(
|
||||
contactDist,
|
||||
replaceBreakingThreshold,
|
||||
polyData,
|
||||
polyMap,
|
||||
convexScaling,
|
||||
idtConvexScale,
|
||||
convexTransform,
|
||||
heightfieldTransform,
|
||||
transform1,
|
||||
multiManifold,
|
||||
contactBuffer,
|
||||
hfUtil,
|
||||
&delayedContacts,
|
||||
!(hf.getFlags() & PxHeightFieldFlag::eNO_BOUNDARY_EDGES),
|
||||
renderOutput
|
||||
);
|
||||
|
||||
hfUtil.overlapAABBTriangles(transform0, transform1, hullAABB, blockCallback);
|
||||
|
||||
PX_ASSERT(multiManifold.mNumManifolds <= GU_MAX_MANIFOLD_SIZE);
|
||||
blockCallback.mGeneration.generateLastContacts();
|
||||
blockCallback.mGeneration.processContacts(GU_SINGLE_MANIFOLD_CACHE_SIZE, false);
|
||||
}
|
||||
else
|
||||
{
|
||||
const PxMatTransformV aToB(curTransform);
|
||||
|
||||
const FloatV projectBreakingThreshold = FMul(minMargin, FLoad(0.6f));
|
||||
multiManifold.refreshManifold(aToB, projectBreakingThreshold, contactDist);
|
||||
}
|
||||
|
||||
#if PCM_LOW_LEVEL_DEBUG
|
||||
multiManifold.drawManifold(*renderOutput, convexTransform, heightfieldTransform);
|
||||
#endif
|
||||
return multiManifold.addManifoldContactsToContactBuffer(contactBuffer, heightfieldTransform);
|
||||
}
|
||||
|
||||
bool Gu::pcmContactConvexHeightField(GU_CONTACT_METHOD_ARGS)
|
||||
{
|
||||
const PxConvexMeshGeometry& shapeConvex = checkedCast<PxConvexMeshGeometry>(shape0);
|
||||
const PxHeightFieldGeometry& shapeHeightField = checkedCast<PxHeightFieldGeometry>(shape1);
|
||||
|
||||
const ConvexHullData* hullData = _getHullData(shapeConvex);
|
||||
MultiplePersistentContactManifold& multiManifold = cache.getMultipleManifold();
|
||||
|
||||
const QuatV q0 = QuatVLoadA(&transform0.q.x);
|
||||
const Vec3V p0 = V3LoadA(&transform0.p.x);
|
||||
|
||||
const PxTransformV convexTransform(p0, q0);
|
||||
|
||||
//const bool idtScaleMesh = shapeMesh.scale.isIdentity();
|
||||
|
||||
//Cm::FastVertex2ShapeScaling meshScaling;
|
||||
//if(!idtScaleMesh)
|
||||
// meshScaling.init(shapeMesh.scale);
|
||||
|
||||
Cm::FastVertex2ShapeScaling convexScaling;
|
||||
PxBounds3 hullAABB;
|
||||
PolygonalData polyData;
|
||||
const bool idtScaleConvex = getPCMConvexData(shapeConvex, convexScaling, hullAABB, polyData);
|
||||
|
||||
const Vec3V vScale = V3LoadU_SafeReadW(shapeConvex.scale.scale); // PT: safe because 'rotation' follows 'scale' in PxMeshScale
|
||||
|
||||
const PxReal toleranceLength = params.mToleranceLength;
|
||||
const FloatV minMargin = CalculatePCMConvexMargin(hullData, vScale, toleranceLength, GU_PCM_MESH_MANIFOLD_EPSILON);
|
||||
|
||||
const QuatV vQuat = QuatVLoadU(&shapeConvex.scale.rotation.x);
|
||||
const ConvexHullV convexHull(hullData, V3Zero(), vScale, vQuat, shapeConvex.scale.isIdentity());
|
||||
|
||||
if(idtScaleConvex)
|
||||
{
|
||||
SupportLocalImpl<ConvexHullNoScaleV> convexMap(static_cast<const ConvexHullNoScaleV&>(convexHull), convexTransform, convexHull.vertex2Shape, convexHull.shape2Vertex, idtScaleConvex);
|
||||
return PCMContactConvexHeightfield(polyData, &convexMap, minMargin, hullAABB, shapeHeightField, transform0, transform1, params.mContactDistance, contactBuffer, convexScaling,
|
||||
idtScaleConvex, multiManifold, renderOutput);
|
||||
}
|
||||
else
|
||||
{
|
||||
SupportLocalImpl<ConvexHullV> convexMap(convexHull, convexTransform, convexHull.vertex2Shape, convexHull.shape2Vertex, idtScaleConvex);
|
||||
return PCMContactConvexHeightfield(polyData, &convexMap, minMargin, hullAABB, shapeHeightField, transform0, transform1, params.mContactDistance, contactBuffer, convexScaling,
|
||||
idtScaleConvex, multiManifold, renderOutput);
|
||||
}
|
||||
}
|
||||
|
||||
bool Gu::pcmContactBoxHeightField(GU_CONTACT_METHOD_ARGS)
|
||||
{
|
||||
MultiplePersistentContactManifold& multiManifold = cache.getMultipleManifold();
|
||||
|
||||
const PxBoxGeometry& shapeBox = checkedCast<PxBoxGeometry>(shape0);
|
||||
const PxHeightFieldGeometry& shapeHeightField = checkedCast<PxHeightFieldGeometry>(shape1);
|
||||
|
||||
const PxVec3 ext = shapeBox.halfExtents + PxVec3(params.mContactDistance);
|
||||
const PxBounds3 hullAABB(-ext, ext);
|
||||
|
||||
const Cm::FastVertex2ShapeScaling idtScaling;
|
||||
|
||||
const QuatV q0 = QuatVLoadA(&transform0.q.x);
|
||||
const Vec3V p0 = V3LoadA(&transform0.p.x);
|
||||
|
||||
const Vec3V boxExtents = V3LoadU(shapeBox.halfExtents);
|
||||
|
||||
const PxReal toranceLength = params.mToleranceLength;
|
||||
const FloatV minMargin = CalculatePCMBoxMargin(boxExtents, toranceLength, GU_PCM_MESH_MANIFOLD_EPSILON);
|
||||
|
||||
const BoxV boxV(V3Zero(), boxExtents);
|
||||
|
||||
const PxTransformV boxTransform(p0, q0);//box
|
||||
|
||||
PolygonalData polyData;
|
||||
PCMPolygonalBox polyBox(shapeBox.halfExtents);
|
||||
polyBox.getPolygonalData(&polyData);
|
||||
|
||||
const Mat33V identity = M33Identity();
|
||||
//SupportLocalImpl<BoxV> boxMap(boxV, boxTransform, identity, identity);
|
||||
SupportLocalImpl<BoxV> boxMap(boxV, boxTransform, identity, identity, true);
|
||||
|
||||
return PCMContactConvexHeightfield(polyData, &boxMap, minMargin, hullAABB, shapeHeightField, transform0, transform1, params.mContactDistance, contactBuffer,
|
||||
idtScaling, true, multiManifold, renderOutput);
|
||||
}
|
||||
245
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactConvexMesh.cpp
vendored
Normal file
245
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactConvexMesh.cpp
vendored
Normal file
@@ -0,0 +1,245 @@
|
||||
// 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/PxTriangleMesh.h"
|
||||
#include "geomutils/PxContactBuffer.h"
|
||||
#include "GuVecBox.h"
|
||||
#include "GuVecConvexHull.h"
|
||||
#include "GuVecConvexHullNoScale.h"
|
||||
#include "GuVecTriangle.h"
|
||||
#include "GuContactMethodImpl.h"
|
||||
#include "GuPCMShapeConvex.h"
|
||||
#include "GuConvexUtilsInternal.h"
|
||||
#include "GuPCMContactConvexCommon.h"
|
||||
#include "GuPCMContactMeshCallback.h"
|
||||
#include "GuIntersectionTriangleBox.h"
|
||||
#include "GuBox.h"
|
||||
#include "CmMatrix34.h"
|
||||
|
||||
using namespace physx;
|
||||
using namespace Gu;
|
||||
using namespace Cm;
|
||||
using namespace aos;
|
||||
|
||||
namespace
|
||||
{
|
||||
struct PCMConvexVsMeshContactGenerationCallback : PCMMeshContactGenerationCallback<PCMConvexVsMeshContactGenerationCallback>
|
||||
{
|
||||
PCMConvexVsMeshContactGenerationCallback& operator=(const PCMConvexVsMeshContactGenerationCallback&);
|
||||
|
||||
PCMConvexVsMeshContactGeneration mGeneration;
|
||||
const BoxPadded& mBox;
|
||||
|
||||
PCMConvexVsMeshContactGenerationCallback(
|
||||
const FloatVArg contactDistance,
|
||||
const FloatVArg replaceBreakingThreshold,
|
||||
const PxTransformV& convexTransform,
|
||||
const PxTransformV& meshTransform,
|
||||
MultiplePersistentContactManifold& multiManifold,
|
||||
PxContactBuffer& contactBuffer,
|
||||
const PolygonalData& polyData,
|
||||
const SupportLocal* polyMap,
|
||||
PxInlineArray<PxU32, LOCAL_PCM_CONTACTS_SIZE>* delayedContacts,
|
||||
const FastVertex2ShapeScaling& convexScaling,
|
||||
bool idtConvexScale,
|
||||
const FastVertex2ShapeScaling& meshScaling,
|
||||
const PxU8* extraTriData,
|
||||
bool idtMeshScale,
|
||||
bool silhouetteEdgesAreActive,
|
||||
const BoxPadded& box,
|
||||
PxRenderOutput* renderOutput = NULL
|
||||
) :
|
||||
PCMMeshContactGenerationCallback<PCMConvexVsMeshContactGenerationCallback>(meshScaling, extraTriData, idtMeshScale),
|
||||
mGeneration(contactDistance, replaceBreakingThreshold, convexTransform, meshTransform, multiManifold, contactBuffer, polyData,
|
||||
polyMap, delayedContacts, convexScaling, idtConvexScale, silhouetteEdgesAreActive, renderOutput),
|
||||
mBox(box)
|
||||
{
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE PxIntBool doTest(const PxVec3& v0, const PxVec3& v1, const PxVec3& v2)
|
||||
{
|
||||
// PT: this one is safe because midphase vertices are directly passed to the function
|
||||
return intersectTriangleBox(mBox, v0, v1, v2);
|
||||
}
|
||||
|
||||
template<PxU32 CacheSize>
|
||||
void processTriangleCache(TriangleCache<CacheSize>& cache)
|
||||
{
|
||||
mGeneration.processTriangleCache<CacheSize, PCMConvexVsMeshContactGeneration>(cache);
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
bool Gu::PCMContactConvexMesh(const PolygonalData& polyData, const SupportLocal* polyMap, const FloatVArg minMargin, const PxBounds3& hullAABB, const PxTriangleMeshGeometry& shapeMesh,
|
||||
const PxTransform& transform0, const PxTransform& transform1,
|
||||
PxReal contactDistance, PxContactBuffer& contactBuffer,
|
||||
const FastVertex2ShapeScaling& convexScaling, const FastVertex2ShapeScaling& meshScaling,
|
||||
bool idtConvexScale, bool idtMeshScale, MultiplePersistentContactManifold& multiManifold,
|
||||
PxRenderOutput* renderOutput)
|
||||
{
|
||||
const QuatV q0 = QuatVLoadA(&transform0.q.x);
|
||||
const Vec3V p0 = V3LoadA(&transform0.p.x);
|
||||
|
||||
const QuatV q1 = QuatVLoadA(&transform1.q.x);
|
||||
const Vec3V p1 = V3LoadA(&transform1.p.x);
|
||||
|
||||
const FloatV contactDist = FLoad(contactDistance);
|
||||
//Transfer A into the local space of B
|
||||
const PxTransformV convexTransform(p0, q0);//box
|
||||
const PxTransformV meshTransform(p1, q1);//triangleMesh
|
||||
const PxTransformV curTransform = meshTransform.transformInv(convexTransform);
|
||||
|
||||
if(multiManifold.invalidate(curTransform, minMargin))
|
||||
{
|
||||
const FloatV replaceBreakingThreshold = FMul(minMargin, FLoad(0.05f));
|
||||
multiManifold.mNumManifolds = 0;
|
||||
multiManifold.setRelativeTransform(curTransform);
|
||||
|
||||
////////////////////
|
||||
const TriangleMesh* PX_RESTRICT meshData = _getMeshData(shapeMesh);
|
||||
|
||||
const Matrix34FromTransform world0(transform0);
|
||||
const Matrix34FromTransform world1(transform1);
|
||||
BoxPadded hullOBB;
|
||||
computeHullOBB(hullOBB, hullAABB, contactDistance, world0, world1, meshScaling, idtMeshScale);
|
||||
|
||||
// Setup the collider
|
||||
|
||||
PxInlineArray<PxU32, LOCAL_PCM_CONTACTS_SIZE> delayedContacts;
|
||||
|
||||
const PxU8* PX_RESTRICT extraData = meshData->getExtraTrigData();
|
||||
PCMConvexVsMeshContactGenerationCallback blockCallback(
|
||||
contactDist, replaceBreakingThreshold, convexTransform, meshTransform, multiManifold, contactBuffer,
|
||||
polyData, polyMap, &delayedContacts, convexScaling, idtConvexScale, meshScaling, extraData, idtMeshScale, true,
|
||||
hullOBB, renderOutput);
|
||||
|
||||
Midphase::intersectOBB(meshData, hullOBB, blockCallback, true);
|
||||
|
||||
PX_ASSERT(multiManifold.mNumManifolds <= GU_MAX_MANIFOLD_SIZE);
|
||||
|
||||
blockCallback.flushCache();
|
||||
//This is very important
|
||||
blockCallback.mGeneration.generateLastContacts();
|
||||
blockCallback.mGeneration.processContacts(GU_SINGLE_MANIFOLD_CACHE_SIZE, false);
|
||||
|
||||
#if PCM_LOW_LEVEL_DEBUG
|
||||
multiManifold.drawManifold(*renderOutput, transform0, transform1);
|
||||
#endif
|
||||
}
|
||||
else
|
||||
{
|
||||
const PxMatTransformV aToB(curTransform);
|
||||
const FloatV projectBreakingThreshold = FMul(minMargin, FLoad(0.8f));
|
||||
multiManifold.refreshManifold(aToB, projectBreakingThreshold, contactDist);
|
||||
}
|
||||
|
||||
return multiManifold.addManifoldContactsToContactBuffer(contactBuffer, meshTransform);
|
||||
}
|
||||
|
||||
bool Gu::pcmContactConvexMesh(GU_CONTACT_METHOD_ARGS)
|
||||
{
|
||||
PX_UNUSED(renderOutput);
|
||||
|
||||
const PxConvexMeshGeometry& shapeConvex = checkedCast<PxConvexMeshGeometry>(shape0);
|
||||
const PxTriangleMeshGeometry& shapeMesh = checkedCast<PxTriangleMeshGeometry>(shape1);
|
||||
|
||||
const ConvexHullData* hullData = _getHullData(shapeConvex);
|
||||
MultiplePersistentContactManifold& multiManifold = cache.getMultipleManifold();
|
||||
|
||||
const PxTransformV convexTransform = loadTransformA(transform0);
|
||||
|
||||
const bool idtScaleMesh = shapeMesh.scale.isIdentity();
|
||||
|
||||
FastVertex2ShapeScaling meshScaling;
|
||||
if(!idtScaleMesh)
|
||||
meshScaling.init(shapeMesh.scale);
|
||||
|
||||
FastVertex2ShapeScaling convexScaling;
|
||||
PxBounds3 hullAABB;
|
||||
PolygonalData polyData;
|
||||
const bool idtScaleConvex = getPCMConvexData(shapeConvex, convexScaling, hullAABB, polyData);
|
||||
const QuatV vQuat = QuatVLoadU(&shapeConvex.scale.rotation.x);
|
||||
|
||||
const Vec3V vScale = V3LoadU_SafeReadW(shapeConvex.scale.scale); // PT: safe because 'rotation' follows 'scale' in PxMeshScale
|
||||
|
||||
const PxReal toleranceScale = params.mToleranceLength;
|
||||
const FloatV minMargin = CalculatePCMConvexMargin(hullData, vScale, toleranceScale, GU_PCM_MESH_MANIFOLD_EPSILON);
|
||||
|
||||
const ConvexHullV convexHull(hullData, V3Zero(), vScale, vQuat, idtScaleConvex);
|
||||
|
||||
if(idtScaleConvex)
|
||||
{
|
||||
SupportLocalImpl<ConvexHullNoScaleV> convexMap(static_cast<const ConvexHullNoScaleV&>(convexHull), convexTransform, convexHull.vertex2Shape, convexHull.shape2Vertex, true);
|
||||
return PCMContactConvexMesh(polyData, &convexMap, minMargin, hullAABB, shapeMesh, transform0, transform1, params.mContactDistance, contactBuffer, convexScaling,
|
||||
meshScaling, idtScaleConvex, idtScaleMesh, multiManifold, renderOutput);
|
||||
}
|
||||
else
|
||||
{
|
||||
SupportLocalImpl<ConvexHullV> convexMap(convexHull, convexTransform, convexHull.vertex2Shape, convexHull.shape2Vertex, false);
|
||||
return PCMContactConvexMesh(polyData, &convexMap, minMargin, hullAABB, shapeMesh, transform0, transform1, params.mContactDistance, contactBuffer, convexScaling,
|
||||
meshScaling, idtScaleConvex, idtScaleMesh, multiManifold, renderOutput);
|
||||
}
|
||||
}
|
||||
|
||||
bool Gu::pcmContactBoxMesh(GU_CONTACT_METHOD_ARGS)
|
||||
{
|
||||
PX_UNUSED(renderOutput);
|
||||
|
||||
MultiplePersistentContactManifold& multiManifold = cache.getMultipleManifold();
|
||||
|
||||
const PxBoxGeometry& shapeBox = checkedCast<PxBoxGeometry>(shape0);
|
||||
const PxTriangleMeshGeometry& shapeMesh = checkedCast<PxTriangleMeshGeometry>(shape1);
|
||||
|
||||
const PxBounds3 hullAABB(-shapeBox.halfExtents, shapeBox.halfExtents);
|
||||
|
||||
const bool idtMeshScale = shapeMesh.scale.isIdentity();
|
||||
|
||||
FastVertex2ShapeScaling meshScaling;
|
||||
if(!idtMeshScale)
|
||||
meshScaling.init(shapeMesh.scale);
|
||||
|
||||
FastVertex2ShapeScaling idtScaling;
|
||||
|
||||
const Vec3V boxExtents = V3LoadU(shapeBox.halfExtents);
|
||||
const PxReal toleranceLength = params.mToleranceLength;
|
||||
const FloatV minMargin = CalculatePCMBoxMargin(boxExtents, toleranceLength, GU_PCM_MESH_MANIFOLD_EPSILON);
|
||||
|
||||
const BoxV boxV(V3Zero(), boxExtents);
|
||||
|
||||
const PxTransformV boxTransform = loadTransformA(transform0);//box
|
||||
|
||||
PolygonalData polyData;
|
||||
PCMPolygonalBox polyBox(shapeBox.halfExtents);
|
||||
polyBox.getPolygonalData(&polyData);
|
||||
|
||||
const Mat33V identity = M33Identity();
|
||||
SupportLocalImpl<BoxV> boxMap(boxV, boxTransform, identity, identity, true);
|
||||
|
||||
return PCMContactConvexMesh(polyData, &boxMap, minMargin, hullAABB, shapeMesh, transform0, transform1, params.mContactDistance, contactBuffer, idtScaling, meshScaling,
|
||||
true, idtMeshScale, multiManifold, renderOutput);
|
||||
}
|
||||
140
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactCustomGeometry.cpp
vendored
Normal file
140
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactCustomGeometry.cpp
vendored
Normal file
@@ -0,0 +1,140 @@
|
||||
// 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 "geomutils/PxContactBuffer.h"
|
||||
#include "GuVecBox.h"
|
||||
#include "GuBounds.h"
|
||||
#include "GuContactMethodImpl.h"
|
||||
#include "GuPCMShapeConvex.h"
|
||||
#include "GuPCMContactGen.h"
|
||||
|
||||
using namespace physx;
|
||||
using namespace aos;
|
||||
using namespace Gu;
|
||||
|
||||
static bool pcmContactCustomGeometryGeometry(GU_CONTACT_METHOD_ARGS)
|
||||
{
|
||||
PX_UNUSED(renderOutput);
|
||||
|
||||
const PxCustomGeometry& customGeom = checkedCast<PxCustomGeometry>(shape0);
|
||||
const PxGeometry& otherGeom = shape1;
|
||||
|
||||
float breakingThreshold = 0.01f * params.mToleranceLength;
|
||||
bool usePCM = customGeom.callbacks->usePersistentContactManifold(customGeom, breakingThreshold);
|
||||
if (otherGeom.getType() == PxGeometryType::eCUSTOM)
|
||||
{
|
||||
float breakingThreshold1 = breakingThreshold;
|
||||
if (checkedCast<PxCustomGeometry>(shape1).callbacks->usePersistentContactManifold(otherGeom, breakingThreshold1))
|
||||
{
|
||||
breakingThreshold = PxMin(breakingThreshold, breakingThreshold1);
|
||||
usePCM = true;
|
||||
}
|
||||
}
|
||||
else if (otherGeom.getType() > PxGeometryType::eCONVEXMESH)
|
||||
{
|
||||
usePCM = true;
|
||||
}
|
||||
|
||||
if (usePCM)
|
||||
{
|
||||
MultiplePersistentContactManifold& multiManifold = cache.getMultipleManifold();
|
||||
|
||||
const PxTransformV transf0 = loadTransformA(transform0), transf1 = loadTransformA(transform1);
|
||||
const PxTransformV curRTrans = transf1.transformInv(transf0);
|
||||
const FloatV cos5 = FLoad(0.9962f); // Cos of 5 degrees
|
||||
if (multiManifold.invalidate(curRTrans, FLoad(breakingThreshold)))
|
||||
{
|
||||
customGeom.callbacks->generateContacts(customGeom, otherGeom, transform0, transform1,
|
||||
params.mContactDistance, params.mMeshContactMargin, params.mToleranceLength,
|
||||
contactBuffer);
|
||||
|
||||
multiManifold.initialize();
|
||||
multiManifold.setRelativeTransform(curRTrans);
|
||||
|
||||
for (PxU32 ci = 0; ci < contactBuffer.count; ++ci)
|
||||
{
|
||||
const PxContactPoint& c = contactBuffer.contacts[ci];
|
||||
const Vec3V cP = V3LoadU(c.point);
|
||||
const Vec3V cN = V3LoadU(c.normal);
|
||||
const FloatV cD = FLoad(c.separation);
|
||||
|
||||
SinglePersistentContactManifold* manifold = NULL;
|
||||
for (PxU8 mi = 0; mi < multiManifold.mNumManifolds; ++mi)
|
||||
{
|
||||
const Vec3V mN = multiManifold.mManifolds[mi].getWorldNormal(transf1);
|
||||
const BoolV cmp = FIsGrtr(V3Dot(cN, mN), cos5);
|
||||
if (BAllEqTTTT(cmp))
|
||||
{
|
||||
manifold = &multiManifold.mManifolds[mi];
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (!manifold)
|
||||
{
|
||||
manifold = multiManifold.getEmptyManifold();
|
||||
if (manifold) multiManifold.mNumManifolds++;
|
||||
}
|
||||
|
||||
if (manifold)
|
||||
{
|
||||
SinglePersistentContactManifold& m = *manifold;
|
||||
MeshPersistentContact pc;
|
||||
pc.mLocalPointA = transf0.transformInv(cP);
|
||||
pc.mLocalPointB = transf1.transformInv(cP);
|
||||
pc.mLocalNormalPen = V4SetW(transf1.rotateInv(cN), cD);
|
||||
pc.mFaceIndex = c.internalFaceIndex1;
|
||||
m.mContactPoints[m.mNumContacts++] = pc;
|
||||
m.mNumContacts = SinglePersistentContactManifold::reduceContacts(m.mContactPoints, m.mNumContacts);
|
||||
}
|
||||
}
|
||||
|
||||
contactBuffer.count = 0;
|
||||
}
|
||||
|
||||
#if PCM_LOW_LEVEL_DEBUG
|
||||
multiManifold.drawManifold(*renderOutput, transf0, transf1);
|
||||
#endif
|
||||
return multiManifold.addManifoldContactsToContactBuffer(contactBuffer, transf1);
|
||||
}
|
||||
|
||||
return customGeom.callbacks->generateContacts(customGeom, otherGeom, transform0, transform1,
|
||||
params.mContactDistance, params.mMeshContactMargin, params.mToleranceLength,
|
||||
contactBuffer);
|
||||
}
|
||||
|
||||
bool Gu::pcmContactGeometryCustomGeometry(GU_CONTACT_METHOD_ARGS)
|
||||
{
|
||||
bool res = pcmContactCustomGeometryGeometry(shape1, shape0, transform1, transform0, params, cache, contactBuffer, renderOutput);
|
||||
|
||||
for (PxU32 i = 0; i < contactBuffer.count; ++i)
|
||||
contactBuffer.contacts[i].normal = -contactBuffer.contacts[i].normal;
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
74
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactGen.h
vendored
Normal file
74
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactGen.h
vendored
Normal file
@@ -0,0 +1,74 @@
|
||||
// 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_PCM_CONTACT_GEN_H
|
||||
#define GU_PCM_CONTACT_GEN_H
|
||||
|
||||
#include "GuConvexSupportTable.h"
|
||||
#include "GuPersistentContactManifold.h"
|
||||
#include "GuShapeConvex.h"
|
||||
#include "GuSeparatingAxes.h"
|
||||
#include "GuGJKType.h"
|
||||
#include "GuGJKUtil.h"
|
||||
|
||||
namespace physx
|
||||
{
|
||||
namespace Gu
|
||||
{
|
||||
//full contact gen code for box/convexhull vs convexhull
|
||||
bool generateFullContactManifold(const Gu::PolygonalData& polyData0, const Gu::PolygonalData& polyData1, const Gu::SupportLocal* map0, const Gu::SupportLocal* map1, Gu::PersistentContact* manifoldContacts,
|
||||
PxU32& numContacts, const aos::FloatVArg contactDist, const aos::Vec3VArg normal, const aos::Vec3VArg closestA, const aos::Vec3VArg closestB,
|
||||
PxReal toleranceA, PxReal toleranceB, bool doOverlapTest, PxRenderOutput* renderOutput, PxReal toleranceLength);
|
||||
|
||||
//full contact gen code for capsule vs convexhulll
|
||||
bool generateFullContactManifold(const Gu::CapsuleV& capsule, const Gu::PolygonalData& polyData, const Gu::SupportLocal* map, const aos::PxMatTransformV& aToB, Gu::PersistentContact* manifoldContacts,
|
||||
PxU32& numContacts, const aos::FloatVArg contactDist, aos::Vec3V& normal, const aos::Vec3VArg closest, PxReal tolerance, bool doOverlapTest, PxReal toleranceScale);
|
||||
|
||||
//full contact gen code for capsule vs box
|
||||
bool generateCapsuleBoxFullContactManifold(const Gu::CapsuleV& capsule, const Gu::PolygonalData& polyData, const Gu::SupportLocal* map, const aos::PxMatTransformV& aToB, Gu::PersistentContact* manifoldContacts, PxU32& numContacts,
|
||||
const aos::FloatVArg contactDist, aos::Vec3V& normal, const aos::Vec3VArg closest, PxReal boxMargin, bool doOverlapTest, PxReal toleranceScale, PxRenderOutput* renderOutput);
|
||||
|
||||
//based on the gjk status to decide whether we should do full contact gen with GJK/EPA normal. Also, this method store
|
||||
//GJK/EPA point to the manifold in case full contact gen doesn't generate any contact
|
||||
bool addGJKEPAContacts(const Gu::GjkConvex* relativeConvex, const Gu::GjkConvex* localConvex, const aos::PxMatTransformV& aToB, Gu::GjkStatus status,
|
||||
Gu::PersistentContact* manifoldContacts, const aos::FloatV replaceBreakingThreshold, const aos::FloatV toleranceLength, GjkOutput& output,
|
||||
Gu::PersistentContactManifold& manifold);
|
||||
|
||||
//MTD code for box/convexhull vs box/convexhull
|
||||
bool computeMTD(const Gu::PolygonalData& polyData0, const Gu::PolygonalData& polyData1, const SupportLocal* map0, const SupportLocal* map1, aos::FloatV& penDepth, aos::Vec3V& normal);
|
||||
|
||||
//MTD code for capsule vs box/convexhull
|
||||
bool computeMTD(const Gu::CapsuleV& capsule, const Gu::PolygonalData& polyData, const Gu::SupportLocal* map, aos::FloatV& penDepth, aos::Vec3V& normal);
|
||||
|
||||
//full contact gen code for sphere vs convexhull
|
||||
bool generateSphereFullContactManifold(const Gu::CapsuleV& capsule, const Gu::PolygonalData& polyData, const Gu::SupportLocal* map, Gu::PersistentContact* manifoldContacts, PxU32& numContacts,
|
||||
const aos::FloatVArg contactDist, aos::Vec3V& normal, bool doOverlapTest);
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
762
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactGenBoxConvex.cpp
vendored
Normal file
762
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactGenBoxConvex.cpp
vendored
Normal file
@@ -0,0 +1,762 @@
|
||||
// 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 "GuPCMContactGen.h"
|
||||
#include "GuPCMShapeConvex.h"
|
||||
#include "GuPCMContactGenUtil.h"
|
||||
#include "foundation/PxVecMath.h"
|
||||
#include "foundation/PxAlloca.h"
|
||||
#include "GuVecCapsule.h"
|
||||
#include "GuVecBox.h"
|
||||
#include "GuEPA.h"
|
||||
#include "geomutils/PxContactBuffer.h"
|
||||
#include "common/PxRenderOutput.h"
|
||||
|
||||
#define PCM_USE_INTERNAL_OBJECT 1
|
||||
|
||||
using namespace physx;
|
||||
using namespace Gu;
|
||||
using namespace aos;
|
||||
|
||||
//Precompute the convex data
|
||||
// 7+------+6 0 = ---
|
||||
// /| /| 1 = +--
|
||||
// / | / | 2 = ++-
|
||||
// / 4+---/--+5 3 = -+-
|
||||
// 3+------+2 / y z 4 = --+
|
||||
// | / | / | / 5 = +-+
|
||||
// |/ |/ |/ 6 = +++
|
||||
// 0+------+1 *---x 7 = -++
|
||||
|
||||
static bool testFaceNormal(const PolygonalData& polyData0, const PolygonalData& polyData1, const SupportLocal* map0, const SupportLocal* map1, const PxMatTransformV& transform0To1, const PxMatTransformV& transform1To0, const FloatVArg contactDist,
|
||||
FloatV& minOverlap, PxU32& feature, Vec3V& faceNormal, FeatureStatus faceStatus, FeatureStatus& status)
|
||||
{
|
||||
PX_UNUSED(polyData1);
|
||||
|
||||
FloatV _minOverlap = FMax();//minOverlap;
|
||||
PxU32 _feature = 0;
|
||||
Vec3V _faceNormal = faceNormal;
|
||||
FloatV min0, max0;
|
||||
FloatV min1, max1;
|
||||
|
||||
const Vec3V center1To0 = transform1To0.p;
|
||||
|
||||
#if PCM_USE_INTERNAL_OBJECT
|
||||
const Vec3V zeroV = V3Zero();
|
||||
const Vec3V shapeSpaceCenter1 = V3LoadU(polyData1.mCenter);
|
||||
const Vec3V internalCenter1In0 = transform1To0.transform(shapeSpaceCenter1);
|
||||
const FloatV internalRadius1 = FLoad(polyData1.mInternal.mInternalRadius);
|
||||
const Vec3V internalExtents1 = V3LoadU_SafeReadW(polyData1.mInternal.mInternalExtents);
|
||||
const Vec3V negInternalExtents1 = V3Neg(internalExtents1);
|
||||
#endif
|
||||
|
||||
//in the local space of polyData0
|
||||
for(PxU32 i=0; i<polyData0.mNbPolygons; ++i)
|
||||
{
|
||||
const HullPolygonData& polygon = polyData0.mPolygons[i];
|
||||
|
||||
const Vec3V minVert = V3LoadU(polyData0.mVerts[polygon.mMinIndex]);
|
||||
const FloatV planeDist = FLoad(polygon.mPlane.d);
|
||||
const Vec3V vertexSpacePlaneNormal = V3LoadU(polygon.mPlane.n);
|
||||
|
||||
//transform plane n to shape space
|
||||
const Vec3V shapeSpacePlaneNormal = M33TrnspsMulV3(map0->shape2Vertex, vertexSpacePlaneNormal);
|
||||
|
||||
const FloatV magnitude = FRecip(V3Length(shapeSpacePlaneNormal));
|
||||
//ML::use this to avoid LHS
|
||||
min0 = FMul(V3Dot(vertexSpacePlaneNormal, minVert), magnitude);
|
||||
max0 = FMul(FNeg(planeDist), magnitude);
|
||||
|
||||
//normalize shape space normal
|
||||
const Vec3V n0 = V3Scale(shapeSpacePlaneNormal, magnitude);
|
||||
|
||||
//calculate polyData1 projection
|
||||
//rotate polygon's normal into the local space of polyData1
|
||||
const Vec3V n1 = transform0To1.rotate(n0);
|
||||
|
||||
#if PCM_USE_INTERNAL_OBJECT
|
||||
//test internal object
|
||||
//ML: we don't need to transform the normal into the vertex space. If polyData1 don't have scale,
|
||||
//the vertex2Shape matrix will be identity, shape space normal will be the same as vertex space's normal.
|
||||
//If polyData0 have scale, internalExtens1 will be 0.
|
||||
const Vec3V proj = V3Sel(V3IsGrtr(n1, zeroV), internalExtents1, negInternalExtents1);
|
||||
const FloatV radius = FMax(V3Dot(n1, proj), internalRadius1);
|
||||
const FloatV internalTrans = V3Dot(internalCenter1In0, n0);
|
||||
|
||||
const FloatV _min1 = FSub(internalTrans, radius);
|
||||
const FloatV _max1 = FAdd(internalTrans, radius);
|
||||
|
||||
const FloatV _min = FMax(min0, _min1);
|
||||
const FloatV _max = FMin(max0, _max1);
|
||||
|
||||
const FloatV _tempOverlap = FSub(_max, _min);
|
||||
//const FloatV _tempOverlap = FSub(max0, _min1);
|
||||
|
||||
//Internal object overlaps more than current min, so can skip it
|
||||
//because (a) it isn't a separating axis and (b) it isn't the smallest axis
|
||||
if(FAllGrtr(_tempOverlap, _minOverlap))
|
||||
continue;
|
||||
#endif
|
||||
const FloatV translate = V3Dot(center1To0, n0);
|
||||
map1->doSupport(n1, min1, max1);
|
||||
|
||||
min1 = FAdd(translate, min1);
|
||||
max1 = FAdd(translate, max1);
|
||||
|
||||
const BoolV con = BOr(FIsGrtr(min1, FAdd(max0, contactDist)), FIsGrtr(min0, FAdd(max1, contactDist)));
|
||||
|
||||
if(BAllEqTTTT(con))
|
||||
return false;
|
||||
|
||||
const FloatV tempOverlap = FSub(max0, min1);
|
||||
|
||||
if(FAllGrtr(_minOverlap, tempOverlap))
|
||||
{
|
||||
_minOverlap = tempOverlap;
|
||||
_feature = i;
|
||||
_faceNormal = n0;
|
||||
}
|
||||
}
|
||||
|
||||
if(FAllGrtr(minOverlap, _minOverlap))
|
||||
{
|
||||
faceNormal = _faceNormal;
|
||||
minOverlap = _minOverlap;
|
||||
status = faceStatus;
|
||||
}
|
||||
|
||||
feature = _feature;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
//plane is in the shape space of polyData
|
||||
static void buildPartialHull(const PolygonalData& polyData, const SupportLocal* map, SeparatingAxes& validAxes, const Vec3VArg planeP, const Vec3VArg planeDir)
|
||||
{
|
||||
const FloatV zero = FZero();
|
||||
const Vec3V dir = V3Normalize(planeDir);
|
||||
for(PxU32 i=0; i<polyData.mNbPolygons; ++i)
|
||||
{
|
||||
const HullPolygonData& polygon = polyData.mPolygons[i];
|
||||
const PxU8* inds = polyData.mPolygonVertexRefs + polygon.mVRef8;
|
||||
|
||||
Vec3V v0 = M33MulV3(map->vertex2Shape, V3LoadU_SafeReadW(polyData.mVerts[inds[polygon.mNbVerts - 1]])); // PT: safe because of the way vertex memory is allocated in ConvexHullData
|
||||
|
||||
FloatV dist0 = V3Dot(dir, V3Sub(v0, planeP));
|
||||
|
||||
for (PxU32 iStart = 0; iStart < polygon.mNbVerts; iStart++)
|
||||
{
|
||||
const Vec3V v1 = M33MulV3(map->vertex2Shape, V3LoadU_SafeReadW(polyData.mVerts[inds[iStart]])); // PT: safe because of the way vertex memory is allocated in ConvexHullData
|
||||
|
||||
const FloatV dist1 = V3Dot(dir, V3Sub(v1, planeP));
|
||||
|
||||
const BoolV con = BOr(FIsGrtr(dist0, zero), FIsGrtr(dist1, zero));
|
||||
|
||||
//cull edge if either of the vertex will on the positive size of the plane
|
||||
if(BAllEqTTTT(con))
|
||||
{
|
||||
const Vec3V tempV = V3Sub(v0, v1);
|
||||
PxVec3 temp;
|
||||
V3StoreU(tempV, temp);
|
||||
validAxes.addAxis(temp.getNormalized());
|
||||
}
|
||||
|
||||
v0 = v1;
|
||||
dist0 = dist1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static bool testEdgeNormal(const PolygonalData& polyData0, const PolygonalData& polyData1, const SupportLocal* map0, const SupportLocal* map1, const PxMatTransformV& transform0To1, const PxMatTransformV& transform1To0, const FloatVArg contactDist,
|
||||
FloatV& minOverlap, Vec3V& edgeNormalIn0, FeatureStatus edgeStatus, FeatureStatus& status)
|
||||
{
|
||||
FloatV overlap = minOverlap;
|
||||
FloatV min0, max0;
|
||||
FloatV min1, max1;
|
||||
const FloatV eps = FEps();
|
||||
|
||||
const Vec3V shapeSpaceCenter0 = V3LoadU(polyData0.mCenter);
|
||||
const Vec3V shapeSpaceCenter1 = V3LoadU(polyData1.mCenter);
|
||||
|
||||
#if PCM_USE_INTERNAL_OBJECT
|
||||
const Vec3V zeroV = V3Zero();
|
||||
const Vec3V internalCenter1In0 = V3Sub(transform1To0.transform(shapeSpaceCenter1), shapeSpaceCenter0);
|
||||
const FloatV internalRadius1 = FLoad(polyData1.mInternal.mInternalRadius);
|
||||
const Vec3V internalExtents1 = V3LoadU_SafeReadW(polyData1.mInternal.mInternalExtents);
|
||||
const Vec3V negInternalExtents1 = V3Neg(internalExtents1);
|
||||
|
||||
const FloatV internalRadius0 = FLoad(polyData0.mInternal.mInternalRadius);
|
||||
const Vec3V internalExtents0 = V3LoadU_SafeReadW(polyData0.mInternal.mInternalExtents);
|
||||
const Vec3V negInternalExtents0 = V3Neg(internalExtents0);
|
||||
#endif
|
||||
|
||||
const Vec3V center1To0 = transform1To0.p;
|
||||
|
||||
//in polyData0 shape space
|
||||
const Vec3V dir0 = V3Sub(transform1To0.transform(shapeSpaceCenter1), shapeSpaceCenter0);
|
||||
const Vec3V support0 = map0->doSupport(dir0);
|
||||
|
||||
//in polyData1 shape space
|
||||
const Vec3V dir1 = transform0To1.rotate(V3Neg(dir0));
|
||||
const Vec3V support1 = map1->doSupport(dir1);
|
||||
|
||||
const Vec3V support0In1 = transform0To1.transform(support0);
|
||||
const Vec3V support1In0 = transform1To0.transform(support1);
|
||||
|
||||
SeparatingAxes mSA0;
|
||||
SeparatingAxes mSA1;
|
||||
mSA0.reset();
|
||||
mSA1.reset();
|
||||
|
||||
buildPartialHull(polyData0, map0, mSA0, support1In0, dir0);
|
||||
buildPartialHull(polyData1, map1, mSA1, support0In1, dir1);
|
||||
|
||||
const PxVec3* axe0 = mSA0.getAxes();
|
||||
const PxVec3* axe1 = mSA1.getAxes();
|
||||
const PxU32 numAxe0 = mSA0.getNumAxes();
|
||||
const PxU32 numAxe1 = mSA1.getNumAxes();
|
||||
for(PxU32 i=0; i < numAxe0; ++i)
|
||||
{
|
||||
//axe0[i] is in the shape space of polyData0
|
||||
const Vec3V v0 = V3LoadU(axe0[i]);
|
||||
|
||||
for(PxU32 j=0; j< numAxe1; ++j)
|
||||
{
|
||||
//axe1[j] is in the shape space of polyData1
|
||||
const Vec3V v1 = V3LoadU(axe1[j]);
|
||||
|
||||
const Vec3V dir = V3Cross(v0, transform1To0.rotate(v1));
|
||||
const FloatV lenSq = V3Dot(dir, dir);
|
||||
if(FAllGrtr(eps, lenSq))
|
||||
continue;
|
||||
|
||||
//n0 is in polyData0's local space
|
||||
const Vec3V n0 = V3Scale(dir, FRsqrt(lenSq));
|
||||
|
||||
//n1 is in polyData1's local space
|
||||
const Vec3V n1 = transform0To1.rotate(n0);
|
||||
|
||||
#if PCM_USE_INTERNAL_OBJECT
|
||||
//ML: we don't need to transform the normal into the vertex space. If polyData1 don't have scale,
|
||||
//the vertex2Shape matrix will be identity, shape space normal will be the same as vertex space's normal.
|
||||
//If polyData0 have scale, internalExtens1 will be 0.
|
||||
//vertex space n1
|
||||
const Vec3V proj = V3Sel(V3IsGrtr(n1, zeroV), internalExtents1, negInternalExtents1);
|
||||
const FloatV radius = FMax(V3Dot(n1, proj), internalRadius1);
|
||||
|
||||
const FloatV internalTrans = V3Dot(internalCenter1In0, n0);
|
||||
|
||||
const FloatV _min1 = FSub(internalTrans, radius);
|
||||
const FloatV _max1 = FAdd(internalTrans, radius);
|
||||
|
||||
const Vec3V proj0 = V3Sel(V3IsGrtr(n0, zeroV), internalExtents0, negInternalExtents0);
|
||||
const FloatV radius0 = FMax(V3Dot(n0, proj0), internalRadius0);
|
||||
|
||||
const FloatV _max0 = radius0;
|
||||
const FloatV _min0 = FNeg(radius0);
|
||||
|
||||
PX_ASSERT(FAllGrtrOrEq(_max0, _min0));
|
||||
PX_ASSERT(FAllGrtrOrEq(_max1, _min1));
|
||||
|
||||
const FloatV _min = FMax(_min0, _min1);
|
||||
const FloatV _max = FMin(_max0, _max1);
|
||||
|
||||
const FloatV _tempOverlap = FSub(_max, _min);
|
||||
|
||||
//Internal object overlaps more than current min, so can skip it
|
||||
//because (a) it isn't a separating axis and (b) it isn't the smallest axis
|
||||
if(FAllGrtr(_tempOverlap, overlap))
|
||||
continue;
|
||||
#endif
|
||||
//get polyData0's projection
|
||||
map0->doSupport(n0, min0, max0);
|
||||
|
||||
const FloatV translate = V3Dot(center1To0, n0);
|
||||
|
||||
//get polyData1's projection
|
||||
map1->doSupport(n1, min1, max1);
|
||||
|
||||
min1 = FAdd(translate, min1);
|
||||
max1 = FAdd(translate, max1);
|
||||
|
||||
const BoolV con = BOr(FIsGrtr(min1, FAdd(max0, contactDist)), FIsGrtr(min0, FAdd(max1, contactDist)));
|
||||
if(BAllEqTTTT(con))
|
||||
return false;
|
||||
|
||||
const FloatV tempOverlap = FSub(max0, min1);
|
||||
|
||||
#if PCM_USE_INTERNAL_OBJECT
|
||||
PX_ASSERT(FAllGrtrOrEq(tempOverlap, _tempOverlap));
|
||||
#endif
|
||||
if(FAllGrtr(overlap, tempOverlap))
|
||||
{
|
||||
overlap = tempOverlap;
|
||||
edgeNormalIn0 = n0;
|
||||
status = edgeStatus;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
minOverlap = overlap;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
//contactNormal is in the space of polyData0
|
||||
static void generatedContacts(const PolygonalData& polyData0, const PolygonalData& polyData1, const HullPolygonData& referencePolygon, const HullPolygonData& incidentPolygon,
|
||||
const SupportLocal* map0, const SupportLocal* map1, const PxMatTransformV& transform0To1, PersistentContact* manifoldContacts,
|
||||
PxU32& numContacts, const FloatVArg contactDist, PxRenderOutput* renderOutput)
|
||||
{
|
||||
PX_UNUSED(renderOutput);
|
||||
const FloatV zero = FZero();
|
||||
|
||||
const PxU8* inds0 = polyData0.mPolygonVertexRefs + referencePolygon.mVRef8;
|
||||
|
||||
//transform the plane normal to shape space
|
||||
const Vec3V contactNormal = V3Normalize(M33TrnspsMulV3(map0->shape2Vertex, V3LoadU(referencePolygon.mPlane.n)));
|
||||
|
||||
//this is the matrix transform all points to the 2d plane
|
||||
const Mat33V rot = findRotationMatrixFromZAxis(contactNormal);
|
||||
|
||||
const PxU8* inds1 = polyData1.mPolygonVertexRefs + incidentPolygon.mVRef8;
|
||||
|
||||
Vec3V* points0In0 = reinterpret_cast<Vec3V*>(PxAllocaAligned(sizeof(Vec3V)*referencePolygon.mNbVerts, 16));
|
||||
Vec3V* points1In0 = reinterpret_cast<Vec3V*>(PxAllocaAligned(sizeof(Vec3V)*incidentPolygon.mNbVerts, 16));
|
||||
bool* points1In0Penetration = reinterpret_cast<bool*>(PxAlloca(sizeof(bool)*incidentPolygon.mNbVerts));
|
||||
FloatV* points1In0TValue = reinterpret_cast<FloatV*>(PxAllocaAligned(sizeof(FloatV)*incidentPolygon.mNbVerts, 16));
|
||||
|
||||
//Transform all the verts from vertex space to shape space
|
||||
map0->populateVerts(inds0, referencePolygon.mNbVerts, polyData0.mVerts, points0In0);
|
||||
map1->populateVerts(inds1, incidentPolygon.mNbVerts, polyData1.mVerts, points1In0);
|
||||
|
||||
#if PCM_LOW_LEVEL_DEBUG
|
||||
PersistentContactManifold::drawPolygon(*renderOutput, map0->transform, points0In0, (PxU32)referencePolygon.mNbVerts, 0x00ff0000);
|
||||
PersistentContactManifold::drawPolygon(*renderOutput, map1->transform, points1In0, (PxU32)incidentPolygon.mNbVerts, 0x0000ff00);
|
||||
#endif
|
||||
|
||||
//This is used to calculate the project point when the 2D reference face points is inside the 2D incident face point
|
||||
const Vec3V sPoint = points1In0[0];
|
||||
|
||||
PX_ASSERT(incidentPolygon.mNbVerts <= 64);
|
||||
|
||||
Vec3V eps = Vec3V_From_FloatV(FEps());
|
||||
Vec3V max = Vec3V_From_FloatV(FMax());
|
||||
Vec3V nmax = V3Neg(max);
|
||||
|
||||
//transform reference polygon to 2d, calculate min and max
|
||||
Vec3V rPolygonMin = max;
|
||||
Vec3V rPolygonMax = nmax;
|
||||
for(PxU32 i=0; i<referencePolygon.mNbVerts; ++i)
|
||||
{
|
||||
points0In0[i] = M33MulV3(rot, points0In0[i]);
|
||||
rPolygonMin = V3Min(rPolygonMin, points0In0[i]);
|
||||
rPolygonMax = V3Max(rPolygonMax, points0In0[i]);
|
||||
}
|
||||
|
||||
rPolygonMin = V3Sub(rPolygonMin, eps);
|
||||
rPolygonMax = V3Add(rPolygonMax, eps);
|
||||
|
||||
const FloatV d = V3GetZ(points0In0[0]);
|
||||
const FloatV rd = FAdd(d, contactDist);
|
||||
|
||||
Vec3V iPolygonMin = max;
|
||||
Vec3V iPolygonMax = nmax;
|
||||
|
||||
PxU32 inside = 0;
|
||||
for(PxU32 i=0; i<incidentPolygon.mNbVerts; ++i)
|
||||
{
|
||||
const Vec3V vert1 = points1In0[i]; //this still in polyData1's local space
|
||||
const Vec3V a = transform0To1.transformInv(vert1);
|
||||
points1In0[i] = M33MulV3(rot, a);
|
||||
const FloatV z = V3GetZ(points1In0[i]);
|
||||
points1In0TValue[i] = FSub(z, d);
|
||||
points1In0[i] = V3SetZ(points1In0[i], d);
|
||||
iPolygonMin = V3Min(iPolygonMin, points1In0[i]);
|
||||
iPolygonMax = V3Max(iPolygonMax, points1In0[i]);
|
||||
if(FAllGrtr(rd, z))
|
||||
{
|
||||
points1In0Penetration[i] = true;
|
||||
|
||||
if(contains(points0In0, referencePolygon.mNbVerts, points1In0[i], rPolygonMin, rPolygonMax))
|
||||
{
|
||||
inside++;
|
||||
|
||||
if (numContacts == PxContactBuffer::MAX_CONTACTS)
|
||||
return;
|
||||
|
||||
const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(contactNormal), points1In0TValue[i]);
|
||||
manifoldContacts[numContacts].mLocalPointA = vert1;
|
||||
manifoldContacts[numContacts].mLocalPointB = M33TrnspsMulV3(rot, points1In0[i]);
|
||||
manifoldContacts[numContacts++].mLocalNormalPen = localNormalPen;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
points1In0Penetration[i] = false;
|
||||
}
|
||||
}
|
||||
|
||||
if(inside == incidentPolygon.mNbVerts)
|
||||
return;
|
||||
|
||||
inside = 0;
|
||||
iPolygonMin = V3Sub(iPolygonMin, eps);
|
||||
iPolygonMax = V3Add(iPolygonMax, eps);
|
||||
|
||||
const Vec3V incidentNormal = V3Normalize(M33TrnspsMulV3(map1->shape2Vertex, V3LoadU(incidentPolygon.mPlane.n)));
|
||||
|
||||
const Vec3V contactNormalIn1 = transform0To1.rotate(contactNormal);
|
||||
|
||||
for(PxU32 i=0; i<referencePolygon.mNbVerts; ++i)
|
||||
{
|
||||
if(contains(points1In0, incidentPolygon.mNbVerts, points0In0[i], iPolygonMin, iPolygonMax))
|
||||
{
|
||||
//const Vec3V vert0=Vec3V_From_PxVec3(polyData0.mVerts[inds0[i]]);
|
||||
const Vec3V vert0 = M33TrnspsMulV3(rot, points0In0[i]);
|
||||
const Vec3V a = transform0To1.transform(vert0);
|
||||
|
||||
const FloatV nom = V3Dot(incidentNormal, V3Sub(sPoint, a));
|
||||
const FloatV denom = V3Dot(incidentNormal, contactNormalIn1);
|
||||
PX_ASSERT(FAllEq(denom, zero)==0);
|
||||
const FloatV t = FDiv(nom, denom);
|
||||
|
||||
if(FAllGrtr(t, contactDist))
|
||||
continue;
|
||||
|
||||
inside++;
|
||||
|
||||
if (numContacts == PxContactBuffer::MAX_CONTACTS)
|
||||
return;
|
||||
|
||||
const Vec3V projPoint = V3ScaleAdd(contactNormalIn1, t, a);
|
||||
const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(contactNormal), t);
|
||||
|
||||
manifoldContacts[numContacts].mLocalPointA = projPoint;
|
||||
manifoldContacts[numContacts].mLocalPointB = vert0;
|
||||
manifoldContacts[numContacts++].mLocalNormalPen = localNormalPen;
|
||||
}
|
||||
}
|
||||
|
||||
if(inside == referencePolygon.mNbVerts)
|
||||
return;
|
||||
|
||||
//(2) segment intersection
|
||||
for (PxU32 iStart = 0, iEnd = PxU32(incidentPolygon.mNbVerts - 1); iStart < incidentPolygon.mNbVerts; iEnd = iStart++)
|
||||
{
|
||||
if((!points1In0Penetration[iStart] && !points1In0Penetration[iEnd] ) )//|| (points1In0[i].status == POINT_OUTSIDE && points1In0[incidentIndex].status == POINT_OUTSIDE))
|
||||
continue;
|
||||
|
||||
const Vec3V ipA = points1In0[iStart];
|
||||
const Vec3V ipB = points1In0[iEnd];
|
||||
|
||||
Vec3V ipAOri = V3SetZ(points1In0[iStart], FAdd(points1In0TValue[iStart], d));
|
||||
Vec3V ipBOri = V3SetZ(points1In0[iEnd], FAdd(points1In0TValue[iEnd], d));
|
||||
|
||||
const Vec3V iMin = V3Min(ipA, ipB);
|
||||
const Vec3V iMax = V3Max(ipA, ipB);
|
||||
|
||||
for (PxU32 rStart = 0, rEnd = PxU32(referencePolygon.mNbVerts - 1); rStart < referencePolygon.mNbVerts; rEnd = rStart++)
|
||||
{
|
||||
const Vec3V rpA = points0In0[rStart];
|
||||
const Vec3V rpB = points0In0[rEnd];
|
||||
|
||||
const Vec3V rMin = V3Min(rpA, rpB);
|
||||
const Vec3V rMax = V3Max(rpA, rpB);
|
||||
|
||||
const BoolV tempCon = BOr(V3IsGrtr(iMin, rMax), V3IsGrtr(rMin, iMax));
|
||||
const BoolV con = BOr(BGetX(tempCon), BGetY(tempCon));
|
||||
|
||||
if(BAllEqTTTT(con))
|
||||
continue;
|
||||
|
||||
FloatV a1 = signed2DTriArea(rpA, rpB, ipA);
|
||||
FloatV a2 = signed2DTriArea(rpA, rpB, ipB);
|
||||
|
||||
if(FAllGrtr(zero, FMul(a1, a2)))
|
||||
{
|
||||
FloatV a3 = signed2DTriArea(ipA, ipB, rpA);
|
||||
FloatV a4 = signed2DTriArea(ipA, ipB, rpB);
|
||||
|
||||
if(FAllGrtr(zero, FMul(a3, a4)))
|
||||
{
|
||||
//these two segment intersect
|
||||
|
||||
const FloatV t = FDiv(a1, FSub(a2, a1));
|
||||
|
||||
const Vec3V pBB = V3NegScaleSub(V3Sub(ipBOri, ipAOri), t, ipAOri);
|
||||
const Vec3V pAA = V3SetZ(pBB, d);
|
||||
const Vec3V pA = M33TrnspsMulV3(rot, pAA);
|
||||
const Vec3V pB = transform0To1.transform(M33TrnspsMulV3(rot, pBB));
|
||||
const FloatV pen = FSub(V3GetZ(pBB), V3GetZ(pAA));
|
||||
if(FAllGrtr(pen, contactDist))
|
||||
continue;
|
||||
|
||||
if (numContacts == PxContactBuffer::MAX_CONTACTS)
|
||||
return;
|
||||
|
||||
const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(contactNormal), pen);
|
||||
manifoldContacts[numContacts].mLocalPointA = pB;
|
||||
manifoldContacts[numContacts].mLocalPointB = pA;
|
||||
manifoldContacts[numContacts++].mLocalNormalPen = localNormalPen;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool Gu::generateFullContactManifold(const PolygonalData& polyData0, const PolygonalData& polyData1, const SupportLocal* map0, const SupportLocal* map1, PersistentContact* manifoldContacts, PxU32& numContacts,
|
||||
const FloatVArg contactDist, const Vec3VArg normal, const Vec3VArg closestA, const Vec3VArg closestB, PxReal marginA, PxReal marginB, bool doOverlapTest,
|
||||
PxRenderOutput* renderOutput, PxReal toleranceLength)
|
||||
{
|
||||
const PxMatTransformV transform1To0V = map0->transform.transformInv(map1->transform);
|
||||
const PxMatTransformV transform0To1V = map1->transform.transformInv(map0->transform);
|
||||
|
||||
if(doOverlapTest)
|
||||
{
|
||||
//if gjk fail, SAT based yes/no test
|
||||
FeatureStatus status = POLYDATA0;
|
||||
FloatV minOverlap = FMax();
|
||||
Vec3V minNormal = V3Zero();
|
||||
|
||||
PxU32 feature0;
|
||||
//in the local space of polyData0, minNormal is in polyData0 space
|
||||
if(!testFaceNormal(polyData0, polyData1, map0, map1, transform0To1V, transform1To0V, contactDist, minOverlap, feature0, minNormal, POLYDATA0, status))
|
||||
return false;
|
||||
|
||||
PxU32 feature1;
|
||||
//in the local space of polyData1, if minOverlap is overwrite inside this function, minNormal will be in polyData1 space
|
||||
if(!testFaceNormal(polyData1, polyData0, map1, map0, transform1To0V, transform0To1V, contactDist, minOverlap, feature1, minNormal, POLYDATA1, status))
|
||||
return false;
|
||||
|
||||
bool doEdgeTest = false;
|
||||
|
||||
EdgeTest:
|
||||
if(doEdgeTest)
|
||||
{
|
||||
if(!testEdgeNormal(polyData0, polyData1, map0, map1, transform0To1V, transform1To0V, contactDist, minOverlap, minNormal, EDGE, status))
|
||||
return false;
|
||||
|
||||
if(status != EDGE)
|
||||
return true;
|
||||
}
|
||||
|
||||
if(status == POLYDATA0)
|
||||
{
|
||||
//minNormal is in the local space of polydata0
|
||||
|
||||
const HullPolygonData& referencePolygon = polyData0.mPolygons[feature0];
|
||||
const Vec3V n = transform0To1V.rotate(minNormal);
|
||||
const HullPolygonData& incidentPolygon = polyData1.mPolygons[getPolygonIndex(polyData1, map1, n)];
|
||||
|
||||
generatedContacts(polyData0, polyData1, referencePolygon, incidentPolygon, map0, map1, transform0To1V, manifoldContacts, numContacts, contactDist, renderOutput);
|
||||
|
||||
if (numContacts > 0)
|
||||
{
|
||||
const Vec3V nn = V3Neg(n);
|
||||
//flip the contacts
|
||||
for(PxU32 i=0; i<numContacts; ++i)
|
||||
{
|
||||
const Vec3V localPointB = manifoldContacts[i].mLocalPointB;
|
||||
manifoldContacts[i].mLocalPointB = manifoldContacts[i].mLocalPointA;
|
||||
manifoldContacts[i].mLocalPointA = localPointB;
|
||||
manifoldContacts[i].mLocalNormalPen = V4SetW(nn, V4GetW(manifoldContacts[i].mLocalNormalPen));
|
||||
}
|
||||
}
|
||||
}
|
||||
else if(status == POLYDATA1)
|
||||
{
|
||||
//minNormal is in the local space of polydata1
|
||||
const HullPolygonData& referencePolygon = polyData1.mPolygons[feature1];
|
||||
const HullPolygonData& incidentPolygon = polyData0.mPolygons[getPolygonIndex(polyData0, map0, transform1To0V.rotate(minNormal))];
|
||||
|
||||
//reference face is polyData1
|
||||
generatedContacts(polyData1, polyData0, referencePolygon, incidentPolygon, map1, map0, transform1To0V, manifoldContacts, numContacts, contactDist, renderOutput);
|
||||
|
||||
}
|
||||
else //if(status == EDGE0)
|
||||
{
|
||||
//minNormal is in the local space of polydata0
|
||||
|
||||
const HullPolygonData& incidentPolygon = polyData0.mPolygons[getPolygonIndex(polyData0, map0, V3Neg(minNormal))];
|
||||
const HullPolygonData& referencePolygon = polyData1.mPolygons[getPolygonIndex(polyData1, map1, transform0To1V.rotate(minNormal))];
|
||||
generatedContacts(polyData1, polyData0, referencePolygon, incidentPolygon, map1, map0, transform1To0V, manifoldContacts, numContacts, contactDist, renderOutput);
|
||||
}
|
||||
|
||||
if(numContacts == 0 && !doEdgeTest)
|
||||
{
|
||||
doEdgeTest = true;
|
||||
goto EdgeTest;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
const PxReal lowerEps = toleranceLength * PCM_WITNESS_POINT_LOWER_EPS;
|
||||
const PxReal upperEps = toleranceLength * PCM_WITNESS_POINT_UPPER_EPS;
|
||||
const PxReal toleranceA = PxClamp(marginA, lowerEps, upperEps);
|
||||
const PxReal toleranceB = PxClamp(marginB, lowerEps, upperEps);
|
||||
|
||||
const Vec3V negNormal = V3Neg(normal);
|
||||
const Vec3V normalIn0 = transform0To1V.rotateInv(normal);
|
||||
|
||||
PxU32 faceIndex1 = getWitnessPolygonIndex(polyData1, map1, negNormal, closestB, toleranceB);
|
||||
PxU32 faceIndex0 = getWitnessPolygonIndex(polyData0, map0, normalIn0, transform0To1V.transformInv(closestA),
|
||||
toleranceA);
|
||||
|
||||
const HullPolygonData& referencePolygon = polyData1.mPolygons[faceIndex1];
|
||||
const HullPolygonData& incidentPolygon = polyData0.mPolygons[faceIndex0];
|
||||
|
||||
const Vec3V referenceNormal = V3Normalize(M33TrnspsMulV3(map1->shape2Vertex, V3LoadU(referencePolygon.mPlane.n)));
|
||||
const Vec3V incidentNormal = V3Normalize(M33TrnspsMulV3(map0->shape2Vertex, V3LoadU(incidentPolygon.mPlane.n)));
|
||||
|
||||
const FloatV referenceProject = FAbs(V3Dot(referenceNormal, negNormal));
|
||||
const FloatV incidentProject = FAbs(V3Dot(incidentNormal, normalIn0));
|
||||
|
||||
if (FAllGrtrOrEq(referenceProject, incidentProject))
|
||||
{
|
||||
generatedContacts(polyData1, polyData0, referencePolygon, incidentPolygon, map1, map0, transform1To0V, manifoldContacts, numContacts, contactDist, renderOutput);
|
||||
}
|
||||
else
|
||||
{
|
||||
generatedContacts(polyData0, polyData1, incidentPolygon, referencePolygon, map0, map1, transform0To1V, manifoldContacts, numContacts, contactDist, renderOutput);
|
||||
|
||||
if (numContacts > 0)
|
||||
{
|
||||
const Vec3V n = transform0To1V.rotate(incidentNormal);
|
||||
|
||||
const Vec3V nn = V3Neg(n);
|
||||
//flip the contacts
|
||||
for (PxU32 i = 0; i<numContacts; ++i)
|
||||
{
|
||||
const Vec3V localPointB = manifoldContacts[i].mLocalPointB;
|
||||
manifoldContacts[i].mLocalPointB = manifoldContacts[i].mLocalPointA;
|
||||
manifoldContacts[i].mLocalPointA = localPointB;
|
||||
manifoldContacts[i].mLocalNormalPen = V4SetW(nn, V4GetW(manifoldContacts[i].mLocalNormalPen));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
//This function called by box/convex and convex/convex pcm function
|
||||
bool Gu::addGJKEPAContacts(const GjkConvex* relativeConvex, const GjkConvex* localConvex, const PxMatTransformV& aToB, GjkStatus status,
|
||||
PersistentContact* manifoldContacts, const FloatV replaceBreakingThreshold, const FloatV tolerenceLength, GjkOutput& output,
|
||||
PersistentContactManifold& manifold)
|
||||
{
|
||||
bool doOverlapTest = false;
|
||||
if (status == GJK_DEGENERATE)
|
||||
{
|
||||
const Vec3V normal = output.normal;
|
||||
const FloatV costheta = V3Dot(output.searchDir, normal);
|
||||
if (FAllGrtr(costheta, FLoad(0.9999f)))
|
||||
{
|
||||
const Vec3V centreA = relativeConvex->getCenter();
|
||||
const Vec3V centreB = localConvex->getCenter();
|
||||
|
||||
const Vec3V dir = V3Normalize(V3Sub(centreA, centreB));
|
||||
const FloatV theta = V3Dot(dir, normal);
|
||||
|
||||
if (FAllGrtr(theta, FLoad(0.707f)))
|
||||
{
|
||||
addManifoldPoint(manifoldContacts, manifold, output, aToB, replaceBreakingThreshold);
|
||||
}
|
||||
else
|
||||
{
|
||||
doOverlapTest = true;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
doOverlapTest = true;
|
||||
}
|
||||
}
|
||||
else if (status == GJK_CONTACT)
|
||||
{
|
||||
addManifoldPoint(manifoldContacts, manifold, output, aToB, replaceBreakingThreshold);
|
||||
}
|
||||
else
|
||||
{
|
||||
PX_ASSERT(status == EPA_CONTACT);
|
||||
|
||||
status = epaPenetration(*relativeConvex, *localConvex, manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints,
|
||||
true, tolerenceLength, output);
|
||||
if (status == EPA_CONTACT)
|
||||
{
|
||||
addManifoldPoint(manifoldContacts, manifold, output, aToB, replaceBreakingThreshold);
|
||||
}
|
||||
else
|
||||
{
|
||||
doOverlapTest = true;
|
||||
}
|
||||
}
|
||||
|
||||
return doOverlapTest;
|
||||
}
|
||||
|
||||
bool Gu::computeMTD(const PolygonalData& polyData0, const PolygonalData& polyData1, const SupportLocal* map0, const SupportLocal* map1, FloatV& penDepth, Vec3V& normal)
|
||||
{
|
||||
using namespace aos;
|
||||
|
||||
const PxMatTransformV transform1To0V = map0->transform.transformInv(map1->transform);
|
||||
const PxMatTransformV transform0To1V = map1->transform.transformInv(map0->transform);
|
||||
|
||||
FeatureStatus status = POLYDATA0;
|
||||
FloatV minOverlap = FMax();
|
||||
Vec3V minNormal = V3Zero();
|
||||
const FloatV contactDist = FZero();
|
||||
|
||||
PxU32 feature0;
|
||||
//in the local space of polyData0, minNormal is in polyData0 space
|
||||
if(!testFaceNormal(polyData0, polyData1, map0, map1, transform0To1V, transform1To0V, contactDist, minOverlap, feature0, minNormal, POLYDATA0, status))
|
||||
return false;
|
||||
|
||||
PxU32 feature1;
|
||||
//in the local space of polyData1, if minOverlap is overwrite inside this function, minNormal will be in polyData1 space
|
||||
if(!testFaceNormal(polyData1, polyData0, map1, map0, transform1To0V, transform0To1V, contactDist, minOverlap, feature1, minNormal, POLYDATA1, status))
|
||||
return false;
|
||||
|
||||
if(!testEdgeNormal(polyData0, polyData1, map0, map1, transform0To1V, transform1To0V, contactDist, minOverlap, minNormal, EDGE, status))
|
||||
return false;
|
||||
|
||||
penDepth = minOverlap;
|
||||
if(status == POLYDATA1)
|
||||
{
|
||||
//minNormal is in the local space of polydata1
|
||||
normal = map1->transform.rotate(minNormal);
|
||||
}
|
||||
else
|
||||
{
|
||||
PX_ASSERT(status == POLYDATA0 || status == EDGE);
|
||||
//ML: status == POLYDATA0 or status == EDGE, minNormal is in the local space of polydata0
|
||||
normal = V3Neg(map0->transform.rotate(minNormal));
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
512
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactGenSphereCapsule.cpp
vendored
Normal file
512
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactGenSphereCapsule.cpp
vendored
Normal file
@@ -0,0 +1,512 @@
|
||||
// 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 "GuPCMContactGen.h"
|
||||
#include "GuPCMShapeConvex.h"
|
||||
#include "common/PxRenderOutput.h"
|
||||
#include "GuPCMContactGenUtil.h"
|
||||
#include "foundation/PxVecMath.h"
|
||||
#include "foundation/PxAlloca.h"
|
||||
#include "GuVecCapsule.h"
|
||||
#include "GuVecBox.h"
|
||||
|
||||
using namespace physx;
|
||||
using namespace Gu;
|
||||
using namespace aos;
|
||||
|
||||
//ML: this function is shared by the sphere/capsule vs convex hulls full contact gen, capsule in the local space of polyData
|
||||
static bool testPolyDataAxis(const CapsuleV& capsule, const PolygonalData& polyData, const SupportLocal* map, const FloatVArg contactDist, FloatV& minOverlap, Vec3V& separatingAxis)
|
||||
{
|
||||
FloatV _minOverlap = FMax();//minOverlap;
|
||||
FloatV min0, max0;
|
||||
FloatV min1, max1;
|
||||
Vec3V tempAxis = V3UnitY();
|
||||
|
||||
//capsule in the local space of polyData
|
||||
for(PxU32 i=0; i<polyData.mNbPolygons; ++i)
|
||||
{
|
||||
const HullPolygonData& polygon = polyData.mPolygons[i];
|
||||
|
||||
const Vec3V minVert = V3LoadU_SafeReadW(polyData.mVerts[polygon.mMinIndex]); // PT: safe because of the way vertex memory is allocated in ConvexHullData
|
||||
const FloatV planeDist = FLoad(polygon.mPlane.d);
|
||||
const Vec3V vertexSpacePlaneNormal = V3LoadU_SafeReadW(polygon.mPlane.n); // PT: safe because 'd' follows 'n' in the plane class
|
||||
|
||||
//transform plane n to shape space
|
||||
const Vec3V shapeSpacePlaneNormal = M33TrnspsMulV3(map->shape2Vertex, vertexSpacePlaneNormal);
|
||||
|
||||
const FloatV magnitude = FRecip(V3Length(shapeSpacePlaneNormal));
|
||||
//normalize shape space normal
|
||||
const Vec3V planeN = V3Scale(shapeSpacePlaneNormal, magnitude);
|
||||
//ML::use this to avoid LHS
|
||||
min0 = FMul(V3Dot(vertexSpacePlaneNormal, minVert), magnitude);
|
||||
max0 = FMul(FNeg(planeDist), magnitude);
|
||||
|
||||
const FloatV tempMin = V3Dot(capsule.p0, planeN);
|
||||
const FloatV tempMax = V3Dot(capsule.p1, planeN);
|
||||
min1 = FMin(tempMin, tempMax);
|
||||
max1 = FMax(tempMin, tempMax);
|
||||
|
||||
min1 = FSub(min1, capsule.radius);
|
||||
max1 = FAdd(max1, capsule.radius);
|
||||
|
||||
const BoolV con = BOr(FIsGrtr(min1, FAdd(max0, contactDist)), FIsGrtr(min0, FAdd(max1, contactDist)));
|
||||
|
||||
if(BAllEqTTTT(con))
|
||||
return false;
|
||||
|
||||
const FloatV tempOverlap = FSub(max0, min1);
|
||||
|
||||
if(FAllGrtr(_minOverlap, tempOverlap))
|
||||
{
|
||||
_minOverlap = tempOverlap;
|
||||
tempAxis = planeN;
|
||||
}
|
||||
}
|
||||
|
||||
separatingAxis = tempAxis;
|
||||
minOverlap = _minOverlap;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
//ML: this function is shared by sphere/capsule. Point a and direction d need to be in the local space of polyData
|
||||
static bool intersectRayPolyhedron(const Vec3VArg a, const Vec3VArg dir, const PolygonalData& polyData, FloatV& tEnter, FloatV& tExit)
|
||||
{
|
||||
const FloatV zero = FZero();
|
||||
const FloatV eps = FLoad(1e-7f);
|
||||
FloatV tFirst = zero;
|
||||
FloatV tLast = FMax();
|
||||
|
||||
for(PxU32 k=0; k<polyData.mNbPolygons; ++k)
|
||||
{
|
||||
const HullPolygonData& data = polyData.mPolygons[k];
|
||||
const Vec3V n = V3LoadU_SafeReadW(data.mPlane.n); // PT: safe because 'd' follows 'n' in the plane class
|
||||
|
||||
FloatV d = FLoad(data.mPlane.d);
|
||||
|
||||
const FloatV denominator = V3Dot(n, dir);
|
||||
const FloatV distToPlane = FAdd(V3Dot(n, a), d);
|
||||
|
||||
if(FAllGrtr(eps, FAbs(denominator)))
|
||||
{
|
||||
if(FAllGrtr(distToPlane, zero))
|
||||
return false;
|
||||
}
|
||||
else
|
||||
{
|
||||
FloatV tTemp = FNeg(FDiv(distToPlane, denominator));
|
||||
|
||||
//ML: denominator < 0 means the ray is entering halfspace; denominator > 0 means the ray is exiting halfspace
|
||||
const BoolV con = FIsGrtr(zero, denominator);
|
||||
const BoolV con0 = FIsGrtr(tTemp, tFirst);
|
||||
const BoolV con1 = FIsGrtr(tLast, tTemp);
|
||||
|
||||
tFirst = FSel(BAnd(con, con0), tTemp, tFirst);
|
||||
tLast = FSel(BAndNot(con1, con), tTemp, tLast);
|
||||
}
|
||||
|
||||
if(FAllGrtr(tFirst, tLast))
|
||||
return false;
|
||||
}
|
||||
|
||||
//calculate the intersect p in the local space
|
||||
tEnter = tFirst;
|
||||
tExit = tLast;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
//Precompute the convex data
|
||||
// 7+------+6 0 = ---
|
||||
// /| /| 1 = +--
|
||||
// / | / | 2 = ++-
|
||||
// / 4+---/--+5 3 = -+-
|
||||
// 3+------+2 / y z 4 = --+
|
||||
// | / | / | / 5 = +-+
|
||||
// |/ |/ |/ 6 = +++
|
||||
// 0+------+1 *---x 7 = -++
|
||||
|
||||
static bool testSATCapsulePoly(const CapsuleV& capsule, const PolygonalData& polyData, const SupportLocal* map, const FloatVArg contactDist, FloatV& minOverlap, Vec3V& separatingAxis)
|
||||
{
|
||||
FloatV _minOverlap = FMax();//minOverlap;
|
||||
FloatV min0, max0;
|
||||
FloatV min1, max1;
|
||||
Vec3V tempAxis = V3UnitY();
|
||||
const FloatV eps = FEps();
|
||||
|
||||
//ML: project capsule to polydata axis
|
||||
if(!testPolyDataAxis(capsule, polyData, map, contactDist, _minOverlap, tempAxis))
|
||||
return false;
|
||||
|
||||
const Vec3V capsuleAxis = V3Sub(capsule.p1, capsule.p0);
|
||||
|
||||
for(PxU32 i=0;i<polyData.mNbPolygons;i++)
|
||||
{
|
||||
const HullPolygonData& polygon = polyData.mPolygons[i];
|
||||
const PxU8* inds1 = polyData.mPolygonVertexRefs + polygon.mVRef8;
|
||||
|
||||
for(PxU32 lStart = 0, lEnd = PxU32(polygon.mNbVerts-1); lStart<polygon.mNbVerts; lEnd = lStart++)
|
||||
{
|
||||
//in the vertex space
|
||||
const Vec3V p10 = V3LoadU_SafeReadW(polyData.mVerts[inds1[lStart]]); // PT: safe because of the way vertex memory is allocated in ConvexHullData
|
||||
const Vec3V p11 = V3LoadU_SafeReadW(polyData.mVerts[inds1[lEnd]]); // PT: safe because of the way vertex memory is allocated in ConvexHullData
|
||||
|
||||
const Vec3V vertexSpaceV = V3Sub(p11, p10);
|
||||
|
||||
//transform v to shape space
|
||||
const Vec3V shapeSpaceV = M33TrnspsMulV3(map->shape2Vertex, vertexSpaceV);
|
||||
|
||||
const Vec3V dir = V3Cross(capsuleAxis, shapeSpaceV);
|
||||
const FloatV lenSq = V3Dot(dir, dir);
|
||||
if(FAllGrtr(eps, lenSq))
|
||||
continue;
|
||||
const Vec3V normal = V3ScaleInv(dir, FSqrt(lenSq));
|
||||
|
||||
map->doSupport(normal, min0, max0);
|
||||
|
||||
const FloatV tempMin = V3Dot(capsule.p0, normal);
|
||||
const FloatV tempMax = V3Dot(capsule.p1, normal);
|
||||
min1 = FMin(tempMin, tempMax);
|
||||
max1 = FMax(tempMin, tempMax);
|
||||
|
||||
min1 = FSub(min1, capsule.radius);
|
||||
max1 = FAdd(max1, capsule.radius);
|
||||
|
||||
const BoolV con = BOr(FIsGrtr(min1, FAdd(max0, contactDist)), FIsGrtr(min0, FAdd(max1, contactDist)));
|
||||
|
||||
if(BAllEqTTTT(con))
|
||||
return false;
|
||||
|
||||
const FloatV tempOverlap = FSub(max0, min1);
|
||||
|
||||
if(FAllGrtr(_minOverlap, tempOverlap))
|
||||
{
|
||||
_minOverlap = tempOverlap;
|
||||
tempAxis = normal;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
separatingAxis = tempAxis;
|
||||
minOverlap = _minOverlap;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static void generatedCapsuleBoxFaceContacts(const CapsuleV& capsule, const PolygonalData& polyData, const HullPolygonData& referencePolygon, const SupportLocal* map, const PxMatTransformV& aToB, PersistentContact* manifoldContacts, PxU32& numContacts,
|
||||
const FloatVArg contactDist, const Vec3VArg normal)
|
||||
{
|
||||
const FloatV radius = FAdd(capsule.radius, contactDist);
|
||||
|
||||
//calculate the intersect point of ray to plane
|
||||
const Vec3V planeNormal = V3Normalize(M33TrnspsMulV3(map->shape2Vertex, V3LoadU(referencePolygon.mPlane.n)));
|
||||
const PxU8* inds = polyData.mPolygonVertexRefs + referencePolygon.mVRef8;
|
||||
const Vec3V a = M33MulV3(map->vertex2Shape, V3LoadU_SafeReadW(polyData.mVerts[inds[0]])); // PT: safe because of the way vertex memory is allocated in ConvexHullData
|
||||
|
||||
const FloatV denom0 = V3Dot(planeNormal, V3Sub(capsule.p0, a));
|
||||
const FloatV denom1 = V3Dot(planeNormal, V3Sub(capsule.p1, a));
|
||||
const FloatV projPlaneN = V3Dot(planeNormal, normal);
|
||||
const FloatV numer = FSel(FIsGrtr(projPlaneN, FZero()), FRecip(projPlaneN), FZero());
|
||||
const FloatV t0 = FMul(denom0, numer);
|
||||
const FloatV t1 = FMul(denom1, numer);
|
||||
|
||||
const BoolV con0 = FIsGrtrOrEq(radius, t0);
|
||||
const BoolV con1 = FIsGrtrOrEq(radius, t1);
|
||||
if(BAllEqTTTT(BOr(con0, con1)))
|
||||
{
|
||||
const Mat33V rot = findRotationMatrixFromZAxis(planeNormal);
|
||||
Vec3V* points0In0 = reinterpret_cast<Vec3V*>(PxAllocaAligned(sizeof(Vec3V)*referencePolygon.mNbVerts, 16));
|
||||
map->populateVerts(inds, referencePolygon.mNbVerts, polyData.mVerts, points0In0);
|
||||
|
||||
Vec3V rPolygonMin = V3Splat(FMax());
|
||||
Vec3V rPolygonMax = V3Neg(rPolygonMin);
|
||||
for(PxU32 i=0; i<referencePolygon.mNbVerts; ++i)
|
||||
{
|
||||
points0In0[i] = M33MulV3(rot, points0In0[i]);
|
||||
rPolygonMin = V3Min(rPolygonMin, points0In0[i]);
|
||||
rPolygonMax = V3Max(rPolygonMax, points0In0[i]);
|
||||
}
|
||||
|
||||
if(BAllEqTTTT(con0))
|
||||
{
|
||||
//add contact
|
||||
const Vec3V proj = V3NegScaleSub(normal, t0, capsule.p0);//V3ScaleAdd(t0, normal, capsule.p0);
|
||||
//transform proj0 to 2D
|
||||
const Vec3V point = M33MulV3(rot, proj);
|
||||
|
||||
if(contains(points0In0, referencePolygon.mNbVerts, point, rPolygonMin, rPolygonMax))
|
||||
{
|
||||
manifoldContacts[numContacts].mLocalPointA = aToB.transformInv(capsule.p0);
|
||||
manifoldContacts[numContacts].mLocalPointB = proj;
|
||||
manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(normal), t0);
|
||||
}
|
||||
}
|
||||
|
||||
if(BAllEqTTTT(con1))
|
||||
{
|
||||
const Vec3V proj = V3NegScaleSub(normal, t1, capsule.p1);//V3ScaleAdd(t1, normal, capsule.p1);
|
||||
//transform proj0 to 2D
|
||||
const Vec3V point = M33MulV3(rot, proj);
|
||||
|
||||
if(contains(points0In0, referencePolygon.mNbVerts, point, rPolygonMin, rPolygonMax))
|
||||
{
|
||||
manifoldContacts[numContacts].mLocalPointA = aToB.transformInv(capsule.p1);
|
||||
manifoldContacts[numContacts].mLocalPointB = proj;
|
||||
manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(normal),t1);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void generatedFaceContacts(const CapsuleV& capsule, const PolygonalData& polyData, const SupportLocal* map, const PxMatTransformV& aToB, PersistentContact* manifoldContacts, PxU32& numContacts,
|
||||
const FloatVArg contactDist, const Vec3VArg normal)
|
||||
{
|
||||
const FloatV zero = FZero();
|
||||
FloatV tEnter = zero, tExit = zero;
|
||||
const FloatV inflatedRadius = FAdd(capsule.radius, contactDist);
|
||||
const Vec3V dir = V3Neg(normal);
|
||||
const Vec3V vertexSpaceDir = M33MulV3(map->shape2Vertex, dir);
|
||||
const Vec3V p0 = M33MulV3(map->shape2Vertex, capsule.p0);
|
||||
|
||||
if(intersectRayPolyhedron(p0, vertexSpaceDir, polyData, tEnter, tExit) && FAllGrtrOrEq(inflatedRadius, tEnter))
|
||||
{
|
||||
manifoldContacts[numContacts].mLocalPointA = aToB.transformInv(capsule.p0);
|
||||
manifoldContacts[numContacts].mLocalPointB = V3ScaleAdd(dir, tEnter, capsule.p0);
|
||||
manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(normal), tEnter);
|
||||
}
|
||||
|
||||
const Vec3V p1 = M33MulV3(map->shape2Vertex, capsule.p1);
|
||||
if(intersectRayPolyhedron(p1, vertexSpaceDir, polyData, tEnter, tExit) && FAllGrtrOrEq(inflatedRadius, tEnter))
|
||||
{
|
||||
manifoldContacts[numContacts].mLocalPointA = aToB.transformInv(capsule.p1);
|
||||
manifoldContacts[numContacts].mLocalPointB = V3ScaleAdd(dir, tEnter, capsule.p1);
|
||||
manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(normal), tEnter);
|
||||
}
|
||||
}
|
||||
|
||||
static void generateEE(const Vec3VArg p, const Vec3VArg q, const Vec3VArg normal, const Vec3VArg a, const Vec3VArg b, const PxMatTransformV& aToB,
|
||||
PersistentContact* manifoldContacts, PxU32& numContacts, const FloatVArg inflatedRadius)
|
||||
{
|
||||
const FloatV zero = FZero();
|
||||
const FloatV expandedRatio = FLoad(0.005f);
|
||||
const Vec3V ab = V3Sub(b, a);
|
||||
const Vec3V n = V3Cross(ab, normal);
|
||||
const FloatV d = V3Dot(n, a);
|
||||
const FloatV np = V3Dot(n, p);
|
||||
const FloatV nq = V3Dot(n,q);
|
||||
const FloatV signP = FSub(np, d);
|
||||
const FloatV signQ = FSub(nq, d);
|
||||
const FloatV temp = FMul(signP, signQ);
|
||||
if(FAllGrtr(temp, zero)) return;//If both points in the same side as the plane, no intersect points
|
||||
|
||||
// if colliding edge (p3,p4) and plane are parallel return no collision
|
||||
const Vec3V pq = V3Sub(q, p);
|
||||
const FloatV npq = V3Dot(n, pq);
|
||||
if(FAllEq(npq, zero)) return;
|
||||
|
||||
//calculate the intersect point in the segment pq with plane n(x - a).
|
||||
const FloatV segTValue = FDiv(FSub(d, np), npq);
|
||||
const Vec3V localPointA = V3ScaleAdd(pq, segTValue, p);
|
||||
|
||||
//ML: ab, localPointA and normal is in the same plane, so that we can do 2D segment segment intersection
|
||||
//calculate a normal perpendicular to ray localPointA + normal*t, then do 2D segment segment intersection
|
||||
const Vec3V perNormal = V3Cross(normal, pq);
|
||||
const Vec3V ap = V3Sub(localPointA, a);
|
||||
const FloatV nom = V3Dot(perNormal, ap);
|
||||
const FloatV denom = V3Dot(perNormal, ab);
|
||||
|
||||
const FloatV tValue = FDiv(nom, denom);
|
||||
|
||||
const FloatV max = FAdd(FOne(), expandedRatio);
|
||||
const FloatV min = FSub(zero, expandedRatio);
|
||||
if(FAllGrtr(tValue, max) || FAllGrtr(min, tValue))
|
||||
return;
|
||||
|
||||
const Vec3V v = V3NegScaleSub(ab, tValue, ap);
|
||||
const FloatV signedDist = V3Dot(v, normal);
|
||||
if(FAllGrtrOrEq(inflatedRadius, signedDist))
|
||||
{
|
||||
const Vec3V localPointB = V3Sub(localPointA, v);
|
||||
manifoldContacts[numContacts].mLocalPointA = aToB.transformInv(localPointA);
|
||||
manifoldContacts[numContacts].mLocalPointB = localPointB;
|
||||
manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(normal), signedDist);
|
||||
}
|
||||
}
|
||||
|
||||
static void generatedContactsEEContacts(const CapsuleV& capsule, const PolygonalData& polyData, const HullPolygonData& referencePolygon, const SupportLocal* map, const PxMatTransformV& aToB,
|
||||
PersistentContact* manifoldContacts, PxU32& numContacts, const FloatVArg contactDist, const Vec3VArg contactNormal)
|
||||
{
|
||||
const PxU8* inds = polyData.mPolygonVertexRefs + referencePolygon.mVRef8;
|
||||
|
||||
Vec3V* points0In0 = reinterpret_cast<Vec3V*>(PxAllocaAligned(sizeof(Vec3V)*referencePolygon.mNbVerts, 16));
|
||||
|
||||
//Transform all the verts from vertex space to shape space
|
||||
map->populateVerts(inds, referencePolygon.mNbVerts, polyData.mVerts, points0In0);
|
||||
|
||||
const FloatV inflatedRadius = FAdd(capsule.radius, contactDist);
|
||||
|
||||
for (PxU32 rStart = 0, rEnd = PxU32(referencePolygon.mNbVerts - 1); rStart < referencePolygon.mNbVerts; rEnd = rStart++)
|
||||
{
|
||||
generateEE(capsule.p0, capsule.p1, contactNormal,points0In0[rStart], points0In0[rEnd], aToB, manifoldContacts, numContacts, inflatedRadius);
|
||||
}
|
||||
}
|
||||
|
||||
bool Gu::generateCapsuleBoxFullContactManifold(const CapsuleV& capsule, const PolygonalData& polyData, const SupportLocal* map, const PxMatTransformV& aToB, PersistentContact* manifoldContacts, PxU32& numContacts,
|
||||
const FloatVArg contactDist, Vec3V& normal, const Vec3VArg closest, PxReal margin, bool doOverlapTest, PxReal toleranceScale, PxRenderOutput* renderOutput)
|
||||
{
|
||||
PX_UNUSED(renderOutput);
|
||||
const PxU32 originalContacts = numContacts;
|
||||
|
||||
const HullPolygonData* referencePolygon = NULL;
|
||||
|
||||
if(doOverlapTest)
|
||||
{
|
||||
FloatV minOverlap;
|
||||
//overwrite the normal
|
||||
if(!testSATCapsulePoly(capsule, polyData, map, contactDist, minOverlap, normal))
|
||||
return false;
|
||||
|
||||
referencePolygon = &polyData.mPolygons[getPolygonIndex(polyData, map, V3Neg(normal))];
|
||||
}
|
||||
else
|
||||
{
|
||||
const PxReal lowerEps = toleranceScale * PCM_WITNESS_POINT_LOWER_EPS;
|
||||
const PxReal upperEps = toleranceScale * PCM_WITNESS_POINT_UPPER_EPS;
|
||||
const PxReal tolerance = PxClamp(margin, lowerEps, upperEps);
|
||||
|
||||
const PxU32 featureIndex = getWitnessPolygonIndex(polyData, map, V3Neg(normal), closest, tolerance);
|
||||
referencePolygon = &polyData.mPolygons[featureIndex];
|
||||
}
|
||||
|
||||
#if PCM_LOW_LEVEL_DEBUG
|
||||
const PxU8* inds = polyData.mPolygonVertexRefs + referencePolygon->mVRef8;
|
||||
Vec3V* pointsInRef = reinterpret_cast<Vec3V*>(PxAllocaAligned(sizeof(Vec3V)*referencePolygon->mNbVerts, 16));
|
||||
map->populateVerts(inds, referencePolygon->mNbVerts, polyData.mVerts, pointsInRef);
|
||||
PersistentContactManifold::drawPolygon(*renderOutput, map->transform, pointsInRef, referencePolygon->mNbVerts, 0x00ff0000);
|
||||
#endif
|
||||
generatedCapsuleBoxFaceContacts(capsule, polyData, *referencePolygon, map, aToB, manifoldContacts, numContacts, contactDist, normal);
|
||||
const PxU32 faceContacts = numContacts - originalContacts;
|
||||
if(faceContacts < 2)
|
||||
{
|
||||
generatedContactsEEContacts(capsule, polyData, *referencePolygon, map, aToB, manifoldContacts, numContacts, contactDist, normal);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
//capsule is in the local space of polyData
|
||||
bool Gu::generateFullContactManifold(const CapsuleV& capsule, const PolygonalData& polyData, const SupportLocal* map, const PxMatTransformV& aToB, PersistentContact* manifoldContacts, PxU32& numContacts,
|
||||
const FloatVArg contactDist, Vec3V& normal, const Vec3VArg closest, PxReal margin, bool doOverlapTest, PxReal toleranceLength)
|
||||
{
|
||||
const PxU32 originalContacts = numContacts;
|
||||
|
||||
Vec3V tNormal = normal;
|
||||
|
||||
if(doOverlapTest)
|
||||
{
|
||||
FloatV minOverlap;
|
||||
|
||||
//overwrite the normal with the sperating axis
|
||||
if(!testSATCapsulePoly(capsule, polyData, map, contactDist, minOverlap, tNormal))
|
||||
return false;
|
||||
|
||||
generatedFaceContacts(capsule, polyData, map, aToB, manifoldContacts, numContacts, contactDist, tNormal);
|
||||
|
||||
const PxU32 faceContacts = numContacts - originalContacts;
|
||||
if(faceContacts < 2)
|
||||
{
|
||||
const HullPolygonData& referencePolygon = polyData.mPolygons[getPolygonIndex(polyData, map, V3Neg(tNormal))];
|
||||
generatedContactsEEContacts(capsule, polyData,referencePolygon, map, aToB, manifoldContacts, numContacts, contactDist, tNormal);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
generatedFaceContacts(capsule, polyData, map, aToB, manifoldContacts, numContacts, contactDist, tNormal);
|
||||
|
||||
const PxU32 faceContacts = numContacts - originalContacts;
|
||||
if(faceContacts < 2)
|
||||
{
|
||||
const PxReal lowerEps = toleranceLength * PCM_WITNESS_POINT_LOWER_EPS;
|
||||
const PxReal upperEps = toleranceLength * PCM_WITNESS_POINT_UPPER_EPS;
|
||||
const PxReal tolerance = PxClamp(margin, lowerEps, upperEps);
|
||||
|
||||
const PxU32 featureIndex = getWitnessPolygonIndex(polyData, map, V3Neg(tNormal), closest, tolerance);
|
||||
const HullPolygonData& referencePolygon = polyData.mPolygons[featureIndex];
|
||||
generatedContactsEEContacts(capsule, polyData,referencePolygon, map, aToB, manifoldContacts, numContacts, contactDist, tNormal);
|
||||
}
|
||||
}
|
||||
|
||||
normal = tNormal;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
//sphere is in the local space of polyData, we treate sphere as capsule
|
||||
bool Gu::generateSphereFullContactManifold(const CapsuleV& capsule, const PolygonalData& polyData, const SupportLocal* map, PersistentContact* manifoldContacts, PxU32& numContacts,
|
||||
const FloatVArg contactDist, Vec3V& normal, bool doOverlapTest)
|
||||
{
|
||||
if(doOverlapTest)
|
||||
{
|
||||
FloatV minOverlap;
|
||||
//overwrite the normal with the sperating axis
|
||||
if(!testPolyDataAxis(capsule, polyData, map, contactDist, minOverlap, normal))
|
||||
return false;
|
||||
}
|
||||
|
||||
const FloatV zero = FZero();
|
||||
FloatV tEnter = zero, tExit = zero;
|
||||
const FloatV inflatedRadius = FAdd(capsule.radius, contactDist);
|
||||
const Vec3V dir = V3Neg(normal);
|
||||
const Vec3V vertexSpaceDir = M33MulV3(map->shape2Vertex, dir);
|
||||
const Vec3V p0 = M33MulV3(map->shape2Vertex, capsule.p0);
|
||||
if(intersectRayPolyhedron(p0, vertexSpaceDir, polyData, tEnter, tExit) && FAllGrtrOrEq(inflatedRadius, tEnter))
|
||||
{
|
||||
//ML: for sphere, the contact point A is always zeroV in its local space
|
||||
manifoldContacts[numContacts].mLocalPointA = V3Zero();
|
||||
manifoldContacts[numContacts].mLocalPointB = V3ScaleAdd(dir, tEnter, capsule.p0);
|
||||
manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(normal), tEnter);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
//capsule is in the shape space of polyData
|
||||
bool Gu::computeMTD(const CapsuleV& capsule, const PolygonalData& polyData, const SupportLocal* map, FloatV& penDepth, Vec3V& normal)
|
||||
{
|
||||
const FloatV contactDist = FZero();
|
||||
Vec3V separatingAxis;
|
||||
FloatV minOverlap;
|
||||
if(!testSATCapsulePoly(capsule, polyData, map, contactDist, minOverlap, separatingAxis))
|
||||
return false;
|
||||
|
||||
//transform normal in to the worl space
|
||||
normal = map->transform.rotate(separatingAxis);
|
||||
penDepth = minOverlap;
|
||||
|
||||
return true;
|
||||
}
|
||||
314
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactGenUtil.cpp
vendored
Normal file
314
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactGenUtil.cpp
vendored
Normal file
@@ -0,0 +1,314 @@
|
||||
// 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 "GuPCMContactGenUtil.h"
|
||||
|
||||
using namespace physx;
|
||||
using namespace Gu;
|
||||
using namespace aos;
|
||||
|
||||
bool Gu::contains(Vec3V* verts, PxU32 numVerts, const Vec3VArg p, const Vec3VArg min, const Vec3VArg max)
|
||||
{
|
||||
const BoolV tempCon = BOr(V3IsGrtr(min, p), V3IsGrtr(p, max));
|
||||
const BoolV con = BOr(BGetX(tempCon), BGetY(tempCon));
|
||||
|
||||
if (BAllEqTTTT(con))
|
||||
return false;
|
||||
|
||||
const FloatV tx = V3GetX(p);
|
||||
const FloatV ty = V3GetY(p);
|
||||
|
||||
const FloatV eps = FEps();
|
||||
const FloatV zero = FZero();
|
||||
PxU32 intersectionPoints = 0;
|
||||
|
||||
PxU32 i = 0, j = numVerts - 1;
|
||||
|
||||
for (; i < numVerts; j = i++)
|
||||
{
|
||||
const FloatV jy = V3GetY(verts[j]);
|
||||
const FloatV iy = V3GetY(verts[i]);
|
||||
|
||||
const FloatV jx = V3GetX(verts[j]);
|
||||
const FloatV ix = V3GetX(verts[i]);
|
||||
|
||||
//if p is one of the end point, we will return intersect
|
||||
const BoolV con0 = BAnd(FIsEq(tx, jx), FIsEq(ty, jy));
|
||||
const BoolV con1 = BAnd(FIsEq(tx, ix), FIsEq(ty, iy));
|
||||
|
||||
if (BAllEqTTTT(BOr(con0, con1)))
|
||||
return true;
|
||||
|
||||
//(verts[i].y > test.y) != (points[j].y > test.y)
|
||||
const PxU32 yflag0 = FAllGrtr(jy, ty);
|
||||
const PxU32 yflag1 = FAllGrtr(iy, ty);
|
||||
|
||||
//ML: the only case the ray will intersect this segment is when the p's y is in between two segments y
|
||||
if (yflag0 != yflag1)
|
||||
{
|
||||
//ML: choose ray, which start at p and every points in the ray will have the same y component
|
||||
//t1 = (yp - yj)/(yi - yj)
|
||||
//qx = xj + t1*(xi-xj)
|
||||
//t = qx - xp > 0 for the ray and segment intersection happen
|
||||
const FloatV jix = FSub(ix, jx);
|
||||
const FloatV jiy = FSub(iy, jy);
|
||||
//const FloatV jtx = FSub(tx, jy);
|
||||
const FloatV jty = FSub(ty, jy);
|
||||
const FloatV part1 = FMul(jty, jix);
|
||||
//const FloatV part2 = FMul(jx, jiy);
|
||||
//const FloatV part3 = FMul(V3Sub(tx, eps), jiy);
|
||||
const FloatV part2 = FMul(FAdd(jx, eps), jiy);
|
||||
const FloatV part3 = FMul(tx, jiy);
|
||||
|
||||
const BoolV comp = FIsGrtr(jiy, zero);
|
||||
const FloatV tmp = FAdd(part1, part2);
|
||||
const FloatV comp1 = FSel(comp, tmp, part3);
|
||||
const FloatV comp2 = FSel(comp, part3, tmp);
|
||||
|
||||
if (FAllGrtrOrEq(comp1, comp2))
|
||||
{
|
||||
if (intersectionPoints == 1)
|
||||
return false;
|
||||
|
||||
intersectionPoints++;
|
||||
}
|
||||
}
|
||||
}
|
||||
return intersectionPoints> 0;
|
||||
}
|
||||
|
||||
PxI32 Gu::getPolygonIndex(const PolygonalData& polyData, const SupportLocal* map, const Vec3VArg normal, PxI32& polyIndex2)
|
||||
{
|
||||
//normal is in shape space, need to transform the vertex space
|
||||
const Vec3V n = M33TrnspsMulV3(map->vertex2Shape, normal);
|
||||
const Vec3V nnormal = V3Neg(n);
|
||||
const Vec3V planeN_ = V3LoadU_SafeReadW(polyData.mPolygons[0].mPlane.n); // PT: safe because 'd' follows 'n' in the plane class
|
||||
FloatV minProj = V3Dot(n, planeN_);
|
||||
|
||||
const FloatV zero = FZero();
|
||||
PxI32 closestFaceIndex = 0;
|
||||
|
||||
polyIndex2 = -1;
|
||||
|
||||
for (PxU32 i = 1; i < polyData.mNbPolygons; ++i)
|
||||
{
|
||||
const Vec3V planeN = V3LoadU_SafeReadW(polyData.mPolygons[i].mPlane.n); // PT: safe because 'd' follows 'n' in the plane class
|
||||
const FloatV proj = V3Dot(n, planeN);
|
||||
if (FAllGrtr(minProj, proj))
|
||||
{
|
||||
minProj = proj;
|
||||
closestFaceIndex = PxI32(i);
|
||||
}
|
||||
}
|
||||
|
||||
const PxU32 numEdges = polyData.mNbEdges;
|
||||
const PxU8* const edgeToFace = polyData.mFacesByEdges;
|
||||
|
||||
//Loop through edges
|
||||
PxU32 closestEdge = 0xffffffff;
|
||||
FloatV maxDpSq = FMul(minProj, minProj);
|
||||
|
||||
FloatV maxDpSq2 = FLoad(-10.0f);
|
||||
|
||||
for (PxU32 i = 0; i < numEdges; ++i)//, inc = VecI32V_Add(inc, vOne))
|
||||
{
|
||||
const PxU32 index = i * 2;
|
||||
const PxU8 f0 = edgeToFace[index];
|
||||
const PxU8 f1 = edgeToFace[index + 1];
|
||||
|
||||
const Vec3V planeNormal0 = V3LoadU_SafeReadW(polyData.mPolygons[f0].mPlane.n); // PT: safe because 'd' follows 'n' in the plane class
|
||||
const Vec3V planeNormal1 = V3LoadU_SafeReadW(polyData.mPolygons[f1].mPlane.n); // PT: safe because 'd' follows 'n' in the plane class
|
||||
|
||||
// unnormalized edge normal
|
||||
const Vec3V edgeNormal = V3Add(planeNormal0, planeNormal1);//polys[f0].mPlane.n + polys[f1].mPlane.n;
|
||||
const FloatV enMagSq = V3Dot(edgeNormal, edgeNormal);//edgeNormal.magnitudeSquared();
|
||||
//Test normal of current edge - squared test is valid if dp and maxDp both >= 0
|
||||
const FloatV dp = V3Dot(edgeNormal, nnormal);//edgeNormal.dot(normal);
|
||||
const FloatV sqDp = FMul(dp, dp);
|
||||
|
||||
if(f0==closestFaceIndex || f1==closestFaceIndex)
|
||||
{
|
||||
if(BAllEqTTTT(FIsGrtrOrEq(sqDp, maxDpSq2)))
|
||||
{
|
||||
maxDpSq2 = sqDp;
|
||||
polyIndex2 = f0==closestFaceIndex ? PxI32(f1) : PxI32(f0);
|
||||
}
|
||||
}
|
||||
|
||||
const BoolV con0 = FIsGrtrOrEq(dp, zero);
|
||||
const BoolV con1 = FIsGrtr(sqDp, FMul(maxDpSq, enMagSq));
|
||||
const BoolV con = BAnd(con0, con1);
|
||||
if (BAllEqTTTT(con))
|
||||
{
|
||||
maxDpSq = FDiv(sqDp, enMagSq);
|
||||
closestEdge = i;
|
||||
}
|
||||
}
|
||||
|
||||
if (closestEdge != 0xffffffff)
|
||||
{
|
||||
const PxU8* FBE = edgeToFace;
|
||||
|
||||
const PxU32 index = closestEdge * 2;
|
||||
const PxU32 f0 = FBE[index];
|
||||
const PxU32 f1 = FBE[index + 1];
|
||||
|
||||
const Vec3V planeNormal0 = V3LoadU_SafeReadW(polyData.mPolygons[f0].mPlane.n); // PT: safe because 'd' follows 'n' in the plane class
|
||||
const Vec3V planeNormal1 = V3LoadU_SafeReadW(polyData.mPolygons[f1].mPlane.n); // PT: safe because 'd' follows 'n' in the plane class
|
||||
|
||||
const FloatV dp0 = V3Dot(planeNormal0, nnormal);
|
||||
const FloatV dp1 = V3Dot(planeNormal1, nnormal);
|
||||
if (FAllGrtr(dp0, dp1))
|
||||
{
|
||||
closestFaceIndex = PxI32(f0);
|
||||
polyIndex2 = PxI32(f1);
|
||||
}
|
||||
else
|
||||
{
|
||||
closestFaceIndex = PxI32(f1);
|
||||
polyIndex2 = PxI32(f0);
|
||||
}
|
||||
}
|
||||
|
||||
return closestFaceIndex;
|
||||
}
|
||||
|
||||
PxU32 Gu::getWitnessPolygonIndex(const PolygonalData& polyData, const SupportLocal* map, const Vec3VArg normal, const Vec3VArg closest, PxReal tolerance)
|
||||
{
|
||||
PxReal pd[256];
|
||||
//first pass : calculate the smallest distance from the closest point to the polygon face
|
||||
|
||||
//transform the closest p to vertex space
|
||||
const Vec3V p = M33MulV3(map->shape2Vertex, closest);
|
||||
PxU32 closestFaceIndex = 0;
|
||||
|
||||
PxVec3 closestP;
|
||||
V3StoreU(p, closestP);
|
||||
|
||||
const PxReal eps = -tolerance;
|
||||
|
||||
PxPlane plane = polyData.mPolygons[0].mPlane;
|
||||
PxReal dist = plane.distance(closestP);
|
||||
PxReal minDist = dist >= eps ? PxAbs(dist) : PX_MAX_F32;
|
||||
pd[0] = minDist;
|
||||
PxReal maxDist = dist;
|
||||
PxU32 maxFaceIndex = 0;
|
||||
|
||||
for (PxU32 i = 1; i < polyData.mNbPolygons; ++i)
|
||||
{
|
||||
plane = polyData.mPolygons[i].mPlane;
|
||||
dist = plane.distance(closestP);
|
||||
pd[i] = dist >= eps ? PxAbs(dist) : PX_MAX_F32;
|
||||
if (minDist > pd[i])
|
||||
{
|
||||
minDist = pd[i];
|
||||
closestFaceIndex = i;
|
||||
}
|
||||
if (dist > maxDist)
|
||||
{
|
||||
maxDist = dist;
|
||||
maxFaceIndex = i;
|
||||
}
|
||||
}
|
||||
|
||||
if (minDist == PX_MAX_F32)
|
||||
return maxFaceIndex;
|
||||
|
||||
//second pass : select the face which has the normal most close to the gjk/epa normal
|
||||
Vec4V plane4 = V4LoadU(&polyData.mPolygons[closestFaceIndex].mPlane.n.x);
|
||||
Vec3V n = Vec3V_From_Vec4V(plane4);
|
||||
n = V3Normalize(M33TrnspsMulV3(map->shape2Vertex, n));
|
||||
FloatV bestProj = V3Dot(n, normal);
|
||||
|
||||
const PxU32 firstPassIndex = closestFaceIndex;
|
||||
|
||||
for (PxU32 i = 0; i< polyData.mNbPolygons; ++i)
|
||||
{
|
||||
//if the difference between the minimum distance and the distance of p to plane i is within tolerance, we use the normal to chose the best plane
|
||||
if ((tolerance >(pd[i] - minDist)) && (firstPassIndex != i))
|
||||
{
|
||||
plane4 = V4LoadU(&polyData.mPolygons[i].mPlane.n.x);
|
||||
n = Vec3V_From_Vec4V(plane4);
|
||||
//rotate the plane normal to shape space. We can roate normal into the vertex space. The reason for
|
||||
//that is because it will lose some numerical precision if the object has large scale and this algorithm
|
||||
//will choose different face
|
||||
n = V3Normalize(M33TrnspsMulV3(map->shape2Vertex, n));
|
||||
FloatV proj = V3Dot(n, normal);
|
||||
|
||||
if (FAllGrtr(bestProj, proj))
|
||||
{
|
||||
closestFaceIndex = i;
|
||||
bestProj = proj;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return closestFaceIndex;
|
||||
}
|
||||
|
||||
/* PX_FORCE_INLINE bool boxContainsInXY(const FloatVArg x, const FloatVArg y, const Vec3VArg p, const Vec3V* verts, const Vec3VArg min, const Vec3VArg max)
|
||||
{
|
||||
using namespace aos;
|
||||
|
||||
const BoolV tempCon = BOr(V3IsGrtr(min, p), V3IsGrtr(p, max));
|
||||
const BoolV con = BOr(BGetX(tempCon), BGetY(tempCon));
|
||||
|
||||
if(BAllEqTTTT(con))
|
||||
return false;
|
||||
|
||||
const FloatV zero = FZero();
|
||||
FloatV PreviousX = V3GetX(verts[3]);
|
||||
FloatV PreviousY = V3GetY(verts[3]);
|
||||
|
||||
// Loop through quad vertices
|
||||
for(PxI32 i=0; i<4; i++)
|
||||
{
|
||||
const FloatV CurrentX = V3GetX(verts[i]);
|
||||
const FloatV CurrentY = V3GetY(verts[i]);
|
||||
|
||||
// |CurrentX - PreviousX x - PreviousX|
|
||||
// |CurrentY - PreviousY y - PreviousY|
|
||||
// => similar to backface culling, check each one of the 4 triangles are consistent, in which case
|
||||
// the point is within the parallelogram.
|
||||
const FloatV v00 = FSub(CurrentX, PreviousX);
|
||||
const FloatV v01 = FSub(y, PreviousY);
|
||||
const FloatV v10 = FSub(CurrentY, PreviousY);
|
||||
const FloatV v11 = FSub(x, PreviousX);
|
||||
const FloatV temp0 = FMul(v00, v01);
|
||||
const FloatV temp1 = FMul(v10, v11);
|
||||
if(FAllGrtrOrEq(FSub(temp0, temp1), zero))
|
||||
return false;
|
||||
|
||||
PreviousX = CurrentX;
|
||||
PreviousY = CurrentY;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
*/
|
||||
|
||||
111
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactGenUtil.h
vendored
Normal file
111
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactGenUtil.h
vendored
Normal file
@@ -0,0 +1,111 @@
|
||||
// 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_PCM_CONTACT_GEN_UTIL_H
|
||||
#define GU_PCM_CONTACT_GEN_UTIL_H
|
||||
|
||||
#include "foundation/PxVecMath.h"
|
||||
#include "geomutils/PxContactBuffer.h"
|
||||
#include "GuShapeConvex.h"
|
||||
#include "GuVecCapsule.h"
|
||||
#include "GuConvexSupportTable.h"
|
||||
|
||||
//The smallest epsilon we will permit (scaled by PxTolerancesScale.length)
|
||||
#define PCM_WITNESS_POINT_LOWER_EPS 1e-2f
|
||||
//The largest epsilon we will permit (scaled by PxTolerancesScale.length)
|
||||
#define PCM_WITNESS_POINT_UPPER_EPS 5e-2f
|
||||
|
||||
namespace physx
|
||||
{
|
||||
namespace Gu
|
||||
{
|
||||
enum FeatureStatus
|
||||
{
|
||||
POLYDATA0,
|
||||
POLYDATA1,
|
||||
EDGE
|
||||
};
|
||||
|
||||
bool contains(aos::Vec3V* verts, PxU32 numVerts, const aos::Vec3VArg p, const aos::Vec3VArg min, const aos::Vec3VArg max);
|
||||
|
||||
PX_FORCE_INLINE aos::FloatV signed2DTriArea(const aos::Vec3VArg a, const aos::Vec3VArg b, const aos::Vec3VArg c)
|
||||
{
|
||||
using namespace aos;
|
||||
const Vec3V ca = V3Sub(a, c);
|
||||
const Vec3V cb = V3Sub(b, c);
|
||||
|
||||
const FloatV t0 = FMul(V3GetX(ca), V3GetY(cb));
|
||||
const FloatV t1 = FMul(V3GetY(ca), V3GetX(cb));
|
||||
|
||||
return FSub(t0, t1);
|
||||
}
|
||||
|
||||
PxI32 getPolygonIndex(const Gu::PolygonalData& polyData, const SupportLocal* map, const aos::Vec3VArg normal, PxI32& polyIndex2);
|
||||
|
||||
PX_FORCE_INLINE PxI32 getPolygonIndex(const Gu::PolygonalData& polyData, const SupportLocal* map, const aos::Vec3VArg normal)
|
||||
{
|
||||
using namespace aos;
|
||||
|
||||
PxI32 index2;
|
||||
return getPolygonIndex(polyData, map, normal, index2);
|
||||
}
|
||||
|
||||
PxU32 getWitnessPolygonIndex( const Gu::PolygonalData& polyData, const SupportLocal* map, const aos::Vec3VArg normal,
|
||||
const aos::Vec3VArg closest, PxReal tolerance);
|
||||
|
||||
PX_FORCE_INLINE void outputPCMContact(PxContactBuffer& contactBuffer, PxU32& contactCount, const aos::Vec3VArg point, const aos::Vec3VArg normal,
|
||||
const aos::FloatVArg penetration, PxU32 internalFaceIndex1 = PXC_CONTACT_NO_FACE_INDEX)
|
||||
{
|
||||
using namespace aos;
|
||||
|
||||
// PT: TODO: the PCM capsule-capsule code was using this alternative version, is it better?
|
||||
// const Vec4V normalSep = V4SetW(Vec4V_From_Vec3V(normal), separation);
|
||||
// V4StoreA(normalSep, &point.normal.x);
|
||||
// Also aren't we overwriting maxImpulse with the position now? Ok to do so?
|
||||
PX_ASSERT(contactCount < PxContactBuffer::MAX_CONTACTS);
|
||||
PxContactPoint& contact = contactBuffer.contacts[contactCount++];
|
||||
V4StoreA(Vec4V_From_Vec3V(normal), &contact.normal.x);
|
||||
V4StoreA(Vec4V_From_Vec3V(point), &contact.point.x);
|
||||
FStore(penetration, &contact.separation);
|
||||
PX_ASSERT(contact.point.isFinite());
|
||||
PX_ASSERT(contact.normal.isFinite());
|
||||
PX_ASSERT(PxIsFinite(contact.separation));
|
||||
contact.internalFaceIndex1 = internalFaceIndex1;
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE bool outputSimplePCMContact(PxContactBuffer& contactBuffer, const aos::Vec3VArg point, const aos::Vec3VArg normal,
|
||||
const aos::FloatVArg penetration, PxU32 internalFaceIndex1 = PXC_CONTACT_NO_FACE_INDEX)
|
||||
{
|
||||
outputPCMContact(contactBuffer, contactBuffer.count, point, normal, penetration, internalFaceIndex1);
|
||||
return true;
|
||||
}
|
||||
|
||||
}//Gu
|
||||
}//physx
|
||||
|
||||
#endif
|
||||
259
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactMeshCallback.h
vendored
Normal file
259
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactMeshCallback.h
vendored
Normal file
@@ -0,0 +1,259 @@
|
||||
// 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_PCM_CONTACT_MESH_CALLBACK_H
|
||||
#define GU_PCM_CONTACT_MESH_CALLBACK_H
|
||||
|
||||
#include "GuMidphaseInterface.h"
|
||||
#include "GuEntityReport.h"
|
||||
#include "GuHeightFieldUtil.h"
|
||||
#include "GuTriangleCache.h"
|
||||
#include "GuConvexEdgeFlags.h"
|
||||
|
||||
namespace physx
|
||||
{
|
||||
namespace Gu
|
||||
{
|
||||
template <typename Derived>
|
||||
struct PCMMeshContactGenerationCallback : MeshHitCallback<PxGeomRaycastHit>
|
||||
{
|
||||
public:
|
||||
const Cm::FastVertex2ShapeScaling& mMeshScaling;
|
||||
const PxU8* PX_RESTRICT mExtraTrigData;
|
||||
bool mIdtMeshScale;
|
||||
static const PxU32 CacheSize = 16;
|
||||
Gu::TriangleCache<CacheSize> mCache;
|
||||
|
||||
PCMMeshContactGenerationCallback(const Cm::FastVertex2ShapeScaling& meshScaling, const PxU8* extraTrigData, bool idtMeshScale)
|
||||
: MeshHitCallback<PxGeomRaycastHit>(CallbackMode::eMULTIPLE),
|
||||
mMeshScaling(meshScaling), mExtraTrigData(extraTrigData), mIdtMeshScale(idtMeshScale)
|
||||
{
|
||||
}
|
||||
|
||||
void flushCache()
|
||||
{
|
||||
if (!mCache.isEmpty())
|
||||
{
|
||||
(static_cast<Derived*>(this))->template processTriangleCache< CacheSize >(mCache);
|
||||
mCache.reset();
|
||||
}
|
||||
}
|
||||
|
||||
virtual PxAgain processHit(
|
||||
const PxGeomRaycastHit& hit, const PxVec3& v0, const PxVec3& v1, const PxVec3& v2, PxReal&, const PxU32* vinds)
|
||||
{
|
||||
if (!(static_cast<Derived*>(this))->doTest(v0, v1, v2))
|
||||
return true;
|
||||
|
||||
const PxU32 triangleIndex = hit.faceIndex;
|
||||
PxU8 extraData = getConvexEdgeFlags(mExtraTrigData, triangleIndex);
|
||||
|
||||
const PxU32* vertexIndices = vinds;
|
||||
PxVec3 v[3];
|
||||
PxU32 localStorage[3];
|
||||
if(mIdtMeshScale)
|
||||
{
|
||||
v[0] = v0;
|
||||
v[1] = v1;
|
||||
v[2] = v2;
|
||||
}
|
||||
else
|
||||
{
|
||||
const PxI32 winding = mMeshScaling.flipsNormal() ? 1 : 0;
|
||||
v[0] = mMeshScaling * v0;
|
||||
v[1 + winding] = mMeshScaling * v1;
|
||||
v[2 - winding] = mMeshScaling * v2;
|
||||
|
||||
if(winding)
|
||||
{
|
||||
flipConvexEdgeFlags(extraData);
|
||||
localStorage[0] = vinds[0];
|
||||
localStorage[1] = vinds[2];
|
||||
localStorage[2] = vinds[1];
|
||||
vertexIndices = localStorage;
|
||||
}
|
||||
}
|
||||
|
||||
if (mCache.isFull())
|
||||
{
|
||||
(static_cast<Derived*>(this))->template processTriangleCache< CacheSize >(mCache);
|
||||
mCache.reset();
|
||||
}
|
||||
mCache.addTriangle(v, vertexIndices, triangleIndex, extraData);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
protected:
|
||||
PCMMeshContactGenerationCallback& operator=(const PCMMeshContactGenerationCallback&);
|
||||
};
|
||||
|
||||
template <typename Derived>
|
||||
struct PCMHeightfieldContactGenerationCallback : Gu::OverlapReport
|
||||
{
|
||||
public:
|
||||
const Gu::HeightFieldUtil& mHfUtil;
|
||||
const PxTransform& mHeightfieldTransform;
|
||||
bool mBoundaryCollisions;
|
||||
|
||||
PCMHeightfieldContactGenerationCallback(const Gu::HeightFieldUtil& hfUtil, const PxTransform& heightfieldTransform) :
|
||||
mHfUtil(hfUtil), mHeightfieldTransform(heightfieldTransform)
|
||||
{
|
||||
mBoundaryCollisions = !(hfUtil.getHeightField().getFlags() & PxHeightFieldFlag::eNO_BOUNDARY_EDGES);
|
||||
}
|
||||
|
||||
// PT: TODO: refactor/unify with similar code in other places
|
||||
virtual bool reportTouchedTris(PxU32 nb, const PxU32* indices)
|
||||
{
|
||||
const PxU32 CacheSize = 16;
|
||||
Gu::TriangleCache<CacheSize> cache;
|
||||
|
||||
const PxU32 nbPasses = (nb+(CacheSize-1))/CacheSize;
|
||||
PxU32 nbTrigs = nb;
|
||||
const PxU32* inds0 = indices;
|
||||
|
||||
const PxU8 nextInd[] = {2,0,1};
|
||||
|
||||
for(PxU32 i = 0; i < nbPasses; ++i)
|
||||
{
|
||||
cache.mNumTriangles = 0;
|
||||
PxU32 trigCount = PxMin(nbTrigs, CacheSize);
|
||||
nbTrigs -= trigCount;
|
||||
while(trigCount--)
|
||||
{
|
||||
PxU32 triangleIndex = *(inds0++);
|
||||
PxU32 vertIndices[3];
|
||||
|
||||
PxTriangle currentTriangle; // in world space
|
||||
|
||||
PxU32 adjInds[3];
|
||||
mHfUtil.getTriangle(mHeightfieldTransform, currentTriangle, vertIndices, adjInds, triangleIndex, false, false);
|
||||
|
||||
PxVec3 normal;
|
||||
currentTriangle.normal(normal);
|
||||
|
||||
PxU8 triFlags = 0; //KS - temporary until we can calculate triFlags for HF
|
||||
|
||||
for(PxU32 a = 0; a < 3; ++a)
|
||||
{
|
||||
if (adjInds[a] != 0xFFFFFFFF)
|
||||
{
|
||||
PxTriangle adjTri;
|
||||
PxU32 inds[3];
|
||||
mHfUtil.getTriangle(mHeightfieldTransform, adjTri, inds, NULL, adjInds[a], false, false);
|
||||
//We now compare the triangles to see if this edge is active
|
||||
|
||||
PX_ASSERT(inds[0] == vertIndices[a] || inds[1] == vertIndices[a] || inds[2] == vertIndices[a]);
|
||||
PX_ASSERT(inds[0] == vertIndices[(a + 1) % 3] || inds[1] == vertIndices[(a + 1) % 3] || inds[2] == vertIndices[(a + 1) % 3]);
|
||||
|
||||
PxVec3 adjNormal;
|
||||
adjTri.denormalizedNormal(adjNormal);
|
||||
PxU32 otherIndex = nextInd[a];
|
||||
PxF32 projD = adjNormal.dot(currentTriangle.verts[otherIndex] - adjTri.verts[0]);
|
||||
|
||||
if (projD < 0.f)
|
||||
{
|
||||
adjNormal.normalize();
|
||||
|
||||
PxF32 proj = adjNormal.dot(normal);
|
||||
|
||||
if (proj < 0.997f)
|
||||
{
|
||||
triFlags |= (1 << (a + 3));
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (mBoundaryCollisions)
|
||||
{
|
||||
triFlags |= (1 << (a + 3)); //Mark boundary edge active
|
||||
}
|
||||
else
|
||||
triFlags |= (1 << a); //Mark as silhouette edge
|
||||
}
|
||||
|
||||
cache.addTriangle(currentTriangle.verts, vertIndices, triangleIndex, triFlags);
|
||||
}
|
||||
PX_ASSERT(cache.mNumTriangles <= 16);
|
||||
|
||||
(static_cast<Derived*>(this))->template processTriangleCache< CacheSize >(cache);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
protected:
|
||||
PCMHeightfieldContactGenerationCallback& operator=(const PCMHeightfieldContactGenerationCallback&);
|
||||
};
|
||||
|
||||
template <typename Derived>
|
||||
struct PCMTetMeshContactGenerationCallback : TetMeshHitCallback<PxGeomRaycastHit>
|
||||
{
|
||||
public:
|
||||
|
||||
static const PxU32 CacheSize = 16;
|
||||
Gu::TetrahedronCache<CacheSize> mCache;
|
||||
|
||||
PCMTetMeshContactGenerationCallback(): TetMeshHitCallback<PxGeomRaycastHit>(CallbackMode::eMULTIPLE)
|
||||
{
|
||||
}
|
||||
|
||||
void flushCache()
|
||||
{
|
||||
if (!mCache.isEmpty())
|
||||
{
|
||||
(static_cast<Derived*>(this))->template processTetrahedronCache< CacheSize >(mCache);
|
||||
mCache.reset();
|
||||
}
|
||||
}
|
||||
|
||||
virtual PxAgain processHit(
|
||||
const PxGeomRaycastHit& hit, const PxVec3& v0, const PxVec3& v1, const PxVec3& v2, const PxVec3& v3, PxReal&, const PxU32* vinds)
|
||||
{
|
||||
if (!(static_cast<Derived*>(this))->doTest(v0, v1, v2, v3))
|
||||
return true;
|
||||
|
||||
PxVec3 v[4] = { v0, v1, v2, v3 };
|
||||
|
||||
const PxU32 tetIndex = hit.faceIndex;
|
||||
|
||||
if (mCache.isFull())
|
||||
{
|
||||
(static_cast<Derived*>(this))->template processTetrahedronCache< CacheSize >(mCache);
|
||||
mCache.reset();
|
||||
}
|
||||
mCache.addTetrahedrons(v, vinds, tetIndex);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
protected:
|
||||
PCMTetMeshContactGenerationCallback& operator=(const PCMTetMeshContactGenerationCallback&);
|
||||
};
|
||||
|
||||
}//Gu
|
||||
}//physx
|
||||
|
||||
#endif
|
||||
209
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactPlaneBox.cpp
vendored
Normal file
209
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactPlaneBox.cpp
vendored
Normal file
@@ -0,0 +1,209 @@
|
||||
// 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 "geomutils/PxContactBuffer.h"
|
||||
#include "GuVecBox.h"
|
||||
#include "GuContactMethodImpl.h"
|
||||
#include "GuPersistentContactManifold.h"
|
||||
|
||||
using namespace physx;
|
||||
|
||||
bool Gu::pcmContactPlaneBox(GU_CONTACT_METHOD_ARGS)
|
||||
{
|
||||
PX_UNUSED(shape0);
|
||||
PX_UNUSED(renderOutput);
|
||||
|
||||
using namespace aos;
|
||||
|
||||
PersistentContactManifold& manifold = cache.getManifold();
|
||||
PxPrefetchLine(&manifold, 256);
|
||||
|
||||
// Get actual shape data
|
||||
const PxBoxGeometry& shapeBox = checkedCast<PxBoxGeometry>(shape1);
|
||||
|
||||
const PxTransformV transf0 = loadTransformA(transform1);//box transform
|
||||
const PxTransformV transf1 = loadTransformA(transform0);//plane transform
|
||||
|
||||
//box to plane
|
||||
const PxTransformV curTransf(transf1.transformInv(transf0));
|
||||
|
||||
//in world space
|
||||
const Vec3V negPlaneNormal = V3Normalize(V3Neg(QuatGetBasisVector0(transf1.q)));
|
||||
|
||||
const FloatV contactDist = FLoad(params.mContactDistance);
|
||||
|
||||
const Vec3V boxExtents = V3LoadU(shapeBox.halfExtents);
|
||||
|
||||
const PxReal toleranceLength = params.mToleranceLength;
|
||||
const FloatV boxMargin = CalculatePCMBoxMargin(boxExtents, toleranceLength);
|
||||
const FloatV projectBreakingThreshold = FMul(boxMargin, FLoad(0.2f));
|
||||
const PxU32 initialContacts = manifold.mNumContacts;
|
||||
|
||||
manifold.refreshContactPoints(curTransf, projectBreakingThreshold, contactDist);
|
||||
|
||||
const PxU32 newContacts = manifold.mNumContacts;
|
||||
const bool bLostContacts = (newContacts != initialContacts);//((initialContacts == 0) || (newContacts != initialContacts));
|
||||
|
||||
if(bLostContacts || manifold.invalidate_PrimitivesPlane(curTransf, boxMargin, FLoad(0.2f)))
|
||||
{
|
||||
//ML:localNormal is the local space of plane normal, however, because shape1 is box and shape0 is plane, we need to use the reverse of contact normal(which will be the plane normal) to make the refreshContactPoints
|
||||
//work out the correct pentration for points
|
||||
const Vec3V localNormal = V3UnitX();
|
||||
|
||||
manifold.mNumContacts = 0;
|
||||
manifold.setRelativeTransform(curTransf);
|
||||
|
||||
const PxMatTransformV aToB(curTransf);
|
||||
const FloatV bx = V3GetX(boxExtents);
|
||||
const FloatV by = V3GetY(boxExtents);
|
||||
const FloatV bz = V3GetZ(boxExtents);
|
||||
|
||||
const FloatV nbx = FNeg(bx);
|
||||
const FloatV nby = FNeg(by);
|
||||
const FloatV nbz = FNeg(bz);
|
||||
|
||||
const Vec3V temp0 = V3Scale(aToB.getCol0(), bx);
|
||||
const Vec3V temp1 = V3Scale(aToB.getCol1(), by);
|
||||
const Vec3V temp2 = V3Scale(aToB.getCol2(), bz);
|
||||
|
||||
const Vec3V ntemp2 = V3Neg(temp2);
|
||||
|
||||
const FloatV px = V3GetX(aToB.p);
|
||||
|
||||
//box's points in the local space of plane
|
||||
const Vec3V temp01 = V3Add(temp0, temp1);//(x, y)
|
||||
const Vec3V temp02 = V3Sub(temp0, temp1);//(x, -y)
|
||||
|
||||
const FloatV s0 = V3GetX(V3Add(temp2, temp01));//(x, y, z)
|
||||
const FloatV s1 = V3GetX(V3Add(ntemp2, temp01));//(x, y, -z)
|
||||
const FloatV s2 = V3GetX(V3Add(temp2, temp02));//(x, -y, z)
|
||||
const FloatV s3 = V3GetX(V3Add(ntemp2, temp02));//(x, -y, -z)
|
||||
const FloatV s4 = V3GetX(V3Sub(temp2, temp02));//(-x, y, z)
|
||||
const FloatV s5 = V3GetX(V3Sub(ntemp2, temp02));//(-x, y, -z)
|
||||
const FloatV s6 = V3GetX(V3Sub(temp2, temp01));//(-x, -y, z)
|
||||
const FloatV s7 = V3GetX(V3Sub(ntemp2, temp01));//(-x, -y, -z)
|
||||
|
||||
const FloatV acceptanceDist = FSub(contactDist, px);
|
||||
|
||||
PersistentContact* manifoldContacts = PX_CP_TO_PCP(contactBuffer.contacts);
|
||||
PxU32 numContacts = 0;
|
||||
|
||||
if(FAllGrtr(acceptanceDist, s0))
|
||||
{
|
||||
const FloatV pen = FAdd(s0, px);
|
||||
//(x, y, z)
|
||||
manifoldContacts[numContacts].mLocalPointA = boxExtents;//aToB.transformInv(p);
|
||||
manifoldContacts[numContacts].mLocalPointB = V3NegScaleSub(localNormal, pen, aToB.transform(boxExtents));
|
||||
manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(localNormal), pen);
|
||||
}
|
||||
|
||||
if(FAllGrtr(acceptanceDist, s1))
|
||||
{
|
||||
const FloatV pen = FAdd(s1, px);
|
||||
//(x, y, -z)
|
||||
const Vec3V p = V3Merge(bx, by, nbz);
|
||||
//add to contact stream
|
||||
manifoldContacts[numContacts].mLocalPointA = p;//aToB.transformInv(p);
|
||||
manifoldContacts[numContacts].mLocalPointB = V3NegScaleSub(localNormal, pen, aToB.transform(p));
|
||||
manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(localNormal), pen);
|
||||
}
|
||||
|
||||
if(FAllGrtr(acceptanceDist, s2))
|
||||
{
|
||||
const FloatV pen = FAdd(s2, px);
|
||||
//(x, -y, z)
|
||||
const Vec3V p = V3Merge(bx, nby, bz);
|
||||
manifoldContacts[numContacts].mLocalPointA = p;//aToB.transformInv(p);
|
||||
manifoldContacts[numContacts].mLocalPointB = V3NegScaleSub(localNormal, pen, aToB.transform(p));
|
||||
manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(localNormal), pen);
|
||||
}
|
||||
|
||||
if(FAllGrtr(acceptanceDist, s3))
|
||||
{
|
||||
const FloatV pen = FAdd(s3, px);
|
||||
//(x, -y, -z)
|
||||
const Vec3V p = V3Merge(bx, nby, nbz);
|
||||
manifoldContacts[numContacts].mLocalPointA = p;
|
||||
manifoldContacts[numContacts].mLocalPointB = V3NegScaleSub(localNormal, pen, aToB.transform(p));
|
||||
manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(localNormal), pen);
|
||||
}
|
||||
|
||||
if(FAllGrtr(acceptanceDist, s4))
|
||||
{
|
||||
const FloatV pen = FAdd(s4, px);
|
||||
//(-x, y, z)
|
||||
const Vec3V p = V3Merge(nbx, by, bz);
|
||||
manifoldContacts[numContacts].mLocalPointA = p;
|
||||
manifoldContacts[numContacts].mLocalPointB = V3NegScaleSub(localNormal, pen, aToB.transform(p));
|
||||
manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(localNormal), pen);
|
||||
}
|
||||
|
||||
if(FAllGrtr(acceptanceDist, s5))
|
||||
{
|
||||
const FloatV pen = FAdd(s5, px);
|
||||
//(-x, y, -z)
|
||||
const Vec3V p = V3Merge(nbx, by, nbz);
|
||||
manifoldContacts[numContacts].mLocalPointA = p;//aToB.transformInv(p);
|
||||
manifoldContacts[numContacts].mLocalPointB = V3NegScaleSub(localNormal, pen, aToB.transform(p));
|
||||
manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(localNormal), pen);
|
||||
}
|
||||
|
||||
if(FAllGrtr(acceptanceDist, s6))
|
||||
{
|
||||
const FloatV pen = FAdd(s6, px);
|
||||
//(-x, -y, z)
|
||||
const Vec3V p = V3Merge(nbx, nby, bz);
|
||||
manifoldContacts[numContacts].mLocalPointA = p;//aToB.transformInv(p);
|
||||
manifoldContacts[numContacts].mLocalPointB = V3NegScaleSub(localNormal, pen, aToB.transform(p));
|
||||
manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(localNormal), pen);
|
||||
}
|
||||
|
||||
if(FAllGrtr(acceptanceDist, s7))
|
||||
{
|
||||
const FloatV pen = FAdd(s7, px);
|
||||
//(-x, -y, -z)
|
||||
const Vec3V p = V3Merge(nbx, nby, nbz);
|
||||
manifoldContacts[numContacts].mLocalPointA = p;
|
||||
manifoldContacts[numContacts].mLocalPointB = V3NegScaleSub(localNormal, pen, aToB.transform(p));
|
||||
manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(localNormal), pen);
|
||||
}
|
||||
|
||||
//reduce contacts
|
||||
manifold.addBatchManifoldContactsCluster(manifoldContacts, numContacts);
|
||||
manifold.addManifoldContactsToContactBuffer(contactBuffer, negPlaneNormal, transf1, contactDist);
|
||||
|
||||
return manifold.getNumContacts() > 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
manifold.addManifoldContactsToContactBuffer(contactBuffer, negPlaneNormal, transf1, contactDist);
|
||||
|
||||
//manifold.drawManifold(*gRenderOutPut, transf0, transf1);
|
||||
return manifold.getNumContacts() > 0;
|
||||
}
|
||||
}
|
||||
123
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactPlaneCapsule.cpp
vendored
Normal file
123
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactPlaneCapsule.cpp
vendored
Normal file
@@ -0,0 +1,123 @@
|
||||
// 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 "geomutils/PxContactBuffer.h"
|
||||
#include "GuVecCapsule.h"
|
||||
#include "GuContactMethodImpl.h"
|
||||
#include "GuPersistentContactManifold.h"
|
||||
|
||||
using namespace physx;
|
||||
|
||||
bool Gu::pcmContactPlaneCapsule(GU_CONTACT_METHOD_ARGS)
|
||||
{
|
||||
PX_UNUSED(shape0);
|
||||
PX_UNUSED(renderOutput);
|
||||
|
||||
using namespace aos;
|
||||
|
||||
PersistentContactManifold& manifold = cache.getManifold();
|
||||
PxPrefetchLine(&manifold, 256);
|
||||
|
||||
// Get actual shape data
|
||||
const PxCapsuleGeometry& shapeCapsule = checkedCast<PxCapsuleGeometry>(shape1);
|
||||
|
||||
const PxTransformV transf0 = loadTransformA(transform1);//capsule transform
|
||||
const PxTransformV transf1 = loadTransformA(transform0);//plane transform
|
||||
//capsule to plane
|
||||
const PxTransformV aToB(transf1.transformInv(transf0));
|
||||
|
||||
//in world space
|
||||
const Vec3V planeNormal = V3Normalize(QuatGetBasisVector0(transf1.q));
|
||||
const Vec3V contactNormal = V3Neg(planeNormal);
|
||||
|
||||
//ML:localNormal is the local space of plane normal, however, because shape1 is capulse and shape0 is plane, we need to use the reverse of contact normal(which will be the plane normal) to make the refreshContactPoints
|
||||
//work out the correct pentration for points
|
||||
const Vec3V localNormal = V3UnitX();
|
||||
|
||||
const FloatV contactDist = FLoad(params.mContactDistance);
|
||||
|
||||
const FloatV radius = FLoad(shapeCapsule.radius);
|
||||
const FloatV halfHeight = FLoad(shapeCapsule.halfHeight);
|
||||
|
||||
//capsule is in the local space of plane(n = (1.f, 0.f, 0.f), d=0.f)
|
||||
const Vec3V basisVector = QuatGetBasisVector0(aToB.q);
|
||||
const Vec3V tmp = V3Scale(basisVector, halfHeight);
|
||||
const Vec3V s = V3Add(aToB.p, tmp);
|
||||
const Vec3V e = V3Sub(aToB.p, tmp);
|
||||
|
||||
const FloatV inflatedRadius = FAdd(radius, contactDist);
|
||||
const FloatV replaceBreakingThreshold = FMul(radius, FLoad(0.001f));
|
||||
const FloatV projectBreakingThreshold = FMul(radius, FLoad(0.05f));
|
||||
const PxU32 initialContacts = manifold.mNumContacts;
|
||||
|
||||
//manifold.refreshContactPoints(curRTrans, projectBreakingThreshold, contactDist);
|
||||
const FloatV refreshDist = FAdd(contactDist, radius);
|
||||
manifold.refreshContactPoints(aToB, projectBreakingThreshold, refreshDist);
|
||||
|
||||
const PxU32 newContacts = manifold.mNumContacts;
|
||||
const bool bLostContacts = (newContacts != initialContacts);//((initialContacts == 0) || (newContacts != initialContacts));
|
||||
|
||||
if(bLostContacts || manifold.invalidate_PrimitivesPlane(aToB, radius, FLoad(0.02f)))
|
||||
{
|
||||
manifold.mNumContacts = 0;
|
||||
manifold.setRelativeTransform(aToB);
|
||||
//calculate the distance from s to the plane
|
||||
const FloatV signDist0 = V3GetX(s);//V3Dot(localNormal, s);
|
||||
if(FAllGrtr(inflatedRadius, signDist0))
|
||||
{
|
||||
const Vec3V localPointA = aToB.transformInv(s);
|
||||
const Vec3V localPointB = V3NegScaleSub(localNormal, signDist0, s);
|
||||
const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(localNormal), signDist0);
|
||||
//add to manifold
|
||||
manifold.addManifoldPoint2(localPointA, localPointB, localNormalPen, replaceBreakingThreshold);
|
||||
}
|
||||
|
||||
const FloatV signDist1 = V3GetX(e);//V3Dot(localNormal, e);
|
||||
if(FAllGrtr(inflatedRadius, signDist1))
|
||||
{
|
||||
const Vec3V localPointA = aToB.transformInv(e);
|
||||
const Vec3V localPointB = V3NegScaleSub(localNormal, signDist1, e);
|
||||
const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(localNormal), signDist1);
|
||||
//add to manifold
|
||||
manifold.addManifoldPoint2(localPointA, localPointB, localNormalPen, replaceBreakingThreshold);
|
||||
}
|
||||
|
||||
manifold.addManifoldContactsToContactBuffer(contactBuffer, contactNormal, planeNormal, transf0, radius, contactDist);
|
||||
|
||||
#if PCM_LOW_LEVEL_DEBUG
|
||||
manifold.drawManifold(*renderOutput, transf0, transf1);
|
||||
#endif
|
||||
|
||||
return manifold.getNumContacts() > 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
manifold.addManifoldContactsToContactBuffer(contactBuffer, contactNormal, planeNormal, transf0, radius, contactDist);
|
||||
return manifold.getNumContacts() > 0;
|
||||
}
|
||||
}
|
||||
227
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactPlaneConvex.cpp
vendored
Normal file
227
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactPlaneConvex.cpp
vendored
Normal file
@@ -0,0 +1,227 @@
|
||||
// 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 "geomutils/PxContactBuffer.h"
|
||||
#include "GuVecConvexHull.h"
|
||||
#include "GuContactMethodImpl.h"
|
||||
#include "GuPersistentContactManifold.h"
|
||||
|
||||
using namespace physx;
|
||||
|
||||
bool Gu::pcmContactPlaneConvex(GU_CONTACT_METHOD_ARGS)
|
||||
{
|
||||
PX_UNUSED(shape0);
|
||||
PX_UNUSED(renderOutput);
|
||||
|
||||
using namespace aos;
|
||||
|
||||
PersistentContactManifold& manifold = cache.getManifold();
|
||||
PxPrefetchLine(&manifold, 256);
|
||||
|
||||
// Get actual shape data
|
||||
const PxConvexMeshGeometry& shapeConvex = checkedCast<PxConvexMeshGeometry>(shape1);
|
||||
|
||||
const PxTransformV transf0 = loadTransformA(transform1);//convex transform
|
||||
const PxTransformV transf1 = loadTransformA(transform0);//plane transform
|
||||
//convex to plane
|
||||
const PxTransformV curTransf(transf1.transformInv(transf0));
|
||||
|
||||
const Vec3V vScale = V3LoadU_SafeReadW(shapeConvex.scale.scale); // PT: safe because 'rotation' follows 'scale' in PxMeshScale
|
||||
const ConvexHullData* hullData = _getHullData(shapeConvex);
|
||||
|
||||
const PxReal toleranceLength = params.mToleranceLength;
|
||||
const FloatV convexMargin = CalculatePCMConvexMargin(hullData, vScale, toleranceLength);
|
||||
|
||||
//in world space
|
||||
const Vec3V planeNormal = V3Normalize(QuatGetBasisVector0(transf1.q));
|
||||
const Vec3V negPlaneNormal = V3Neg(planeNormal);
|
||||
|
||||
const FloatV contactDist = FLoad(params.mContactDistance);
|
||||
|
||||
//const FloatV replaceBreakingThreshold = FMul(convexMargin, FLoad(0.001f));
|
||||
const FloatV projectBreakingThreshold = FMul(convexMargin, FLoad(0.2f));
|
||||
const PxU32 initialContacts = manifold.mNumContacts;
|
||||
|
||||
manifold.refreshContactPoints(curTransf, projectBreakingThreshold, contactDist);
|
||||
|
||||
const PxU32 newContacts = manifold.mNumContacts;
|
||||
const bool bLostContacts = (newContacts != initialContacts);//((initialContacts == 0) || (newContacts != initialContacts));
|
||||
|
||||
if(bLostContacts || manifold.invalidate_PrimitivesPlane(curTransf, convexMargin, FLoad(0.2f)))
|
||||
{
|
||||
const PxMatTransformV aToB(curTransf);
|
||||
const QuatV vQuat = QuatVLoadU(&shapeConvex.scale.rotation.x);
|
||||
|
||||
const Mat33V vertex2Shape = ConstructVertex2ShapeMatrix(vScale, vQuat);
|
||||
|
||||
//ML:localNormal is the local space of plane normal, however, because shape1 is box and shape0 is plane, we need to use the reverse of contact normal(which will be the plane normal) to make the refreshContactPoints
|
||||
//work out the correct pentration for points
|
||||
const Vec3V localNormal = V3UnitX();
|
||||
|
||||
manifold.mNumContacts = 0;
|
||||
manifold.setRelativeTransform(curTransf);
|
||||
const PxVec3* PX_RESTRICT verts = hullData->getHullVertices();
|
||||
|
||||
const PxU32 nbPolygons = hullData->mNbPolygons;
|
||||
|
||||
const Vec3V n = V3Normalize(M33MulV3(vertex2Shape, aToB.rotateInv(localNormal)));
|
||||
const Vec3V nnormal = V3Neg(n);
|
||||
const FloatV zero = FZero();
|
||||
|
||||
PersistentContact* manifoldContacts = PX_CP_TO_PCP(contactBuffer.contacts);
|
||||
PxU32 numContacts = 0;
|
||||
|
||||
const PxMatTransformV aToBVertexSpace(aToB.p, M33MulM33(aToB.rot, vertex2Shape));
|
||||
|
||||
FloatV minProj = FMax();
|
||||
PxU32 closestFaceIndex = 0;
|
||||
PxU32 polyIndex2 = 0xFFFFFFFF;
|
||||
|
||||
for (PxU32 i = 0; i < nbPolygons; ++i)
|
||||
{
|
||||
const HullPolygonData& polyData = hullData->mPolygons[i];
|
||||
const Vec3V planeN = V3LoadU_SafeReadW(polyData.mPlane.n); // PT: safe because 'd' follows 'n' in the plane class
|
||||
const FloatV proj = V3Dot(n, planeN);
|
||||
if (FAllGrtr(minProj, proj))
|
||||
{
|
||||
minProj = proj;
|
||||
closestFaceIndex = PxI32(i);
|
||||
}
|
||||
}
|
||||
|
||||
const PxU32 numEdges = hullData->mNbEdges;
|
||||
const PxU8* const edgeToFace = hullData->getFacesByEdges8();
|
||||
|
||||
//Loop through edges
|
||||
PxU32 closestEdge = 0xffffffff;
|
||||
//We subtract a small bias to increase the chances of selecting an edge below
|
||||
minProj = FSub(minProj, FLoad(5e-4f));
|
||||
FloatV maxDpSq = FMul(minProj, minProj);
|
||||
|
||||
for (PxU32 i = 0; i < numEdges; ++i)//, inc = VecI32V_Add(inc, vOne))
|
||||
{
|
||||
const PxU32 index = i * 2;
|
||||
const PxU8 f0 = edgeToFace[index];
|
||||
const PxU8 f1 = edgeToFace[index + 1];
|
||||
|
||||
const Vec3V planeNormal0 = V3LoadU_SafeReadW(hullData->mPolygons[f0].mPlane.n); // PT: safe because 'd' follows 'n' in the plane class
|
||||
const Vec3V planeNormal1 = V3LoadU_SafeReadW(hullData->mPolygons[f1].mPlane.n); // PT: safe because 'd' follows 'n' in the plane class
|
||||
|
||||
// unnormalized edge normal
|
||||
const Vec3V edgeNormal = V3Add(planeNormal0, planeNormal1);//polys[f0].mPlane.n + polys[f1].mPlane.n;
|
||||
const FloatV enMagSq = V3Dot(edgeNormal, edgeNormal);//edgeNormal.magnitudeSquared();
|
||||
//Test normal of current edge - squared test is valid if dp and maxDp both >= 0
|
||||
const FloatV dp = V3Dot(edgeNormal, nnormal);//edgeNormal.dot(normal);
|
||||
const FloatV sqDp = FMul(dp, dp);
|
||||
|
||||
const BoolV con0 = FIsGrtrOrEq(dp, zero);
|
||||
const BoolV con1 = FIsGrtr(sqDp, FMul(maxDpSq, enMagSq));
|
||||
const BoolV con = BAnd(con0, con1);
|
||||
if (BAllEqTTTT(con))
|
||||
{
|
||||
maxDpSq = FDiv(sqDp, enMagSq);
|
||||
closestEdge = i;
|
||||
}
|
||||
}
|
||||
|
||||
if (closestEdge != 0xffffffff)
|
||||
{
|
||||
const PxU8* FBE = edgeToFace;
|
||||
|
||||
const PxU32 index = closestEdge * 2;
|
||||
const PxU32 f0 = FBE[index];
|
||||
const PxU32 f1 = FBE[index + 1];
|
||||
|
||||
const Vec3V planeNormal0 = V3LoadU_SafeReadW(hullData->mPolygons[f0].mPlane.n); // PT: safe because 'd' follows 'n' in the plane class
|
||||
const Vec3V planeNormal1 = V3LoadU_SafeReadW(hullData->mPolygons[f1].mPlane.n); // PT: safe because 'd' follows 'n' in the plane class
|
||||
|
||||
const FloatV dp0 = V3Dot(planeNormal0, nnormal);
|
||||
const FloatV dp1 = V3Dot(planeNormal1, nnormal);
|
||||
if (FAllGrtr(dp0, dp1))
|
||||
{
|
||||
closestFaceIndex = PxI32(f0);
|
||||
polyIndex2 = PxI32(f1);
|
||||
}
|
||||
else
|
||||
{
|
||||
closestFaceIndex = PxI32(f1);
|
||||
polyIndex2 = PxI32(f0);
|
||||
}
|
||||
}
|
||||
|
||||
for (PxU32 index = closestFaceIndex; index != 0xFFFFFFFF; index = polyIndex2, polyIndex2 = 0xFFFFFFFF)
|
||||
{
|
||||
const HullPolygonData& face = hullData->mPolygons[closestFaceIndex];
|
||||
const PxU32 nbFaceVerts = face.mNbVerts;
|
||||
|
||||
const PxU8* vertInds = hullData->getVertexData8() + face.mVRef8;
|
||||
|
||||
for (PxU32 i = 0; i < nbFaceVerts; ++i)
|
||||
{
|
||||
const Vec3V pInVertexSpace = V3LoadU(verts[vertInds[i]]);
|
||||
|
||||
//transform p into plane space
|
||||
const Vec3V pInPlaneSpace = aToBVertexSpace.transform(pInVertexSpace);//V3Add(aToB.p, M33MulV3(temp1, pInVertexSpace));
|
||||
|
||||
const FloatV signDist = V3GetX(pInPlaneSpace);
|
||||
|
||||
if (FAllGrtr(contactDist, signDist))
|
||||
{
|
||||
//transform p into shape space
|
||||
const Vec3V pInShapeSpace = M33MulV3(vertex2Shape, pInVertexSpace);
|
||||
//add to manifold
|
||||
|
||||
manifoldContacts[numContacts].mLocalPointA = pInShapeSpace;
|
||||
manifoldContacts[numContacts].mLocalPointB = V3NegScaleSub(localNormal, signDist, pInPlaneSpace);
|
||||
manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(localNormal), signDist);
|
||||
|
||||
if (numContacts == 64)
|
||||
{
|
||||
manifold.reduceBatchContactsCluster(manifoldContacts, numContacts);
|
||||
numContacts = GU_MANIFOLD_CACHE_SIZE;
|
||||
|
||||
for (PxU32 c = 0; c < GU_MANIFOLD_CACHE_SIZE; ++c)
|
||||
manifoldContacts[c] = manifold.mContactPoints[c];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//reduce contacts
|
||||
//manifold.addBatchManifoldContactsCluster(manifoldContacts, numContacts);
|
||||
|
||||
manifold.addBatchManifoldContacts(manifoldContacts, numContacts, toleranceLength);
|
||||
}
|
||||
|
||||
manifold.addManifoldContactsToContactBuffer(contactBuffer, negPlaneNormal, transf1, contactDist);
|
||||
#if PCM_LOW_LEVEL_DEBUG
|
||||
manifold.drawManifold(*renderOutput, transf0, transf1);
|
||||
#endif
|
||||
return manifold.getNumContacts() > 0;
|
||||
}
|
||||
|
||||
131
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactSphereBox.cpp
vendored
Normal file
131
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactSphereBox.cpp
vendored
Normal file
@@ -0,0 +1,131 @@
|
||||
// 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 "geomutils/PxContactBuffer.h"
|
||||
#include "GuVecBox.h"
|
||||
#include "GuVecSphere.h"
|
||||
#include "GuContactMethodImpl.h"
|
||||
#include "GuPCMContactGenUtil.h"
|
||||
|
||||
using namespace physx;
|
||||
|
||||
bool Gu::pcmContactSphereBox(GU_CONTACT_METHOD_ARGS)
|
||||
{
|
||||
PX_UNUSED(renderOutput);
|
||||
PX_UNUSED(cache);
|
||||
|
||||
using namespace aos;
|
||||
// Get actual shape data
|
||||
const PxSphereGeometry& shapeSphere = checkedCast<PxSphereGeometry>(shape0);
|
||||
const PxBoxGeometry& shapeBox = checkedCast<PxBoxGeometry>(shape1);
|
||||
|
||||
//const PsTransformV transf0(transform0);
|
||||
const Vec3V sphereOrigin = V3LoadA(&transform0.p.x);
|
||||
//const PsTransformV transf1(transform1);
|
||||
|
||||
const QuatV q1 = QuatVLoadA(&transform1.q.x);
|
||||
const Vec3V p1 = V3LoadA(&transform1.p.x);
|
||||
|
||||
const FloatV radius = FLoad(shapeSphere.radius);
|
||||
|
||||
const PxTransformV transf1(p1, q1);
|
||||
|
||||
const FloatV cDist = FLoad(params.mContactDistance);
|
||||
|
||||
const Vec3V boxExtents = V3LoadU(shapeBox.halfExtents);
|
||||
|
||||
//translate sphere center into the box space
|
||||
const Vec3V sphereCenter = transf1.transformInv(sphereOrigin);
|
||||
|
||||
const Vec3V nBoxExtents = V3Neg(boxExtents);
|
||||
|
||||
//const FloatV radSq = FMul(radius, radius);
|
||||
|
||||
const FloatV inflatedSum = FAdd(radius, cDist);
|
||||
const FloatV sqInflatedSum = FMul(inflatedSum, inflatedSum);
|
||||
|
||||
const Vec3V p = V3Clamp(sphereCenter, nBoxExtents, boxExtents);
|
||||
const Vec3V v = V3Sub(sphereCenter, p);
|
||||
const FloatV lengthSq = V3Dot(v, v);
|
||||
|
||||
PX_ASSERT(contactBuffer.count < PxContactBuffer::MAX_CONTACTS);
|
||||
|
||||
if(FAllGrtr(sqInflatedSum, lengthSq))//intersect
|
||||
{
|
||||
//check whether the spherCenter is inside the box
|
||||
const BoolV bInsideBox = V3IsGrtrOrEq(boxExtents, V3Abs(sphereCenter));
|
||||
// PT: TODO: ??? revisit this, why do we have both BAllEqTTTT and BAllTrue3?
|
||||
if(BAllEqTTTT(BAllTrue3(bInsideBox)))//sphere center inside the box
|
||||
{
|
||||
//Pick directions and sign
|
||||
const Vec3V absP = V3Abs(p);
|
||||
const Vec3V distToSurface = V3Sub(boxExtents, absP);//dist from embedded center to box surface along 3 dimensions.
|
||||
|
||||
const FloatV x = V3GetX(distToSurface);
|
||||
const FloatV y = V3GetY(distToSurface);
|
||||
const FloatV z = V3GetZ(distToSurface);
|
||||
|
||||
const Vec3V xV = V3Splat(x);
|
||||
const Vec3V zV = V3Splat(z);
|
||||
|
||||
//find smallest element of distToSurface
|
||||
const BoolV con0 = BAllTrue3(V3IsGrtrOrEq(distToSurface, zV));
|
||||
const BoolV con1 = BAllTrue3(V3IsGrtrOrEq(distToSurface, xV));
|
||||
const Vec3V sign = V3Sign(p);
|
||||
|
||||
const Vec3V tmpX = V3Mul(V3UnitX(), sign);
|
||||
const Vec3V tmpY = V3Mul(V3UnitY(), sign);
|
||||
const Vec3V tmpZ = V3Mul(V3UnitZ(), sign);
|
||||
|
||||
const Vec3V locNorm = V3Sel(con0, tmpZ, V3Sel(con1, tmpX, tmpY));////local coords contact normal
|
||||
const FloatV dist = FNeg(FSel(con0, z, FSel(con1, x, y)));
|
||||
|
||||
//separation so far is just the embedding of the center point; we still have to push out all of the radius.
|
||||
const Vec3V normal = transf1.rotate(locNorm);
|
||||
const FloatV penetration = FSub(dist, radius);
|
||||
const Vec3V point = V3Sub(sphereOrigin , V3Scale(normal, dist));
|
||||
|
||||
outputSimplePCMContact(contactBuffer, point, normal, penetration);
|
||||
}
|
||||
else
|
||||
{
|
||||
//get the closest point from the center to the box surface
|
||||
const FloatV recipLength = FRsqrt(lengthSq);
|
||||
const FloatV length = FRecip(recipLength);
|
||||
const Vec3V locNorm = V3Scale(v, recipLength);
|
||||
const FloatV penetration = FSub(length, radius);
|
||||
const Vec3V normal = transf1.rotate(locNorm);
|
||||
const Vec3V point = transf1.transform(p);
|
||||
|
||||
outputSimplePCMContact(contactBuffer, point, normal, penetration);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
102
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactSphereCapsule.cpp
vendored
Normal file
102
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactSphereCapsule.cpp
vendored
Normal file
@@ -0,0 +1,102 @@
|
||||
// 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 "geomutils/PxContactBuffer.h"
|
||||
#include "GuVecSphere.h"
|
||||
#include "GuVecCapsule.h"
|
||||
#include "GuContactMethodImpl.h"
|
||||
#include "GuPCMContactGenUtil.h"
|
||||
|
||||
using namespace physx;
|
||||
using namespace aos;
|
||||
|
||||
static PX_FORCE_INLINE FloatV distancePointSegmentSquared(const Vec3VArg a, const Vec3VArg b, const Vec3VArg p, FloatV& param)
|
||||
{
|
||||
const FloatV zero = FZero();
|
||||
const FloatV one = FOne();
|
||||
|
||||
const Vec3V ap = V3Sub(p, a);
|
||||
const Vec3V ab = V3Sub(b, a);
|
||||
const FloatV nom = V3Dot(ap, ab);
|
||||
|
||||
const FloatV denom = V3Dot(ab, ab);
|
||||
const FloatV tValue = FClamp(FDiv(nom, denom), zero, one);
|
||||
|
||||
const FloatV t = FSel(FIsEq(denom, zero), zero, tValue);
|
||||
const Vec3V v = V3NegScaleSub(ab, t, ap);
|
||||
param = t;
|
||||
return V3Dot(v, v);
|
||||
}
|
||||
|
||||
bool Gu::pcmContactSphereCapsule(GU_CONTACT_METHOD_ARGS)
|
||||
{
|
||||
PX_UNUSED(cache);
|
||||
PX_UNUSED(renderOutput);
|
||||
|
||||
const PxSphereGeometry& shapeSphere = checkedCast<PxSphereGeometry>(shape0);
|
||||
const PxCapsuleGeometry& shapeCapsule = checkedCast<PxCapsuleGeometry>(shape1);
|
||||
|
||||
//Sphere in world space
|
||||
const Vec3V sphereCenter = V3LoadA(&transform0.p.x);
|
||||
const QuatV q1 = QuatVLoadA(&transform1.q.x);
|
||||
const Vec3V p1 = V3LoadA(&transform1.p.x);
|
||||
|
||||
const FloatV sphereRadius = FLoad(shapeSphere.radius);
|
||||
const FloatV capsuleRadius = FLoad(shapeCapsule.radius);
|
||||
const FloatV cDist = FLoad(params.mContactDistance);
|
||||
|
||||
//const FloatV r0 = FloatV_From_F32(shapeCapsule.radius);
|
||||
const FloatV halfHeight0 = FLoad(shapeCapsule.halfHeight);
|
||||
const Vec3V basisVector0 = QuatGetBasisVector0(q1);
|
||||
const Vec3V tmp0 = V3Scale(basisVector0, halfHeight0);
|
||||
const Vec3V s = V3Add(p1, tmp0);
|
||||
const Vec3V e = V3Sub(p1, tmp0);
|
||||
|
||||
const FloatV radiusSum = FAdd(sphereRadius, capsuleRadius);
|
||||
const FloatV inflatedSum = FAdd(radiusSum, cDist);
|
||||
|
||||
// Collision detection
|
||||
FloatV t;
|
||||
const FloatV squareDist = distancePointSegmentSquared(s, e, sphereCenter, t);
|
||||
const FloatV sqInflatedSum = FMul(inflatedSum, inflatedSum);
|
||||
|
||||
if(FAllGrtr(sqInflatedSum, squareDist))//BAllEq(con, bTrue))
|
||||
{
|
||||
const Vec3V p = V3ScaleAdd(V3Sub(e, s), t, s);
|
||||
const Vec3V dir = V3Sub(sphereCenter, p);
|
||||
const Vec3V normal = V3NormalizeSafe(dir, V3UnitX());
|
||||
const Vec3V point = V3NegScaleSub(normal, sphereRadius, sphereCenter);//transform back to the world space
|
||||
|
||||
const FloatV dist = FSub(FSqrt(squareDist), radiusSum);
|
||||
//context.mContactBuffer.contact(point, normal, FSub(FSqrt(squareDist), radiusSum));
|
||||
|
||||
return outputSimplePCMContact(contactBuffer, point, normal, dist);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
246
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactSphereConvex.cpp
vendored
Normal file
246
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactSphereConvex.cpp
vendored
Normal file
@@ -0,0 +1,246 @@
|
||||
// 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 "geomutils/PxContactBuffer.h"
|
||||
#include "GuGJKPenetration.h"
|
||||
#include "GuEPA.h"
|
||||
#include "GuVecCapsule.h"
|
||||
#include "GuVecConvexHull.h"
|
||||
#include "GuVecConvexHullNoScale.h"
|
||||
#include "GuContactMethodImpl.h"
|
||||
#include "GuPCMContactGen.h"
|
||||
#include "GuPCMShapeConvex.h"
|
||||
#include "GuPCMContactGenUtil.h"
|
||||
|
||||
using namespace physx;
|
||||
using namespace Gu;
|
||||
using namespace aos;
|
||||
|
||||
static void addToContactBuffer(PxContactBuffer& contactBuffer, const Vec3VArg worldNormal, const Vec3VArg worldPoint, const FloatVArg penDep)
|
||||
{
|
||||
outputSimplePCMContact(contactBuffer, worldPoint, worldNormal, penDep);
|
||||
}
|
||||
|
||||
static bool fullContactsGenerationSphereConvex(const CapsuleV& capsule, const ConvexHullV& convexHull, const PxTransformV& transf0, const PxTransformV& transf1,
|
||||
PersistentContact* manifoldContacts, PxContactBuffer& contactBuffer, const bool idtScale, PersistentContactManifold& manifold,
|
||||
Vec3VArg normal, const FloatVArg contactDist, bool doOverlapTest, PxRenderOutput* renderOutput)
|
||||
{
|
||||
PX_UNUSED(renderOutput);
|
||||
|
||||
PolygonalData polyData;
|
||||
getPCMConvexData(convexHull,idtScale, polyData);
|
||||
|
||||
PX_ALIGN(16, PxU8 buff[sizeof(SupportLocalImpl<ConvexHullV>)]);
|
||||
SupportLocal* map = (idtScale ? static_cast<SupportLocal*>(PX_PLACEMENT_NEW(buff, SupportLocalImpl<ConvexHullNoScaleV>)(static_cast<const ConvexHullNoScaleV&>(convexHull), transf1, convexHull.vertex2Shape, convexHull.shape2Vertex, idtScale)) :
|
||||
static_cast<SupportLocal*>(PX_PLACEMENT_NEW(buff, SupportLocalImpl<ConvexHullV>)(convexHull, transf1, convexHull.vertex2Shape, convexHull.shape2Vertex, idtScale)));
|
||||
|
||||
PxU32 numContacts = 0;
|
||||
if(generateSphereFullContactManifold(capsule, polyData, map, manifoldContacts, numContacts, contactDist, normal, doOverlapTest))
|
||||
{
|
||||
if(numContacts > 0)
|
||||
{
|
||||
PersistentContact& p = manifold.getContactPoint(0);
|
||||
|
||||
p.mLocalPointA = manifoldContacts[0].mLocalPointA;
|
||||
p.mLocalPointB = manifoldContacts[0].mLocalPointB;
|
||||
p.mLocalNormalPen = manifoldContacts[0].mLocalNormalPen;
|
||||
manifold.mNumContacts =1;
|
||||
|
||||
//transform normal to world space
|
||||
const Vec3V worldNormal = transf1.rotate(normal);
|
||||
const Vec3V worldP = V3NegScaleSub(worldNormal, capsule.radius, transf0.p);
|
||||
const FloatV penDep = FSub(V4GetW(manifoldContacts[0].mLocalNormalPen), capsule.radius);
|
||||
|
||||
#if PCM_LOW_LEVEL_DEBUG
|
||||
manifold.drawManifold(*renderOutput, transf0, transf1, capsule.radius);
|
||||
#endif
|
||||
addToContactBuffer(contactBuffer, worldNormal, worldP, penDep);
|
||||
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool Gu::pcmContactSphereConvex(GU_CONTACT_METHOD_ARGS)
|
||||
{
|
||||
PX_ASSERT(transform1.q.isSane());
|
||||
PX_ASSERT(transform0.q.isSane());
|
||||
|
||||
const PxConvexMeshGeometry& shapeConvex = checkedCast<PxConvexMeshGeometry>(shape1);
|
||||
const PxSphereGeometry& shapeSphere = checkedCast<PxSphereGeometry>(shape0);
|
||||
|
||||
PersistentContactManifold& manifold = cache.getManifold();
|
||||
|
||||
const Vec3V zeroV = V3Zero();
|
||||
|
||||
const ConvexHullData* hullData = _getHullData(shapeConvex);
|
||||
PxPrefetchLine(hullData);
|
||||
const Vec3V vScale = V3LoadU_SafeReadW(shapeConvex.scale.scale); // PT: safe because 'rotation' follows 'scale' in PxMeshScale
|
||||
const FloatV sphereRadius = FLoad(shapeSphere.radius);
|
||||
const FloatV contactDist = FLoad(params.mContactDistance);
|
||||
|
||||
//Transfer A into the local space of B
|
||||
const PxTransformV transf0 = loadTransformA(transform0);
|
||||
const PxTransformV transf1 = loadTransformA(transform1);
|
||||
const PxTransformV curRTrans(transf1.transformInv(transf0));
|
||||
const PxMatTransformV aToB(curRTrans);
|
||||
|
||||
const PxReal toleranceLength = params.mToleranceLength;
|
||||
const FloatV convexMargin = CalculatePCMConvexMargin(hullData, vScale, toleranceLength);
|
||||
|
||||
const PxU32 initialContacts = manifold.mNumContacts;
|
||||
const FloatV minMargin = FMin(convexMargin, sphereRadius);
|
||||
const FloatV projectBreakingThreshold = FMul(minMargin, FLoad(0.05f));
|
||||
|
||||
const FloatV refreshDistance = FAdd(sphereRadius, contactDist);
|
||||
manifold.refreshContactPoints(aToB, projectBreakingThreshold, refreshDistance);
|
||||
//ML: after refreshContactPoints, we might lose some contacts
|
||||
const bool bLostContacts = (manifold.mNumContacts != initialContacts);
|
||||
|
||||
if(bLostContacts || manifold.invalidate_SphereCapsule(curRTrans, minMargin))
|
||||
{
|
||||
GjkStatus status = manifold.mNumContacts > 0 ? GJK_UNDEFINED : GJK_NON_INTERSECT;
|
||||
|
||||
manifold.setRelativeTransform(curRTrans);
|
||||
|
||||
const QuatV vQuat = QuatVLoadU(&shapeConvex.scale.rotation.x);
|
||||
|
||||
const bool idtScale = shapeConvex.scale.isIdentity();
|
||||
//use the original shape
|
||||
const ConvexHullV convexHull(hullData, V3LoadU(hullData->mCenterOfMass), vScale, vQuat, idtScale);
|
||||
//transform capsule into the local space of convexHull
|
||||
const CapsuleV capsule(aToB.p, sphereRadius);
|
||||
|
||||
GjkOutput output;
|
||||
const LocalConvex<CapsuleV> convexA(capsule);
|
||||
const Vec3V initialSearchDir = V3Sub(capsule.getCenter(), convexHull.getCenter());
|
||||
if(idtScale)
|
||||
{
|
||||
const LocalConvex<ConvexHullNoScaleV> convexB(*PX_CONVEX_TO_NOSCALECONVEX(&convexHull));
|
||||
status = gjkPenetration<LocalConvex<CapsuleV>, LocalConvex<ConvexHullNoScaleV> >(convexA, convexB, initialSearchDir, contactDist, true,
|
||||
manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, output);
|
||||
}
|
||||
else
|
||||
{
|
||||
const LocalConvex<ConvexHullV> convexB(convexHull);
|
||||
status = gjkPenetration<LocalConvex<CapsuleV>, LocalConvex<ConvexHullV> >(convexA, convexB, initialSearchDir, contactDist, true,
|
||||
manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, output);
|
||||
}
|
||||
|
||||
if(status == GJK_NON_INTERSECT)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
else if(status == GJK_CONTACT)
|
||||
{
|
||||
PersistentContact& p = manifold.getContactPoint(0);
|
||||
p.mLocalPointA = zeroV;//sphere center
|
||||
p.mLocalPointB = output.closestB;
|
||||
p.mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(output.normal), output.penDep);
|
||||
manifold.mNumContacts =1;
|
||||
|
||||
#if PCM_LOW_LEVEL_DEBUG
|
||||
manifold.drawManifold(*renderOutput, transf0, transf1, capsule.radius);
|
||||
#endif
|
||||
|
||||
//transform normal to world space
|
||||
const Vec3V worldNormal = transf1.rotate(output.normal);
|
||||
const Vec3V worldP = V3NegScaleSub(worldNormal, sphereRadius, transf0.p);
|
||||
const FloatV penDep = FSub(output.penDep, sphereRadius);
|
||||
addToContactBuffer(contactBuffer, worldNormal, worldP, penDep);
|
||||
return true;
|
||||
}
|
||||
else if(status == GJK_DEGENERATE)
|
||||
{
|
||||
PersistentContact* manifoldContacts = PX_CP_TO_PCP(contactBuffer.contacts);
|
||||
|
||||
return fullContactsGenerationSphereConvex(capsule, convexHull, transf0, transf1, manifoldContacts, contactBuffer, idtScale,
|
||||
manifold, output.normal, contactDist, true, renderOutput);
|
||||
}
|
||||
else if (status == EPA_CONTACT)
|
||||
{
|
||||
if (idtScale)
|
||||
{
|
||||
const LocalConvex<ConvexHullNoScaleV> convexB(*PX_CONVEX_TO_NOSCALECONVEX(&convexHull));
|
||||
status = epaPenetration(convexA, convexB, manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints,
|
||||
true, FLoad(toleranceLength), output);
|
||||
}
|
||||
else
|
||||
{
|
||||
const LocalConvex<ConvexHullV> convexB(convexHull);
|
||||
status = epaPenetration(convexA, convexB, manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints,
|
||||
true, FLoad(toleranceLength), output);
|
||||
}
|
||||
|
||||
if (status == EPA_CONTACT)
|
||||
{
|
||||
PersistentContact& p = manifold.getContactPoint(0);
|
||||
p.mLocalPointA = zeroV;//sphere center
|
||||
p.mLocalPointB = output.closestB;
|
||||
p.mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(output.normal), output.penDep);
|
||||
manifold.mNumContacts = 1;
|
||||
|
||||
#if PCM_LOW_LEVEL_DEBUG
|
||||
manifold.drawManifold(*renderOutput, transf0, transf1, capsule.radius);
|
||||
#endif
|
||||
//transform normal to world space
|
||||
const Vec3V worldNormal = transf1.rotate(output.normal);
|
||||
const Vec3V worldP = V3NegScaleSub(worldNormal, sphereRadius, transf0.p);
|
||||
const FloatV penDep = FSub(output.penDep, sphereRadius);
|
||||
|
||||
addToContactBuffer(contactBuffer, worldNormal, worldP, penDep);
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
PersistentContact* manifoldContacts = PX_CP_TO_PCP(contactBuffer.contacts);
|
||||
return fullContactsGenerationSphereConvex(capsule, convexHull, transf0, transf1, manifoldContacts, contactBuffer, idtScale,
|
||||
manifold, output.normal, contactDist, true, renderOutput);
|
||||
}
|
||||
}
|
||||
}
|
||||
else if(manifold.mNumContacts > 0)
|
||||
{
|
||||
//ML:: the manifold originally has contacts
|
||||
PersistentContact& p = manifold.getContactPoint(0);
|
||||
const Vec3V worldNormal = transf1.rotate(Vec3V_From_Vec4V(p.mLocalNormalPen));
|
||||
const Vec3V worldP = V3NegScaleSub(worldNormal, sphereRadius, transf0.p);
|
||||
const FloatV penDep = FSub(V4GetW(p.mLocalNormalPen), sphereRadius);
|
||||
|
||||
#if PCM_LOW_LEVEL_DEBUG
|
||||
manifold.drawManifold(*renderOutput, transf0, transf1, sphereRadius);
|
||||
#endif
|
||||
addToContactBuffer(contactBuffer, worldNormal, worldP, penDep);
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
143
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactSphereHeightField.cpp
vendored
Normal file
143
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactSphereHeightField.cpp
vendored
Normal file
@@ -0,0 +1,143 @@
|
||||
// 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/PxTriangleMesh.h"
|
||||
#include "geomutils/PxContactBuffer.h"
|
||||
#include "GuVecBox.h"
|
||||
#include "GuVecConvexHull.h"
|
||||
#include "GuVecConvexHullNoScale.h"
|
||||
#include "GuVecTriangle.h"
|
||||
#include "GuContactMethodImpl.h"
|
||||
#include "GuHeightField.h"
|
||||
#include "GuPCMContactConvexCommon.h"
|
||||
#include "GuPCMContactMeshCallback.h"
|
||||
|
||||
using namespace physx;
|
||||
using namespace Gu;
|
||||
using namespace aos;
|
||||
|
||||
namespace
|
||||
{
|
||||
struct PCMSphereVsHeightfieldContactGenerationCallback : PCMHeightfieldContactGenerationCallback<PCMSphereVsHeightfieldContactGenerationCallback>
|
||||
{
|
||||
PCMSphereVsMeshContactGeneration mGeneration;
|
||||
|
||||
PCMSphereVsHeightfieldContactGenerationCallback(
|
||||
const Vec3VArg sphereCenter,
|
||||
const FloatVArg sphereRadius,
|
||||
const FloatVArg contactDistance,
|
||||
const FloatVArg replaceBreakingThreshold,
|
||||
const PxTransformV& sphereTransform,
|
||||
const PxTransformV& heightfieldTransform,
|
||||
const PxTransform& heightfieldTransform1,
|
||||
MultiplePersistentContactManifold& multiManifold,
|
||||
PxContactBuffer& contactBuffer,
|
||||
PxInlineArray<PxU32, LOCAL_PCM_CONTACTS_SIZE>* deferredContacts,
|
||||
HeightFieldUtil& hfUtil
|
||||
) :
|
||||
PCMHeightfieldContactGenerationCallback<PCMSphereVsHeightfieldContactGenerationCallback>(hfUtil, heightfieldTransform1),
|
||||
mGeneration(sphereCenter, sphereRadius, contactDistance, replaceBreakingThreshold, sphereTransform,
|
||||
heightfieldTransform, multiManifold, contactBuffer, deferredContacts)
|
||||
{
|
||||
}
|
||||
|
||||
template<PxU32 CacheSize>
|
||||
void processTriangleCache(TriangleCache<CacheSize>& cache)
|
||||
{
|
||||
mGeneration.processTriangleCache<CacheSize, PCMSphereVsMeshContactGeneration>(cache);
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
bool Gu::pcmContactSphereHeightField(GU_CONTACT_METHOD_ARGS)
|
||||
{
|
||||
PX_UNUSED(renderOutput);
|
||||
|
||||
const PxSphereGeometry& shapeSphere = checkedCast<PxSphereGeometry>(shape0);
|
||||
const PxHeightFieldGeometry& shapeHeight = checkedCast<PxHeightFieldGeometry>(shape1);
|
||||
|
||||
MultiplePersistentContactManifold& multiManifold = cache.getMultipleManifold();
|
||||
|
||||
const QuatV q0 = QuatVLoadA(&transform0.q.x);
|
||||
const Vec3V p0 = V3LoadA(&transform0.p.x);
|
||||
|
||||
const QuatV q1 = QuatVLoadA(&transform1.q.x);
|
||||
const Vec3V p1 = V3LoadA(&transform1.p.x);
|
||||
|
||||
const FloatV sphereRadius = FLoad(shapeSphere.radius);
|
||||
const FloatV contactDist = FLoad(params.mContactDistance);
|
||||
|
||||
const PxTransformV sphereTransform(p0, q0);//sphere transform
|
||||
const PxTransformV heightfieldTransform(p1, q1);//height feild
|
||||
const PxTransformV curTransform = heightfieldTransform.transformInv(sphereTransform);
|
||||
|
||||
// We must be in local space to use the cache
|
||||
|
||||
if(multiManifold.invalidate(curTransform, sphereRadius, FLoad(0.02f)))
|
||||
{
|
||||
multiManifold.mNumManifolds = 0;
|
||||
multiManifold.setRelativeTransform(curTransform);
|
||||
|
||||
const FloatV replaceBreakingThreshold = FMul(sphereRadius, FLoad(0.001f));
|
||||
HeightFieldUtil hfUtil(shapeHeight);
|
||||
|
||||
PxBounds3 localBounds;
|
||||
const PxVec3 localSphereCenter = getLocalSphereData(localBounds, transform0, transform1, shapeSphere.radius + params.mContactDistance);
|
||||
|
||||
const Vec3V sphereCenter = V3LoadU(localSphereCenter);
|
||||
|
||||
PxInlineArray<PxU32, LOCAL_PCM_CONTACTS_SIZE> delayedContacts;
|
||||
|
||||
PCMSphereVsHeightfieldContactGenerationCallback blockCallback(
|
||||
sphereCenter,
|
||||
sphereRadius,
|
||||
contactDist,
|
||||
replaceBreakingThreshold,
|
||||
sphereTransform,
|
||||
heightfieldTransform,
|
||||
transform1,
|
||||
multiManifold,
|
||||
contactBuffer,
|
||||
&delayedContacts,
|
||||
hfUtil);
|
||||
|
||||
hfUtil.overlapAABBTriangles(localBounds, blockCallback);
|
||||
|
||||
blockCallback.mGeneration.generateLastContacts();
|
||||
blockCallback.mGeneration.processContacts(GU_SPHERE_MANIFOLD_CACHE_SIZE, false);
|
||||
}
|
||||
else
|
||||
{
|
||||
const PxMatTransformV aToB(curTransform);
|
||||
const FloatV projectBreakingThreshold = FMul(sphereRadius, FLoad(0.05f));
|
||||
const FloatV refereshDistance = FAdd(sphereRadius, contactDist);
|
||||
multiManifold.refreshManifold(aToB, projectBreakingThreshold, refereshDistance);
|
||||
}
|
||||
|
||||
return multiManifold.addManifoldContactsToContactBuffer(contactBuffer, sphereTransform, heightfieldTransform, sphereRadius);
|
||||
}
|
||||
501
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactSphereMesh.cpp
vendored
Normal file
501
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactSphereMesh.cpp
vendored
Normal file
@@ -0,0 +1,501 @@
|
||||
// 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 "foundation/PxSort.h"
|
||||
#include "GuContactMethodImpl.h"
|
||||
#include "GuPCMContactConvexCommon.h"
|
||||
#include "GuPCMContactMeshCallback.h"
|
||||
#include "GuFeatureCode.h"
|
||||
#include "GuBox.h"
|
||||
|
||||
using namespace physx;
|
||||
using namespace Gu;
|
||||
using namespace aos;
|
||||
|
||||
namespace
|
||||
{
|
||||
struct PCMSphereVsMeshContactGenerationCallback : PCMMeshContactGenerationCallback< PCMSphereVsMeshContactGenerationCallback >
|
||||
{
|
||||
PCMSphereVsMeshContactGeneration mGeneration;
|
||||
|
||||
PCMSphereVsMeshContactGenerationCallback(
|
||||
const Vec3VArg sphereCenter,
|
||||
const FloatVArg sphereRadius,
|
||||
const FloatVArg contactDist,
|
||||
const FloatVArg replaceBreakingThreshold,
|
||||
const PxTransformV& sphereTransform,
|
||||
const PxTransformV& meshTransform,
|
||||
MultiplePersistentContactManifold& multiManifold,
|
||||
PxContactBuffer& contactBuffer,
|
||||
const PxU8* extraTriData,
|
||||
const Cm::FastVertex2ShapeScaling& meshScaling,
|
||||
bool idtMeshScale,
|
||||
PxInlineArray<PxU32, LOCAL_PCM_CONTACTS_SIZE>* deferredContacts,
|
||||
PxRenderOutput* renderOutput = NULL
|
||||
) :
|
||||
PCMMeshContactGenerationCallback<PCMSphereVsMeshContactGenerationCallback>(meshScaling, extraTriData, idtMeshScale),
|
||||
mGeneration(sphereCenter, sphereRadius, contactDist, replaceBreakingThreshold, sphereTransform, meshTransform, multiManifold, contactBuffer, deferredContacts, renderOutput)
|
||||
{
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE bool doTest(const PxVec3&, const PxVec3&, const PxVec3&) { return true; }
|
||||
|
||||
template<PxU32 CacheSize>
|
||||
void processTriangleCache(TriangleCache<CacheSize>& cache)
|
||||
{
|
||||
mGeneration.processTriangleCache<CacheSize, PCMSphereVsMeshContactGeneration>(cache);
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
bool Gu::pcmContactSphereMesh(GU_CONTACT_METHOD_ARGS)
|
||||
{
|
||||
PX_UNUSED(renderOutput);
|
||||
|
||||
const PxSphereGeometry& shapeSphere = checkedCast<PxSphereGeometry>(shape0);
|
||||
const PxTriangleMeshGeometry& shapeMesh = checkedCast<PxTriangleMeshGeometry>(shape1);
|
||||
|
||||
MultiplePersistentContactManifold& multiManifold = cache.getMultipleManifold();
|
||||
|
||||
const QuatV q0 = QuatVLoadA(&transform0.q.x);
|
||||
const Vec3V p0 = V3LoadA(&transform0.p.x);
|
||||
|
||||
const QuatV q1 = QuatVLoadA(&transform1.q.x);
|
||||
const Vec3V p1 = V3LoadA(&transform1.p.x);
|
||||
|
||||
const FloatV sphereRadius = FLoad(shapeSphere.radius);
|
||||
const FloatV contactDist = FLoad(params.mContactDistance);
|
||||
|
||||
const PxTransformV sphereTransform(p0, q0);//sphere transform
|
||||
const PxTransformV meshTransform(p1, q1);//triangleMesh
|
||||
const PxTransformV curTransform = meshTransform.transformInv(sphereTransform);
|
||||
|
||||
// We must be in local space to use the cache
|
||||
if(multiManifold.invalidate(curTransform, sphereRadius, FLoad(0.02f)))
|
||||
{
|
||||
const FloatV replaceBreakingThreshold = FMul(sphereRadius, FLoad(0.001f));
|
||||
const PxVec3 sphereCenterShape1Space = transform1.transformInv(transform0.p);
|
||||
PxReal inflatedRadius = shapeSphere.radius + params.mContactDistance;
|
||||
|
||||
const Vec3V sphereCenter = V3LoadU(sphereCenterShape1Space);
|
||||
|
||||
const TriangleMesh* meshData = _getMeshData(shapeMesh);
|
||||
|
||||
Cm::FastVertex2ShapeScaling meshScaling; // PT: TODO: get rid of default ctor :(
|
||||
const bool idtMeshScale = shapeMesh.scale.isIdentity();
|
||||
if(!idtMeshScale)
|
||||
meshScaling.init(shapeMesh.scale);
|
||||
|
||||
multiManifold.mNumManifolds = 0;
|
||||
multiManifold.setRelativeTransform(curTransform);
|
||||
|
||||
PxInlineArray<PxU32, LOCAL_PCM_CONTACTS_SIZE> delayedContacts;
|
||||
|
||||
const PxU8* PX_RESTRICT extraData = meshData->getExtraTrigData();
|
||||
// mesh scale is not baked into cached verts
|
||||
PCMSphereVsMeshContactGenerationCallback callback(
|
||||
sphereCenter,
|
||||
sphereRadius,
|
||||
contactDist,
|
||||
replaceBreakingThreshold,
|
||||
sphereTransform,
|
||||
meshTransform,
|
||||
multiManifold,
|
||||
contactBuffer,
|
||||
extraData,
|
||||
meshScaling,
|
||||
idtMeshScale,
|
||||
&delayedContacts,
|
||||
renderOutput);
|
||||
|
||||
PxVec3 obbCenter = sphereCenterShape1Space;
|
||||
PxVec3 obbExtents = PxVec3(inflatedRadius);
|
||||
PxMat33 obbRot(PxIdentity);
|
||||
if(!idtMeshScale)
|
||||
meshScaling.transformQueryBounds(obbCenter, obbExtents, obbRot);
|
||||
const Box obb(obbCenter, obbExtents, obbRot);
|
||||
|
||||
Midphase::intersectOBB(meshData, obb, callback, true);
|
||||
|
||||
callback.flushCache();
|
||||
|
||||
callback.mGeneration.generateLastContacts();
|
||||
callback.mGeneration.processContacts(GU_SPHERE_MANIFOLD_CACHE_SIZE, false);
|
||||
}
|
||||
else
|
||||
{
|
||||
const PxMatTransformV aToB(curTransform);
|
||||
const FloatV projectBreakingThreshold = FMul(sphereRadius, FLoad(0.05f));
|
||||
const FloatV refereshDistance = FAdd(sphereRadius, contactDist);
|
||||
multiManifold.refreshManifold(aToB, projectBreakingThreshold, refereshDistance);
|
||||
}
|
||||
|
||||
//multiManifold.drawManifold(*gRenderOutPut, sphereTransform, meshTransform);
|
||||
return multiManifold.addManifoldContactsToContactBuffer(contactBuffer, sphereTransform, meshTransform, sphereRadius);
|
||||
}
|
||||
|
||||
static FloatV pcmDistancePointTriangleSquared( const Vec3VArg p,
|
||||
const Vec3VArg a,
|
||||
const Vec3VArg b,
|
||||
const Vec3VArg c,
|
||||
Vec3V& closestP,
|
||||
FeatureCode& fc)
|
||||
{
|
||||
const FloatV zero = FZero();
|
||||
const Vec3V ab = V3Sub(b, a);
|
||||
const Vec3V ac = V3Sub(c, a);
|
||||
const Vec3V bc = V3Sub(c, b);
|
||||
const Vec3V ap = V3Sub(p, a);
|
||||
const Vec3V bp = V3Sub(p, b);
|
||||
const Vec3V cp = V3Sub(p, c);
|
||||
|
||||
const FloatV d1 = V3Dot(ab, ap); // snom
|
||||
const FloatV d2 = V3Dot(ac, ap); // tnom
|
||||
const FloatV d3 = V3Dot(ab, bp); // -sdenom
|
||||
const FloatV d4 = V3Dot(ac, bp); // unom = d4 - d3
|
||||
const FloatV d5 = V3Dot(ab, cp); // udenom = d5 - d6
|
||||
const FloatV d6 = V3Dot(ac, cp); // -tdenom
|
||||
const FloatV unom = FSub(d4, d3);
|
||||
const FloatV udenom = FSub(d5, d6);
|
||||
|
||||
const Vec3V n = V3Cross(ab, ac);
|
||||
const VecCrossV crossA = V3PrepareCross(ap);
|
||||
const VecCrossV crossB = V3PrepareCross(bp);
|
||||
const VecCrossV crossC = V3PrepareCross(cp);
|
||||
const Vec3V bCrossC = V3Cross(crossB, crossC);
|
||||
const Vec3V cCrossA = V3Cross(crossC, crossA);
|
||||
const Vec3V aCrossB = V3Cross(crossA, crossB);
|
||||
|
||||
//const FloatV va = V3Dot(n, bCrossC);//edge region of BC, signed area rbc, u = S(rbc)/S(abc) for a
|
||||
//const FloatV vb = V3Dot(n, cCrossA);//edge region of AC, signed area rac, v = S(rca)/S(abc) for b
|
||||
//const FloatV vc = V3Dot(n, aCrossB);//edge region of AB, signed area rab, w = S(rab)/S(abc) for c
|
||||
|
||||
//check if p in vertex region outside a
|
||||
const BoolV con00 = FIsGrtr(zero, d1); // snom <= 0
|
||||
const BoolV con01 = FIsGrtr(zero, d2); // tnom <= 0
|
||||
const BoolV con0 = BAnd(con00, con01); // vertex region a
|
||||
|
||||
if(BAllEqTTTT(con0))
|
||||
{
|
||||
//Vertex 0
|
||||
fc = FC_VERTEX0;
|
||||
closestP = a;
|
||||
return V3Dot(ap, ap);
|
||||
}
|
||||
|
||||
//check if p in vertex region outside b
|
||||
const BoolV con10 = FIsGrtrOrEq(d3, zero);
|
||||
const BoolV con11 = FIsGrtrOrEq(d3, d4);
|
||||
const BoolV con1 = BAnd(con10, con11); // vertex region b
|
||||
if(BAllEqTTTT(con1))
|
||||
{
|
||||
//Vertex 1
|
||||
fc = FC_VERTEX1;
|
||||
closestP = b;
|
||||
return V3Dot(bp, bp);
|
||||
}
|
||||
|
||||
//check if p in vertex region outside c
|
||||
const BoolV con20 = FIsGrtrOrEq(d6, zero);
|
||||
const BoolV con21 = FIsGrtrOrEq(d6, d5);
|
||||
const BoolV con2 = BAnd(con20, con21); // vertex region c
|
||||
if(BAllEqTTTT(con2))
|
||||
{
|
||||
//Vertex 2
|
||||
fc = FC_VERTEX2;
|
||||
closestP = c;
|
||||
return V3Dot(cp, cp);
|
||||
}
|
||||
|
||||
//check if p in edge region of AB
|
||||
//const FloatV vc = FSub(FMul(d1, d4), FMul(d3, d2));
|
||||
const FloatV vc = V3Dot(n, aCrossB);//edge region of AB, signed area rab, w = S(rab)/S(abc) for c
|
||||
const BoolV con30 = FIsGrtr(zero, vc);
|
||||
const BoolV con31 = FIsGrtrOrEq(d1, zero);
|
||||
const BoolV con32 = FIsGrtr(zero, d3);
|
||||
const BoolV con3 = BAnd(con30, BAnd(con31, con32));
|
||||
if(BAllEqTTTT(con3))
|
||||
{
|
||||
// Edge 01
|
||||
fc = FC_EDGE01;
|
||||
const FloatV sScale = FDiv(d1, FSub(d1, d3));
|
||||
const Vec3V closest3 = V3ScaleAdd(ab, sScale, a);//V3Add(a, V3Scale(ab, sScale));
|
||||
const Vec3V vv = V3Sub(p, closest3);
|
||||
closestP = closest3;
|
||||
return V3Dot(vv, vv);
|
||||
}
|
||||
|
||||
//check if p in edge region of BC
|
||||
//const FloatV va = FSub(FMul(d3, d6),FMul(d5, d4));
|
||||
const FloatV va = V3Dot(n, bCrossC);//edge region of BC, signed area rbc, u = S(rbc)/S(abc) for a
|
||||
|
||||
const BoolV con40 = FIsGrtr(zero, va);
|
||||
const BoolV con41 = FIsGrtrOrEq(d4, d3);
|
||||
const BoolV con42 = FIsGrtrOrEq(d5, d6);
|
||||
const BoolV con4 = BAnd(con40, BAnd(con41, con42));
|
||||
if(BAllEqTTTT(con4))
|
||||
{
|
||||
// Edge 12
|
||||
fc = FC_EDGE12;
|
||||
const FloatV uScale = FDiv(unom, FAdd(unom, udenom));
|
||||
const Vec3V closest4 = V3ScaleAdd(bc, uScale, b);//V3Add(b, V3Scale(bc, uScale));
|
||||
const Vec3V vv = V3Sub(p, closest4);
|
||||
closestP = closest4;
|
||||
return V3Dot(vv, vv);
|
||||
}
|
||||
|
||||
//check if p in edge region of AC
|
||||
//const FloatV vb = FSub(FMul(d5, d2), FMul(d1, d6));
|
||||
const FloatV vb = V3Dot(n, cCrossA);//edge region of AC, signed area rac, v = S(rca)/S(abc) for b
|
||||
const BoolV con50 = FIsGrtr(zero, vb);
|
||||
const BoolV con51 = FIsGrtrOrEq(d2, zero);
|
||||
const BoolV con52 = FIsGrtr(zero, d6);
|
||||
const BoolV con5 = BAnd(con50, BAnd(con51, con52));
|
||||
if(BAllEqTTTT(con5))
|
||||
{
|
||||
//Edge 20
|
||||
fc = FC_EDGE20;
|
||||
const FloatV tScale = FDiv(d2, FSub(d2, d6));
|
||||
const Vec3V closest5 = V3ScaleAdd(ac, tScale, a);//V3Add(a, V3Scale(ac, tScale));
|
||||
const Vec3V vv = V3Sub(p, closest5);
|
||||
closestP = closest5;
|
||||
return V3Dot(vv, vv);
|
||||
}
|
||||
|
||||
fc = FC_FACE;
|
||||
|
||||
//P must project inside face region. Compute Q using Barycentric coordinates
|
||||
const FloatV nn = V3Dot(n, n);
|
||||
const FloatV t = FDiv(V3Dot(n, V3Sub(a, p)), nn);
|
||||
const Vec3V vv = V3Scale(n, t);
|
||||
closestP = V3Add(p, vv);
|
||||
return V3Dot(vv, vv);
|
||||
}
|
||||
|
||||
bool Gu::PCMSphereVsMeshContactGeneration::processTriangle(const PxVec3* verts, PxU32 triangleIndex, PxU8 triFlags, const PxU32* vertInds)
|
||||
{
|
||||
const FloatV zero = FZero();
|
||||
|
||||
const Vec3V v0 = V3LoadU(verts[0]);
|
||||
const Vec3V v1 = V3LoadU(verts[1]);
|
||||
const Vec3V v2 = V3LoadU(verts[2]);
|
||||
|
||||
const Vec3V v10 = V3Sub(v1, v0);
|
||||
const Vec3V v20 = V3Sub(v2, v0);
|
||||
|
||||
const Vec3V n = V3Normalize(V3Cross(v10, v20));//(p1 - p0).cross(p2 - p0).getNormalized();
|
||||
const FloatV d = V3Dot(v0, n);//d = -p0.dot(n);
|
||||
|
||||
const FloatV dist0 = FSub(V3Dot(mSphereCenter, n), d);//p.dot(n) + d;
|
||||
|
||||
// Backface culling
|
||||
if(FAllGrtr(zero, dist0))
|
||||
return false;
|
||||
|
||||
Vec3V closestP;
|
||||
//mSphereCenter will be in the local space of the triangle mesh
|
||||
FeatureCode fc;
|
||||
FloatV sqDist = pcmDistancePointTriangleSquared(mSphereCenter, v0, v1, v2, closestP, fc);
|
||||
|
||||
//sphere overlap with triangles
|
||||
if (FAllGrtr(mSqInflatedSphereRadius, sqDist))
|
||||
{
|
||||
//sphere center is on the triangle surface, we take triangle normal as the patchNormal. Otherwise, we need to calculate the patchNormal
|
||||
if(fc==FC_FACE)
|
||||
{
|
||||
const Vec3V patchNormal = n;
|
||||
|
||||
const FloatV dist = FSqrt(sqDist);
|
||||
|
||||
mEdgeCache.addData(CachedEdge(vertInds[0], vertInds[1]));
|
||||
mEdgeCache.addData(CachedEdge(vertInds[1], vertInds[2]));
|
||||
mEdgeCache.addData(CachedEdge(vertInds[2], vertInds[0]));
|
||||
|
||||
mVertexCache.addData(CachedVertex(vertInds[0]));
|
||||
mVertexCache.addData(CachedVertex(vertInds[1]));
|
||||
mVertexCache.addData(CachedVertex(vertInds[2]));
|
||||
|
||||
addToPatch(closestP, patchNormal, dist, triangleIndex);
|
||||
}
|
||||
else
|
||||
{
|
||||
const Vec3V patchNormal = V3Normalize(V3Sub(mSphereCenter, closestP));
|
||||
|
||||
//ML : defer the contacts generation
|
||||
const PxU32 nb = sizeof(PCMDeferredPolyData) / sizeof(PxU32);
|
||||
PxU32 newSize = nb + mDeferredContacts->size();
|
||||
if(mDeferredContacts->capacity() < newSize)
|
||||
mDeferredContacts->reserve(2*(newSize+1));
|
||||
PCMDeferredPolyData* PX_RESTRICT data = reinterpret_cast<PCMDeferredPolyData*>(mDeferredContacts->end());
|
||||
mDeferredContacts->forceSize_Unsafe(newSize);
|
||||
|
||||
SortedTriangle sortedTriangle;
|
||||
sortedTriangle.mSquareDist = sqDist;
|
||||
sortedTriangle.mIndex = mSortedTriangle.size();
|
||||
mSortedTriangle.pushBack(sortedTriangle);
|
||||
|
||||
data->mTriangleIndex = triangleIndex;
|
||||
data->mFeatureIndex = fc;
|
||||
data->triFlags32 = PxU32(triFlags);
|
||||
data->mInds[0] = vertInds[0];
|
||||
data->mInds[1] = vertInds[1];
|
||||
data->mInds[2] = vertInds[2];
|
||||
V3StoreU(closestP, data->mVerts[0]);
|
||||
V3StoreU(patchNormal, data->mVerts[1]);
|
||||
V3StoreU(V3Splat(sqDist), data->mVerts[2]);
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void Gu::PCMSphereVsMeshContactGeneration::addToPatch(const Vec3VArg contactP, const Vec3VArg patchNormal, const FloatV dist, PxU32 triangleIndex)
|
||||
{
|
||||
PX_ASSERT(mNumContactPatch < PCM_MAX_CONTACTPATCH_SIZE);
|
||||
|
||||
const Vec3V sphereCenter = V3Zero(); // in sphere local space
|
||||
|
||||
bool foundPatch = false;
|
||||
if (mNumContactPatch > 0)
|
||||
{
|
||||
if (FAllGrtr(V3Dot(mContactPatch[mNumContactPatch - 1].mPatchNormal, patchNormal), mAcceptanceEpsilon))
|
||||
{
|
||||
PCMContactPatch& patch = mContactPatch[mNumContactPatch - 1];
|
||||
|
||||
PX_ASSERT((patch.mEndIndex - patch.mStartIndex) == 1);
|
||||
|
||||
if (FAllGrtr(patch.mPatchMaxPen, dist))
|
||||
{
|
||||
//overwrite the old contact
|
||||
mManifoldContacts[patch.mStartIndex].mLocalPointA = sphereCenter;//in sphere's space
|
||||
mManifoldContacts[patch.mStartIndex].mLocalPointB = contactP;
|
||||
mManifoldContacts[patch.mStartIndex].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(patchNormal), dist);
|
||||
mManifoldContacts[patch.mStartIndex].mFaceIndex = triangleIndex;
|
||||
patch.mPatchMaxPen = dist;
|
||||
}
|
||||
|
||||
foundPatch = true;
|
||||
}
|
||||
}
|
||||
if (!foundPatch)
|
||||
{
|
||||
mManifoldContacts[mNumContacts].mLocalPointA = sphereCenter;//in sphere's space
|
||||
mManifoldContacts[mNumContacts].mLocalPointB = contactP;
|
||||
mManifoldContacts[mNumContacts].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(patchNormal), dist);
|
||||
mManifoldContacts[mNumContacts++].mFaceIndex = triangleIndex;
|
||||
|
||||
mContactPatch[mNumContactPatch].mStartIndex = mNumContacts - 1;
|
||||
mContactPatch[mNumContactPatch].mEndIndex = mNumContacts;
|
||||
mContactPatch[mNumContactPatch].mPatchMaxPen = dist;
|
||||
mContactPatch[mNumContactPatch++].mPatchNormal = patchNormal;
|
||||
}
|
||||
|
||||
PX_ASSERT(mNumContactPatch < PCM_MAX_CONTACTPATCH_SIZE);
|
||||
|
||||
if (mNumContacts >= 16)
|
||||
{
|
||||
PX_ASSERT(mNumContacts <= 64);
|
||||
processContacts(GU_SPHERE_MANIFOLD_CACHE_SIZE);
|
||||
}
|
||||
}
|
||||
|
||||
void Gu::PCMSphereVsMeshContactGeneration::generateLastContacts()
|
||||
{
|
||||
// Process delayed contacts
|
||||
PxU32 nbSortedTriangle = mSortedTriangle.size();
|
||||
|
||||
if (nbSortedTriangle)
|
||||
{
|
||||
PxSort(mSortedTriangle.begin(), mSortedTriangle.size(), PxLess<SortedTriangle>());
|
||||
|
||||
const PCMDeferredPolyData* PX_RESTRICT cd = reinterpret_cast<const PCMDeferredPolyData*>(mDeferredContacts->begin());
|
||||
|
||||
for (PxU32 i = 0; i < nbSortedTriangle; ++i)
|
||||
{
|
||||
const PCMDeferredPolyData& currentContact = cd[mSortedTriangle[i].mIndex];
|
||||
const PxU32 ref0 = currentContact.mInds[0];
|
||||
const PxU32 ref1 = currentContact.mInds[1];
|
||||
const PxU32 ref2 = currentContact.mInds[2];
|
||||
|
||||
//if addData sucessful, which means mEdgeCache doesn't have the edge
|
||||
const bool noEdge01 = mEdgeCache.addData(CachedEdge(ref0, ref1));
|
||||
const bool noEdge12 = mEdgeCache.addData(CachedEdge(ref1, ref2));
|
||||
const bool noEdge20 = mEdgeCache.addData(CachedEdge(ref2, ref0));
|
||||
|
||||
const bool noVertex0 = mVertexCache.addData(CachedVertex(ref0));
|
||||
const bool noVertex1 = mVertexCache.addData(CachedVertex(ref1));
|
||||
const bool noVertex2 = mVertexCache.addData(CachedVertex(ref2));
|
||||
|
||||
bool needsProcessing = false;
|
||||
{
|
||||
switch(currentContact.mFeatureIndex)
|
||||
{
|
||||
case FC_VERTEX0:
|
||||
needsProcessing = noVertex0;
|
||||
break;
|
||||
|
||||
case FC_VERTEX1:
|
||||
needsProcessing = noVertex1;
|
||||
break;
|
||||
|
||||
case FC_VERTEX2:
|
||||
needsProcessing = noVertex2;
|
||||
break;
|
||||
|
||||
case FC_EDGE01:
|
||||
needsProcessing = noEdge01;
|
||||
break;
|
||||
|
||||
case FC_EDGE12:
|
||||
needsProcessing = noEdge12;
|
||||
break;
|
||||
|
||||
case FC_EDGE20:
|
||||
needsProcessing = noEdge20;
|
||||
break;
|
||||
|
||||
case FC_FACE:
|
||||
case FC_UNDEFINED:
|
||||
PX_ASSERT(0); // PT: should not be possible
|
||||
break;
|
||||
};
|
||||
}
|
||||
|
||||
if (needsProcessing)
|
||||
{
|
||||
//we store the contact, patch normal and sq distance in the vertex memory in PCMDeferredPolyData
|
||||
const Vec3V contactP = V3LoadU(currentContact.mVerts[0]);
|
||||
const Vec3V patchNormal = V3LoadU(currentContact.mVerts[1]);
|
||||
const FloatV sqDist = FLoad(currentContact.mVerts[2].x);
|
||||
const FloatV dist = FSqrt(sqDist);
|
||||
addToPatch(contactP, patchNormal, dist, currentContact.mTriangleIndex);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
73
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactSpherePlane.cpp
vendored
Normal file
73
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactSpherePlane.cpp
vendored
Normal file
@@ -0,0 +1,73 @@
|
||||
// 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 "geomutils/PxContactBuffer.h"
|
||||
#include "foundation/PxVecTransform.h"
|
||||
#include "GuContactMethodImpl.h"
|
||||
#include "GuPCMContactGenUtil.h"
|
||||
|
||||
using namespace physx;
|
||||
|
||||
bool Gu::pcmContactSpherePlane(GU_CONTACT_METHOD_ARGS)
|
||||
{
|
||||
using namespace aos;
|
||||
PX_UNUSED(renderOutput);
|
||||
PX_UNUSED(cache);
|
||||
PX_UNUSED(shape1);
|
||||
|
||||
// Get actual shape data
|
||||
const PxSphereGeometry& shapeSphere = checkedCast<PxSphereGeometry>(shape0);
|
||||
|
||||
//sphere transform
|
||||
const Vec3V p0 = V3LoadU_SafeReadW(transform0.p); // PT: safe because 'mRefCount' follows 'mTransform' in PxsTransform
|
||||
|
||||
//plane transform
|
||||
const Vec3V p1 = V3LoadU_SafeReadW(transform1.p); // PT: safe because 'mRefCount' follows 'mTransform' in PxsTransform
|
||||
const QuatV q1 = QuatVLoadU(&transform1.q.x);
|
||||
|
||||
const FloatV radius = FLoad(shapeSphere.radius);
|
||||
const FloatV contactDist = FLoad(params.mContactDistance);
|
||||
|
||||
const PxTransformV transf1(p1, q1);
|
||||
//Sphere in plane space
|
||||
const Vec3V sphereCenterInPlaneSpace = transf1.transformInv(p0);
|
||||
|
||||
//Separation
|
||||
const FloatV separation = FSub(V3GetX(sphereCenterInPlaneSpace), radius);
|
||||
|
||||
if(FAllGrtrOrEq(contactDist, separation))
|
||||
{
|
||||
//get the plane normal
|
||||
const Vec3V worldNormal = QuatGetBasisVector0(q1);
|
||||
const Vec3V worldPoint = V3NegScaleSub(worldNormal, radius, p0);
|
||||
|
||||
return outputSimplePCMContact(contactBuffer, worldPoint, worldNormal, separation);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
69
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactSphereSphere.cpp
vendored
Normal file
69
engine/third_party/physx/source/geomutils/src/pcm/GuPCMContactSphereSphere.cpp
vendored
Normal file
@@ -0,0 +1,69 @@
|
||||
// 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 "geomutils/PxContactBuffer.h"
|
||||
#include "GuContactMethodImpl.h"
|
||||
#include "foundation/PxVecTransform.h"
|
||||
#include "GuPCMContactGenUtil.h"
|
||||
|
||||
using namespace physx;
|
||||
|
||||
bool Gu::pcmContactSphereSphere(GU_CONTACT_METHOD_ARGS)
|
||||
{
|
||||
PX_UNUSED(cache);
|
||||
PX_UNUSED(renderOutput);
|
||||
|
||||
using namespace aos;
|
||||
const PxSphereGeometry& shapeSphere0 = checkedCast<PxSphereGeometry>(shape0);
|
||||
const PxSphereGeometry& shapeSphere1 = checkedCast<PxSphereGeometry>(shape1);
|
||||
|
||||
const FloatV cDist = FLoad(params.mContactDistance);
|
||||
const Vec3V p0 = V3LoadA(&transform0.p.x);
|
||||
const Vec3V p1 = V3LoadA(&transform1.p.x);
|
||||
|
||||
const FloatV r0 = FLoad(shapeSphere0.radius);
|
||||
const FloatV r1 = FLoad(shapeSphere1.radius);
|
||||
|
||||
const Vec3V _delta = V3Sub(p0, p1);
|
||||
const FloatV distanceSq = V3Dot(_delta, _delta);
|
||||
const FloatV radiusSum = FAdd(r0, r1);
|
||||
const FloatV inflatedSum = FAdd(radiusSum, cDist);
|
||||
|
||||
if(FAllGrtr(FMul(inflatedSum, inflatedSum), distanceSq))
|
||||
{
|
||||
const FloatV eps = FLoad(0.00001f);
|
||||
const FloatV dist = FSqrt(distanceSq);
|
||||
const BoolV bCon = FIsGrtrOrEq(eps, dist);
|
||||
const Vec3V normal = V3Sel(bCon, V3UnitX(), V3ScaleInv(_delta, dist));
|
||||
const Vec3V point = V3ScaleAdd(normal, r1, p1);
|
||||
const FloatV pen = FSub(dist, radiusSum);
|
||||
|
||||
return outputSimplePCMContact(contactBuffer, point, normal, pen);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
206
engine/third_party/physx/source/geomutils/src/pcm/GuPCMShapeConvex.cpp
vendored
Normal file
206
engine/third_party/physx/source/geomutils/src/pcm/GuPCMShapeConvex.cpp
vendored
Normal file
@@ -0,0 +1,206 @@
|
||||
// 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 "GuPCMShapeConvex.h"
|
||||
#include "GuVecConvexHull.h"
|
||||
#include "GuPCMContactGen.h"
|
||||
#include "common/PxRenderOutput.h"
|
||||
|
||||
using namespace physx;
|
||||
using namespace Gu;
|
||||
using namespace aos;
|
||||
|
||||
namespace physx
|
||||
{
|
||||
namespace Gu
|
||||
{
|
||||
const PxU8 gPCMBoxPolygonData[24] =
|
||||
{
|
||||
0, 3, 2, 1,
|
||||
1, 2, 6, 5,
|
||||
5, 6, 7, 4,
|
||||
4, 7, 3, 0,
|
||||
3, 7, 6, 2,
|
||||
4, 0, 1, 5,
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
PCMPolygonalBox::PCMPolygonalBox(const PxVec3& halfSide) : mHalfSide(halfSide)
|
||||
{
|
||||
//Precompute the convex data
|
||||
// 7+------+6 0 = ---
|
||||
// /| /| 1 = +--
|
||||
// / | / | 2 = ++-
|
||||
// / 4+---/--+5 3 = -+-
|
||||
// 3+------+2 / y z 4 = --+
|
||||
// | / | / | / 5 = +-+
|
||||
// |/ |/ |/ 6 = +++
|
||||
// 0+------+1 *---x 7 = -++
|
||||
|
||||
PxVec3 minimum = -mHalfSide;
|
||||
PxVec3 maximum = mHalfSide;
|
||||
// Generate 8 corners of the bbox
|
||||
mVertices[0] = PxVec3(minimum.x, minimum.y, minimum.z);
|
||||
mVertices[1] = PxVec3(maximum.x, minimum.y, minimum.z);
|
||||
mVertices[2] = PxVec3(maximum.x, maximum.y, minimum.z);
|
||||
mVertices[3] = PxVec3(minimum.x, maximum.y, minimum.z);
|
||||
mVertices[4] = PxVec3(minimum.x, minimum.y, maximum.z);
|
||||
mVertices[5] = PxVec3(maximum.x, minimum.y, maximum.z);
|
||||
mVertices[6] = PxVec3(maximum.x, maximum.y, maximum.z);
|
||||
mVertices[7] = PxVec3(minimum.x, maximum.y, maximum.z);
|
||||
|
||||
//Setup the polygons
|
||||
for(PxU8 i=0; i < 6; i++)
|
||||
{
|
||||
mPolygons[i].mNbVerts = 4;
|
||||
mPolygons[i].mVRef8 = PxU16(i*4);
|
||||
}
|
||||
|
||||
// ### planes needs *very* careful checks
|
||||
// X axis
|
||||
mPolygons[1].mPlane.n = PxVec3(1.0f, 0.0f, 0.0f);
|
||||
mPolygons[1].mPlane.d = -mHalfSide.x;
|
||||
mPolygons[3].mPlane.n = PxVec3(-1.0f, 0.0f, 0.0f);
|
||||
mPolygons[3].mPlane.d = -mHalfSide.x;
|
||||
|
||||
mPolygons[1].mMinIndex = 0;
|
||||
mPolygons[3].mMinIndex = 1;
|
||||
|
||||
PX_ASSERT(mPolygons[1].getMin(mVertices) == -mHalfSide.x);
|
||||
PX_ASSERT(mPolygons[3].getMin(mVertices) == -mHalfSide.x);
|
||||
|
||||
// Y axis
|
||||
mPolygons[4].mPlane.n = PxVec3(0.f, 1.0f, 0.0f);
|
||||
mPolygons[4].mPlane.d = -mHalfSide.y;
|
||||
mPolygons[5].mPlane.n = PxVec3(0.0f, -1.0f, 0.0f);
|
||||
mPolygons[5].mPlane.d = -mHalfSide.y;
|
||||
|
||||
mPolygons[4].mMinIndex = 0;
|
||||
mPolygons[5].mMinIndex = 2;
|
||||
|
||||
PX_ASSERT(mPolygons[4].getMin(mVertices) == -mHalfSide.y);
|
||||
PX_ASSERT(mPolygons[5].getMin(mVertices) == -mHalfSide.y);
|
||||
|
||||
// Z axis
|
||||
mPolygons[2].mPlane.n = PxVec3(0.f, 0.0f, 1.0f);
|
||||
mPolygons[2].mPlane.d = -mHalfSide.z;
|
||||
mPolygons[0].mPlane.n = PxVec3(0.0f, 0.0f, -1.0f);
|
||||
mPolygons[0].mPlane.d = -mHalfSide.z;
|
||||
|
||||
mPolygons[2].mMinIndex = 0;
|
||||
mPolygons[0].mMinIndex = 4;
|
||||
PX_ASSERT(mPolygons[2].getMin(mVertices) == -mHalfSide.z);
|
||||
PX_ASSERT(mPolygons[0].getMin(mVertices) == -mHalfSide.z);
|
||||
}
|
||||
|
||||
void PCMPolygonalBox::getPolygonalData(PolygonalData* PX_RESTRICT dst) const
|
||||
{
|
||||
dst->mCenter = PxVec3(0.0f, 0.0f, 0.0f);
|
||||
dst->mNbVerts = 8;
|
||||
dst->mNbPolygons = 6;
|
||||
dst->mPolygons = mPolygons;
|
||||
dst->mNbEdges = 0;
|
||||
dst->mVerts = mVertices;
|
||||
dst->mPolygonVertexRefs = gPCMBoxPolygonData;
|
||||
dst->mFacesByEdges = NULL;
|
||||
dst->mVerticesByEdges = NULL;
|
||||
|
||||
dst->mBigData = NULL;
|
||||
|
||||
dst->mInternal.mInternalRadius = 0.0f;
|
||||
dst->mInternal.mInternalExtents = mHalfSide;
|
||||
|
||||
dst->mScale = PxMeshScale();
|
||||
}
|
||||
|
||||
static void getPCMPolygonalData_Convex(PolygonalData* PX_RESTRICT dst, const ConvexHullData* PX_RESTRICT src, const Mat33V& vertexToShape)
|
||||
{
|
||||
using namespace aos;
|
||||
const Vec3V vertexSpaceCenterOfMass = V3LoadU(src->mCenterOfMass);
|
||||
const Vec3V shapeSpaceCenterOfMass = M33MulV3(vertexToShape, vertexSpaceCenterOfMass);
|
||||
V3StoreU(shapeSpaceCenterOfMass, dst->mCenter);
|
||||
dst->mNbVerts = src->mNbHullVertices;
|
||||
dst->mNbPolygons = src->mNbPolygons;
|
||||
dst->mNbEdges = src->mNbEdges;
|
||||
dst->mPolygons = src->mPolygons;
|
||||
dst->mVerts = src->getHullVertices();
|
||||
dst->mPolygonVertexRefs = src->getVertexData8();
|
||||
dst->mFacesByEdges = src->getFacesByEdges8();
|
||||
dst->mVerticesByEdges = src->getVerticesByEdges16();
|
||||
|
||||
dst->mBigData = src->mBigConvexRawData;
|
||||
|
||||
dst->mInternal = src->mInternal;
|
||||
|
||||
dst->mScale = PxMeshScale();
|
||||
}
|
||||
|
||||
static void getPCMPolygonalData_Convex(PolygonalData* PX_RESTRICT dst, const ConvexHullData* PX_RESTRICT src,
|
||||
const Cm::FastVertex2ShapeScaling& scaling, const PxMeshScale& convexScale)
|
||||
{
|
||||
dst->mCenter = scaling * src->mCenterOfMass;
|
||||
dst->mNbVerts = src->mNbHullVertices;
|
||||
dst->mNbPolygons = src->mNbPolygons;
|
||||
dst->mNbEdges = src->mNbEdges;
|
||||
dst->mPolygons = src->mPolygons;
|
||||
dst->mVerts = src->getHullVertices();
|
||||
dst->mPolygonVertexRefs = src->getVertexData8();
|
||||
dst->mFacesByEdges = src->getFacesByEdges8();
|
||||
dst->mVerticesByEdges = src->getVerticesByEdges16();
|
||||
|
||||
dst->mBigData = src->mBigConvexRawData;
|
||||
dst->mInternal = src->mInternal;
|
||||
dst->mScale = convexScale;
|
||||
}
|
||||
|
||||
void Gu::getPCMConvexData(const ConvexHullV& convexHull, bool idtScale, PolygonalData& polyData)
|
||||
{
|
||||
PX_ASSERT(!convexHull.hullData->mAABB.isEmpty());
|
||||
|
||||
//this is used to calculate the convex hull's center of mass
|
||||
getPCMPolygonalData_Convex(&polyData, convexHull.hullData, convexHull.vertex2Shape);
|
||||
|
||||
if(!idtScale)
|
||||
polyData.mInternal.reset();
|
||||
}
|
||||
|
||||
bool Gu::getPCMConvexData(const PxConvexMeshGeometry& shapeConvex, Cm::FastVertex2ShapeScaling& scaling, PxBounds3& bounds, PolygonalData& polyData)
|
||||
{
|
||||
const bool idtScale = shapeConvex.scale.isIdentity();
|
||||
if(!idtScale)
|
||||
scaling.init(shapeConvex.scale);
|
||||
|
||||
const ConvexHullData* hullData = _getHullData(shapeConvex);
|
||||
PX_ASSERT(!hullData->mAABB.isEmpty());
|
||||
bounds = hullData->mAABB.transformFast(scaling.getVertex2ShapeSkew());
|
||||
|
||||
getPCMPolygonalData_Convex(&polyData, hullData, scaling, shapeConvex.scale);
|
||||
|
||||
return idtScale;
|
||||
}
|
||||
63
engine/third_party/physx/source/geomutils/src/pcm/GuPCMShapeConvex.h
vendored
Normal file
63
engine/third_party/physx/source/geomutils/src/pcm/GuPCMShapeConvex.h
vendored
Normal file
@@ -0,0 +1,63 @@
|
||||
// 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_PCM_SHAPE_CONVEX_H
|
||||
#define GU_PCM_SHAPE_CONVEX_H
|
||||
|
||||
#include "GuConvexSupportTable.h"
|
||||
#include "GuPersistentContactManifold.h"
|
||||
#include "GuShapeConvex.h"
|
||||
|
||||
namespace physx
|
||||
{
|
||||
class PxConvexMeshGeometry;
|
||||
|
||||
namespace Gu
|
||||
{
|
||||
extern const PxU8 gPCMBoxPolygonData[24];
|
||||
|
||||
class PCMPolygonalBox
|
||||
{
|
||||
public:
|
||||
PCMPolygonalBox(const PxVec3& halfSide);
|
||||
|
||||
void getPolygonalData(Gu::PolygonalData* PX_RESTRICT dst) const;
|
||||
|
||||
const PxVec3& mHalfSide;
|
||||
PxVec3 mVertices[8];
|
||||
Gu::HullPolygonData mPolygons[6];
|
||||
private:
|
||||
PCMPolygonalBox& operator=(const PCMPolygonalBox&);
|
||||
};
|
||||
|
||||
void getPCMConvexData(const Gu::ConvexHullV& convexHull, bool idtScale, Gu::PolygonalData& polyData);
|
||||
bool getPCMConvexData(const PxConvexMeshGeometry& shapeConvex, Cm::FastVertex2ShapeScaling& scaling, PxBounds3& bounds, Gu::PolygonalData& polyData);
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
1158
engine/third_party/physx/source/geomutils/src/pcm/GuPCMTriangleContactGen.cpp
vendored
Normal file
1158
engine/third_party/physx/source/geomutils/src/pcm/GuPCMTriangleContactGen.cpp
vendored
Normal file
File diff suppressed because it is too large
Load Diff
57
engine/third_party/physx/source/geomutils/src/pcm/GuPCMTriangleContactGen.h
vendored
Normal file
57
engine/third_party/physx/source/geomutils/src/pcm/GuPCMTriangleContactGen.h
vendored
Normal file
@@ -0,0 +1,57 @@
|
||||
// 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_PCM_TRIANGLE_CONTACT_GEN_H
|
||||
#define GU_PCM_TRIANGLE_CONTACT_GEN_H
|
||||
|
||||
#include "GuPCMContactGenUtil.h"
|
||||
#include "GuPersistentContactManifold.h"
|
||||
|
||||
namespace physx
|
||||
{
|
||||
class PxTriangleMeshGeometry;
|
||||
class PxHeightFieldGeometry;
|
||||
|
||||
namespace Gu
|
||||
{
|
||||
bool PCMContactConvexMesh(const Gu::PolygonalData& polyData0, const Gu::SupportLocal* polyMap, const aos::FloatVArg minMargin, const PxBounds3& hullAABB,
|
||||
const PxTriangleMeshGeometry& shapeMesh,
|
||||
const PxTransform& transform0, const PxTransform& transform1, PxReal contactDistance, PxContactBuffer& contactBuffer,
|
||||
const Cm::FastVertex2ShapeScaling& convexScaling, const Cm::FastVertex2ShapeScaling& meshScaling,
|
||||
bool idtConvexScale, bool idtMeshScale,
|
||||
Gu::MultiplePersistentContactManifold& multiManifold, PxRenderOutput* renderOutput);
|
||||
|
||||
bool PCMContactConvexHeightfield(const Gu::PolygonalData& polyData0, const Gu::SupportLocal* polyMap, const aos::FloatVArg minMargin, const PxBounds3& hullAABB,
|
||||
const PxHeightFieldGeometry& shapeHeightfield,
|
||||
const PxTransform& transform0, const PxTransform& transform1, PxReal contactDistance, PxContactBuffer& contactBuffer,
|
||||
const Cm::FastVertex2ShapeScaling& convexScaling, bool idtConvexScale,
|
||||
Gu::MultiplePersistentContactManifold& multiManifold, PxRenderOutput* renderOutput);
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
2291
engine/third_party/physx/source/geomutils/src/pcm/GuPersistentContactManifold.cpp
vendored
Normal file
2291
engine/third_party/physx/source/geomutils/src/pcm/GuPersistentContactManifold.cpp
vendored
Normal file
File diff suppressed because it is too large
Load Diff
765
engine/third_party/physx/source/geomutils/src/pcm/GuPersistentContactManifold.h
vendored
Normal file
765
engine/third_party/physx/source/geomutils/src/pcm/GuPersistentContactManifold.h
vendored
Normal file
@@ -0,0 +1,765 @@
|
||||
// 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_PERSISTENTCONTACTMANIFOLD_H
|
||||
#define Gu_PERSISTENTCONTACTMANIFOLD_H
|
||||
|
||||
#include "common/PxPhysXCommonConfig.h"
|
||||
#include "foundation/PxUnionCast.h"
|
||||
#include "foundation/PxMemory.h"
|
||||
#include "foundation/PxVecTransform.h"
|
||||
|
||||
#define PCM_LOW_LEVEL_DEBUG 0
|
||||
|
||||
namespace physx
|
||||
{
|
||||
|
||||
#define VISUALIZE_PERSISTENT_CONTACT 1
|
||||
//This is for primitives vs primitives
|
||||
#define GU_MANIFOLD_CACHE_SIZE 4
|
||||
//These are for primitives vs mesh
|
||||
#define GU_SINGLE_MANIFOLD_SINGLE_POLYGONE_CACHE_SIZE 5
|
||||
#define GU_SINGLE_MANIFOLD_CACHE_SIZE 6
|
||||
#define GU_SPHERE_MANIFOLD_CACHE_SIZE 1
|
||||
#define GU_CAPSULE_MANIFOLD_CACHE_SIZE 3
|
||||
#define GU_MAX_MANIFOLD_SIZE 6 // PT: max nb of manifolds (e.g. for multi-manifolds), NOT the max size of a single manifold
|
||||
#define GU_MESH_CONTACT_REDUCTION_THRESHOLD 16
|
||||
|
||||
#define GU_MANIFOLD_INVALID_INDEX 0xffffffff
|
||||
|
||||
//ML: this is used to compared with the shape's margin to decide the final tolerance used in the manifold to validate the existing contacts.
|
||||
//In the case of big shape and relatively speaking small triangles in the mesh, we need to take a smaller margin. This helps because the PCM
|
||||
//recycling thresholds are proportionate to margin so it makes it less likely to discard previous contacts due to separation
|
||||
#define GU_PCM_MESH_MANIFOLD_EPSILON 0.05f
|
||||
|
||||
class PxRenderOutput;
|
||||
class PxContactBuffer;
|
||||
|
||||
namespace Gu
|
||||
{
|
||||
struct GjkOutput;
|
||||
|
||||
extern const PxF32 invalidateThresholds[5];
|
||||
extern const PxF32 invalidateQuatThresholds[5];
|
||||
extern const PxF32 invalidateThresholds2[3];
|
||||
extern const PxF32 invalidateQuatThresholds2[3];
|
||||
|
||||
aos::Mat33V findRotationMatrixFromZAxis(const aos::Vec3VArg to);
|
||||
|
||||
//This contact is used in the primitives vs primitives contact gen
|
||||
class PersistentContact
|
||||
{
|
||||
public:
|
||||
PersistentContact()
|
||||
{
|
||||
}
|
||||
|
||||
PersistentContact(aos::Vec3V _localPointA, aos::Vec3V _localPointB, aos::Vec4V _localNormalPen) :
|
||||
mLocalPointA(_localPointA), mLocalPointB(_localPointB), mLocalNormalPen(_localNormalPen)
|
||||
{
|
||||
}
|
||||
|
||||
aos::Vec3V mLocalPointA;
|
||||
aos::Vec3V mLocalPointB;
|
||||
aos::Vec4V mLocalNormalPen; // the (x, y, z) is the local normal, and the w is the penetration depth
|
||||
};
|
||||
|
||||
//This contact is used in the mesh contact gen to store an extra variable
|
||||
class MeshPersistentContact : public PersistentContact
|
||||
{
|
||||
public:
|
||||
PxU32 mFaceIndex;
|
||||
};
|
||||
|
||||
//This contact is used in the compress stream buffer(NpCacheStreamPair in the PxcNpThreadContext) to store the data we need
|
||||
PX_ALIGN_PREFIX(16)
|
||||
class CachedMeshPersistentContact
|
||||
{
|
||||
public:
|
||||
PxVec3 mLocalPointA; //16 byte aligned
|
||||
PxU32 mFaceIndex; //face index
|
||||
PxVec3 mLocalPointB; //16 byte aligned
|
||||
PxU32 mPad; //pad
|
||||
PxVec3 mLocalNormal; //16 byte aligned
|
||||
PxReal mPen; //penetration
|
||||
}PX_ALIGN_SUFFIX(16);
|
||||
|
||||
#if PX_VC
|
||||
#pragma warning(push)
|
||||
#pragma warning( disable : 4324 ) // Padding was added at the end of a structure because of a __declspec(align) value.
|
||||
#endif
|
||||
|
||||
//This class is used to store the start and end index of the mesh persistent contacts in an array.
|
||||
struct PCMContactPatch
|
||||
{
|
||||
public:
|
||||
PCMContactPatch()
|
||||
{
|
||||
mNextPatch = NULL;
|
||||
mEndPatch = NULL;
|
||||
mRoot = this;
|
||||
mPatchMaxPen = aos::FMax();
|
||||
}
|
||||
aos::Vec3V mPatchNormal;
|
||||
PCMContactPatch* mNextPatch;//store the next patch pointer in the patch list
|
||||
PCMContactPatch* mEndPatch;//store the last patch pointer in the patch list
|
||||
PCMContactPatch* mRoot;//if this is the start of the patch list which has very similar patch normal, the root will be itself
|
||||
aos::FloatV mPatchMaxPen;//store the deepest penetration of the whole patch
|
||||
PxU32 mStartIndex;//store the start index of the manifold contacts in the manifold contacts stream
|
||||
PxU32 mEndIndex;//store the end index of the manifold contacts in the manifold contacts stream
|
||||
PxU32 mTotalSize;//if this is the root, the total size will store the total number of manifold contacts in the whole patch list
|
||||
};
|
||||
|
||||
#if PX_VC
|
||||
#pragma warning(pop)
|
||||
#endif
|
||||
|
||||
//ML: this is needed because it seems NEON doesn't force the alignment on SIMD type, which cause some of the PCM unit tests fail
|
||||
PX_ALIGN_PREFIX(16)
|
||||
class PersistentContactManifold
|
||||
{
|
||||
public:
|
||||
|
||||
PersistentContactManifold(PersistentContact* contactPointsBuff, PxU8 capacity): mNumContacts(0), mCapacity(capacity), mNumWarmStartPoints(0), mContactPoints(contactPointsBuff)
|
||||
{
|
||||
using namespace physx::aos;
|
||||
mRelativeTransform.invalidate();
|
||||
mQuatA = QuatIdentity();
|
||||
mQuatB = QuatIdentity();
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE PxU32 getNumContacts() const { return mNumContacts;}
|
||||
|
||||
PX_FORCE_INLINE bool isEmpty() const { return mNumContacts==0; }
|
||||
|
||||
PX_FORCE_INLINE PersistentContact& getContactPoint(const PxU32 index)
|
||||
{
|
||||
PX_ASSERT(index < GU_MANIFOLD_CACHE_SIZE);
|
||||
return mContactPoints[index];
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE aos::FloatV maxTransformPositionDelta(const aos::Vec3V& curP) const
|
||||
{
|
||||
using namespace aos;
|
||||
|
||||
const Vec3V deltaP = V3Sub(curP, mRelativeTransform.p);
|
||||
const Vec4V delta = Vec4V_From_Vec3V(V3Abs(deltaP));
|
||||
//need to work out max from a single vector...
|
||||
return V4ExtractMax(delta);
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE void setRelativeTransform(const aos::PxTransformV& transform)
|
||||
{
|
||||
mRelativeTransform = transform;
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE void setRelativeTransform(const aos::PxTransformV& transform, const aos::QuatV quatA, const aos::QuatV quatB)
|
||||
{
|
||||
mRelativeTransform = transform;
|
||||
mQuatA = quatA;
|
||||
mQuatB = quatB;
|
||||
}
|
||||
|
||||
//This is used for the box/convexhull vs box/convexhull contact gen to decide whether the relative movement of a pair of objects are
|
||||
//small enough. In this case, we can skip the collision detection all together
|
||||
PX_FORCE_INLINE PxU32 invalidate_BoxConvex(const aos::PxTransformV& curRTrans, const aos::QuatV& quatA, const aos::QuatV& quatB,
|
||||
const aos::FloatVArg minMargin, const aos::FloatVArg radiusA, const aos::FloatVArg radiusB) const
|
||||
{
|
||||
using namespace aos;
|
||||
PX_ASSERT(mNumContacts <= GU_MANIFOLD_CACHE_SIZE);
|
||||
const FloatV ratio = FLoad(invalidateThresholds[mNumContacts]);
|
||||
const FloatV thresholdP = FMul(minMargin, ratio);
|
||||
const FloatV deltaP = maxTransformPositionDelta(curRTrans.p);
|
||||
|
||||
const FloatV thresholdQ = FLoad(invalidateQuatThresholds[mNumContacts]);
|
||||
const FloatV deltaQA = QuatDot(quatA, mQuatA);
|
||||
const FloatV deltaQB = QuatDot(quatB, mQuatB);
|
||||
|
||||
const BoolV con0 = BOr(FIsGrtr(deltaP, thresholdP), BOr(FIsGrtr(thresholdQ, deltaQA), FIsGrtr(thresholdQ, deltaQB)));
|
||||
PxU32 generateContacts = BAllEqTTTT(con0);
|
||||
if (!generateContacts)
|
||||
{
|
||||
PxReal dqA, dqB;
|
||||
FStore(deltaQA, &dqA);
|
||||
FStore(deltaQB, &dqB);
|
||||
|
||||
const PxReal aRadian = dqA < 1.0f ? PxAcos(dqA) : 0.f;
|
||||
const FloatV travelDistA = FMul(FLoad(aRadian), radiusA);
|
||||
|
||||
const PxReal bRadian = dqB < 1.0f ? PxAcos(dqB) : 0.f;
|
||||
const FloatV travelDistB = FMul(FLoad(bRadian), radiusB);
|
||||
|
||||
const BoolV con = BOr(FIsGrtr(travelDistA, thresholdP), FIsGrtr(travelDistB, thresholdP));
|
||||
|
||||
generateContacts = BAllEqTTTT(con);
|
||||
}
|
||||
|
||||
return generateContacts;
|
||||
}
|
||||
|
||||
//This is used for the sphere/capsule vs other primitives contact gen to decide whether the relative movement of a pair of objects are
|
||||
//small enough. In this case, we can skip the collision detection all together
|
||||
PX_FORCE_INLINE PxU32 invalidate_SphereCapsule(const aos::PxTransformV& curRTrans, const aos::FloatVArg minMargin) const
|
||||
{
|
||||
using namespace aos;
|
||||
PX_ASSERT(mNumContacts <= 2);
|
||||
const FloatV ratio = FLoad(invalidateThresholds2[mNumContacts]);
|
||||
|
||||
const FloatV thresholdP = FMul(minMargin, ratio);
|
||||
const FloatV deltaP = maxTransformPositionDelta(curRTrans.p);
|
||||
|
||||
const FloatV thresholdQ = FLoad(invalidateQuatThresholds2[mNumContacts]);
|
||||
const FloatV deltaQ = QuatDot(curRTrans.q, mRelativeTransform.q);
|
||||
const BoolV con = BOr(FIsGrtr(deltaP, thresholdP), FIsGrtr(thresholdQ, deltaQ));
|
||||
|
||||
return BAllEqTTTT(con);
|
||||
}
|
||||
|
||||
//This is used for plane contact gen to decide whether the relative movement of a pair of objects are small enough. In this case,
|
||||
//we can skip the collision detection all together
|
||||
PX_FORCE_INLINE PxU32 invalidate_PrimitivesPlane(const aos::PxTransformV& curRTrans, const aos::FloatVArg minMargin, const aos::FloatVArg ratio) const
|
||||
{
|
||||
using namespace aos;
|
||||
const FloatV thresholdP = FMul(minMargin, ratio);
|
||||
const FloatV deltaP = maxTransformPositionDelta(curRTrans.p);
|
||||
|
||||
const FloatV thresholdQ = FLoad(0.99996f);//about 0.5 degree
|
||||
const FloatV deltaQ = QuatDot(curRTrans.q, mRelativeTransform.q);
|
||||
const BoolV con = BOr(FIsGrtr(deltaP, thresholdP), FIsGrtr(thresholdQ, deltaQ));
|
||||
|
||||
return BAllEqTTTT(con);
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE void removeContactPoint(PxU32 index)
|
||||
{
|
||||
mNumContacts--;
|
||||
mContactPoints[index] = mContactPoints[mNumContacts];
|
||||
}
|
||||
|
||||
/*bool validContactDistance(const PersistentContact& pt, const aos::FloatVArg breakingThreshold) const
|
||||
{
|
||||
using namespace aos;
|
||||
const FloatV dist = V4GetW(pt.mLocalNormalPen);
|
||||
return FAllGrtr(breakingThreshold, dist) != 0;
|
||||
}*/
|
||||
|
||||
PX_FORCE_INLINE void clearManifold()
|
||||
{
|
||||
mNumWarmStartPoints = 0;
|
||||
mNumContacts = 0;
|
||||
mRelativeTransform.invalidate();
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE void initialize()
|
||||
{
|
||||
clearManifold();
|
||||
}
|
||||
|
||||
//This function is used to replace the existing contact with the newly created contact if their distance are within some threshold
|
||||
bool replaceManifoldPoint(const aos::Vec3VArg localPointA, const aos::Vec3VArg localPointB, const aos::Vec4VArg localNormalPen, const aos::FloatVArg replaceBreakingThreshold);
|
||||
|
||||
//This function is to add a point(in box/convexhull contact gen) to the exising manifold. If the number of manifold is more than 4, we need to do contact reduction
|
||||
PxU32 addManifoldPoint(const aos::Vec3VArg localPointA, const aos::Vec3VArg localPointB, const aos::Vec4VArg localNormalAPen, const aos::FloatVArg replaceBreakingThreshold);
|
||||
//This function is to add a point(in capsule contact gen) to the exising manifold. If the number of manifold is more than 4, we need to do contact reduction
|
||||
PxU32 addManifoldPoint2(const aos::Vec3VArg localPointA, const aos::Vec3VArg localPointB, const aos::Vec4VArg localNormalAPen, const aos::FloatVArg replaceBreakingThreshold);//max two points of contacts
|
||||
//This function is used in box-plane contact gen to add the plane contacts to the manifold
|
||||
void addBatchManifoldContactsCluster(const PersistentContact* manifoldPoints, PxU32 numPoints);
|
||||
|
||||
//This function is used in the capsule full manifold contact genenation(maximum 2 points).
|
||||
void addBatchManifoldContacts2(const PersistentContact* manifoldPoints, PxU32 numPoints);//max two points of contacts
|
||||
|
||||
//This function is used in the box/convexhull full manifold contact generation(maximum 4 points).
|
||||
void addBatchManifoldContacts(const PersistentContact* manifoldPoints, PxU32 numPoints, PxReal toleranceLength);
|
||||
//This function is using the cluster algorithm to reduce contacts
|
||||
void reduceBatchContactsCluster(const PersistentContact* manifoldPoints, PxU32 numPoints);
|
||||
//This function is called by addBatchManifoldContacts2 to reduce the manifold contacts to 2 points;
|
||||
void reduceBatchContacts2(const PersistentContact* manifoldPoints, PxU32 numPoints);
|
||||
//This function is called by addBatchManifoldContacts to reduce the manifold contacts to 4 points
|
||||
void reduceBatchContacts(const PersistentContact* manifoldPoints, PxU32 numPoints, PxReal toleranceLength);
|
||||
|
||||
//This function is used for incremental manifold contact reduction for box/convexhull
|
||||
PxU32 reduceContactsForPCM(const aos::Vec3VArg localPointA, const aos::Vec3VArg localPointB, const aos::Vec4VArg localNormalPen);
|
||||
//This function is used for incremental manifold contact reduction for capsule
|
||||
PxU32 reduceContactSegment(const aos::Vec3VArg localPointA, const aos::Vec3VArg localPointB, const aos::Vec4VArg localNormalPen);
|
||||
|
||||
// This function recalculate the contacts in the manifold based on the current relative transform between a pair of objects. If the recalculated contacts are within some threshold,
|
||||
// we will keep the contacts; Otherwise, we will remove the contacts.
|
||||
void refreshContactPoints(const aos::PxMatTransformV& relTra, const aos::FloatVArg projectBreakingThreshold, const aos::FloatVArg contactOffset);
|
||||
//This function is just used in boxbox contact gen for fast transform
|
||||
void addManifoldContactsToContactBuffer(PxContactBuffer& contactBuffer, const aos::Vec3VArg normal, const aos::PxMatTransformV& transf1);
|
||||
//This function is for adding box/convexhull manifold contacts to the contact buffer
|
||||
void addManifoldContactsToContactBuffer(PxContactBuffer& contactBuffer, const aos::Vec3VArg normal, const aos::PxTransformV& transf1, const aos::FloatVArg contactOffset);
|
||||
//This function is for adding sphere/capsule manifold contacts to the contact buffer
|
||||
void addManifoldContactsToContactBuffer(PxContactBuffer& contactBuffer, const aos::Vec3VArg normal, const aos::Vec3VArg projectionNormal, const aos::PxTransformV& transf0, const aos::FloatVArg radius, const aos::FloatVArg contactOffset);
|
||||
|
||||
//get the average normal in the manifold in world space
|
||||
aos::Vec3V getWorldNormal(const aos::PxTransformV& trB) const;
|
||||
//get the average normal in the manifold in local B object space
|
||||
aos::Vec3V getLocalNormal() const;
|
||||
|
||||
void recordWarmStart(PxU8* aIndices, PxU8* bIndices, PxU8& nbWarmStartPoints);
|
||||
void setWarmStart(const PxU8* aIndices, const PxU8* bIndices, const PxU8 nbWarmStartPoints);
|
||||
void drawManifold(PxRenderOutput& out, const aos::PxTransformV& trA, const aos::PxTransformV& trB) const;
|
||||
void drawManifold(PxRenderOutput& out, const aos::PxTransformV& trA, const aos::PxTransformV& trB, const aos::FloatVArg radius) const;
|
||||
void drawManifold(const PersistentContact& m, PxRenderOutput& out, const aos::PxTransformV& trA, const aos::PxTransformV& trB) const;
|
||||
static void drawPoint(PxRenderOutput& out, const aos::Vec3VArg p, const PxF32 size, PxU32 color = 0x0000ff00);
|
||||
static void drawLine(PxRenderOutput& out, const aos::Vec3VArg p0, const aos::Vec3VArg p1, PxU32 color = 0xff00ffff);
|
||||
static void drawTriangle(PxRenderOutput& out, const aos::Vec3VArg p0, const aos::Vec3VArg p1, const aos::Vec3VArg p2, PxU32 color = 0xffff0000);
|
||||
static void drawTetrahedron(PxRenderOutput& out, const aos::Vec3VArg p0, const aos::Vec3VArg p1, const aos::Vec3VArg p2, const aos::Vec3VArg p3, PxU32 color = 0xffff0000);
|
||||
static void drawPolygon(PxRenderOutput& out, const aos::PxTransformV& transform, const aos::Vec3V* points, PxU32 numVerts, PxU32 color = 0xff00ffff);
|
||||
static void drawPolygon(PxRenderOutput& out, const aos::PxMatTransformV& transform, const aos::Vec3V* points, PxU32 numVerts, PxU32 color = 0xff00ffff);
|
||||
|
||||
aos::PxTransformV mRelativeTransform;//aToB
|
||||
aos::QuatV mQuatA;
|
||||
aos::QuatV mQuatB;
|
||||
PxU8 mNumContacts;
|
||||
PxU8 mCapacity;
|
||||
PxU8 mNumWarmStartPoints;
|
||||
PxU8 mAIndice[4];
|
||||
PxU8 mBIndice[4];
|
||||
PersistentContact* mContactPoints;
|
||||
} PX_ALIGN_SUFFIX(16);
|
||||
|
||||
PX_ALIGN_PREFIX(16)
|
||||
class LargePersistentContactManifold : public PersistentContactManifold
|
||||
{
|
||||
public:
|
||||
LargePersistentContactManifold() : PersistentContactManifold(mContactPointsBuff, GU_MANIFOLD_CACHE_SIZE)
|
||||
{
|
||||
}
|
||||
|
||||
PersistentContact mContactPointsBuff[GU_MANIFOLD_CACHE_SIZE];
|
||||
}PX_ALIGN_SUFFIX(16);
|
||||
|
||||
PX_ALIGN_PREFIX(16)
|
||||
class SpherePersistentContactManifold : public PersistentContactManifold
|
||||
{
|
||||
public:
|
||||
SpherePersistentContactManifold() : PersistentContactManifold(mContactPointsBuff, GU_SPHERE_MANIFOLD_CACHE_SIZE)
|
||||
{
|
||||
}
|
||||
|
||||
PersistentContact mContactPointsBuff[GU_SPHERE_MANIFOLD_CACHE_SIZE];
|
||||
}PX_ALIGN_SUFFIX(16);
|
||||
|
||||
PX_ALIGN_PREFIX(16)
|
||||
class SinglePersistentContactManifold
|
||||
{
|
||||
public:
|
||||
|
||||
SinglePersistentContactManifold(): mNumContacts(0)
|
||||
{
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE PxU32 getNumContacts() const { return mNumContacts;}
|
||||
|
||||
PX_FORCE_INLINE bool isEmpty() const { return mNumContacts==0; }
|
||||
|
||||
PX_FORCE_INLINE MeshPersistentContact& getContactPoint(const PxU32 index)
|
||||
{
|
||||
PX_ASSERT(index < GU_SINGLE_MANIFOLD_CACHE_SIZE);
|
||||
return mContactPoints[index];
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE void removeContactPoint (PxU32 index)
|
||||
{
|
||||
mNumContacts--;
|
||||
mContactPoints[index] = mContactPoints[mNumContacts];
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE void clearManifold()
|
||||
{
|
||||
mNumContacts = 0;
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE void initialize()
|
||||
{
|
||||
clearManifold();
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE aos::Vec3V getWorldNormal(const aos::PxTransformV& trB) const
|
||||
{
|
||||
using namespace aos;
|
||||
Vec4V nPen = mContactPoints[0].mLocalNormalPen;
|
||||
for(PxU32 i=1; i<mNumContacts; ++i)
|
||||
{
|
||||
nPen = V4Add(nPen, mContactPoints[i].mLocalNormalPen);
|
||||
}
|
||||
|
||||
const Vec3V n = Vec3V_From_Vec4V(nPen);
|
||||
return V3Normalize(trB.rotate(n));
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE aos::Vec3V getLocalNormal() const
|
||||
{
|
||||
using namespace aos;
|
||||
Vec4V nPen = mContactPoints[0].mLocalNormalPen;
|
||||
for(PxU32 i=1; i<mNumContacts; ++i)
|
||||
{
|
||||
nPen = V4Add(nPen, mContactPoints[i].mLocalNormalPen);
|
||||
}
|
||||
return V3Normalize(Vec3V_From_Vec4V(nPen));
|
||||
}
|
||||
|
||||
//This function reduces the manifold contact list in a patch for box/convexhull vs mesh
|
||||
aos::FloatV reduceBatchContactsConvex(const MeshPersistentContact* manifoldContactExt, PxU32 numContacts, PCMContactPatch& patch);
|
||||
//This function reduces the manifold contact list in a patch for sphere vs mesh
|
||||
aos::FloatV reduceBatchContactsSphere(const MeshPersistentContact* manifoldContactExt, PxU32 numContacts, PCMContactPatch& patch);
|
||||
//This function reduces the manifold contact list in a patch for capsuel vs mesh
|
||||
aos::FloatV reduceBatchContactsCapsule(const MeshPersistentContact* manifoldContactExt, PxU32 numContacts, PCMContactPatch& patch);
|
||||
|
||||
//This function adds the manifold contact list in a patch for box/convexhull vs mesh
|
||||
aos::FloatV addBatchManifoldContactsConvex(const MeshPersistentContact* manifoldContact, PxU32 numContacts, PCMContactPatch& patch, const aos::FloatVArg replaceBreakingThreshold);
|
||||
//This function adds the manifold contact list in a patch for sphere vs mesh
|
||||
aos::FloatV addBatchManifoldContactsSphere(const MeshPersistentContact* manifoldContact, PxU32 numContacts, PCMContactPatch& patch, const aos::FloatVArg replaceBreakingThreshold);
|
||||
//This function adds the manifold contact list in a patch for capsule vs mesh
|
||||
aos::FloatV addBatchManifoldContactsCapsule(const MeshPersistentContact* manifoldContact, PxU32 numContacts, PCMContactPatch& patch, const aos::FloatVArg replaceBreakingThreshold);
|
||||
|
||||
//This is used for in the addContactsToPatch for convex mesh contact gen.
|
||||
static PxU32 reduceContacts(MeshPersistentContact* manifoldContactExt, PxU32 numContacts);
|
||||
|
||||
//This function is to recalculate the contacts based on the relative transform between a pair of objects
|
||||
aos::FloatV refreshContactPoints(const aos::PxMatTransformV& relTra, const aos::FloatVArg projectBreakingThreshold, const aos::FloatVArg contactOffset);
|
||||
|
||||
void drawManifold(PxRenderOutput& out, const aos::PxTransformV& trA, const aos::PxTransformV& trB) const;
|
||||
|
||||
MeshPersistentContact mContactPoints[GU_SINGLE_MANIFOLD_CACHE_SIZE];//384 bytes
|
||||
PxU32 mNumContacts;//400 bytes
|
||||
|
||||
} PX_ALIGN_SUFFIX(16);
|
||||
|
||||
//This is a structure used to cache a multi-persistent-manifold in the cache stream
|
||||
struct MultiPersistentManifoldHeader
|
||||
{
|
||||
aos::PxTransformV mRelativeTransform;//aToB
|
||||
PxU32 mNumManifolds;
|
||||
PxU32 pad[3];
|
||||
};
|
||||
|
||||
struct SingleManifoldHeader
|
||||
{
|
||||
PxU32 mNumContacts;
|
||||
PxU32 pad[3];
|
||||
};
|
||||
|
||||
#if PX_VC
|
||||
#pragma warning(push)
|
||||
#pragma warning( disable : 4251 ) // class needs to have dll-interface to be used by clients of class
|
||||
#endif
|
||||
|
||||
PX_ALIGN_PREFIX(16)
|
||||
class PX_PHYSX_COMMON_API MultiplePersistentContactManifold
|
||||
{
|
||||
public:
|
||||
MultiplePersistentContactManifold():mNumManifolds(0), mNumTotalContacts(0)
|
||||
{
|
||||
mRelativeTransform.invalidate();
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE void setRelativeTransform(const aos::PxTransformV& transform)
|
||||
{
|
||||
mRelativeTransform = transform;
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE aos::FloatV maxTransformPositionDelta(const aos::Vec3V& curP) const
|
||||
{
|
||||
using namespace aos;
|
||||
|
||||
const Vec3V deltaP = V3Sub(curP, mRelativeTransform.p);
|
||||
const Vec4V delta = Vec4V_From_Vec3V(V3Abs(deltaP));
|
||||
//need to work out max from a single vector...
|
||||
return V4ExtractMax(delta);
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE PxU32 invalidate(const aos::PxTransformV& curRTrans, const aos::FloatVArg minMargin, const aos::FloatVArg ratio) const
|
||||
{
|
||||
using namespace aos;
|
||||
|
||||
const FloatV thresholdP = FMul(minMargin, ratio);
|
||||
const FloatV deltaP = maxTransformPositionDelta(curRTrans.p);
|
||||
const FloatV thresholdQ = FLoad(0.9998f);//about 1 degree
|
||||
const FloatV deltaQ = QuatDot(curRTrans.q, mRelativeTransform.q);
|
||||
const BoolV con = BOr(FIsGrtr(deltaP, thresholdP), FIsGrtr(thresholdQ, deltaQ));
|
||||
|
||||
return BAllEqTTTT(con);
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE PxU32 invalidate(const aos::PxTransformV& curRTrans, const aos::FloatVArg minMargin) const
|
||||
{
|
||||
using namespace aos;
|
||||
return invalidate(curRTrans, minMargin, FLoad(0.2f));
|
||||
}
|
||||
|
||||
// This function work out the contact patch connectivity. If two patches's normal are within 5 degree, we would link these two patches together and reset the total size.
|
||||
PX_FORCE_INLINE void refineContactPatchConnective(PCMContactPatch** contactPatch, PxU32 numContactPatch, MeshPersistentContact* manifoldContacts, const aos::FloatVArg acceptanceEpsilon) const
|
||||
{
|
||||
PX_UNUSED(manifoldContacts);
|
||||
|
||||
using namespace aos;
|
||||
|
||||
//work out the contact patch connectivity, the patchNormal should be in the local space of mesh
|
||||
for(PxU32 i=0; i<numContactPatch; ++i)
|
||||
{
|
||||
PCMContactPatch* patch = contactPatch[i];
|
||||
patch->mRoot = patch;
|
||||
patch->mEndPatch = patch;
|
||||
patch->mTotalSize = patch->mEndIndex - patch->mStartIndex;
|
||||
patch->mNextPatch = NULL;
|
||||
|
||||
for(PxU32 j=i; j>0; --j)
|
||||
{
|
||||
PCMContactPatch* other = contactPatch[j-1];
|
||||
const FloatV d = V3Dot(patch->mPatchNormal, other->mRoot->mPatchNormal);
|
||||
if(FAllGrtrOrEq(d, acceptanceEpsilon))//less than 5 degree
|
||||
{
|
||||
other->mNextPatch = patch;
|
||||
other->mRoot->mEndPatch = patch;
|
||||
patch->mRoot = other->mRoot;
|
||||
other->mRoot->mTotalSize += patch->mEndIndex - patch->mStartIndex;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// This function uses to reduce the manifold contacts which are in different connected patchs but are within replace breaking threshold
|
||||
PX_FORCE_INLINE PxU32 reduceManifoldContactsInDifferentPatches(PCMContactPatch** contactPatch, PxU32 numContactPatch, MeshPersistentContact* manifoldContacts, PxU32 numContacts, const aos::FloatVArg sqReplaceBreaking) const
|
||||
{
|
||||
using namespace aos;
|
||||
|
||||
for(PxU32 i=0; i<numContactPatch; ++i)
|
||||
{
|
||||
PCMContactPatch* currentPatch = contactPatch[i];
|
||||
//this make sure the patch is the root before we do the contact reduction, otherwise, we will do duplicate work
|
||||
if(currentPatch->mRoot == currentPatch)
|
||||
{
|
||||
while(currentPatch)
|
||||
{
|
||||
PCMContactPatch* nextPatch = currentPatch->mNextPatch;
|
||||
if(nextPatch)
|
||||
{
|
||||
for(PxU32 k = currentPatch->mStartIndex; k<currentPatch->mEndIndex; ++k)
|
||||
{
|
||||
for(PxU32 l = nextPatch->mStartIndex; l < nextPatch->mEndIndex; ++l)
|
||||
{
|
||||
Vec3V dif = V3Sub(manifoldContacts[l].mLocalPointB, manifoldContacts[k].mLocalPointB);
|
||||
FloatV d = V3Dot(dif, dif);
|
||||
if(FAllGrtr(sqReplaceBreaking, d))
|
||||
{
|
||||
//if two manifold contacts are within threshold, we will get rid of the manifold contacts in the other contact patch
|
||||
manifoldContacts[l] = manifoldContacts[nextPatch->mEndIndex-1];
|
||||
nextPatch->mEndIndex--;
|
||||
numContacts--;
|
||||
l--;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
currentPatch = nextPatch;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return numContacts;
|
||||
}
|
||||
|
||||
// This function is for the multiple manifold loop through each individual single manifold to recalculate the contacts based on the relative transform between a pair of objects
|
||||
PX_FORCE_INLINE void refreshManifold(const aos::PxMatTransformV& relTra, const aos::FloatVArg projectBreakingThreshold, const aos::FloatVArg contactDist)
|
||||
{
|
||||
using namespace aos;
|
||||
|
||||
//refresh manifold contacts
|
||||
for(PxU32 i=0; i<mNumManifolds; ++i)
|
||||
{
|
||||
const PxU8 ind = mManifoldIndices[i];
|
||||
PX_ASSERT(mManifoldIndices[i] < GU_MAX_MANIFOLD_SIZE);
|
||||
const PxU32 nextInd = PxMin(i, mNumManifolds-2u)+1;
|
||||
PxPrefetchLine(&mManifolds[mManifoldIndices[nextInd]]);
|
||||
PxPrefetchLine(&mManifolds[mManifoldIndices[nextInd]],128);
|
||||
PxPrefetchLine(&mManifolds[mManifoldIndices[nextInd]],256);
|
||||
const FloatV _maxPen = mManifolds[ind].refreshContactPoints(relTra, projectBreakingThreshold, contactDist);
|
||||
if(mManifolds[ind].isEmpty())
|
||||
{
|
||||
//swap the index with the next manifolds
|
||||
const PxU8 index = mManifoldIndices[--mNumManifolds];
|
||||
mManifoldIndices[mNumManifolds] = ind;
|
||||
mManifoldIndices[i] = index;
|
||||
i--;
|
||||
}
|
||||
else
|
||||
{
|
||||
FStore(_maxPen, &mMaxPen[ind]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE void initialize()
|
||||
{
|
||||
mNumManifolds = 0;
|
||||
mNumTotalContacts = 0;
|
||||
mRelativeTransform.invalidate();
|
||||
for(PxU8 i=0; i<GU_MAX_MANIFOLD_SIZE; ++i)
|
||||
{
|
||||
mManifolds[i].initialize();
|
||||
mManifoldIndices[i] = i;
|
||||
}
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE void clearManifold()
|
||||
{
|
||||
for(PxU8 i=0; i<mNumManifolds; ++i)
|
||||
{
|
||||
mManifolds[i].clearManifold();
|
||||
}
|
||||
mNumManifolds = 0;
|
||||
mNumTotalContacts = 0;
|
||||
mRelativeTransform.invalidate();
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE const SinglePersistentContactManifold* getManifold(PxU32 index) const
|
||||
{
|
||||
PX_ASSERT(index < GU_MAX_MANIFOLD_SIZE);
|
||||
return &mManifolds[mManifoldIndices[index]];
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE SinglePersistentContactManifold* getManifold(PxU32 index)
|
||||
{
|
||||
PX_ASSERT(index < GU_MAX_MANIFOLD_SIZE);
|
||||
return &mManifolds[mManifoldIndices[index]];
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE SinglePersistentContactManifold* getEmptyManifold()
|
||||
{
|
||||
if(mNumManifolds < GU_MAX_MANIFOLD_SIZE)
|
||||
return &mManifolds[mManifoldIndices[mNumManifolds]];
|
||||
return NULL;
|
||||
}
|
||||
|
||||
//This function adds the manifold contacts with different patches into the corresponding single persistent contact manifold
|
||||
void addManifoldContactPoints(MeshPersistentContact* manifoldContact, PxU32 numManifoldContacts, PCMContactPatch** contactPatch, PxU32 numPatch,
|
||||
const aos::FloatVArg sqReplaceBreakingThreshold, const aos::FloatVArg acceptanceEpsilon, PxU8 maxContactsPerManifold);
|
||||
//This function adds the box/convexhull manifold contacts to the contact buffer
|
||||
bool addManifoldContactsToContactBuffer(PxContactBuffer& contactBuffer, const aos::PxTransformV& transf1);
|
||||
//This function adds the sphere/capsule manifold contacts to the contact buffer
|
||||
bool addManifoldContactsToContactBuffer(PxContactBuffer& contactBuffer, const aos::PxTransformV& trA, const aos::PxTransformV& trB, const aos::FloatVArg radius);
|
||||
void drawManifold(PxRenderOutput& out, const aos::PxTransformV& trA, const aos::PxTransformV& trB) const;
|
||||
|
||||
//Code to load from a buffer and store to a buffer.
|
||||
void fromBuffer(PxU8* PX_RESTRICT buffer);
|
||||
void toBuffer(PxU8* PX_RESTRICT buffer) const;
|
||||
|
||||
static void drawLine(PxRenderOutput& out, const aos::Vec3VArg p0, const aos::Vec3VArg p1, PxU32 color = 0xff00ffff);
|
||||
static void drawLine(PxRenderOutput& out, const PxVec3 p0, const PxVec3 p1, PxU32 color = 0xff00ffff);
|
||||
static void drawPoint(PxRenderOutput& out, const aos::Vec3VArg p, const PxF32 size, PxU32 color = 0x00ff0000);
|
||||
static void drawPolygon(PxRenderOutput& out, const aos::PxTransformV& transform, aos::Vec3V* points, PxU32 numVerts, PxU32 color = 0xff00ffff);
|
||||
|
||||
aos::PxTransformV mRelativeTransform;//aToB
|
||||
PxF32 mMaxPen[GU_MAX_MANIFOLD_SIZE];
|
||||
PxU8 mManifoldIndices[GU_MAX_MANIFOLD_SIZE];
|
||||
PxU8 mNumManifolds;
|
||||
PxU8 mNumTotalContacts;
|
||||
SinglePersistentContactManifold mManifolds[GU_MAX_MANIFOLD_SIZE];
|
||||
|
||||
} PX_ALIGN_SUFFIX(16);
|
||||
|
||||
#if PX_VC
|
||||
#pragma warning(pop)
|
||||
#endif
|
||||
|
||||
// This function calculates the average normal in the manifold in world space
|
||||
PX_FORCE_INLINE aos::Vec3V PersistentContactManifold::getWorldNormal(const aos::PxTransformV& trB) const
|
||||
{
|
||||
using namespace aos;
|
||||
|
||||
Vec4V nPen = mContactPoints[0].mLocalNormalPen;
|
||||
for(PxU32 i=1; i<mNumContacts; ++i)
|
||||
nPen = V4Add(nPen, mContactPoints[i].mLocalNormalPen);
|
||||
|
||||
const Vec3V n = Vec3V_From_Vec4V(nPen);
|
||||
const FloatV sqLength = V3Dot(n, n);
|
||||
const Vec3V nn = V3Sel(FIsGrtr(sqLength, FEps()), n, Vec3V_From_Vec4V(mContactPoints[0].mLocalNormalPen));
|
||||
return trB.rotateAndNormalize(nn);
|
||||
}
|
||||
|
||||
// This function calculates the average normal in the manifold in local B space
|
||||
PX_FORCE_INLINE aos::Vec3V PersistentContactManifold::getLocalNormal() const
|
||||
{
|
||||
using namespace aos;
|
||||
|
||||
Vec4V nPen = mContactPoints[0].mLocalNormalPen;
|
||||
for(PxU32 i=1; i<mNumContacts; ++i)
|
||||
nPen = V4Add(nPen, mContactPoints[i].mLocalNormalPen);
|
||||
|
||||
return V3Normalize(Vec3V_From_Vec4V(nPen));
|
||||
}
|
||||
|
||||
// This function recalculates the contacts in the manifold based on the current relative transform between a pair of objects. If the recalculated contacts are within some threshold,
|
||||
// we will keep the contacts; Otherwise, we will remove the contacts.
|
||||
PX_FORCE_INLINE void PersistentContactManifold::refreshContactPoints(const aos::PxMatTransformV& aToB, const aos::FloatVArg projectBreakingThreshold, const aos::FloatVArg /*contactOffset*/)
|
||||
{
|
||||
using namespace aos;
|
||||
const FloatV sqProjectBreakingThreshold = FMul(projectBreakingThreshold, projectBreakingThreshold);
|
||||
|
||||
// first refresh worldspace positions and distance
|
||||
for (PxU32 i=mNumContacts; i > 0; --i)
|
||||
{
|
||||
PersistentContact& manifoldPoint = mContactPoints[i-1];
|
||||
const Vec3V localAInB = aToB.transform( manifoldPoint.mLocalPointA ); // from a to b
|
||||
const Vec3V localBInB = manifoldPoint.mLocalPointB;
|
||||
const Vec3V v = V3Sub(localAInB, localBInB);
|
||||
|
||||
const Vec3V localNormal = Vec3V_From_Vec4V(manifoldPoint.mLocalNormalPen); // normal in b space
|
||||
const FloatV dist = V3Dot(v, localNormal);
|
||||
|
||||
const Vec3V projectedPoint = V3NegScaleSub(localNormal, dist, localAInB);//manifoldPoint.worldPointA - manifoldPoint.worldPointB * manifoldPoint.m_distance1;
|
||||
const Vec3V projectedDifference = V3Sub(localBInB, projectedPoint);
|
||||
|
||||
const FloatV distance2d = V3Dot(projectedDifference, projectedDifference);
|
||||
//const BoolV con = BOr(FIsGrtr(dist, contactOffset), FIsGrtr(distance2d, sqProjectBreakingThreshold));
|
||||
const BoolV con = FIsGrtr(distance2d, sqProjectBreakingThreshold);
|
||||
if(BAllEqTTTT(con))
|
||||
{
|
||||
removeContactPoint(i-1);
|
||||
}
|
||||
else
|
||||
{
|
||||
manifoldPoint.mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(localNormal), dist);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#define PX_CP_TO_PCP(contactPoint) (reinterpret_cast<PersistentContact*>(contactPoint)) //this is used in the normal pcm contact gen
|
||||
#define PX_CP_TO_MPCP(contactPoint) (reinterpret_cast<MeshPersistentContact*>(contactPoint))//this is used in the mesh pcm contact gen
|
||||
|
||||
void addManifoldPoint(PersistentContact* manifoldContacts, PersistentContactManifold& manifold, GjkOutput& output,
|
||||
const aos::PxMatTransformV& aToB, const aos::FloatV replaceBreakingThreshold);
|
||||
|
||||
}//Gu
|
||||
}//physx
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user