bool pcmContactCapsuleConvex(GU_CONTACT_METHOD_ARGS)
{
PX_UNUSED(renderOutput);
const PxConvexMeshGeometryLL& shapeConvex = shape1.get<const PxConvexMeshGeometryLL>();
const PxCapsuleGeometry& shapeCapsule = shape0.get<const PxCapsuleGeometry>();
PersistentContactManifold& manifold = cache.getManifold();
Ps::prefetchLine(shapeConvex.hullData);
PX_ASSERT(transform1.q.isSane());
PX_ASSERT(transform0.q.isSane());
const Vec3V zeroV = V3Zero();
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);
const ConvexHullData* hullData =shapeConvex.hullData;
//Transfer A into the local space of B
const PsTransformV transf0 = loadTransformA(transform0);
const PsTransformV transf1 = loadTransformA(transform1);
const PsTransformV curRTrans(transf1.transformInv(transf0));
const PsMatTransformV aToB(curRTrans);
const FloatV convexMargin = Gu::CalculatePCMConvexMargin(hullData, vScale);
const FloatV capsuleMinMargin = Gu::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;
Vec3V closestA(zeroV), closestB(zeroV), normal(zeroV); // from a to b
const FloatV zero = FZero();
FloatV penDep = zero;
PX_UNUSED(bLostContacts);
if(bLostContacts || manifold.invalidate_SphereCapsule(curRTrans, minMargin))
{
const bool idtScale = shapeConvex.scale.isIdentity();
manifold.setRelativeTransform(curRTrans);
const QuatV vQuat = QuatVLoadU(&shapeConvex.scale.rotation.x);
ConvexHullV convexHull(hullData, zeroV, vScale, vQuat, idtScale);
convexHull.setMargin(zero);
//transform capsule(a) into the local space of convexHull(b)
CapsuleV capsule(aToB.p, aToB.rotate(V3Scale(V3UnitX(), capsuleHalfHeight)), capsuleRadius);
LocalConvex<CapsuleV> convexA(capsule);
const Vec3V initialSearchDir = V3Sub(capsule.getCenter(), convexHull.getCenter());
if(idtScale)
{
LocalConvex<ConvexHullNoScaleV> convexB(*PX_CONVEX_TO_NOSCALECONVEX(&convexHull));
status = gjkPenetration<LocalConvex<CapsuleV>, LocalConvex<ConvexHullNoScaleV> >(convexA, convexB, initialSearchDir, contactDist, closestA, closestB, normal, penDep,
manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, true);
}
else
{
LocalConvex<ConvexHullV> convexB(convexHull);
status = gjkPenetration<LocalConvex<CapsuleV>, LocalConvex<ConvexHullV> >(convexA, convexB, initialSearchDir, contactDist, closestA, closestB, normal, penDep,
manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, true);
}
Gu::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, normal,
closestB, convexHull.getMargin(), contactDist, true, renderOutput, FLoad(params.mToleranceLength));
}
else
{
const FloatV replaceBreakingThreshold = FMul(minMargin, FLoad(0.05f));
if(status == GJK_CONTACT)
{
const Vec3V localPointA = aToB.transformInv(closestA);//curRTrans.transformInv(closestA);
const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(normal), penDep);
//Add contact to contact stream
manifoldContacts[0].mLocalPointA = localPointA;
manifoldContacts[0].mLocalPointB = closestB;
manifoldContacts[0].mLocalNormalPen = localNormalPen;
//Add contact to manifold
manifold.addManifoldPoint2(localPointA, closestB, localNormalPen, replaceBreakingThreshold);
}
else
{
PX_ASSERT(status == EPA_CONTACT);
if(idtScale)
{
LocalConvex<ConvexHullNoScaleV> convexB(*PX_CONVEX_TO_NOSCALECONVEX(&convexHull));
status= Gu::epaPenetration(convexA, convexB, manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints,
closestA, closestB, normal, penDep, true);
}
else
{
LocalConvex<ConvexHullV> convexB(convexHull);
status= Gu::epaPenetration(convexA, convexB, manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints,
closestA, closestB, normal, penDep, true);
}
if(status == EPA_CONTACT)
{
const Vec3V localPointA = aToB.transformInv(closestA);//curRTrans.transformInv(closestA);
const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(normal), penDep);
//Add contact to contact stream
manifoldContacts[0].mLocalPointA = localPointA;
manifoldContacts[0].mLocalPointB = closestB;
manifoldContacts[0].mLocalNormalPen = localNormalPen;
//Add contact to manifold
manifold.addManifoldPoint2(localPointA, closestB, localNormalPen, replaceBreakingThreshold);
}
else
{
doOverlapTest = true;
}
}
if(initialContacts == 0 || bLostContacts || doOverlapTest)
{
return fullContactsGenerationCapsuleConvex(capsule, convexHull, aToB, transf0, transf1, manifoldContacts, contactBuffer, idtScale, manifold, normal,
closestB, convexHull.getMargin(), contactDist, doOverlapTest, renderOutput, FLoad(params.mToleranceLength));
}
else
{
//This contact is either come from GJK or EPA
normal = transf1.rotate(normal);
manifold.addManifoldContactsToContactBuffer(contactBuffer, normal, transf0, capsuleRadius, contactDist);
#if PCM_LOW_LEVEL_DEBUG
manifold.drawManifold(*renderOutput, transf0, transf1);
#endif
return true;
}
}
}
else if (manifold.getNumContacts() > 0)
{
normal = manifold.getWorldNormal(transf1);
manifold.addManifoldContactsToContactBuffer(contactBuffer, normal, transf0, capsuleRadius, contactDist);
#if PCM_LOW_LEVEL_DEBUG
manifold.drawManifold(*renderOutput, transf0, transf1);
#endif
return true;
}
return false;
}
void CalcNormalBlob(Object *obj, Vec3 *Q, Vec3 *N)
{
BlobData *b;
Bloblet *be;
double dist, x, y, z, a;
Vec3 p;
b = obj->data.blob;
V3Copy(&p, Q);
if (obj->T != NULL)
PointToObject(&p, obj->T);
V3Zero(N);
for (be = b->elems; be != NULL; be = be->next)
{
x = p.x - be->loc.x;
y = p.y - be->loc.y;
z = p.z - be->loc.z;
if (be->type == BLOB_CYLINDER)
{
double t;
/* get distance of point along cylinder axis from cylinder origin */
t = x * be->dir.x + y * be->dir.y + z * be->dir.z;
/* are we within cylinder length? */
if (t >= 0.0 && t < be->len) /* yes */
{
/* get radius-squared from correponding point along cylinder axis */
x -= be->dir.x * t;
y -= be->dir.y * t;
z -= be->dir.z * t;
/* If point is outside radius of influence, it doesn't count. */
if ((dist = x*x + y*y + z*z) >= be->rsq)
continue;
}
else
continue; /* Outside of cylinder length. */
}
else if (be->type == BLOB_PLANE)
{
if ((dist = x * be->dir.x + y * be->dir.y + z * be->dir.z)
>= be->rad)
continue;
x = be->dir.x;
y = be->dir.y;
z = be->dir.z;
}
else /* Spheres or hemi-spheres. */
{
/* If point is outside sphere of influence, it doesn't count. */
if ((dist = x*x + y*y + z*z) >= be->rsq)
continue;
/* See that point is also within the plane if this is a hemisphere. */
if (be->type == BLOB_HEMISPHERE)
if ((x * be->dir.x + y * be->dir.y + z * be->dir.z) > 0.0)
continue;
}
/*
* Note: Since this is actually the gradient for the inward facing
* surface of the density equation(s), we need to flip the sign so
* that the normal is pointing out from the blob. The outer surface
* of the density equation is "mirrored" on the outside of the
* field of influence for the blob element (due to the "W" shape
* of the density function) and does not fall within the interval
* containing valid blob intersections. These are checked for
* and skipped in the intersection tests above.
*/
a = - 4.0 * be->r4 * dist - 2.0 * be->r2;
N->x += x * a;
N->y += y * a;
N->z += z * a;
}
if (obj->T != NULL)
NormToWorld(N, obj->T);
V3Normalize(N);
}
