bool sweepConvex_ConvexGeom(const PxGeometry& geom, const PxTransform& pose, const PxConvexMeshGeometry& convexGeom, const PxTransform& convexPose,
const PxVec3& unitDir, const PxReal distance, PxSweepHit& sweepHit, PxHitFlags hintFlags, const PxReal inflation)
{
using namespace Ps::aos;
PX_ASSERT(geom.getType() == PxGeometryType::eCONVEXMESH);
const PxConvexMeshGeometry& otherConvexGeom = static_cast<const PxConvexMeshGeometry&>(geom);
ConvexMesh& otherConvexMesh = *static_cast<ConvexMesh*>(otherConvexGeom.convexMesh);
FETCH_CONVEX_HULL_DATA(convexGeom)
// PT: TODO: find a way to use the FETCH_CONVEX_HULL_DATA macro for the second hull as well
#ifdef __SPU__
PX_COMPILE_TIME_ASSERT(&((ConvexMesh*)NULL)->getHull()==NULL);
PX_ALIGN_PREFIX(16) PxU8 otherconvexMeshBuffer[sizeof(ConvexMesh)+32] PX_ALIGN_SUFFIX(16);
ConvexMesh* otherMesh = memFetchAsync<ConvexMesh>(otherconvexMeshBuffer, (uintptr_t)(&otherConvexMesh), sizeof(ConvexMesh),1);
memFetchWait(1); // convexMesh
PxU32 otherNPolys = otherMesh->getNbPolygonsFast();
const HullPolygonData* PX_RESTRICT otherPolysEA = otherMesh->getPolygons();
const PxU32 otherPolysSize = sizeof(HullPolygonData)*otherNPolys + sizeof(PxVec3)*otherMesh->getNbVerts();
//TODO: Need optimization with dma cache --jiayang
void* otherHullBuffer = PxAlloca(CELL_ALIGN_SIZE_16(otherPolysSize+32));
HullPolygonData* otherPolys = memFetchAsync<HullPolygonData>(otherHullBuffer, (uintptr_t)(otherPolysEA), otherPolysSize, 1);
ConvexHullData* otherHullData = &otherMesh->getHull();
otherHullData->mPolygons = otherPolys;
memFetchWait(1); // convexMesh
#else
ConvexHullData* otherHullData = &otherConvexMesh.getHull();
#endif
const Vec3V zeroV = V3Zero();
const FloatV zero = FZero();
const Vec3V otherVScale = V3LoadU(otherConvexGeom.scale.scale);
const QuatV otherVQuat = QuatVLoadU(&otherConvexGeom.scale.rotation.x);
const Vec3V vScale = Vec3V_From_Vec4V(V4LoadU(&convexGeom.scale.scale.x));
const QuatV vQuat = QuatVLoadU(&convexGeom.scale.rotation.x);
const PsTransformV otherTransf = loadTransformU(pose);
const PsTransformV convexTransf = loadTransformU(convexPose);
const Vec3V worldDir = V3LoadU(unitDir);
const FloatV dist = FLoad(distance);
const Vec3V dir = convexTransf.rotateInv(V3Scale(worldDir, dist));
const PsMatTransformV aToB(convexTransf.transformInv(otherTransf));
ConvexHullV otherConvexHull(otherHullData, zeroV, otherVScale, otherVQuat);
ConvexHullV convexHull(hullData, zeroV, vScale, vQuat);
bool isMtd = hintFlags & PxHitFlag::eMTD;
FloatV toi;
Vec3V closestA, normal;
bool hit = GJKRelativeRayCast(otherConvexHull, convexHull, aToB, zero, zeroV, dir, toi, normal, closestA,
inflation, isMtd);
if(hit)
{
sweepHit.flags = PxHitFlag::eDISTANCE | PxHitFlag::eNORMAL;
if(FAllGrtrOrEq(zero, toi))
{
//initial overlap
if(!(PX_IS_SPU) && isMtd)
{
sweepHit.flags |= PxHitFlag::ePOSITION;
const Vec3V worldPointA = convexTransf.transform(closestA);
const Vec3V destNormal = V3Neg(V3Normalize(convexTransf.rotate(normal)));
const FloatV length = toi;
V3StoreU(destNormal, sweepHit.normal);
V3StoreU(worldPointA, sweepHit.position);
FStore(length, &sweepHit.distance);
}
else
{
sweepHit.distance = 0.0f;
sweepHit.normal = -unitDir;
}
}
else
{
sweepHit.flags |= PxHitFlag::ePOSITION;
const Vec3V worldPointA = convexTransf.transform(closestA);
const Vec3V destNormal = V3Neg(V3Normalize(convexTransf.rotate(normal)));
const FloatV length = FMul(dist, toi);
V3StoreU(destNormal, sweepHit.normal);
V3StoreU(worldPointA, sweepHit.position);
FStore(length, &sweepHit.distance);
}
// PT: compute closest polygon using the same tweak as in swept-capsule-vs-mesh
sweepHit.faceIndex = computeSweepConvexPlane(convexGeom,hullData,nbPolys,pose,sweepHit.position,unitDir);
return true;
}
return false;
}
PxU32 raycast_convexMesh(GU_RAY_FUNC_PARAMS)
{
PX_UNUSED(maxHits);
PX_ASSERT(geom.getType() == PxGeometryType::eCONVEXMESH);
PX_ASSERT(maxHits && hits);
PX_ASSERT(PxAbs(rayDir.magnitudeSquared()-1)<1e-4f);
const PxConvexMeshGeometry& convexGeom = static_cast<const PxConvexMeshGeometry&>(geom);
ConvexMesh* convexMesh = static_cast<ConvexMesh*>(convexGeom.convexMesh);
PxRaycastHit& hit = *hits;
//scaling: transform the ray to vertex space
const Cm::Matrix34 world2vertexSkew = convexGeom.scale.getInverse() * pose.getInverse();
//ConvexMesh* cmesh = static_cast<ConvexMesh*>(convexGeom.convexMesh);
const PxU32 nPolys = convexMesh->getNbPolygonsFast();
const HullPolygonData* PX_RESTRICT polysEA = convexMesh->getPolygons();
const HullPolygonData* polys = polysEA;
const PxVec3 vrayOrig = world2vertexSkew.transform(rayOrigin);
const PxVec3 vrayDir = world2vertexSkew.rotate(rayDir);
/*
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
*/
bool originInsideAllPlanes = true;
PxReal latestEntry = -FLT_MAX;
PxReal earliestExit = FLT_MAX;
// PxU32 bestPolygonIndex = 0;
hit.faceIndex = 0xffffffff;
for(PxU32 i=0; i<nPolys; i++)
{
const HullPolygonData& poly = polys[i];
const PxPlane& vertSpacePlane = poly.mPlane;
const PxReal distToPlane = vertSpacePlane.distance(vrayOrig);
const PxReal dn = vertSpacePlane.n.dot(vrayDir);
const PxReal distAlongRay = -distToPlane/dn; // PT: TODO: potential divide by zero here!
// PT: TODO: this is computed again in the last branch!
if(distToPlane > 0.0f)
originInsideAllPlanes = false; //origin not behind plane == ray starts outside the convex.
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;
hit.faceIndex = i;
}
}
else
{
//plane normal and ray dir are orthogonal
if(distToPlane > 0.0f)
return 0; //a plane is parallel with ray -- and we're outside the ray -- we definitely miss the entire convex!
}
}
if(originInsideAllPlanes) //ray starts inside convex
{
hit.distance = 0.0f;
hit.faceIndex = 0xffffffff;
hit.u = 0.0f;
hit.v = 0.0f;
hit.position = rayOrigin;
hit.normal = -rayDir;
hit.flags = PxHitFlag::eDISTANCE|PxHitFlag::eNORMAL|PxHitFlag::ePOSITION;
return 1;
}
// AP: changed to latestEntry < maxDist-1e-5f so that we have a conservatively negative result near end of ray
if(latestEntry < earliestExit && latestEntry > 0.0f && latestEntry < maxDist-1e-5f)
{
PxHitFlags outFlags = PxHitFlag::eDISTANCE | PxHitFlag::eFACE_INDEX;
if(hitFlags & PxHitFlag::ePOSITION)
{
outFlags |= PxHitFlag::ePOSITION;
const PxVec3 pointOnPlane = vrayOrig + latestEntry * vrayDir;
hit.position = pose.transform(convexGeom.scale.toMat33() * pointOnPlane);
}
hit.distance = latestEntry;
hit.u = 0.0f;
hit.v = 0.0f;
hit.normal = PxVec3(0.0f);
// Compute additional information if needed
if(hitFlags & PxHitFlag::eNORMAL)
{
outFlags |= PxHitFlag::eNORMAL;
//when we have nonuniform scaling we actually have to transform by the transpose of the inverse of vertex2worldSkew.M == transpose of world2vertexSkew:
hit.normal = world2vertexSkew.rotateTranspose(polys[hit.faceIndex].mPlane.n);
hit.normal.normalize();
}
hit.flags = outFlags;
return 1;
}
return 0;
}