feat(physics): wire physx sdk into build
This commit is contained in:
538
engine/third_party/physx/source/geomutils/src/gjk/GuVecConvexHull.h
vendored
Normal file
538
engine/third_party/physx/source/geomutils/src/gjk/GuVecConvexHull.h
vendored
Normal file
@@ -0,0 +1,538 @@
|
||||
// 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_VEC_CONVEXHULL_H
|
||||
#define GU_VEC_CONVEXHULL_H
|
||||
|
||||
#include "common/PxPhysXCommonConfig.h"
|
||||
#include "geometry/PxMeshScale.h"
|
||||
#include "GuConvexMesh.h"
|
||||
#include "GuVecConvex.h"
|
||||
#include "GuConvexMeshData.h"
|
||||
#include "GuBigConvexData.h"
|
||||
#include "GuConvexSupportTable.h"
|
||||
#include "GuCubeIndex.h"
|
||||
#include "foundation/PxFPU.h"
|
||||
#include "foundation/PxVecQuat.h"
|
||||
#include "GuShapeConvex.h"
|
||||
|
||||
namespace physx
|
||||
{
|
||||
namespace Gu
|
||||
{
|
||||
#define CONVEX_MARGIN_RATIO 0.1f
|
||||
#define CONVEX_MIN_MARGIN_RATIO 0.05f
|
||||
#define CONVEX_SWEEP_MARGIN_RATIO 0.025f
|
||||
#define TOLERANCE_MARGIN_RATIO 0.08f
|
||||
#define TOLERANCE_MIN_MARGIN_RATIO 0.05f
|
||||
|
||||
//This margin is used in Persistent contact manifold
|
||||
PX_SUPPORT_FORCE_INLINE aos::FloatV CalculatePCMConvexMargin(const Gu::ConvexHullData* hullData, const aos::Vec3VArg scale,
|
||||
const PxReal toleranceLength, const PxReal toleranceRatio = TOLERANCE_MIN_MARGIN_RATIO)
|
||||
{
|
||||
|
||||
using namespace aos;
|
||||
const Vec3V extents = V3Mul(V3LoadU_SafeReadW(hullData->mInternal.mInternalExtents), scale);
|
||||
const FloatV min = V3ExtractMin(extents);
|
||||
const FloatV toleranceMargin = FLoad(toleranceLength * toleranceRatio);
|
||||
//ML: 25% of the minimum extents of the internal AABB as this convex hull's margin
|
||||
return FMin(FMul(min, FLoad(0.25f)), toleranceMargin);
|
||||
}
|
||||
|
||||
PX_SUPPORT_FORCE_INLINE aos::FloatV CalculateMTDConvexMargin(const Gu::ConvexHullData* hullData, const aos::Vec3VArg scale)
|
||||
{
|
||||
using namespace aos;
|
||||
const Vec3V extents = V3Mul(V3LoadU_SafeReadW(hullData->mInternal.mInternalExtents), scale);
|
||||
const FloatV min = V3ExtractMin(extents);
|
||||
//ML: 25% of the minimum extents of the internal AABB as this convex hull's margin
|
||||
return FMul(min, FLoad(0.25f));
|
||||
}
|
||||
|
||||
//This minMargin is used in PCM contact gen
|
||||
PX_SUPPORT_FORCE_INLINE void CalculateConvexMargin(const InternalObjectsData& internalObject, PxReal& margin, PxReal& minMargin, PxReal& sweepMargin,
|
||||
const aos::Vec3VArg scale)
|
||||
{
|
||||
using namespace aos;
|
||||
|
||||
const Vec3V extents = V3Mul(V3LoadU_SafeReadW(internalObject.mInternalExtents), scale);
|
||||
const FloatV min_ = V3ExtractMin(extents);
|
||||
|
||||
PxReal minExtent;
|
||||
FStore(min_, &minExtent);
|
||||
|
||||
//margin is used as acceptanceTolerance for overlap.
|
||||
margin = minExtent * CONVEX_MARGIN_RATIO;
|
||||
//minMargin is used in the GJK termination condition
|
||||
minMargin = minExtent * CONVEX_MIN_MARGIN_RATIO;
|
||||
//this is used for sweep(gjkRaycast)
|
||||
sweepMargin = minExtent * CONVEX_SWEEP_MARGIN_RATIO;
|
||||
}
|
||||
|
||||
|
||||
PX_SUPPORT_FORCE_INLINE aos::Mat33V ConstructSkewMatrix(const aos::Vec3VArg scale, const aos::QuatVArg rotation)
|
||||
{
|
||||
using namespace aos;
|
||||
Mat33V rot;
|
||||
QuatGetMat33V(rotation, rot.col0, rot.col1, rot.col2);
|
||||
Mat33V trans = M33Trnsps(rot);
|
||||
trans.col0 = V3Scale(trans.col0, V3GetX(scale));
|
||||
trans.col1 = V3Scale(trans.col1, V3GetY(scale));
|
||||
trans.col2 = V3Scale(trans.col2, V3GetZ(scale));
|
||||
return M33MulM33(trans, rot);
|
||||
}
|
||||
|
||||
PX_SUPPORT_FORCE_INLINE void ConstructSkewMatrix(const aos::Vec3VArg scale, const aos::QuatVArg rotation, aos::Mat33V& vertex2Shape, aos::Mat33V& shape2Vertex, aos::Vec3V& center, const bool idtScale)
|
||||
{
|
||||
using namespace aos;
|
||||
|
||||
PX_ASSERT(!V3AllEq(scale, V3Zero()));
|
||||
|
||||
if(idtScale)
|
||||
{
|
||||
//create identity buffer
|
||||
const Mat33V identity = M33Identity();
|
||||
vertex2Shape = identity;
|
||||
shape2Vertex = identity;
|
||||
}
|
||||
else
|
||||
{
|
||||
const FloatV scaleX = V3GetX(scale);
|
||||
const Vec3V invScale = V3Recip(scale);
|
||||
|
||||
//this is uniform scale
|
||||
if(V3AllEq(V3Splat(scaleX), scale))
|
||||
{
|
||||
vertex2Shape = M33Diagonal(scale);
|
||||
shape2Vertex = M33Diagonal(invScale);
|
||||
}
|
||||
else
|
||||
{
|
||||
Mat33V rot;
|
||||
QuatGetMat33V(rotation, rot.col0, rot.col1, rot.col2);
|
||||
const Mat33V trans = M33Trnsps(rot);
|
||||
/*
|
||||
vertex2shape
|
||||
skewMat = Inv(R)*Diagonal(scale)*R;
|
||||
*/
|
||||
|
||||
const Mat33V temp(V3Scale(trans.col0, scaleX), V3Scale(trans.col1, V3GetY(scale)), V3Scale(trans.col2, V3GetZ(scale)));
|
||||
vertex2Shape = M33MulM33(temp, rot);
|
||||
|
||||
//don't need it in the support function
|
||||
/*
|
||||
shape2Vertex
|
||||
invSkewMat =(invSkewMat)= Inv(R)*Diagonal(1/scale)*R;
|
||||
*/
|
||||
|
||||
shape2Vertex.col0 = V3Scale(trans.col0, V3GetX(invScale));
|
||||
shape2Vertex.col1 = V3Scale(trans.col1, V3GetY(invScale));
|
||||
shape2Vertex.col2 = V3Scale(trans.col2, V3GetZ(invScale));
|
||||
shape2Vertex = M33MulM33(shape2Vertex, rot);
|
||||
|
||||
//shape2Vertex = M33Inverse(vertex2Shape);
|
||||
}
|
||||
|
||||
//transform center to shape space
|
||||
center = M33MulV3(vertex2Shape, center);
|
||||
}
|
||||
}
|
||||
|
||||
PX_SUPPORT_FORCE_INLINE aos::Mat33V ConstructVertex2ShapeMatrix(const aos::Vec3VArg scale, const aos::QuatVArg rotation)
|
||||
{
|
||||
using namespace aos;
|
||||
Mat33V rot;
|
||||
QuatGetMat33V(rotation, rot.col0, rot.col1, rot.col2);
|
||||
const Mat33V trans = M33Trnsps(rot);
|
||||
/*
|
||||
vertex2shape
|
||||
skewMat = Inv(R)*Diagonal(scale)*R;
|
||||
*/
|
||||
|
||||
const Mat33V temp(V3Scale(trans.col0, V3GetX(scale)), V3Scale(trans.col1, V3GetY(scale)), V3Scale(trans.col2, V3GetZ(scale)));
|
||||
return M33MulM33(temp, rot);
|
||||
}
|
||||
|
||||
|
||||
class ConvexHullV : public ConvexV
|
||||
{
|
||||
|
||||
class TinyBitMap
|
||||
{
|
||||
public:
|
||||
PxU32 m[8];
|
||||
PX_FORCE_INLINE TinyBitMap() { m[0] = m[1] = m[2] = m[3] = m[4] = m[5] = m[6] = m[7] = 0; }
|
||||
PX_FORCE_INLINE void set(PxU8 v) { m[v >> 5] |= 1 << (v & 31); }
|
||||
PX_FORCE_INLINE bool get(PxU8 v) const { return (m[v >> 5] & 1 << (v & 31)) != 0; }
|
||||
};
|
||||
|
||||
|
||||
public:
|
||||
/**
|
||||
\brief Constructor
|
||||
*/
|
||||
PX_SUPPORT_INLINE ConvexHullV() : ConvexV(ConvexType::eCONVEXHULL)
|
||||
{
|
||||
}
|
||||
|
||||
PX_SUPPORT_INLINE ConvexHullV(const Gu::ConvexHullData* _hullData, const aos::Vec3VArg _center, const aos::Vec3VArg scale, const aos::QuatVArg scaleRot,
|
||||
const bool idtScale) :
|
||||
ConvexV(ConvexType::eCONVEXHULL, _center)
|
||||
{
|
||||
using namespace aos;
|
||||
|
||||
hullData = _hullData;
|
||||
const PxVec3* PX_RESTRICT tempVerts = _hullData->getHullVertices();
|
||||
verts = tempVerts;
|
||||
numVerts = _hullData->mNbHullVertices;
|
||||
CalculateConvexMargin(_hullData->mInternal, margin, minMargin, sweepMargin, scale);
|
||||
ConstructSkewMatrix(scale, scaleRot, vertex2Shape, shape2Vertex, center, idtScale);
|
||||
data = _hullData->mBigConvexRawData;
|
||||
}
|
||||
|
||||
PX_SUPPORT_INLINE ConvexHullV(const Gu::ConvexHullData* _hullData, const aos::Vec3VArg _center) :
|
||||
ConvexV(ConvexType::eCONVEXHULL, _center)
|
||||
{
|
||||
using namespace aos;
|
||||
|
||||
hullData = _hullData;
|
||||
verts = _hullData->getHullVertices();
|
||||
numVerts = _hullData->mNbHullVertices;
|
||||
data = _hullData->mBigConvexRawData;
|
||||
}
|
||||
|
||||
//this is used by CCD system
|
||||
PX_SUPPORT_INLINE ConvexHullV(const PxGeometry& geom) : ConvexV(ConvexType::eCONVEXHULL, aos::V3Zero())
|
||||
{
|
||||
using namespace aos;
|
||||
const PxConvexMeshGeometry& convexGeom = static_cast<const PxConvexMeshGeometry&>(geom);
|
||||
const Gu::ConvexHullData* hData = _getHullData(convexGeom);
|
||||
|
||||
const Vec3V vScale = V3LoadU_SafeReadW(convexGeom.scale.scale); // PT: safe because 'rotation' follows 'scale' in PxMeshScale
|
||||
const QuatV vRot = QuatVLoadU(&convexGeom.scale.rotation.x);
|
||||
const bool idtScale = convexGeom.scale.isIdentity();
|
||||
|
||||
hullData = hData;
|
||||
const PxVec3* PX_RESTRICT tempVerts = hData->getHullVertices();
|
||||
verts = tempVerts;
|
||||
numVerts = hData->mNbHullVertices;
|
||||
CalculateConvexMargin(hData->mInternal, margin, minMargin, sweepMargin, vScale);
|
||||
ConstructSkewMatrix(vScale, vRot, vertex2Shape, shape2Vertex, center, idtScale);
|
||||
|
||||
data = hData->mBigConvexRawData;
|
||||
}
|
||||
|
||||
//this is used by convex vs tetrahedron collision
|
||||
PX_SUPPORT_INLINE ConvexHullV(const Gu::PolygonalData& polyData, const Cm::FastVertex2ShapeScaling& convexScale) :
|
||||
ConvexV(ConvexType::eCONVEXHULL, aos::V3LoadU(polyData.mCenter))
|
||||
{
|
||||
using namespace aos;
|
||||
|
||||
const Vec3V vScale = V3LoadU(polyData.mScale.scale);
|
||||
|
||||
verts = polyData.mVerts;
|
||||
numVerts = PxU8(polyData.mNbVerts);
|
||||
CalculateConvexMargin(polyData.mInternal, margin, minMargin, sweepMargin, vScale);
|
||||
|
||||
const PxMat33& v2s = convexScale.getVertex2ShapeSkew();
|
||||
const PxMat33& s2v = convexScale.getShape2VertexSkew();
|
||||
|
||||
vertex2Shape.col0 = V3LoadU(v2s.column0);
|
||||
vertex2Shape.col1 = V3LoadU(v2s.column1);
|
||||
vertex2Shape.col2 = V3LoadU(v2s.column2);
|
||||
|
||||
shape2Vertex.col0 = V3LoadU(s2v.column0);
|
||||
shape2Vertex.col1 = V3LoadU(s2v.column1);
|
||||
shape2Vertex.col2 = V3LoadU(s2v.column2);
|
||||
|
||||
data = polyData.mBigData;
|
||||
|
||||
}
|
||||
|
||||
PX_SUPPORT_INLINE void initialize(const Gu::ConvexHullData* _hullData, const aos::Vec3VArg _center, const aos::Vec3VArg scale,
|
||||
const aos::QuatVArg scaleRot, const bool idtScale)
|
||||
{
|
||||
using namespace aos;
|
||||
|
||||
const PxVec3* tempVerts = _hullData->getHullVertices();
|
||||
CalculateConvexMargin(_hullData->mInternal, margin, minMargin, sweepMargin, scale);
|
||||
ConstructSkewMatrix(scale, scaleRot, vertex2Shape, shape2Vertex, center, idtScale);
|
||||
|
||||
verts = tempVerts;
|
||||
numVerts = _hullData->mNbHullVertices;
|
||||
//rot = _rot;
|
||||
|
||||
center = _center;
|
||||
|
||||
// searchIndex = 0;
|
||||
data = _hullData->mBigConvexRawData;
|
||||
|
||||
hullData = _hullData;
|
||||
if (_hullData->mBigConvexRawData)
|
||||
{
|
||||
PxPrefetchLine(hullData->mBigConvexRawData->mValencies);
|
||||
PxPrefetchLine(hullData->mBigConvexRawData->mValencies, 128);
|
||||
PxPrefetchLine(hullData->mBigConvexRawData->mAdjacentVerts);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
PX_FORCE_INLINE void resetMargin(const PxReal toleranceLength)
|
||||
{
|
||||
const PxReal toleranceMinMargin = toleranceLength * TOLERANCE_MIN_MARGIN_RATIO;
|
||||
const PxReal toleranceMargin = toleranceLength * TOLERANCE_MARGIN_RATIO;
|
||||
|
||||
margin = PxMin(margin, toleranceMargin);
|
||||
minMargin = PxMin(minMargin, toleranceMinMargin);
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE aos::Vec3V supportPoint(const PxI32 index)const
|
||||
{
|
||||
using namespace aos;
|
||||
|
||||
return M33MulV3(vertex2Shape, V3LoadU_SafeReadW(verts[index])); // PT: safe because of the way vertex memory is allocated in ConvexHullData (and 'verts' is initialized with ConvexHullData::getHullVertices())
|
||||
}
|
||||
|
||||
PX_NOINLINE PxU32 hillClimbing(const aos::Vec3VArg _dir)const
|
||||
{
|
||||
using namespace aos;
|
||||
|
||||
const Gu::Valency* valency = data->mValencies;
|
||||
const PxU8* adjacentVerts = data->mAdjacentVerts;
|
||||
|
||||
//NotSoTinyBitMap visited;
|
||||
PxU32 smallBitMap[8] = {0,0,0,0,0,0,0,0};
|
||||
|
||||
// PxU32 index = searchIndex;
|
||||
PxU32 index = 0;
|
||||
|
||||
{
|
||||
PxVec3 vertexSpaceDirection;
|
||||
V3StoreU(_dir, vertexSpaceDirection);
|
||||
const PxU32 offset = ComputeCubemapNearestOffset(vertexSpaceDirection, data->mSubdiv);
|
||||
//const PxU32 offset = ComputeCubemapOffset(vertexSpaceDirection, data->mSubdiv);
|
||||
index = data->mSamples[offset];
|
||||
}
|
||||
|
||||
Vec3V maxPoint = V3LoadU_SafeReadW(verts[index]); // PT: safe because of the way vertex memory is allocated in ConvexHullData (and 'verts' is initialized with ConvexHullData::getHullVertices())
|
||||
FloatV max = V3Dot(maxPoint, _dir);
|
||||
|
||||
PxU32 initialIndex = index;
|
||||
|
||||
do
|
||||
{
|
||||
initialIndex = index;
|
||||
const PxU32 numNeighbours = valency[index].mCount;
|
||||
const PxU32 offset = valency[index].mOffset;
|
||||
|
||||
for(PxU32 a = 0; a < numNeighbours; ++a)
|
||||
{
|
||||
const PxU32 neighbourIndex = adjacentVerts[offset + a];
|
||||
|
||||
const Vec3V vertex = V3LoadU_SafeReadW(verts[neighbourIndex]); // PT: safe because of the way vertex memory is allocated in ConvexHullData (and 'verts' is initialized with ConvexHullData::getHullVertices())
|
||||
const FloatV dist = V3Dot(vertex, _dir);
|
||||
if(FAllGrtr(dist, max))
|
||||
{
|
||||
const PxU32 ind = neighbourIndex>>5;
|
||||
const PxU32 mask = PxU32(1 << (neighbourIndex & 31));
|
||||
if((smallBitMap[ind] & mask) == 0)
|
||||
{
|
||||
smallBitMap[ind] |= mask;
|
||||
max = dist;
|
||||
index = neighbourIndex;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}while(index != initialIndex);
|
||||
|
||||
return index;
|
||||
}
|
||||
|
||||
PX_SUPPORT_INLINE PxU32 bruteForceSearch(const aos::Vec3VArg _dir)const
|
||||
{
|
||||
using namespace aos;
|
||||
//brute force
|
||||
PxVec3 dir;
|
||||
V3StoreU(_dir, dir);
|
||||
|
||||
PxReal max = verts[0].dot(dir);
|
||||
PxU32 maxIndex = 0;
|
||||
|
||||
for (PxU32 i = 1; i < numVerts; ++i)
|
||||
{
|
||||
const PxReal dist = verts[i].dot(dir);
|
||||
if (dist > max)
|
||||
{
|
||||
max = dist;
|
||||
maxIndex = i;
|
||||
}
|
||||
}
|
||||
return maxIndex;
|
||||
}
|
||||
|
||||
//points are in vertex space, _dir in vertex space
|
||||
PX_NOINLINE PxU32 supportVertexIndex(const aos::Vec3VArg _dir)const
|
||||
{
|
||||
using namespace aos;
|
||||
if(data)
|
||||
return hillClimbing(_dir);
|
||||
else
|
||||
return bruteForceSearch(_dir);
|
||||
}
|
||||
|
||||
//dir is in the vertex space
|
||||
PX_SUPPORT_INLINE void bruteForceSearchMinMax(const aos::Vec3VArg _dir, aos::FloatV& min, aos::FloatV& max)const
|
||||
{
|
||||
using namespace aos;
|
||||
|
||||
//brute force
|
||||
PxVec3 dir;
|
||||
V3StoreU(_dir, dir);
|
||||
//get the support point from the orignal margin
|
||||
PxReal _max = verts[0].dot(dir);
|
||||
PxReal _min = _max;
|
||||
|
||||
for(PxU32 i = 1; i < numVerts; ++i)
|
||||
{
|
||||
const PxReal dist = verts[i].dot(dir);
|
||||
_max = PxMax(dist, _max);
|
||||
_min = PxMin(dist, _min);
|
||||
}
|
||||
min = FLoad(_min);
|
||||
max = FLoad(_max);
|
||||
}
|
||||
|
||||
//This function is used in the full contact manifold generation code, points are in vertex space.
|
||||
//This function support scaling, _dir is in the shape space
|
||||
PX_SUPPORT_INLINE void supportVertexMinMax(const aos::Vec3VArg _dir, aos::FloatV& min, aos::FloatV& max)const
|
||||
{
|
||||
using namespace aos;
|
||||
|
||||
//dir is in the vertex space
|
||||
const Vec3V dir = M33TrnspsMulV3(vertex2Shape, _dir);
|
||||
|
||||
if(data)
|
||||
{
|
||||
const PxU32 maxIndex = hillClimbing(dir);
|
||||
const PxU32 minIndex = hillClimbing(V3Neg(dir));
|
||||
const Vec3V maxPoint = M33MulV3(vertex2Shape, V3LoadU_SafeReadW(verts[maxIndex])); // PT: safe because of the way vertex memory is allocated in ConvexHullData (and 'verts' is initialized with ConvexHullData::getHullVertices())
|
||||
const Vec3V minPoint = M33MulV3(vertex2Shape, V3LoadU_SafeReadW(verts[minIndex])); // PT: safe because of the way vertex memory is allocated in ConvexHullData (and 'verts' is initialized with ConvexHullData::getHullVertices())
|
||||
min = V3Dot(_dir, minPoint);
|
||||
max = V3Dot(_dir, maxPoint);
|
||||
}
|
||||
else
|
||||
{
|
||||
//dir is in the vertex space
|
||||
bruteForceSearchMinMax(dir, min, max);
|
||||
}
|
||||
}
|
||||
|
||||
//This function is used in the full contact manifold generation code
|
||||
PX_SUPPORT_INLINE void populateVerts(const PxU8* inds, PxU32 numInds, const PxVec3* originalVerts, aos::Vec3V* _verts)const
|
||||
{
|
||||
using namespace aos;
|
||||
|
||||
for(PxU32 i=0; i<numInds; ++i)
|
||||
_verts[i] = M33MulV3(vertex2Shape, V3LoadU_SafeReadW(originalVerts[inds[i]])); // PT: safe because of the way vertex memory is allocated in ConvexHullData (and 'populateVerts' is always called with polyData.mVerts)
|
||||
}
|
||||
|
||||
//This function is used in epa
|
||||
//dir is in the shape space
|
||||
PX_SUPPORT_INLINE aos::Vec3V supportLocal(const aos::Vec3VArg dir)const
|
||||
{
|
||||
using namespace aos;
|
||||
//scale dir and put it in the vertex space
|
||||
const Vec3V _dir = M33TrnspsMulV3(vertex2Shape, dir);
|
||||
const PxU32 maxIndex = supportVertexIndex(_dir);
|
||||
return M33MulV3(vertex2Shape, V3LoadU_SafeReadW(verts[maxIndex])); // PT: safe because of the way vertex memory is allocated in ConvexHullData (and 'verts' is initialized with ConvexHullData::getHullVertices())
|
||||
}
|
||||
|
||||
//this is used in the sat test for the full contact gen
|
||||
PX_SUPPORT_INLINE void supportLocal(const aos::Vec3VArg dir, aos::FloatV& min, aos::FloatV& max)const
|
||||
{
|
||||
using namespace aos;
|
||||
//dir is in the shape space
|
||||
supportVertexMinMax(dir, min, max);
|
||||
}
|
||||
|
||||
//This function is used in epa
|
||||
PX_SUPPORT_INLINE aos::Vec3V supportRelative(const aos::Vec3VArg dir, const aos::PxMatTransformV& aTob, const aos::PxMatTransformV& aTobT) const
|
||||
{
|
||||
using namespace aos;
|
||||
|
||||
//transform dir into the shape space
|
||||
// const Vec3V dir_ = aTob.rotateInv(dir);//relTra.rotateInv(dir);
|
||||
const Vec3V dir_ = aTobT.rotate(dir);//relTra.rotateInv(dir);
|
||||
const Vec3V maxPoint = supportLocal(dir_);
|
||||
//translate maxPoint from shape space of a back to the b space
|
||||
return aTob.transform(maxPoint);//relTra.transform(maxPoint);
|
||||
}
|
||||
|
||||
//dir in the shape space, this function is used in gjk
|
||||
PX_SUPPORT_INLINE aos::Vec3V supportLocal(const aos::Vec3VArg dir, PxI32& index)const
|
||||
{
|
||||
using namespace aos;
|
||||
//scale dir and put it in the vertex space, for non-uniform scale, we don't want the scale in the dir, therefore, we are using
|
||||
//the transpose of the inverse of shape2Vertex(which is vertex2shape). This will allow us igore the scale and keep the rotation
|
||||
const Vec3V dir_ = M33TrnspsMulV3(vertex2Shape, dir);
|
||||
//get the extreme point index
|
||||
const PxU32 maxIndex = supportVertexIndex(dir_);
|
||||
index = PxI32(maxIndex);
|
||||
//p is in the shape space
|
||||
return M33MulV3(vertex2Shape, V3LoadU_SafeReadW(verts[index])); // PT: safe because of the way vertex memory is allocated in ConvexHullData (and 'verts' is initialized with ConvexHullData::getHullVertices())
|
||||
}
|
||||
|
||||
//this function is used in gjk
|
||||
PX_SUPPORT_INLINE aos::Vec3V supportRelative( const aos::Vec3VArg dir, const aos::PxMatTransformV& aTob,
|
||||
const aos::PxMatTransformV& aTobT, PxI32& index)const
|
||||
{
|
||||
using namespace aos;
|
||||
|
||||
//transform dir from b space to the shape space of a space
|
||||
// const Vec3V dir_ = aTob.rotateInv(dir);//relTra.rotateInv(dir);//M33MulV3(skewInvRot, dir);
|
||||
const Vec3V dir_ = aTobT.rotate(dir);//relTra.rotateInv(dir);//M33MulV3(skewInvRot, dir);
|
||||
const Vec3V p = supportLocal(dir_, index);
|
||||
//transfrom from a to b space
|
||||
return aTob.transform(p);
|
||||
}
|
||||
|
||||
aos::Mat33V vertex2Shape;//inv(R)*S*R
|
||||
aos::Mat33V shape2Vertex;//inv(vertex2Shape)
|
||||
|
||||
const Gu::ConvexHullData* hullData;
|
||||
const BigConvexRawData* data;
|
||||
const PxVec3* verts;
|
||||
PxU8 numVerts;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif //
|
||||
Reference in New Issue
Block a user