static void getTriangle(const Gu::TriangleMesh& mesh, PxU32 i, PxVec3* wp, const PxVec3* vertices, const void* indices, const Cm::Matrix34& absPose, bool has16BitIndices)
{
PxVec3 localVerts[3];
getTriangle(mesh, i, localVerts, vertices, indices, has16BitIndices);
wp[0] = absPose.transform(localVerts[0]);
wp[1] = absPose.transform(localVerts[1]);
wp[2] = absPose.transform(localVerts[2]);
}
void LSSCollider::InitQuery(const Gu::Capsule& lss, const Cm::Matrix34* worldl, const Cm::Matrix34* worldm)
{
VolumeCollider::InitQuery();
// Compute LSS in model space:
// - Precompute R^2
mRadius = lss.radius;
mRadius2 = lss.radius * lss.radius;
// - Compute segment
mSeg.p0 = lss.p0;
mSeg.p1 = lss.p1;
// -> to world space
if(worldl)
{
mSeg.p0 = worldl->transform(mSeg.p0);
mSeg.p1 = worldl->transform(mSeg.p1);
}
// -> to model space
if(worldm)
{
// Invert model matrix
Cm::Matrix34 InvWorldM = worldm->getInverseRT();
mSeg.p0 = InvWorldM.transform(mSeg.p0);
mSeg.p1 = InvWorldM.transform(mSeg.p1);
}
// Precompute segment data
mSDir = 0.5f * (mSeg.p1 - mSeg.p0);
mFDir.x = PxAbs(mSDir.x);
mFDir.y = PxAbs(mSDir.y);
mFDir.z = PxAbs(mSDir.z);
mSCen = 0.5f * (mSeg.p1 + mSeg.p0);
mOBB.create(Gu::Capsule(mSeg, mRadius)); // Create box around transformed capsule
}
void SphereCollider::InitQuery(const Gu::Sphere& sphere, const Cm::Matrix34* worlds, const Cm::Matrix34* worldm)
{
VolumeCollider::InitQuery();
// Compute sphere in model space:
// - Precompute R^2
mRadius2 = sphere.radius * sphere.radius;
mRadius = sphere.radius;
// - Compute center position
mCenter = sphere.center;
// -> to world space
if(worlds)
{
mCenter = worlds->transform(mCenter);
}
// -> to model space
if(worldm)
{
// Invert model matrix
Cm::Matrix34 InvWorldM = worldm->getInverseRT();
mCenter = InvWorldM.transform(mCenter);
}
}
void Gu::TriangleMesh::debugVisualize(
Cm::RenderOutput& out, const PxTransform& pose, const PxMeshScale& scaling, const PxBounds3& cullbox,
const PxU64 mask, const PxReal fscale, const PxU32 numMaterials) const
{
PX_UNUSED(numMaterials);
//bool cscale = !!(mask & ((PxU64)1 << PxVisualizationParameter::eCULL_BOX));
const PxU64 cullBoxMask = PxU64(1) << PxVisualizationParameter::eCULL_BOX;
bool cscale = ((mask & cullBoxMask) == cullBoxMask);
const PxMat44 midt(PxIdentity);
const Cm::Matrix34 absPose(PxMat33(pose.q) * scaling.toMat33(), pose.p);
PxU32 nbTriangles = getNbTrianglesFast();
const PxU32 nbVertices = getNbVerticesFast();
const PxVec3* vertices = getVerticesFast();
const void* indices = getTrianglesFast();
const PxDebugColor::Enum colors[] =
{
PxDebugColor::eARGB_BLACK,
PxDebugColor::eARGB_RED,
PxDebugColor::eARGB_GREEN,
PxDebugColor::eARGB_BLUE,
PxDebugColor::eARGB_YELLOW,
PxDebugColor::eARGB_MAGENTA,
PxDebugColor::eARGB_CYAN,
PxDebugColor::eARGB_WHITE,
PxDebugColor::eARGB_GREY,
PxDebugColor::eARGB_DARKRED,
PxDebugColor::eARGB_DARKGREEN,
PxDebugColor::eARGB_DARKBLUE,
};
const PxU32 colorCount = sizeof(colors)/sizeof(PxDebugColor::Enum);
if(cscale)
{
const Gu::Box worldBox(
(cullbox.maximum + cullbox.minimum)*0.5f,
(cullbox.maximum - cullbox.minimum)*0.5f,
PxMat33(PxIdentity));
// PT: TODO: use the callback version here to avoid allocating this huge array
PxU32* results = reinterpret_cast<PxU32*>(PX_ALLOC_TEMP(sizeof(PxU32)*nbTriangles, "tmp triangle indices"));
LimitedResults limitedResults(results, nbTriangles, 0);
Midphase::intersectBoxVsMesh(worldBox, *this, pose, scaling, &limitedResults);
nbTriangles = limitedResults.mNbResults;
if (fscale)
{
const PxU32 fcolor = PxU32(PxDebugColor::eARGB_DARKRED);
for (PxU32 i=0; i<nbTriangles; i++)
{
const PxU32 index = results[i];
PxVec3 wp[3];
getTriangle(*this, index, wp, vertices, indices, absPose, has16BitIndices());
const PxVec3 center = (wp[0] + wp[1] + wp[2]) / 3.0f;
PxVec3 normal = (wp[0] - wp[1]).cross(wp[0] - wp[2]);
PX_ASSERT(!normal.isZero());
normal = normal.getNormalized();
out << midt << fcolor <<
Cm::DebugArrow(center, normal * fscale);
}
}
if (mask & (PxU64(1) << PxVisualizationParameter::eCOLLISION_SHAPES))
{
const PxU32 scolor = PxU32(PxDebugColor::eARGB_MAGENTA);
out << midt << scolor; // PT: no need to output this for each segment!
PxDebugLine* segments = out.reserveSegments(nbTriangles*3);
for(PxU32 i=0; i<nbTriangles; i++)
{
const PxU32 index = results[i];
PxVec3 wp[3];
getTriangle(*this, index, wp, vertices, indices, absPose, has16BitIndices());
segments[0] = PxDebugLine(wp[0], wp[1], scolor);
segments[1] = PxDebugLine(wp[1], wp[2], scolor);
segments[2] = PxDebugLine(wp[2], wp[0], scolor);
segments+=3;
}
}
if ((mask & (PxU64(1) << PxVisualizationParameter::eCOLLISION_EDGES)) && mExtraTrigData)
visualizeActiveEdges(out, *this, nbTriangles, results, absPose, midt);
PX_FREE(results);
}
else
{
if (fscale)
{
const PxU32 fcolor = PxU32(PxDebugColor::eARGB_DARKRED);
for (PxU32 i=0; i<nbTriangles; i++)
{
PxVec3 wp[3];
getTriangle(*this, i, wp, vertices, indices, absPose, has16BitIndices());
const PxVec3 center = (wp[0] + wp[1] + wp[2]) / 3.0f;
PxVec3 normal = (wp[0] - wp[1]).cross(wp[0] - wp[2]);
PX_ASSERT(!normal.isZero());
normal = normal.getNormalized();
out << midt << fcolor <<
Cm::DebugArrow(center, normal * fscale);
}
}
if (mask & (PxU64(1) << PxVisualizationParameter::eCOLLISION_SHAPES))
{
PxU32 scolor = PxU32(PxDebugColor::eARGB_MAGENTA);
out << midt << scolor; // PT: no need to output this for each segment!
PxVec3* transformed = reinterpret_cast<PxVec3*>(PX_ALLOC(sizeof(PxVec3)*nbVertices, "PxVec3"));
for(PxU32 i=0;i<nbVertices;i++)
transformed[i] = absPose.transform(vertices[i]);
PxDebugLine* segments = out.reserveSegments(nbTriangles*3);
for (PxU32 i=0; i<nbTriangles; i++)
{
PxVec3 wp[3];
getTriangle(*this, i, wp, transformed, indices, has16BitIndices());
const PxU32 localMaterialIndex = getTriangleMaterialIndex(i);
scolor = colors[localMaterialIndex % colorCount];
segments[0] = PxDebugLine(wp[0], wp[1], scolor);
segments[1] = PxDebugLine(wp[1], wp[2], scolor);
segments[2] = PxDebugLine(wp[2], wp[0], scolor);
segments+=3;
}
PX_FREE(transformed);
}
if ((mask & (PxU64(1) << PxVisualizationParameter::eCOLLISION_EDGES)) && mExtraTrigData)
visualizeActiveEdges(out, *this, nbTriangles, NULL, absPose, midt);
}
}
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;
}
void Gu::TriangleMesh::debugVisualize(
Cm::RenderOutput& out, const Cm::Matrix34& absPose, const PxBounds3& cullbox,
const PxU64 mask, const PxReal fscale) const
{
bool cscale = !!(mask & ((PxU64)1 << PxVisualizationParameter::eCULL_BOX));
const PxMat44 midt = PxMat44::createIdentity();
const PxU32 nbTriangles = mesh.getNumTriangles();
const PxU32 nbVertices = mesh.getNumVertices();
const PxVec3* vertices = mesh.getVertices();
const void* indices = getTrianglesFast();
const bool has16BitIndices = mesh.has16BitIndices();
if (fscale)
{
const PxU32 fcolor = PxDebugColor::eARGB_DARKRED;
for (PxU32 i=0; i<nbTriangles; i++)
{
PxVec3 wp[3];
getTriangle(*this, i, wp, vertices, indices, absPose, has16BitIndices);
const PxVec3 center = (wp[0] + wp[1] + wp[2]) / 3.0f;
PxVec3 normal = (wp[0] - wp[1]).cross(wp[0] - wp[2]);
PX_ASSERT(!normal.isZero());
normal = normal.getNormalized();
if (!cscale || cullbox.contains(center))
out << midt << fcolor <<
Cm::DebugArrow(center, normal * fscale);
}
}
if (mask & ((PxU64)1 << PxVisualizationParameter::eCOLLISION_SHAPES))
{
const PxU32 scolor = PxDebugColor::eARGB_MAGENTA;
out << midt << scolor; // PT: no need to output this for each segment!
// PT: transform vertices only once
PxVec3* transformed = (PxVec3*)PX_ALLOC(sizeof(PxVec3)*nbVertices);
for(PxU32 i=0;i<nbVertices;i++)
transformed[i] = absPose.transform(vertices[i]);
for (PxU32 i=0; i<nbTriangles; i++)
{
PxVec3 wp[3];
getTriangle(*this, i, wp, transformed, indices, has16BitIndices);
if (!cscale || (cullbox.contains(wp[0]) && cullbox.contains(wp[1]) && cullbox.contains(wp[2])))
{
out.outputSegment(wp[0], wp[1]);
out.outputSegment(wp[1], wp[2]);
out.outputSegment(wp[2], wp[0]);
}
}
PX_FREE(transformed);
}
if (mask & ((PxU64)1 << PxVisualizationParameter::eCOLLISION_EDGES))
{
const PxU32 ecolor = PxDebugColor::eARGB_YELLOW;
for (PxU32 i=0; i<nbTriangles; i++)
{
PxVec3 wp[3];
getTriangle(*this, i, wp, vertices, indices, absPose, has16BitIndices);
const PxU32 flags = mesh.getTrigSharedEdgeFlags(i);
if(flags & Gu::ETD_CONVEX_EDGE_01)
{
if (!cscale || (cullbox.contains(wp[0]) && cullbox.contains(wp[1])))
out << midt << ecolor << Cm::RenderOutput::LINES << wp[0] << wp[1];
}
if(flags & Gu::ETD_CONVEX_EDGE_12)
{
if (!cscale || (cullbox.contains(wp[1]) && cullbox.contains(wp[2])))
out << midt << ecolor << Cm::RenderOutput::LINES << wp[1] << wp[2];
}
if(flags & Gu::ETD_CONVEX_EDGE_20)
{
if (!cscale || (cullbox.contains(wp[0]) && cullbox.contains(wp[2])))
out << midt << ecolor << Cm::RenderOutput::LINES << wp[0] << wp[2];
}
}
}
}
Ps::IntBool RayCollider::InitQuery(const PxVec3& orig, const PxVec3& dir, const Cm::Matrix34* world, PxU32* face_id)
{
// Reset stats & contact status
mNbIntersections = 0;
#ifndef OPC_RAYHIT_CALLBACK
if(mStabbedFaces)
mStabbedFaces->Reset();
#endif
// Compute ray in local space
// The (Origin/Dir) form is needed for the ray-triangle test anyway (even for segment tests)
if(world)
{
mDir = world->rotateTranspose(dir);
Cm::Matrix34 World = world->getInverseRT();
mOrigin = World.transform(orig);
}
else
{
mDir = dir;
mOrigin = orig;
}
// 4) Special case: 1-triangle meshes [Opcode 1.3]
// if(mCurrentModel && mCurrentModel->HasSingleNode())
if(mCurrentModel && mCurrentModel->HasSingleNode())
{
// We simply perform the BV-Prim overlap test each time. We assume single triangle has index 0.
if(!SkipPrimitiveTests())
{
// Perform overlap test between the unique triangle and the ray (and set contact status if needed)
PerformSegmentPrimTest(PxU32(0), OPC_CONTACT);
// Return immediately regardless of status
return Ps::IntTrue;
}
}
// Precompute data (moved after temporal coherence since only needed for ray-AABB)
if(mMaxDist!=PX_MAX_F32)
{
SetupSegment();
}
else
{
// For Ray-AABB overlap
// PxU32 x = SIR(mDir.x)-1;
// PxU32 y = SIR(mDir.y)-1;
// PxU32 z = SIR(mDir.z)-1;
// mData.x = FR(x);
// mData.y = FR(y);
// mData.z = FR(z);
// Precompute mFDir;
mFDir.x = PxAbs(mDir.x);
mFDir.y = PxAbs(mDir.y);
mFDir.z = PxAbs(mDir.z);
}
return Ps::IntFalse;
}