1036 lines
37 KiB
C++
1036 lines
37 KiB
C++
//
|
|
// Redistribution and use in source and binary forms, with or without
|
|
// modification, are permitted provided that the following conditions
|
|
// are met:
|
|
// * Redistributions of source code must retain the above copyright
|
|
// notice, this list of conditions and the following disclaimer.
|
|
// * Redistributions in binary form must reproduce the above copyright
|
|
// notice, this list of conditions and the following disclaimer in the
|
|
// documentation and/or other materials provided with the distribution.
|
|
// * Neither the name of NVIDIA CORPORATION nor the names of its
|
|
// contributors may be used to endorse or promote products derived
|
|
// from this software without specific prior written permission.
|
|
//
|
|
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
|
|
// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
|
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
|
|
// PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
|
|
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
|
|
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
|
|
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
|
|
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
|
|
// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
|
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
|
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|
//
|
|
// Copyright (c) 2008-2021 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/GuContactBuffer.h"
|
|
|
|
#include "GuContactPolygonPolygon.h"
|
|
#include "GuGeometryUnion.h"
|
|
#include "GuConvexHelper.h"
|
|
#include "GuInternal.h"
|
|
#include "GuSeparatingAxes.h"
|
|
#include "GuContactMethodImpl.h"
|
|
|
|
#include "PsMathUtils.h"
|
|
#include "PsFPU.h"
|
|
|
|
// csigg: the single reference of gEnableOptims (below) has been
|
|
// replaced with the actual value to prevent ios64 compiler crash.
|
|
// static const int gEnableOptims = 1;
|
|
#define CONVEX_CONVEX_ROUGH_FIRST_PASS
|
|
#define TEST_INTERNAL_OBJECTS
|
|
#ifdef TEST_INTERNAL_OBJECTS
|
|
#define USE_BOX_DATA
|
|
#endif
|
|
|
|
using namespace physx;
|
|
using namespace Gu;
|
|
using namespace shdfnd::aos;
|
|
using namespace intrinsics;
|
|
|
|
#ifdef TEST_INTERNAL_OBJECTS
|
|
#ifdef USE_BOX_DATA
|
|
PX_FORCE_INLINE void BoxSupport(const float extents[3], const PxVec3& sv, float p[3])
|
|
{
|
|
const PxU32* iextents = reinterpret_cast<const PxU32*>(extents);
|
|
const PxU32* isv = reinterpret_cast<const PxU32*>(&sv);
|
|
PxU32* ip = reinterpret_cast<PxU32*>(p);
|
|
|
|
ip[0] = iextents[0]|(isv[0]&PX_SIGN_BITMASK);
|
|
ip[1] = iextents[1]|(isv[1]&PX_SIGN_BITMASK);
|
|
ip[2] = iextents[2]|(isv[2]&PX_SIGN_BITMASK);
|
|
}
|
|
#endif
|
|
|
|
#if PX_DEBUG
|
|
static const PxReal testInternalObjectsEpsilon = 1.0e-3f;
|
|
#endif
|
|
|
|
#ifdef DO_NOT_REMOVE
|
|
PX_FORCE_INLINE void testInternalObjects_( const PxVec3& delta_c, const PxVec3& axis,
|
|
const PolygonalData& polyData0, const PolygonalData& polyData1,
|
|
const Cm::Matrix34& tr0, const Cm::Matrix34& tr1,
|
|
float& dmin, float contactDistance)
|
|
{
|
|
{
|
|
/* float projected0 = axis.dot(tr0.p);
|
|
float projected1 = axis.dot(tr1.p);
|
|
float min0 = projected0 - polyData0.mInternal.mRadius;
|
|
float max0 = projected0 + polyData0.mInternal.mRadius;
|
|
float min1 = projected1 - polyData1.mInternal.mRadius;
|
|
float max1 = projected1 + polyData1.mInternal.mRadius;
|
|
*/
|
|
float MinMaxRadius = polyData0.mInternal.mRadius + polyData1.mInternal.mRadius;
|
|
PxVec3 delta = tr0.p - tr1.p;
|
|
const PxReal dp = axis.dot(delta);
|
|
// const PxReal dp2 = axis.dot(delta_c);
|
|
|
|
// const PxReal d0 = max0 - min1;
|
|
// const PxReal d0 = projected0 + polyData0.mInternal.mRadius - projected1 + polyData1.mInternal.mRadius;
|
|
// const PxReal d0 = projected0 - projected1 + polyData0.mInternal.mRadius + polyData1.mInternal.mRadius;
|
|
// const PxReal d0 = projected0 - projected1 + MinMaxRadius;
|
|
// const PxReal d0 = axis.dot(tr0.p) - axis.dot(tr1.p) + MinMaxRadius;
|
|
// const PxReal d0 = axis.dot(tr0.p - tr1.p) + MinMaxRadius;
|
|
// const PxReal d0 = MinMaxRadius + axis.dot(delta);
|
|
const PxReal d0 = MinMaxRadius + dp;
|
|
|
|
// const PxReal d1 = max1 - min0;
|
|
// const PxReal d1 = projected1 + polyData1.mInternal.mRadius - projected0 + polyData0.mInternal.mRadius;
|
|
// const PxReal d1 = projected1 - projected0 + polyData1.mInternal.mRadius + polyData0.mInternal.mRadius;
|
|
// const PxReal d1 = projected1 - projected0 + MinMaxRadius;
|
|
// const PxReal d1 = axis.dot(tr1.p) - axis.dot(tr0.p) + MinMaxRadius;
|
|
// const PxReal d1 = axis.dot(tr1.p - tr0.p) + MinMaxRadius;
|
|
// const PxReal d1 = MinMaxRadius - axis.dot(delta);
|
|
const PxReal d1 = MinMaxRadius - dp;
|
|
|
|
dmin = selectMin(d0, d1);
|
|
return;
|
|
}
|
|
|
|
#ifdef USE_BOX_DATA
|
|
const PxVec3 localAxis0 = tr0.rotateTranspose(axis);
|
|
const PxVec3 localAxis1 = tr1.rotateTranspose(axis);
|
|
|
|
const float dp = delta_c.dot(axis);
|
|
|
|
float p0[3];
|
|
BoxSupport(polyData0.mInternal.mExtents, localAxis0, p0);
|
|
float p1[3];
|
|
BoxSupport(polyData1.mInternal.mExtents, localAxis1, p1);
|
|
|
|
const float Radius0 = p0[0]*localAxis0.x + p0[1]*localAxis0.y + p0[2]*localAxis0.z;
|
|
const float Radius1 = p1[0]*localAxis1.x + p1[1]*localAxis1.y + p1[2]*localAxis1.z;
|
|
|
|
const float MinRadius = selectMax(Radius0, polyData0.mInternal.mRadius);
|
|
const float MaxRadius = selectMax(Radius1, polyData1.mInternal.mRadius);
|
|
#else
|
|
const float dp = delta_c.dot(axis);
|
|
const float MinRadius = polyData0.mInternal.mRadius;
|
|
const float MaxRadius = polyData1.mInternal.mRadius;
|
|
#endif
|
|
const float MinMaxRadius = MaxRadius + MinRadius;
|
|
const float d0 = MinMaxRadius + dp;
|
|
const float d1 = MinMaxRadius - dp;
|
|
|
|
dmin = selectMin(d0, d1);
|
|
}
|
|
#endif
|
|
|
|
static PX_FORCE_INLINE bool testInternalObjects( const PxVec3& delta_c, const PxVec3& axis,
|
|
const PolygonalData& polyData0, const PolygonalData& polyData1,
|
|
const Cm::Matrix34& tr0, const Cm::Matrix34& tr1,
|
|
float dmin)
|
|
{
|
|
#ifdef USE_BOX_DATA
|
|
const PxVec3 localAxis0 = tr0.rotateTranspose(axis);
|
|
const PxVec3 localAxis1 = tr1.rotateTranspose(axis);
|
|
|
|
const float dp = delta_c.dot(axis);
|
|
|
|
float p0[3];
|
|
BoxSupport(polyData0.mInternal.mExtents, localAxis0, p0);
|
|
float p1[3];
|
|
BoxSupport(polyData1.mInternal.mExtents, localAxis1, p1);
|
|
|
|
const float Radius0 = p0[0]*localAxis0.x + p0[1]*localAxis0.y + p0[2]*localAxis0.z;
|
|
const float Radius1 = p1[0]*localAxis1.x + p1[1]*localAxis1.y + p1[2]*localAxis1.z;
|
|
|
|
const float MinRadius = selectMax(Radius0, polyData0.mInternal.mRadius);
|
|
const float MaxRadius = selectMax(Radius1, polyData1.mInternal.mRadius);
|
|
#else
|
|
const float dp = delta_c.dot(axis);
|
|
const float MinRadius = polyData0.mInternal.mRadius;
|
|
const float MaxRadius = polyData1.mInternal.mRadius;
|
|
#endif
|
|
const float MinMaxRadius = MaxRadius + MinRadius;
|
|
const float d0 = MinMaxRadius + dp;
|
|
const float d1 = MinMaxRadius - dp;
|
|
|
|
const float depth = selectMin(d0, d1);
|
|
if(depth>dmin)
|
|
return false;
|
|
return true;
|
|
}
|
|
#endif
|
|
|
|
PX_FORCE_INLINE float PxcMultiplyAdd3x4(PxU32 i, const PxVec3& p0, const PxVec3& p1, const Cm::Matrix34& world)
|
|
{
|
|
return (p1.x + p0.x) * world.m.column0[i] + (p1.y + p0.y) * world.m.column1[i] + (p1.z + p0.z) * world.m.column2[i] + 2.0f * world.p[i];
|
|
}
|
|
|
|
PX_FORCE_INLINE float PxcMultiplySub3x4(PxU32 i, const PxVec3& p0, const PxVec3& p1, const Cm::Matrix34& world)
|
|
{
|
|
return (p1.x - p0.x) * world.m.column0[i] + (p1.y - p0.y) * world.m.column1[i] + (p1.z - p0.z) * world.m.column2[i];
|
|
}
|
|
|
|
static PX_FORCE_INLINE bool testNormal( const PxVec3& axis, PxReal min0, PxReal max0,
|
|
const PolygonalData& polyData1,
|
|
const Cm::Matrix34& m1to0,
|
|
const Cm::FastVertex2ShapeScaling& scaling1,
|
|
PxReal& depth, PxReal contactDistance)
|
|
{
|
|
//The separating axis we want to test is a face normal of hull0
|
|
PxReal min1, max1;
|
|
(polyData1.mProjectHull)(polyData1, axis, m1to0, scaling1, min1, max1);
|
|
|
|
if(max0+contactDistance<min1 || max1+contactDistance<min0)
|
|
return false;
|
|
|
|
const PxReal d0 = max0 - min1;
|
|
const PxReal d1 = max1 - min0;
|
|
depth = selectMin(d0, d1);
|
|
return true;
|
|
}
|
|
|
|
static PX_FORCE_INLINE bool testSeparatingAxis( const PolygonalData& polyData0, const PolygonalData& polyData1,
|
|
const Cm::Matrix34& world0, const Cm::Matrix34& world1,
|
|
const Cm::FastVertex2ShapeScaling& scaling0, const Cm::FastVertex2ShapeScaling& scaling1,
|
|
const PxVec3& axis, PxReal& depth, PxReal contactDistance)
|
|
{
|
|
PxReal min0, max0;
|
|
(polyData0.mProjectHull)(polyData0, axis, world0, scaling0, min0, max0);
|
|
|
|
return testNormal(axis, min0, max0, polyData1, world1, scaling1, depth, contactDistance);
|
|
}
|
|
|
|
static bool PxcSegmentAABBIntersect(const PxVec3& p0, const PxVec3& p1,
|
|
const PxVec3& minimum, const PxVec3& maximum,
|
|
const Cm::Matrix34& world)
|
|
{
|
|
PxVec3 boxExtent, diff, dir;
|
|
PxReal fAWdU[3];
|
|
|
|
dir.x = PxcMultiplySub3x4(0, p0, p1, world);
|
|
boxExtent.x = maximum.x - minimum.x;
|
|
diff.x = (PxcMultiplyAdd3x4(0, p0, p1, world) - (maximum.x+minimum.x));
|
|
fAWdU[0] = PxAbs(dir.x);
|
|
if(PxAbs(diff.x)> boxExtent.x + fAWdU[0]) return false;
|
|
|
|
dir.y = PxcMultiplySub3x4(1, p0, p1, world);
|
|
boxExtent.y = maximum.y - minimum.y;
|
|
diff.y = (PxcMultiplyAdd3x4(1, p0, p1, world) - (maximum.y+minimum.y));
|
|
fAWdU[1] = PxAbs(dir.y);
|
|
if(PxAbs(diff.y)> boxExtent.y + fAWdU[1]) return false;
|
|
|
|
dir.z = PxcMultiplySub3x4(2, p0, p1, world);
|
|
boxExtent.z = maximum.z - minimum.z;
|
|
diff.z = (PxcMultiplyAdd3x4(2, p0, p1, world) - (maximum.z+minimum.z));
|
|
fAWdU[2] = PxAbs(dir.z);
|
|
if(PxAbs(diff.z)> boxExtent.z + fAWdU[2]) return false;
|
|
|
|
PxReal f;
|
|
f = dir.y * diff.z - dir.z*diff.y; if(PxAbs(f)>boxExtent.y*fAWdU[2] + boxExtent.z*fAWdU[1]) return false;
|
|
f = dir.z * diff.x - dir.x*diff.z; if(PxAbs(f)>boxExtent.x*fAWdU[2] + boxExtent.z*fAWdU[0]) return false;
|
|
f = dir.x * diff.y - dir.y*diff.x; if(PxAbs(f)>boxExtent.x*fAWdU[1] + boxExtent.y*fAWdU[0]) return false;
|
|
return true;
|
|
}
|
|
|
|
#define EXPERIMENT
|
|
|
|
/*
|
|
Edge culling can clearly be improved : in ConvexTest02 for example, edges don't even touch the other mesh sometimes.
|
|
*/
|
|
static void PxcFindSeparatingAxes( SeparatingAxes& sa, const PxU32* PX_RESTRICT indices, PxU32 numPolygons,
|
|
const PolygonalData& polyData,
|
|
const Cm::Matrix34& world0, const PxPlane& plane,
|
|
const Cm::Matrix34& m0to1, const PxBounds3& aabb, PxReal contactDistance,
|
|
const Cm::FastVertex2ShapeScaling& scaling)
|
|
{
|
|
// EdgeCache edgeCache; // PT: TODO: check this is actually useful
|
|
const PxVec3* PX_RESTRICT vertices = polyData.mVerts;
|
|
const Gu::HullPolygonData* PX_RESTRICT polygons = polyData.mPolygons;
|
|
const PxU8* PX_RESTRICT vrefsBase = polyData.mPolygonVertexRefs;
|
|
|
|
while(numPolygons--)
|
|
{
|
|
//Get current polygon
|
|
const Gu::HullPolygonData& P = polygons[*indices++];
|
|
const PxU8* PX_RESTRICT VData = vrefsBase + P.mVRef8;
|
|
|
|
// Loop through polygon vertices == polygon edges
|
|
PxU32 numVerts = P.mNbVerts;
|
|
|
|
#ifdef EXPERIMENT
|
|
PxVec3 p0,p1;
|
|
PxU8 VRef0 = VData[0];
|
|
p0 = scaling * vertices[VRef0];
|
|
bool b0 = plane.distance(p0) <= contactDistance;
|
|
#endif
|
|
|
|
for(PxU32 j = 0; j < numVerts; j++)
|
|
{
|
|
PxU32 j1 = j+1;
|
|
if(j1 >= numVerts) j1 = 0;
|
|
|
|
#ifndef EXPERIMENT
|
|
PxU8 VRef0 = VData[j];
|
|
#endif
|
|
PxU8 VRef1 = VData[j1];
|
|
|
|
// Ps::order(VRef0, VRef1); //make sure edge (a,b) == edge (b,a)
|
|
// if (edgeCache.isInCache(VRef0, VRef1))
|
|
// continue;
|
|
|
|
//transform points //TODO: once this works we could transform plan instead, that is more efficient!!
|
|
|
|
#ifdef EXPERIMENT
|
|
p1 = scaling * vertices[VRef1];
|
|
#else
|
|
const PxVec3 p0 = scaling * vertices[VRef0];
|
|
const PxVec3 p1 = scaling * vertices[VRef1];
|
|
#endif
|
|
|
|
// Cheap but effective culling!
|
|
#ifdef EXPERIMENT
|
|
bool b1 = plane.distance(p1) <= contactDistance;
|
|
if(b0 || b1)
|
|
#else
|
|
if(plane.signedDistanceHessianNormalForm(p0) <= contactDistance ||
|
|
plane.signedDistanceHessianNormalForm(p1) <= contactDistance)
|
|
#endif
|
|
{
|
|
if(PxcSegmentAABBIntersect(p0, p1, aabb.minimum, aabb.maximum, m0to1))
|
|
{
|
|
// Create current edge. We're only interested in different edge directions so we normalize.
|
|
const PxVec3 currentEdge = world0.rotate(p0 - p1).getNormalized();
|
|
sa.addAxis(currentEdge);
|
|
}
|
|
}
|
|
#ifdef EXPERIMENT
|
|
VRef0 = VRef1;
|
|
p0 = p1;
|
|
b0 = b1;
|
|
#endif
|
|
}
|
|
}
|
|
}
|
|
|
|
static bool PxcTestFacesSepAxesBackface(const PolygonalData& polyData0, const PolygonalData& polyData1,
|
|
const Cm::Matrix34& world0, const Cm::Matrix34& world1,
|
|
const Cm::FastVertex2ShapeScaling& scaling0, const Cm::FastVertex2ShapeScaling& scaling1,
|
|
const Cm::Matrix34& m1to0, const PxVec3& delta,
|
|
PxReal& dmin, PxVec3& sep, PxU32& id, PxU32* PX_RESTRICT indices_, PxU32& numIndices,
|
|
PxReal contactDistance, float toleranceLength
|
|
#ifdef TEST_INTERNAL_OBJECTS
|
|
, const PxVec3& worldDelta
|
|
#endif
|
|
)
|
|
{
|
|
PX_UNUSED(toleranceLength); // Only used in Debug
|
|
id = PX_INVALID_U32;
|
|
PxU32* indices = indices_;
|
|
|
|
const PxU32 num = polyData0.mNbPolygons;
|
|
const PxVec3* PX_RESTRICT vertices = polyData0.mVerts;
|
|
const Gu::HullPolygonData* PX_RESTRICT polygons = polyData0.mPolygons;
|
|
|
|
//transform delta from hull0 shape into vertex space:
|
|
const PxVec3 vertSpaceDelta = scaling0 % delta;
|
|
|
|
// PT: prefetch polygon data
|
|
{
|
|
const PxU32 dataSize = num*sizeof(Gu::HullPolygonData);
|
|
for(PxU32 offset=0; offset < dataSize; offset+=128)
|
|
Ps::prefetchLine(polygons, offset);
|
|
}
|
|
|
|
for(PxU32 i=0; i < num; i++)
|
|
{
|
|
const Gu::HullPolygonData& P = polygons[i];
|
|
const PxPlane& PL = P.mPlane;
|
|
|
|
// Do backface-culling
|
|
if(PL.n.dot(vertSpaceDelta) < 0.0f)
|
|
continue;
|
|
|
|
//normals transform by inverse transpose: (and transpose(skew) == skew as its symmetric)
|
|
PxVec3 shapeSpaceNormal = scaling0 % PL.n;
|
|
//renormalize: (Arr!)
|
|
const PxReal magnitude = shapeSpaceNormal.normalize(); // PT: We need to find a way to skip this normalize
|
|
|
|
#ifdef TEST_INTERNAL_OBJECTS
|
|
|
|
/*
|
|
const PxVec3 worldNormal_ = world0.rotate(shapeSpaceNormal);
|
|
PxReal d0;
|
|
bool test0 = PxcTestSeparatingAxis(polyData0, polyData1, world0, world1, scaling0, scaling1, worldNormal_, d0, contactDistance);
|
|
|
|
PxReal d1;
|
|
const float invMagnitude0 = 1.0f / magnitude;
|
|
bool test1 = PxcTestNormal(shapeSpaceNormal, P.getMin(vertices) * invMagnitude0, P.getMax() * invMagnitude0, polyData1, m1to0, scaling1, d1, contactDistance);
|
|
|
|
PxReal d2;
|
|
testInternalObjects_(worldDelta, worldNormal_, polyData0, polyData1, world0, world1, d2, contactDistance);
|
|
*/
|
|
|
|
const PxVec3 worldNormal = world0.rotate(shapeSpaceNormal);
|
|
if(!testInternalObjects(worldDelta, worldNormal, polyData0, polyData1, world0, world1, dmin))
|
|
{
|
|
#if PX_DEBUG
|
|
PxReal d;
|
|
const float invMagnitude = 1.0f / magnitude;
|
|
if(testNormal(shapeSpaceNormal, P.getMin(vertices) * invMagnitude, P.getMax() * invMagnitude, polyData1, m1to0, scaling1, d, contactDistance)) //note how we scale scalars by skew magnitude change as we do plane d-s.
|
|
{
|
|
PX_ASSERT(d + testInternalObjectsEpsilon*toleranceLength >= dmin);
|
|
}
|
|
#endif
|
|
continue;
|
|
}
|
|
#endif
|
|
|
|
*indices++ = i;
|
|
|
|
////////////////////
|
|
/*
|
|
//gotta transform minimum and maximum from vertex to shape space!
|
|
//I think they transform like the 'd' of the plane, basically by magnitude division!
|
|
//unfortunately I am not certain of that, so let's convert them to a point in vertex space, transform that, and then convert back to a plane d.
|
|
|
|
//let's start by transforming the plane's d:
|
|
//a point on the plane:
|
|
|
|
PxVec3 vertSpaceDPoint = PL.normal * -PL.d;
|
|
|
|
//make sure this is on the plane:
|
|
PxReal distZero = PL.signedDistanceHessianNormalForm(vertSpaceDPoint); //should be zero
|
|
|
|
//transform:
|
|
|
|
PxVec3 shapeSpaceDPoint = cache.mVertex2ShapeSkew[skewIndex] * vertSpaceDPoint;
|
|
|
|
//make into a d offset again by projecting along the plane:
|
|
PxcPlane shapeSpacePlane(shapeSpaceNormal, shapeSpaceDPoint);
|
|
|
|
//see what D is!!
|
|
|
|
|
|
//NOTE: for boxes scale[0] is always id so this is all redundant. Hopefully for convex convex it will become useful!
|
|
*/
|
|
|
|
////////////////////
|
|
|
|
PxReal d;
|
|
const float invMagnitude = 1.0f / magnitude;
|
|
if(!testNormal(shapeSpaceNormal, P.getMin(vertices) * invMagnitude, P.getMax() * invMagnitude, polyData1, m1to0, scaling1, d, contactDistance)) //note how we scale scalars by skew magnitude change as we do plane d-s.
|
|
return false;
|
|
|
|
if(d < dmin)
|
|
{
|
|
#ifdef TEST_INTERNAL_OBJECTS
|
|
sep = worldNormal;
|
|
#else
|
|
sep = world0.rotate(shapeSpaceNormal);
|
|
#endif
|
|
dmin = d;
|
|
id = i;
|
|
}
|
|
}
|
|
|
|
numIndices = PxU32(indices - indices_);
|
|
|
|
PX_ASSERT(id!=PX_INVALID_U32); //Should never happen with this version
|
|
|
|
return true;
|
|
}
|
|
|
|
// PT: isolating this piece of code allows us to better see what happens in PIX. For some reason it looks
|
|
// like isolating it creates *less* LHS than before, but I don't understand why. There's still a lot of
|
|
// bad stuff there anyway
|
|
//PX_FORCE_INLINE
|
|
static void prepareData(PxPlane& witnessPlane0, PxPlane& witnessPlane1,
|
|
PxBounds3& aabb0, PxBounds3& aabb1,
|
|
const PxBounds3& hullBounds0, const PxBounds3& hullBounds1,
|
|
const PxPlane& vertSpacePlane0, const PxPlane& vertSpacePlane1,
|
|
const Cm::FastVertex2ShapeScaling& scaling0, const Cm::FastVertex2ShapeScaling& scaling1,
|
|
const Cm::Matrix34& m0to1, const Cm::Matrix34& m1to0,
|
|
PxReal contactDistance
|
|
)
|
|
{
|
|
scaling0.transformPlaneToShapeSpace(vertSpacePlane0.n, vertSpacePlane0.d, witnessPlane0.n, witnessPlane0.d);
|
|
scaling1.transformPlaneToShapeSpace(vertSpacePlane1.n, vertSpacePlane1.d, witnessPlane1.n, witnessPlane1.d);
|
|
|
|
//witnessPlane0 = witnessPlane0.getTransformed(m0to1);
|
|
//witnessPlane1 = witnessPlane1.getTransformed(m1to0);
|
|
const PxVec3 newN0 = m0to1.rotate(witnessPlane0.n);
|
|
witnessPlane0 = PxPlane(newN0, witnessPlane0.d - m0to1.p.dot(newN0));
|
|
const PxVec3 newN1 = m1to0.rotate(witnessPlane1.n);
|
|
witnessPlane1 = PxPlane(newN1, witnessPlane1.d - m1to0.p.dot(newN1));
|
|
|
|
aabb0 = hullBounds0;
|
|
aabb1 = hullBounds1;
|
|
|
|
//gotta inflate // PT: perfect LHS recipe here....
|
|
const PxVec3 inflate(contactDistance);
|
|
aabb0.minimum -= inflate;
|
|
aabb1.minimum -= inflate;
|
|
aabb0.maximum += inflate;
|
|
aabb1.maximum += inflate;
|
|
}
|
|
|
|
static bool PxcBruteForceOverlapBackface( const PxBounds3& hullBounds0, const PxBounds3& hullBounds1,
|
|
const PolygonalData& polyData0, const PolygonalData& polyData1,
|
|
const Cm::Matrix34& world0, const Cm::Matrix34& world1,
|
|
const Cm::FastVertex2ShapeScaling& scaling0, const Cm::FastVertex2ShapeScaling& scaling1,
|
|
const Cm::Matrix34& m0to1, const Cm::Matrix34& m1to0, const PxVec3& delta,
|
|
PxU32& id0, PxU32& id1,
|
|
PxReal& depth, PxVec3& sep, PxcSepAxisType& code, PxReal contactDistance, float toleranceLength)
|
|
{
|
|
const PxVec3 localDelta0 = world0.rotateTranspose(delta);
|
|
PxU32* PX_RESTRICT indices0 = reinterpret_cast<PxU32*>(PxAlloca(polyData0.mNbPolygons*sizeof(PxU32)));
|
|
|
|
PxU32 numIndices0;
|
|
PxReal dmin0 = PX_MAX_REAL;
|
|
PxVec3 vec0;
|
|
if(!PxcTestFacesSepAxesBackface(polyData0, polyData1, world0, world1, scaling0, scaling1, m1to0, localDelta0, dmin0, vec0, id0, indices0, numIndices0, contactDistance, toleranceLength
|
|
#ifdef TEST_INTERNAL_OBJECTS
|
|
, -delta
|
|
#endif
|
|
))
|
|
return false;
|
|
|
|
const PxVec3 localDelta1 = world1.rotateTranspose(delta);
|
|
|
|
PxU32* PX_RESTRICT indices1 = reinterpret_cast<PxU32*>(PxAlloca(polyData1.mNbPolygons*sizeof(PxU32)));
|
|
|
|
PxU32 numIndices1;
|
|
PxReal dmin1 = PX_MAX_REAL;
|
|
PxVec3 vec1;
|
|
if(!PxcTestFacesSepAxesBackface(polyData1, polyData0, world1, world0, scaling1, scaling0, m0to1, -localDelta1, dmin1, vec1, id1, indices1, numIndices1, contactDistance, toleranceLength
|
|
#ifdef TEST_INTERNAL_OBJECTS
|
|
, delta
|
|
#endif
|
|
))
|
|
return false;
|
|
|
|
PxReal dmin = dmin0;
|
|
PxVec3 vec = vec0;
|
|
code = SA_NORMAL0;
|
|
|
|
if(dmin1 < dmin)
|
|
{
|
|
dmin = dmin1;
|
|
vec = vec1;
|
|
code = SA_NORMAL1;
|
|
}
|
|
|
|
PX_ASSERT(id0!=PX_INVALID_U32);
|
|
PX_ASSERT(id1!=PX_INVALID_U32);
|
|
|
|
// Brute-force find a separating axis
|
|
SeparatingAxes mSA0;
|
|
SeparatingAxes mSA1;
|
|
mSA0.reset();
|
|
mSA1.reset();
|
|
PxPlane witnessPlane0;
|
|
PxPlane witnessPlane1;
|
|
PxBounds3 aabb0, aabb1;
|
|
|
|
prepareData(witnessPlane0, witnessPlane1,
|
|
aabb0, aabb1,
|
|
hullBounds0, hullBounds1,
|
|
polyData0.mPolygons[id0].mPlane,
|
|
polyData1.mPolygons[id1].mPlane,
|
|
scaling0, scaling1,
|
|
m0to1, m1to0,
|
|
contactDistance);
|
|
|
|
// Find possibly separating axes
|
|
PxcFindSeparatingAxes(mSA0, indices0, numIndices0, polyData0, world0, witnessPlane1, m0to1, aabb1, contactDistance, scaling0);
|
|
PxcFindSeparatingAxes(mSA1, indices1, numIndices1, polyData1, world1, witnessPlane0, m1to0, aabb0, contactDistance, scaling1);
|
|
// PxcFindSeparatingAxes(context.mSA0, &id0, 1, hull0, world0, witnessPlane1, m0to1, aabbMin1, aabbMax1);
|
|
// PxcFindSeparatingAxes(context.mSA1, &id1, 1, hull1, world1, witnessPlane0, m1to0, aabbMin0, aabbMax0);
|
|
|
|
const PxU32 numEdges0 = mSA0.getNumAxes();
|
|
const PxVec3* PX_RESTRICT edges0 = mSA0.getAxes();
|
|
|
|
const PxU32 numEdges1 = mSA1.getNumAxes();
|
|
const PxVec3* PX_RESTRICT edges1 = mSA1.getAxes();
|
|
|
|
// Worst case = convex test 02 with big meshes: 23 & 23 edges => 23*23 = 529 tests!
|
|
// printf("%d - %d\n", NbEdges0, NbEdges1); // maximum = ~20 in test scenes.
|
|
|
|
for(PxU32 i=0; i < numEdges0; i++)
|
|
{
|
|
const PxVec3& edge0 = edges0[i];
|
|
for(PxU32 j=0; j < numEdges1; j++)
|
|
{
|
|
const PxVec3& edge1 = edges1[j];
|
|
|
|
PxVec3 sepAxis = edge0.cross(edge1);
|
|
if(!Ps::isAlmostZero(sepAxis))
|
|
{
|
|
sepAxis = sepAxis.getNormalized();
|
|
|
|
#ifdef TEST_INTERNAL_OBJECTS
|
|
if(!testInternalObjects(-delta, sepAxis, polyData0, polyData1, world0, world1, dmin))
|
|
{
|
|
#if PX_DEBUG
|
|
PxReal d;
|
|
if(testSeparatingAxis(polyData0, polyData1, world0, world1, scaling0, scaling1, sepAxis, d, contactDistance))
|
|
{
|
|
PX_ASSERT(d + testInternalObjectsEpsilon*toleranceLength >= dmin);
|
|
}
|
|
#endif
|
|
continue;
|
|
}
|
|
#endif
|
|
PxReal d;
|
|
if(!testSeparatingAxis(polyData0, polyData1, world0, world1, scaling0, scaling1, sepAxis, d, contactDistance))
|
|
return false;
|
|
|
|
if(d<dmin)
|
|
{
|
|
dmin = d;
|
|
vec = sepAxis;
|
|
code = SA_EE;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
depth = dmin;
|
|
sep = vec;
|
|
|
|
return true;
|
|
}
|
|
|
|
static bool GuTestFacesSepAxesBackfaceRoughPass(
|
|
const PolygonalData& polyData0, const PolygonalData& polyData1,
|
|
const Cm::Matrix34& world0, const Cm::Matrix34& world1,
|
|
const Cm::FastVertex2ShapeScaling& scaling0, const Cm::FastVertex2ShapeScaling& scaling1,
|
|
const Cm::Matrix34& m1to0, const PxVec3& /*witness*/, const PxVec3& delta,
|
|
PxReal& dmin, PxVec3& sep, PxU32& id,
|
|
PxReal contactDistance, float toleranceLength
|
|
#ifdef TEST_INTERNAL_OBJECTS
|
|
, const PxVec3& worldDelta
|
|
#endif
|
|
)
|
|
{
|
|
PX_UNUSED(toleranceLength); // Only used in Debug
|
|
id = PX_INVALID_U32;
|
|
|
|
const PxU32 num = polyData0.mNbPolygons;
|
|
const Gu::HullPolygonData* PX_RESTRICT polygons = polyData0.mPolygons;
|
|
const PxVec3* PX_RESTRICT vertices = polyData0.mVerts;
|
|
|
|
//transform delta from hull0 shape into vertex space:
|
|
const PxVec3 vertSpaceDelta = scaling0 % delta;
|
|
|
|
for(PxU32 i=0; i < num; i++)
|
|
{
|
|
const Gu::HullPolygonData& P = polygons[i];
|
|
const PxPlane& PL = P.mPlane;
|
|
|
|
if(PL.n.dot(vertSpaceDelta) < 0.0f)
|
|
continue; //backface-cull
|
|
|
|
PxVec3 shapeSpaceNormal = scaling0 % PL.n; //normals transform with inverse transpose
|
|
//renormalize: (Arr!)
|
|
const PxReal magnitude = shapeSpaceNormal.normalize(); // PT: We need to find a way to skip this normalize
|
|
|
|
#ifdef TEST_INTERNAL_OBJECTS
|
|
const PxVec3 worldNormal = world0.rotate(shapeSpaceNormal);
|
|
if(!testInternalObjects(worldDelta, worldNormal, polyData0, polyData1, world0, world1, dmin))
|
|
{
|
|
#if PX_DEBUG
|
|
PxReal d;
|
|
const float invMagnitude = 1.0f / magnitude;
|
|
if(testNormal(shapeSpaceNormal, P.getMin(vertices) * invMagnitude, P.getMax() * invMagnitude, /*hull1,*/ polyData1, m1to0, scaling1, d, contactDistance)) //note how we scale scalars by skew magnitude change as we do plane d-s.
|
|
{
|
|
PX_ASSERT(d + testInternalObjectsEpsilon*toleranceLength >= dmin);
|
|
}
|
|
#endif
|
|
continue;
|
|
}
|
|
#endif
|
|
|
|
PxReal d;
|
|
const float invMagnitude = 1.0f / magnitude;
|
|
if(!testNormal(shapeSpaceNormal, P.getMin(vertices) * invMagnitude, P.getMax() * invMagnitude, /*hull1,*/ polyData1, m1to0, scaling1, d, contactDistance)) //note how we scale scalars by skew magnitude change as we do plane d-s.
|
|
return false;
|
|
|
|
if(d < dmin)
|
|
{
|
|
#ifdef TEST_INTERNAL_OBJECTS
|
|
sep = worldNormal;
|
|
#else
|
|
sep = world0.rotate(shapeSpaceNormal);
|
|
#endif
|
|
dmin = d;
|
|
id = i;
|
|
}
|
|
}
|
|
|
|
PX_ASSERT(id!=PX_INVALID_U32); //Should never happen with this version
|
|
|
|
return true;
|
|
}
|
|
|
|
static bool GuBruteForceOverlapBackfaceRoughPass( const PolygonalData& polyData0, const PolygonalData& polyData1,
|
|
const Cm::Matrix34& world0, const Cm::Matrix34& world1,
|
|
const Cm::FastVertex2ShapeScaling& scaling0, const Cm::FastVertex2ShapeScaling& scaling1,
|
|
const Cm::Matrix34& m0to1, const Cm::Matrix34& m1to0, const PxVec3& delta,
|
|
PxU32& id0, PxU32& id1,
|
|
PxReal& depth, PxVec3& sep, PxcSepAxisType& code, PxReal contactDistance, PxReal toleranceLength)
|
|
{
|
|
PxReal dmin0 = PX_MAX_REAL;
|
|
PxReal dmin1 = PX_MAX_REAL;
|
|
PxVec3 vec0, vec1;
|
|
|
|
const PxVec3 localDelta0 = world0.rotateTranspose(delta);
|
|
const PxVec3 localCenter1in0 = m1to0.transform(polyData1.mCenter);
|
|
if(!GuTestFacesSepAxesBackfaceRoughPass(polyData0, polyData1, world0, world1, scaling0, scaling1, m1to0, localCenter1in0, localDelta0, dmin0, vec0, id0, contactDistance, toleranceLength
|
|
#ifdef TEST_INTERNAL_OBJECTS
|
|
, -delta
|
|
#endif
|
|
))
|
|
return false;
|
|
|
|
const PxVec3 localDelta1 = world1.rotateTranspose(delta);
|
|
const PxVec3 localCenter0in1 = m0to1.transform(polyData0.mCenter);
|
|
if(!GuTestFacesSepAxesBackfaceRoughPass(polyData1, polyData0, world1, world0, scaling1, scaling0, m0to1, localCenter0in1, -localDelta1, dmin1, vec1, id1, contactDistance, toleranceLength
|
|
#ifdef TEST_INTERNAL_OBJECTS
|
|
, delta
|
|
#endif
|
|
))
|
|
return false;
|
|
|
|
PxReal dmin = dmin0;
|
|
PxVec3 vec = vec0;
|
|
code = SA_NORMAL0;
|
|
|
|
if(dmin1 < dmin)
|
|
{
|
|
dmin = dmin1;
|
|
vec = vec1;
|
|
code = SA_NORMAL1;
|
|
}
|
|
|
|
PX_ASSERT(id0!=PX_INVALID_U32);
|
|
PX_ASSERT(id1!=PX_INVALID_U32);
|
|
|
|
depth = dmin;
|
|
sep = vec;
|
|
|
|
return true;
|
|
}
|
|
|
|
static bool GuContactHullHull( const PolygonalData& polyData0, const PolygonalData& polyData1,
|
|
const PxBounds3& hullBounds0, const PxBounds3& hullBounds1,
|
|
const PxTransform& transform0, const PxTransform& transform1,
|
|
const NarrowPhaseParams& params, ContactBuffer& contactBuffer,
|
|
const Cm::FastVertex2ShapeScaling& scaling0, const Cm::FastVertex2ShapeScaling& scaling1,
|
|
bool idtScale0, bool idtScale1);
|
|
|
|
namespace physx
|
|
{
|
|
|
|
namespace Gu
|
|
{
|
|
// Box-convex contact generation
|
|
// this can no longer share code with convex-convex because that case needs scaling for both shapes, while this only needs it for one, which may be significant perf wise.
|
|
//
|
|
// PT: duplicating the full convex-vs-convex codepath is a lot worse. Look at it this way: if scaling is really "significant perf wise" then we made the whole convex-convex
|
|
// codepath significantly slower, and this is a lot more important than just box-vs-convex. The proper approach is to share the code and make sure scaling is NOT a perf hit.
|
|
// PT: please leave this function in the same translation unit as PxcContactHullHull.
|
|
|
|
bool contactBoxConvex(GU_CONTACT_METHOD_ARGS)
|
|
{
|
|
PX_UNUSED(renderOutput);
|
|
PX_UNUSED(cache);
|
|
|
|
const PxBoxGeometry& shapeBox = shape0.get<const PxBoxGeometry>();
|
|
|
|
Cm::FastVertex2ShapeScaling idtScaling;
|
|
|
|
const PxBounds3 boxBounds(-shapeBox.halfExtents, shapeBox.halfExtents);
|
|
|
|
PolygonalData polyData0;
|
|
PolygonalBox polyBox(shapeBox.halfExtents);
|
|
polyBox.getPolygonalData(&polyData0);
|
|
|
|
///////
|
|
|
|
Cm::FastVertex2ShapeScaling convexScaling;
|
|
PxBounds3 convexBounds;
|
|
PolygonalData polyData1;
|
|
const bool idtScale = getConvexData(shape1, convexScaling, convexBounds, polyData1);
|
|
|
|
return GuContactHullHull( polyData0, polyData1, boxBounds, convexBounds,
|
|
transform0, transform1, params, contactBuffer,
|
|
idtScaling, convexScaling, true, idtScale);
|
|
}
|
|
|
|
// PT: please leave this function in the same translation unit as PxcContactHullHull.
|
|
bool contactConvexConvex(GU_CONTACT_METHOD_ARGS)
|
|
{
|
|
PX_UNUSED(cache);
|
|
PX_UNUSED(renderOutput);
|
|
|
|
Cm::FastVertex2ShapeScaling scaling0, scaling1;
|
|
PxBounds3 convexBounds0, convexBounds1;
|
|
PolygonalData polyData0, polyData1;
|
|
const bool idtScale0 = getConvexData(shape0, scaling0, convexBounds0, polyData0);
|
|
const bool idtScale1 = getConvexData(shape1, scaling1, convexBounds1, polyData1);
|
|
|
|
return GuContactHullHull( polyData0, polyData1, convexBounds0, convexBounds1,
|
|
transform0, transform1, params, contactBuffer,
|
|
scaling0, scaling1, idtScale0, idtScale1);
|
|
}
|
|
}//Gu
|
|
}//physx
|
|
|
|
static bool GuContactHullHull( const PolygonalData& polyData0, const PolygonalData& polyData1,
|
|
const PxBounds3& hullBounds0, const PxBounds3& hullBounds1,
|
|
const PxTransform& transform0, const PxTransform& transform1,
|
|
const NarrowPhaseParams& params, ContactBuffer& contactBuffer,
|
|
const Cm::FastVertex2ShapeScaling& scaling0, const Cm::FastVertex2ShapeScaling& scaling1,
|
|
bool idtScale0, bool idtScale1)
|
|
{
|
|
// Compute matrices
|
|
const Cm::Matrix34 world0(transform0);
|
|
const Cm::Matrix34 world1(transform1);
|
|
|
|
const PxVec3 worldCenter0 = world0.transform(polyData0.mCenter);
|
|
const PxVec3 worldCenter1 = world1.transform(polyData1.mCenter);
|
|
|
|
const PxVec3 deltaC = worldCenter1 - worldCenter0;
|
|
|
|
PxReal depth;
|
|
|
|
///////////////////////////////////////////////////////////////////////////
|
|
|
|
// Early-exit test: quickly discard obviously non-colliding cases.
|
|
// PT: we used to skip this when objects were touching, which provided a small speedup (saving 2 full hull projections).
|
|
// We may want to add this back at some point in the future, if possible.
|
|
if(true) //AM: now we definitely want to test the cached axis to get the depth value for the cached axis, even if we had a contact before.
|
|
{
|
|
if(!testSeparatingAxis(polyData0, polyData1, world0, world1, scaling0, scaling1, deltaC, depth, params.mContactDistance))
|
|
//there was no contact previously and we reject using the same prev. axis.
|
|
return false;
|
|
}
|
|
|
|
///////////////////////////////////////////////////////////////////////////
|
|
|
|
// Compute relative transforms
|
|
PxTransform t0to1 = transform1.transformInv(transform0);
|
|
PxTransform t1to0 = transform0.transformInv(transform1);
|
|
|
|
PxU32 c0(0x7FFF);
|
|
PxU32 c1(0x7FFF);
|
|
PxVec3 worldNormal;
|
|
|
|
const Cm::Matrix34 m0to1(t0to1);
|
|
const Cm::Matrix34 m1to0(t1to0);
|
|
|
|
#ifdef CONVEX_CONVEX_ROUGH_FIRST_PASS
|
|
// PT: it is a bad idea to skip the rough pass, for two reasons:
|
|
// 1) performance. The rough pass is obviously a lot faster.
|
|
// 2) stability. The "skipIt" optimization relies on the rough pass being present to catch the cases where it fails. If you disable the rough pass
|
|
// "skipIt" can skip the whole work, contacts get lost, and we never "try again" ==> explosions
|
|
// bool TryRoughPass = (contactDistance == 0.0f); //Rough first pass doesn't work with dist based for some reason.
|
|
for(int TryRoughPass = 1 /*gEnableOptims*/; 0 <= TryRoughPass; --TryRoughPass)
|
|
{
|
|
#endif
|
|
|
|
{
|
|
PxcSepAxisType code;
|
|
PxU32 id0, id1;
|
|
//PxReal depth;
|
|
#ifdef CONVEX_CONVEX_ROUGH_FIRST_PASS
|
|
bool Status;
|
|
if(TryRoughPass)
|
|
{
|
|
Status = GuBruteForceOverlapBackfaceRoughPass(polyData0, polyData1, world0, world1, scaling0, scaling1, m0to1, m1to0, deltaC, id0, id1, depth, worldNormal, code, params.mContactDistance, params.mToleranceLength);
|
|
}
|
|
else
|
|
{
|
|
Status = PxcBruteForceOverlapBackface(
|
|
// hull0, hull1,
|
|
hullBounds0, hullBounds1,
|
|
polyData0, polyData1, world0, world1, scaling0, scaling1, m0to1, m1to0, deltaC, id0, id1, depth, worldNormal, code, params.mContactDistance, params.mToleranceLength);
|
|
}
|
|
|
|
if(!Status)
|
|
#else
|
|
if(!PxcBruteForceOverlapBackface(
|
|
// hull0, hull1,
|
|
hullBounds0, hullBounds1,
|
|
polyData0, polyData1,
|
|
world0, world1, scaling0, scaling1, m0to1, m1to0, deltaC, id0, id1, depth, worldNormal, code, contactDistance))
|
|
#endif
|
|
return false;
|
|
|
|
if(deltaC.dot(worldNormal) < 0.0f)
|
|
worldNormal = -worldNormal;
|
|
|
|
/*
|
|
worldNormal = -partialSep;
|
|
depth = -partialDepth;
|
|
code = SA_EE;
|
|
*/
|
|
|
|
if(code==SA_NORMAL0)
|
|
{
|
|
c0 = id0;
|
|
c1 = (polyData1.mSelectClosestEdgeCB)(polyData1, scaling1, world1.rotateTranspose(-worldNormal));
|
|
}
|
|
else if(code==SA_NORMAL1)
|
|
{
|
|
c0 = (polyData0.mSelectClosestEdgeCB)(polyData0, scaling0, world0.rotateTranspose(worldNormal));
|
|
c1 = id1;
|
|
}
|
|
else if(code==SA_EE)
|
|
{
|
|
c0 = (polyData0.mSelectClosestEdgeCB)(polyData0, scaling0, world0.rotateTranspose(worldNormal));
|
|
c1 = (polyData1.mSelectClosestEdgeCB)(polyData1, scaling1, world1.rotateTranspose(-worldNormal));
|
|
}
|
|
}
|
|
|
|
const Gu::HullPolygonData& HP0 = polyData0.mPolygons[c0];
|
|
const Gu::HullPolygonData& HP1 = polyData1.mPolygons[c1];
|
|
// PT: prefetching those guys saves ~600.000 cycles in convex-convex benchmark
|
|
Ps::prefetchLine(&HP0.mPlane);
|
|
Ps::prefetchLine(&HP1.mPlane);
|
|
|
|
//ok, we have a new depth value. convert to real distance.
|
|
// PxReal separation = -depth; //depth was either computed in initial cached-axis check, or better, when skipIt was false, in sep axis search.
|
|
// if (separation < 0.0f)
|
|
// separation = 0.0f; //we don't want to store penetration values.
|
|
const PxReal separation = fsel(depth, 0.0f, -depth);
|
|
|
|
PxVec3 worldNormal0;
|
|
PX_ALIGN(16, PxPlane) shapeSpacePlane0;
|
|
if(idtScale0)
|
|
{
|
|
V4StoreA(V4LoadU(&HP0.mPlane.n.x), &shapeSpacePlane0.n.x);
|
|
worldNormal0 = world0.rotate(HP0.mPlane.n);
|
|
}
|
|
else
|
|
{
|
|
scaling0.transformPlaneToShapeSpace(HP0.mPlane.n,HP0.mPlane.d,shapeSpacePlane0.n,shapeSpacePlane0.d);
|
|
worldNormal0 = world0.rotate(shapeSpacePlane0.n);
|
|
}
|
|
|
|
PxVec3 worldNormal1;
|
|
PX_ALIGN(16, PxPlane) shapeSpacePlane1;
|
|
if(idtScale1)
|
|
{
|
|
V4StoreA(V4LoadU(&HP1.mPlane.n.x), &shapeSpacePlane1.n.x);
|
|
worldNormal1 = world1.rotate(HP1.mPlane.n);
|
|
}
|
|
else
|
|
{
|
|
scaling1.transformPlaneToShapeSpace(HP1.mPlane.n,HP1.mPlane.d,shapeSpacePlane1.n,shapeSpacePlane1.d);
|
|
worldNormal1 = world1.rotate(shapeSpacePlane1.n);
|
|
}
|
|
|
|
Ps::IntBool flag;
|
|
{
|
|
const PxReal d0 = PxAbs(worldNormal0.dot(worldNormal));
|
|
const PxReal d1 = PxAbs(worldNormal1.dot(worldNormal));
|
|
bool f = d0>d1; //which face normal is the separating axis closest to.
|
|
flag = f;
|
|
}
|
|
|
|
////////////////////NEW DIST HANDLING//////////////////////
|
|
PX_ASSERT(separation >= 0.0f); //be sure this got fetched somewhere in the chaos above.
|
|
|
|
const PxReal cCCDEpsilon = params.mMeshContactMargin;
|
|
const PxReal contactGenPositionShift = separation + cCCDEpsilon; //if we're at a distance, shift so we're in penetration.
|
|
|
|
const PxVec3 contactGenPositionShiftVec = worldNormal * -contactGenPositionShift; //shift one of the bodies this distance toward the other just for Pierre's contact generation. Then the bodies should be penetrating exactly by MIN_SEPARATION_FOR_PENALTY - ideal conditions for this contact generator.
|
|
|
|
//note: for some reason this has to change sign!
|
|
|
|
//this will make contact gen always generate contacts at about MSP. Shift them back to the true real distance, and then to a solver compliant distance given that
|
|
//the solver converges to MSP penetration, while we want it to converge to 0 penetration.
|
|
//to real distance:
|
|
|
|
//The system: We always shift convex 0 (arbitrary). If the contact is attached to convex 0 then we will need to shift the contact point, otherwise not.
|
|
const PxVec3 newp = world0.p - contactGenPositionShiftVec;
|
|
const Cm::Matrix34 world0_Tweaked(world0.m.column0, world0.m.column1, world0.m.column2, newp);
|
|
PxTransform shifted0(newp, transform0.q);
|
|
|
|
t0to1 = transform1.transformInv(shifted0);
|
|
t1to0 = shifted0.transformInv(transform1);
|
|
Cm::Matrix34 m0to1_Tweaked;
|
|
Cm::Matrix34 m1to0_Tweaked;
|
|
m0to1_Tweaked.set(t0to1.q); m0to1_Tweaked.p = t0to1.p;
|
|
m1to0_Tweaked.set(t1to0.q); m1to0_Tweaked.p = t1to0.p;
|
|
|
|
//////////////////////////////////////////////////
|
|
//pretransform convex polygon if we have scaling!
|
|
PxVec3* scaledVertices0;
|
|
PxU8* stackIndices0;
|
|
GET_SCALEX_CONVEX(scaledVertices0, stackIndices0, idtScale0, HP0.mNbVerts, scaling0, polyData0.mVerts, polyData0.getPolygonVertexRefs(HP0))
|
|
|
|
//pretransform convex polygon if we have scaling!
|
|
PxVec3* scaledVertices1;
|
|
PxU8* stackIndices1;
|
|
GET_SCALEX_CONVEX(scaledVertices1, stackIndices1, idtScale1, HP1.mNbVerts, scaling1, polyData1.mVerts, polyData1.getPolygonVertexRefs(HP1))
|
|
|
|
// So we need to switch:
|
|
// - HP0, HP1
|
|
// - scaledVertices0, scaledVertices1
|
|
// - stackIndices0, stackIndices1
|
|
// - world0, world1
|
|
// - shapeSpacePlane0, shapeSpacePlane1
|
|
// - worldNormal0, worldNormal1
|
|
// - m0to1, m1to0
|
|
// - true, false
|
|
const PxMat33 RotT0 = findRotationMatrixFromZ(shapeSpacePlane0.n);
|
|
const PxMat33 RotT1 = findRotationMatrixFromZ(shapeSpacePlane1.n);
|
|
|
|
if(flag)
|
|
{
|
|
if(contactPolygonPolygonExt(HP0.mNbVerts, scaledVertices0, stackIndices0, world0_Tweaked, shapeSpacePlane0, RotT0,
|
|
HP1.mNbVerts, scaledVertices1, stackIndices1, world1, shapeSpacePlane1, RotT1,
|
|
worldNormal0, m0to1_Tweaked, m1to0_Tweaked,
|
|
PXC_CONTACT_NO_FACE_INDEX, PXC_CONTACT_NO_FACE_INDEX,
|
|
contactBuffer,
|
|
true, contactGenPositionShiftVec, contactGenPositionShift))
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
if(contactPolygonPolygonExt(HP1.mNbVerts, scaledVertices1, stackIndices1, world1, shapeSpacePlane1, RotT1,
|
|
HP0.mNbVerts, scaledVertices0, stackIndices0, world0_Tweaked, shapeSpacePlane0, RotT0,
|
|
worldNormal1, m1to0_Tweaked, m0to1_Tweaked,
|
|
PXC_CONTACT_NO_FACE_INDEX, PXC_CONTACT_NO_FACE_INDEX,
|
|
contactBuffer,
|
|
false, contactGenPositionShiftVec, contactGenPositionShift))
|
|
return true;
|
|
}
|
|
|
|
#ifdef CONVEX_CONVEX_ROUGH_FIRST_PASS
|
|
}
|
|
#endif
|
|
return false;
|
|
}
|