feat(physics): wire physx sdk into build
This commit is contained in:
577
engine/third_party/physx/source/geomutils/src/contact/GuContactCapsuleConvex.cpp
vendored
Normal file
577
engine/third_party/physx/source/geomutils/src/contact/GuContactCapsuleConvex.cpp
vendored
Normal file
@@ -0,0 +1,577 @@
|
||||
// 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 "GuConvexMesh.h"
|
||||
#include "GuConvexHelper.h"
|
||||
#include "GuContactMethodImpl.h"
|
||||
#include "GuVecConvexHull.h"
|
||||
#include "GuVecCapsule.h"
|
||||
#include "GuInternal.h"
|
||||
#include "GuGJK.h"
|
||||
#include "CmMatrix34.h"
|
||||
|
||||
using namespace physx;
|
||||
using namespace Gu;
|
||||
using namespace Cm;
|
||||
|
||||
///////////
|
||||
// #include "PxRenderOutput.h"
|
||||
// #include "PxsContext.h"
|
||||
// static void gVisualizeLine(const PxVec3& a, const PxVec3& b, PxcNpThreadContext& context, PxU32 color=0xffffff)
|
||||
// {
|
||||
// PxMat44 m = PxMat44::identity();
|
||||
//
|
||||
// PxRenderOutput& out = context.mRenderOutput;
|
||||
// out << color << m << RenderOutput::LINES << a << b;
|
||||
// }
|
||||
///////////
|
||||
|
||||
static const PxReal fatConvexEdgeCoeff = 0.01f;
|
||||
|
||||
static bool intersectEdgeEdgePreca(const PxVec3& p1, const PxVec3& p2, const PxVec3& v1, const PxPlane& plane, PxU32 i, PxU32 j, float coeff, const PxVec3& dir, const PxVec3& p3, const PxVec3& p4, PxReal& dist, PxVec3& ip, float limit)
|
||||
{
|
||||
// if colliding edge (p3,p4) does not cross plane return no collision
|
||||
// same as if p3 and p4 on same side of plane return 0
|
||||
//
|
||||
// Derivation:
|
||||
// d3 = d(p3, P) = (p3 | plane.n) - plane.d; Reversed sign compared to Plane::Distance() because plane.d is negated.
|
||||
// d4 = d(p4, P) = (p4 | plane.n) - plane.d; Reversed sign compared to Plane::Distance() because plane.d is negated.
|
||||
// if d3 and d4 have the same sign, they're on the same side of the plane => no collision
|
||||
// We test both sides at the same time by only testing Sign(d3 * d4).
|
||||
// ### put that in the Plane class
|
||||
// ### also check that code in the triangle class that might be similar
|
||||
const PxReal d3 = plane.distance(p3);
|
||||
PxReal temp = d3 * plane.distance(p4);
|
||||
if(temp>0.0f)
|
||||
return false;
|
||||
|
||||
// if colliding edge (p3,p4) and plane are parallel return no collision
|
||||
PxVec3 v2 = p4 - p3;
|
||||
|
||||
temp = plane.n.dot(v2);
|
||||
if(temp==0.0f)
|
||||
return false; // ### epsilon would be better
|
||||
|
||||
// compute intersection point of plane and colliding edge (p3,p4)
|
||||
ip = p3-v2*(d3/temp);
|
||||
|
||||
// compute distance of intersection from line (ip, -dir) to line (p1,p2)
|
||||
dist = (v1[i]*(ip[j]-p1[j])-v1[j]*(ip[i]-p1[i]))*coeff;
|
||||
if(dist<limit)
|
||||
return false;
|
||||
|
||||
// compute intersection point on edge (p1,p2) line
|
||||
ip -= dist*dir;
|
||||
|
||||
// check if intersection point (ip) is between edge (p1,p2) vertices
|
||||
temp = (p1.x-ip.x)*(p2.x-ip.x)+(p1.y-ip.y)*(p2.y-ip.y)+(p1.z-ip.z)*(p2.z-ip.z);
|
||||
if(temp<0.0f)
|
||||
return true; // collision found
|
||||
|
||||
return false; // no collision
|
||||
}
|
||||
|
||||
static bool GuTestAxis(const PxVec3& axis, const Segment& segment, PxReal radius,
|
||||
const PolygonalData& polyData, const FastVertex2ShapeScaling& scaling,
|
||||
const PxMat34& worldTM,
|
||||
PxReal& depth)
|
||||
{
|
||||
// Project capsule
|
||||
PxReal min0 = segment.p0.dot(axis);
|
||||
PxReal max0 = segment.p1.dot(axis);
|
||||
if(min0>max0) PxSwap(min0, max0);
|
||||
min0 -= radius;
|
||||
max0 += radius;
|
||||
|
||||
// Project convex
|
||||
PxReal Min1, Max1;
|
||||
(polyData.mProjectHull)(polyData, axis, worldTM, scaling, Min1, Max1);
|
||||
|
||||
// Test projections
|
||||
if(max0<Min1 || Max1<min0)
|
||||
return false;
|
||||
|
||||
const PxReal d0 = max0 - Min1;
|
||||
PX_ASSERT(d0>=0.0f);
|
||||
const PxReal d1 = Max1 - min0;
|
||||
PX_ASSERT(d1>=0.0f);
|
||||
depth = physx::intrinsics::selectMin(d0, d1);
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool GuCapsuleConvexOverlap(const Segment& segment, PxReal radius,
|
||||
const PolygonalData& polyData,
|
||||
const FastVertex2ShapeScaling& scaling,
|
||||
const PxTransform& transform,
|
||||
PxReal* t, PxVec3* pp, bool isSphere)
|
||||
{
|
||||
// TODO:
|
||||
// - test normal & edge in same loop
|
||||
// - local space
|
||||
// - use precomputed face value
|
||||
// - optimize projection
|
||||
|
||||
PxVec3 Sep(0,0,0);
|
||||
PxReal PenDepth = PX_MAX_REAL;
|
||||
|
||||
PxU32 nbPolys = polyData.mNbPolygons;
|
||||
const HullPolygonData* polys = polyData.mPolygons;
|
||||
|
||||
const Matrix34FromTransform worldTM(transform);
|
||||
|
||||
// Test normals
|
||||
for(PxU32 i=0;i<nbPolys;i++)
|
||||
{
|
||||
const HullPolygonData& poly = polys[i];
|
||||
const PxPlane& vertSpacePlane = poly.mPlane;
|
||||
|
||||
const PxVec3 worldNormal = worldTM.rotate(vertSpacePlane.n);
|
||||
|
||||
PxReal d;
|
||||
if(!GuTestAxis(worldNormal, segment, radius, polyData, scaling, worldTM, d))
|
||||
return false;
|
||||
|
||||
if(d<PenDepth)
|
||||
{
|
||||
PenDepth = d;
|
||||
Sep = worldNormal;
|
||||
}
|
||||
}
|
||||
|
||||
// Test edges
|
||||
if(!isSphere)
|
||||
{
|
||||
PxVec3 CapsuleAxis(segment.p1 - segment.p0);
|
||||
CapsuleAxis = CapsuleAxis.getNormalized();
|
||||
for(PxU32 i=0;i<nbPolys;i++)
|
||||
{
|
||||
const HullPolygonData& poly = polys[i];
|
||||
const PxPlane& vertSpacePlane = poly.mPlane;
|
||||
|
||||
const PxVec3 worldNormal = worldTM.rotate(vertSpacePlane.n);
|
||||
|
||||
PxVec3 Cross = CapsuleAxis.cross(worldNormal);
|
||||
if(!isAlmostZero(Cross))
|
||||
{
|
||||
Cross = Cross.getNormalized();
|
||||
PxReal d;
|
||||
if(!GuTestAxis(Cross, segment, radius, polyData, scaling, worldTM, d))
|
||||
return false;
|
||||
|
||||
if(d<PenDepth)
|
||||
{
|
||||
PenDepth = d;
|
||||
Sep = Cross;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
const PxVec3 Witness = segment.computeCenter() - transform.transform(polyData.mCenter);
|
||||
|
||||
if(Sep.dot(Witness) < 0.0f)
|
||||
Sep = -Sep;
|
||||
|
||||
if(t) *t = PenDepth;
|
||||
if(pp) *pp = Sep;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool raycast_convexMesh2( const PolygonalData& polyData,
|
||||
const PxVec3& vrayOrig, const PxVec3& vrayDir,
|
||||
PxReal maxDist, PxF32& t)
|
||||
{
|
||||
PxU32 nPolys = polyData.mNbPolygons;
|
||||
const HullPolygonData* PX_RESTRICT polys = polyData.mPolygons;
|
||||
|
||||
/*
|
||||
Purely convex planes based algorithm
|
||||
Iterate all planes of convex, with following rules:
|
||||
* determine of ray origin is inside them all or not.
|
||||
* planes parallel to ray direction are immediate early out if we're on the outside side (plane normal is sep axis)
|
||||
* else
|
||||
- for all planes the ray direction "enters" from the front side, track the one furthest along the ray direction (A)
|
||||
- for all planes the ray direction "exits" from the back side, track the one furthest along the negative ray direction (B)
|
||||
if the ray origin is outside the convex and if along the ray, A comes before B, the directed line stabs the convex at A
|
||||
*/
|
||||
PxReal latestEntry = -FLT_MAX;
|
||||
PxReal earliestExit = FLT_MAX;
|
||||
|
||||
while(nPolys--)
|
||||
{
|
||||
const HullPolygonData& poly = *polys++;
|
||||
const PxPlane& vertSpacePlane = poly.mPlane;
|
||||
|
||||
const PxReal distToPlane = vertSpacePlane.distance(vrayOrig);
|
||||
const PxReal dn = vertSpacePlane.n.dot(vrayDir);
|
||||
const PxReal distAlongRay = -distToPlane/dn;
|
||||
|
||||
if (dn > 1E-7f) //the ray direction "exits" from the back side
|
||||
{
|
||||
earliestExit = physx::intrinsics::selectMin(earliestExit, distAlongRay);
|
||||
}
|
||||
else if (dn < -1E-7f) //the ray direction "enters" from the front side
|
||||
{
|
||||
/* if (distAlongRay > latestEntry)
|
||||
{
|
||||
latestEntry = distAlongRay;
|
||||
}*/
|
||||
latestEntry = physx::intrinsics::selectMax(latestEntry, distAlongRay);
|
||||
}
|
||||
else
|
||||
{
|
||||
//plane normal and ray dir are orthogonal
|
||||
if(distToPlane > 0.0f)
|
||||
return false; //a plane is parallel with ray -- and we're outside the ray -- we definitely miss the entire convex!
|
||||
}
|
||||
}
|
||||
|
||||
if(latestEntry < earliestExit && latestEntry != -FLT_MAX && latestEntry < maxDist-1e-5f)
|
||||
{
|
||||
t = latestEntry;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
// PT: version based on Gu::raycast_convexMesh to handle scaling, but modified to make sure it works when ray starts inside the convex
|
||||
static void GuGenerateVFContacts2(PxContactBuffer& contactBuffer,
|
||||
//
|
||||
const PxTransform& convexPose,
|
||||
const PolygonalData& polyData, // Convex data
|
||||
const PxMeshScale& scale,
|
||||
//
|
||||
PxU32 nbPts,
|
||||
const PxVec3* PX_RESTRICT points,
|
||||
PxReal radius, // Capsule's radius
|
||||
//
|
||||
const PxVec3& normal,
|
||||
PxReal contactDistance)
|
||||
{
|
||||
PX_ASSERT(PxAbs(normal.magnitudeSquared()-1)<1e-4f);
|
||||
|
||||
//scaling: transform the ray to vertex space
|
||||
const PxMat34 world2vertexSkew = scale.getInverse() * convexPose.getInverse();
|
||||
|
||||
const PxVec3 vrayDir = world2vertexSkew.rotate( -normal );
|
||||
|
||||
const PxReal maxDist = contactDistance + radius;
|
||||
|
||||
for(PxU32 i=0;i<nbPts;i++)
|
||||
{
|
||||
const PxVec3& rayOrigin = points[i];
|
||||
|
||||
const PxVec3 vrayOrig = world2vertexSkew.transform(rayOrigin);
|
||||
PxF32 t;
|
||||
if(raycast_convexMesh2(polyData, vrayOrig, vrayDir, maxDist, t))
|
||||
{
|
||||
contactBuffer.contact(rayOrigin - t * normal, normal, t - radius);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void GuGenerateEEContacts( PxContactBuffer& contactBuffer,
|
||||
//
|
||||
const Segment& segment,
|
||||
PxReal radius,
|
||||
PxReal contactDistance,
|
||||
//
|
||||
const PolygonalData& polyData,
|
||||
const PxTransform& transform,
|
||||
const FastVertex2ShapeScaling& scaling,
|
||||
//
|
||||
const PxVec3& normal)
|
||||
{
|
||||
PxU32 numPolygons = polyData.mNbPolygons;
|
||||
const HullPolygonData* PX_RESTRICT polygons = polyData.mPolygons;
|
||||
const PxU8* PX_RESTRICT vertexData = polyData.mPolygonVertexRefs;
|
||||
|
||||
ConvexEdge edges[512];
|
||||
PxU32 nbEdges = findUniqueConvexEdges(512, edges, numPolygons, polygons, vertexData);
|
||||
|
||||
//
|
||||
PxVec3 s0 = segment.p0;
|
||||
PxVec3 s1 = segment.p1;
|
||||
makeFatEdge(s0, s1, fatConvexEdgeCoeff);
|
||||
|
||||
// PT: precomputed part of edge-edge intersection test
|
||||
// const PxVec3 v1 = segment.p1 - segment.p0;
|
||||
const PxVec3 v1 = s1 - s0;
|
||||
PxPlane plane;
|
||||
plane.n = v1.cross(normal);
|
||||
// plane.d = -(plane.normal|segment.p0);
|
||||
plane.d = -(plane.n.dot(s0));
|
||||
|
||||
PxU32 ii,jj;
|
||||
closestAxis(plane.n, ii, jj);
|
||||
|
||||
const float coeff = 1.0f /(v1[ii]*normal[jj]-v1[jj]*normal[ii]);
|
||||
|
||||
//
|
||||
|
||||
const PxVec3* PX_RESTRICT verts = polyData.mVerts;
|
||||
for(PxU32 i=0;i<nbEdges;i++)
|
||||
{
|
||||
const PxU8 vi0 = edges[i].vref0;
|
||||
const PxU8 vi1 = edges[i].vref1;
|
||||
|
||||
// PxVec3 p1 = transform.transform(verts[vi0]);
|
||||
// PxVec3 p2 = transform.transform(verts[vi1]);
|
||||
// makeFatEdge(p1, p2, fatConvexEdgeCoeff); // PT: TODO: make fat segment instead
|
||||
const PxVec3 p1 = transform.transform(scaling * verts[vi0]);
|
||||
const PxVec3 p2 = transform.transform(scaling * verts[vi1]);
|
||||
|
||||
PxReal dist;
|
||||
PxVec3 ip;
|
||||
// if(intersectEdgeEdgePreca(segment.p0, segment.p1, v1, plane, ii, jj, coeff, normal, p1, p2, dist, ip))
|
||||
// if(intersectEdgeEdgePreca(s0, s1, v1, plane, ii, jj, coeff, normal, p1, p2, dist, ip, -FLT_MAX))
|
||||
if(intersectEdgeEdgePreca(s0, s1, v1, plane, ii, jj, coeff, normal, p1, p2, dist, ip, -radius-contactDistance))
|
||||
// if(intersectEdgeEdgePreca(s0, s1, v1, plane, ii, jj, coeff, normal, p1, p2, dist, ip, 0))
|
||||
{
|
||||
contactBuffer.contact(ip-normal*dist, normal, - (radius + dist));
|
||||
// if(contactBuffer.count==2) // PT: we only need 2 contacts to be stable
|
||||
// return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void GuGenerateEEContacts2b(PxContactBuffer& contactBuffer,
|
||||
//
|
||||
const Segment& segment,
|
||||
PxReal radius,
|
||||
//
|
||||
const PxMat34& transform,
|
||||
const PolygonalData& polyData,
|
||||
const FastVertex2ShapeScaling& scaling,
|
||||
//
|
||||
const PxVec3& normal,
|
||||
PxReal contactDistance)
|
||||
{
|
||||
// TODO:
|
||||
// - local space
|
||||
|
||||
const PxVec3 localDir = transform.rotateTranspose(normal);
|
||||
PxU32 polyIndex = (polyData.mSelectClosestEdgeCB)(polyData, scaling, localDir);
|
||||
|
||||
PxVec3 s0 = segment.p0;
|
||||
PxVec3 s1 = segment.p1;
|
||||
makeFatEdge(s0, s1, fatConvexEdgeCoeff);
|
||||
|
||||
// PT: precomputed part of edge-edge intersection test
|
||||
// const PxVec3 v1 = segment.p1 - segment.p0;
|
||||
const PxVec3 v1 = s1 - s0;
|
||||
PxPlane plane;
|
||||
plane.n = -(v1.cross(normal));
|
||||
// plane.d = -(plane.normal|segment.p0);
|
||||
plane.d = -(plane.n.dot(s0));
|
||||
|
||||
PxU32 ii,jj;
|
||||
closestAxis(plane.n, ii, jj);
|
||||
|
||||
const float coeff = 1.0f /(v1[jj]*normal[ii]-v1[ii]*normal[jj]);
|
||||
//
|
||||
|
||||
const PxVec3* PX_RESTRICT verts = polyData.mVerts;
|
||||
|
||||
const HullPolygonData& polygon = polyData.mPolygons[polyIndex];
|
||||
const PxU8* PX_RESTRICT vRefBase = polyData.mPolygonVertexRefs + polygon.mVRef8;
|
||||
PxU32 numEdges = polygon.mNbVerts;
|
||||
|
||||
PxU32 a = numEdges - 1;
|
||||
PxU32 b = 0;
|
||||
while(numEdges--)
|
||||
{
|
||||
// const PxVec3 p1 = transform.transform(verts[vRefBase[a]]);
|
||||
// const PxVec3 p2 = transform.transform(verts[vRefBase[b]]);
|
||||
const PxVec3 p1 = transform.transform(scaling * verts[vRefBase[a]]);
|
||||
const PxVec3 p2 = transform.transform(scaling * verts[vRefBase[b]]);
|
||||
|
||||
PxReal dist;
|
||||
PxVec3 ip;
|
||||
// bool contact = intersectEdgeEdgePreca(segment.p0, segment.p1, v1, plane, ii, jj, coeff, -normal, p1, p2, dist, ip);
|
||||
bool contact = intersectEdgeEdgePreca(s0, s1, v1, plane, ii, jj, coeff, -normal, p1, p2, dist, ip, 0.0f);
|
||||
if(contact && dist < radius + contactDistance)
|
||||
{
|
||||
contactBuffer.contact(ip-normal*dist, normal, dist - radius);
|
||||
// if(contactBuffer.count==2) // PT: we only need 2 contacts to be stable
|
||||
// return;
|
||||
}
|
||||
|
||||
a = b;
|
||||
b++;
|
||||
}
|
||||
}
|
||||
|
||||
bool Gu::contactCapsuleConvex(GU_CONTACT_METHOD_ARGS)
|
||||
{
|
||||
PX_UNUSED(renderOutput);
|
||||
PX_UNUSED(cache);
|
||||
|
||||
// Get actual shape data
|
||||
// PT: the capsule can be a sphere in this case so we do this special piece of code:
|
||||
PxCapsuleGeometry shapeCapsule = static_cast<const PxCapsuleGeometry&>(shape0);
|
||||
if(shape0.getType()==PxGeometryType::eSPHERE)
|
||||
shapeCapsule.halfHeight = 0.0f;
|
||||
const PxConvexMeshGeometry& shapeConvex = checkedCast<PxConvexMeshGeometry>(shape1);
|
||||
|
||||
PxVec3 onSegment, onConvex;
|
||||
PxReal distance;
|
||||
PxVec3 normal_;
|
||||
{
|
||||
const ConvexMesh* cm = static_cast<const ConvexMesh*>(shapeConvex.convexMesh);
|
||||
|
||||
using namespace aos;
|
||||
Vec3V closA, closB, normalV;
|
||||
GjkStatus status;
|
||||
FloatV dist;
|
||||
{
|
||||
const Vec3V zeroV = V3Zero();
|
||||
const ConvexHullData* hullData = &cm->getHull();
|
||||
|
||||
const FloatV capsuleHalfHeight = FLoad(shapeCapsule.halfHeight);
|
||||
|
||||
const Vec3V vScale = V3LoadU_SafeReadW(shapeConvex.scale.scale); // PT: safe because 'rotation' follows 'scale' in PxMeshScale
|
||||
const QuatV vQuat = QuatVLoadU(&shapeConvex.scale.rotation.x);
|
||||
|
||||
const PxMatTransformV aToB(transform1.transformInv(transform0));
|
||||
|
||||
const ConvexHullV convexHull(hullData, zeroV, vScale, vQuat, shapeConvex.scale.isIdentity());
|
||||
|
||||
//transform capsule(a) into the local space of convexHull(b), treat capsule as segment
|
||||
const CapsuleV capsule(aToB.p, aToB.rotate(V3Scale(V3UnitX(), capsuleHalfHeight)), FZero());
|
||||
|
||||
const LocalConvex<CapsuleV> convexA(capsule);
|
||||
const LocalConvex<ConvexHullV> convexB(convexHull);
|
||||
const Vec3V initialSearchDir = V3Sub(convexA.getCenter(), convexB.getCenter());
|
||||
|
||||
status = gjk<LocalConvex<CapsuleV>, LocalConvex<ConvexHullV> >(convexA, convexB, initialSearchDir, FMax(),closA, closB, normalV, dist);
|
||||
}
|
||||
|
||||
if(status == GJK_CONTACT)
|
||||
distance = 0.f;
|
||||
else
|
||||
{
|
||||
//const FloatV sqDist = FMul(dist, dist);
|
||||
V3StoreU(closB, onConvex);
|
||||
FStore(dist, &distance);
|
||||
V3StoreU(normalV, normal_);
|
||||
onConvex = transform1.transform(onConvex);
|
||||
normal_ = transform1.rotate(normal_);
|
||||
}
|
||||
}
|
||||
|
||||
const PxReal inflatedRadius = shapeCapsule.radius + params.mContactDistance;
|
||||
|
||||
if(distance >= inflatedRadius)
|
||||
return false;
|
||||
|
||||
Segment worldSegment;
|
||||
getCapsuleSegment(transform0, shapeCapsule, worldSegment);
|
||||
|
||||
const bool isSphere = worldSegment.p0 == worldSegment.p1;
|
||||
const PxU32 nbPts = PxU32(isSphere ? 1 : 2);
|
||||
|
||||
PX_ASSERT(contactBuffer.count==0);
|
||||
|
||||
FastVertex2ShapeScaling convexScaling;
|
||||
const bool idtConvexScale = shapeConvex.scale.isIdentity();
|
||||
if(!idtConvexScale)
|
||||
convexScaling.init(shapeConvex.scale);
|
||||
|
||||
PolygonalData polyData;
|
||||
getPolygonalData_Convex(&polyData, _getHullData(shapeConvex), convexScaling);
|
||||
|
||||
// if(0)
|
||||
if(distance > 0.f)
|
||||
{
|
||||
// PT: the capsule segment doesn't intersect the convex => distance-based version
|
||||
PxVec3 normal = -normal_;
|
||||
|
||||
// PT: generate VF contacts for segment's vertices vs convex
|
||||
GuGenerateVFContacts2( contactBuffer,
|
||||
transform1, polyData, shapeConvex.scale,
|
||||
nbPts, &worldSegment.p0, shapeCapsule.radius,
|
||||
normal, params.mContactDistance);
|
||||
|
||||
// PT: early exit if we already have 2 stable contacts
|
||||
if(contactBuffer.count==2)
|
||||
return true;
|
||||
|
||||
// PT: else generate slower EE contacts
|
||||
if(!isSphere)
|
||||
{
|
||||
const Matrix34FromTransform worldTM(transform1);
|
||||
GuGenerateEEContacts2b(contactBuffer, worldSegment, shapeCapsule.radius,
|
||||
worldTM, polyData, convexScaling,
|
||||
normal, params.mContactDistance);
|
||||
}
|
||||
|
||||
// PT: run VF case for convex-vertex-vs-capsule only if we don't have any contact yet
|
||||
if(!contactBuffer.count)
|
||||
{
|
||||
// gVisualizeLine(onConvex, onConvex + normal, context, PxDebugColor::eARGB_RED);
|
||||
//PxReal distance = PxSqrt(sqDistance);
|
||||
contactBuffer.contact(onConvex, normal, distance - shapeCapsule.radius);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// PT: the capsule segment intersects the convex => penetration-based version
|
||||
//printf("Penetration-based:\n");
|
||||
|
||||
// PT: compute penetration vector (MTD)
|
||||
PxVec3 SepAxis;
|
||||
if(!GuCapsuleConvexOverlap(worldSegment, shapeCapsule.radius, polyData, convexScaling, transform1, NULL, &SepAxis, isSphere))
|
||||
{
|
||||
//printf("- no overlap\n");
|
||||
return false;
|
||||
}
|
||||
|
||||
// PT: generate VF contacts for segment's vertices vs convex
|
||||
GuGenerateVFContacts2( contactBuffer,
|
||||
transform1, polyData, shapeConvex.scale,
|
||||
nbPts, &worldSegment.p0, shapeCapsule.radius,
|
||||
SepAxis, params.mContactDistance);
|
||||
|
||||
// PT: early exit if we already have 2 stable contacts
|
||||
//printf("- %d VF contacts\n", contactBuffer.count);
|
||||
if(contactBuffer.count==2)
|
||||
return true;
|
||||
|
||||
// PT: else generate slower EE contacts
|
||||
if(!isSphere)
|
||||
{
|
||||
GuGenerateEEContacts(contactBuffer, worldSegment, shapeCapsule.radius, params.mContactDistance, polyData, transform1, convexScaling, SepAxis);
|
||||
//printf("- %d total contacts\n", contactBuffer.count);
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
Reference in New Issue
Block a user