Revision control
Copy as Markdown
// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
// SPDX-FileCopyrightText: Copyright (c) 2008-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
// SPDX-License-Identifier: Apache-2.0
#include "NpDebugViz.h"
// PT: moving "all" debug viz code to the same file to improve cache locality when debug drawing things,
// share more code, and make sure all actors do thing consistently.
#include "NpScene.h"
#include "NpCheck.h"
#include "common/PxProfileZone.h"
using namespace physx;
#if PX_ENABLE_DEBUG_VISUALIZATION
#include "NpShapeManager.h"
#include "NpRigidStatic.h"
#include "NpRigidDynamic.h"
#include "NpArticulationReducedCoordinate.h"
#if PX_SUPPORT_GPU_PHYSX
#include "NpPBDParticleSystem.h"
#endif
#include "foundation/PxVecMath.h"
#include "geometry/PxMeshQuery.h"
#include "GuHeightFieldUtil.h"
#include "GuConvexEdgeFlags.h"
#include "GuMidphaseInterface.h"
#include "GuEdgeList.h"
#include "GuBounds.h"
#include "BpBroadPhase.h"
#include "BpAABBManager.h"
#include "GuConvexGeometry.h"
using namespace aos;
using namespace Gu;
using namespace Cm;
/////
static const PxU32 gCollisionShapeColor = PxU32(PxDebugColor::eARGB_MAGENTA);
static PX_FORCE_INLINE Vec4V multiply3x3V(const Vec4V p, const PxMat34& mat)
{
Vec4V ResV = V4Scale(V4LoadU(&mat.m.column0.x), V4GetX(p));
ResV = V4Add(ResV, V4Scale(V4LoadU(&mat.m.column1.x), V4GetY(p)));
ResV = V4Add(ResV, V4Scale(V4LoadU(&mat.m.column2.x), V4GetZ(p)));
return ResV;
}
// PT: beware, needs padding at the end of dst/src
static PX_FORCE_INLINE void transformV(PxVec3* dst, const PxVec3* src, const Vec4V p, const PxMat34& mat)
{
const Vec4V vertexV = V4LoadU(&src->x);
const Vec4V transformedV = V4Add(multiply3x3V(vertexV, mat), p);
V4StoreU(transformedV, &dst->x);
}
static void visualizeSphere(const PxSphereGeometry& geometry, PxRenderOutput& out, const PxTransform& absPose)
{
out << gCollisionShapeColor; // PT: no need to output this for each segment!
out << absPose;
renderOutputDebugCircle(out, 100, geometry.radius);
PxMat44 rotPose(absPose);
PxSwap(rotPose.column1, rotPose.column2);
rotPose.column1 = -rotPose.column1;
out << rotPose;
renderOutputDebugCircle(out, 100, geometry.radius);
PxSwap(rotPose.column0, rotPose.column2);
rotPose.column0 = -rotPose.column0;
out << rotPose;
renderOutputDebugCircle(out, 100, geometry.radius);
}
static void visualizePlane(const PxPlaneGeometry& /*geometry*/, PxRenderOutput& out, const PxTransform& absPose)
{
PxMat44 rotPose(absPose);
PxSwap(rotPose.column1, rotPose.column2);
rotPose.column1 = -rotPose.column1;
PxSwap(rotPose.column0, rotPose.column2);
rotPose.column0 = -rotPose.column0;
out << rotPose << gCollisionShapeColor; // PT: no need to output this for each segment!
for(PxReal radius = 2.0f; radius < 20.0f ; radius += 2.0f)
renderOutputDebugCircle(out, 100, radius*radius);
}
static void visualizeCapsule(const PxCapsuleGeometry& geometry, PxRenderOutput& out, const PxTransform& absPose)
{
out << gCollisionShapeColor;
out.outputCapsule(geometry.radius, geometry.halfHeight, absPose);
}
static void visualizeBox(const PxBoxGeometry& geometry, PxRenderOutput& out, const PxTransform& absPose)
{
out << gCollisionShapeColor;
out << absPose;
renderOutputDebugBox(out, PxBounds3(-geometry.halfExtents, geometry.halfExtents));
}
static void visualizeConvexCore(const PxConvexCoreGeometry& geometry, PxRenderOutput& out, const PxTransform& absPose, const PxBounds3& cullbox)
{
Gu::visualize(geometry, absPose, true, cullbox, out);
}
static void visualizeConvexMesh(const PxConvexMeshGeometry& geometry, PxRenderOutput& out, const PxTransform& absPose)
{
const ConvexMesh* convexMesh = static_cast<const ConvexMesh*>(geometry.convexMesh);
const ConvexHullData& hullData = convexMesh->getHull();
const PxVec3* vertices = hullData.getHullVertices();
const PxU8* indexBuffer = hullData.getVertexData8();
const PxU32 nbPolygons = convexMesh->getNbPolygonsFast();
const PxMat33Padded m33(absPose.q);
const PxMat44 m44(m33 * toMat33(geometry.scale), absPose.p);
out << m44 << gCollisionShapeColor; // PT: no need to output this for each segment!
for(PxU32 i=0; i<nbPolygons; i++)
{
const PxU32 pnbVertices = hullData.mPolygons[i].mNbVerts;
PxVec3 begin = m44.transform(vertices[indexBuffer[0]]); // PT: transform it only once before the loop starts
for(PxU32 j=1; j<pnbVertices; j++)
{
const PxVec3 end = m44.transform(vertices[indexBuffer[j]]);
out.outputSegment(begin, end);
begin = end;
}
out.outputSegment(begin, m44.transform(vertices[indexBuffer[0]]));
indexBuffer += pnbVertices;
}
}
static void getTriangle(PxU32 i, PxVec3* wp, const PxVec3* vertices, const void* indices, bool has16BitIndices)
{
PxU32 ref0, ref1, ref2;
getVertexRefs(i, ref0, ref1, ref2, indices, has16BitIndices);
wp[0] = vertices[ref0];
wp[1] = vertices[ref1];
wp[2] = vertices[ref2];
}
// PT: beware with wp, needs padding
static void getWorldTriangle(PxU32 i, PxVec3* wp, const PxVec3* vertices, const void* indices, const PxMat34& absPose, bool has16BitIndices)
{
// PxVec3 localVerts[3];
// getTriangle(i, localVerts, vertices, indices, has16BitIndices);
// wp[0] = absPose.transform(localVerts[0]);
// wp[1] = absPose.transform(localVerts[1]);
// wp[2] = absPose.transform(localVerts[2]);
PxU32 ref0, ref1, ref2;
getVertexRefs(i, ref0, ref1, ref2, indices, has16BitIndices);
const Vec4V posV = Vec4V_From_Vec3V(V3LoadU(&absPose.p.x));
transformV(&wp[0], &vertices[ref0], posV, absPose);
transformV(&wp[1], &vertices[ref1], posV, absPose);
transformV(&wp[2], &vertices[ref2], posV, absPose);
}
static void visualizeActiveEdges(PxRenderOutput& out, const TriangleMesh& mesh, PxU32 nbTriangles, const PxU32* results, const PxMat34& absPose)
{
const PxU8* extraTrigData = mesh.getExtraTrigData();
const PxVec3* vertices = mesh.getVerticesFast();
const void* indices = mesh.getTrianglesFast();
out << PxU32(PxDebugColor::eARGB_YELLOW); // PT: no need to output this for each segment!
const bool has16Bit = mesh.has16BitIndices();
for(PxU32 i=0; i<nbTriangles; i++)
{
const PxU32 index = results ? results[i] : i;
PxVec3 wp[3+1];
getWorldTriangle(index, wp, vertices, indices, absPose, has16Bit);
const PxU32 flags = getConvexEdgeFlags(extraTrigData, index);
if(flags & ETD_CONVEX_EDGE_01)
out.outputSegment(wp[0], wp[1]);
if(flags & ETD_CONVEX_EDGE_12)
out.outputSegment(wp[1], wp[2]);
if(flags & ETD_CONVEX_EDGE_20)
out.outputSegment(wp[0], wp[2]);
}
}
static void visualizeFaceNormals( PxReal fscale, PxRenderOutput& out, PxU32 nbTriangles, const PxVec3* vertices,
const void* indices, bool has16Bit, const PxU32* results, const PxMat34& absPose, const PxMat44& midt)
{
out << midt << PxU32(PxDebugColor::eARGB_DARKRED); // PT: no need to output this for each segment!
const float coeff = 1.0f / 3.0f;
PxDebugLine* segments = out.reserveSegments(nbTriangles);
for(PxU32 i=0; i<nbTriangles; i++)
{
const PxU32 index = results ? results[i] : i;
PxVec3 wp[3+1];
getWorldTriangle(index, wp, vertices, indices, absPose, has16Bit);
const PxVec3 center = (wp[0] + wp[1] + wp[2]) * coeff;
PxVec3 normal = (wp[0] - wp[1]).cross(wp[0] - wp[2]);
PX_ASSERT(!normal.isZero());
normal = normal.getNormalized();
segments->pos0 = center;
segments->pos1 = center + normal * fscale;
segments->color0 = segments->color1 = PxU32(PxDebugColor::eARGB_DARKRED);
segments++;
}
}
static PxU32 MakeSolidColor(PxU32 alpha, PxU32 red, PxU32 green, PxU32 blue)
{
return (alpha<<24) | (red << 16) |
(green << 8) | blue;
}
static void decodeTriple(PxU32 id, PxU32& x, PxU32& y, PxU32& z)
{
x = id & 0x000003FF;
id = id >> 10;
y = id & 0x000003FF;
id = id >> 10;
z = id & 0x000003FF;
}
PX_FORCE_INLINE PxU32 idx(PxU32 x, PxU32 y, PxU32 z, PxU32 width, PxU32 height)
{
return z * (width) * (height) + y * width + x;
}
PX_FORCE_INLINE PxReal decode(PxU8* data, PxU32 bytesPerSparsePixel, PxReal subgridsMinSdfValue, PxReal subgridsMaxSdfValue)
{
switch (bytesPerSparsePixel)
{
case 1:
return PxReal(data[0]) * (1.0f / 255.0f) * (subgridsMaxSdfValue - subgridsMinSdfValue) + subgridsMinSdfValue;
case 2:
{
PxU16* ptr = reinterpret_cast<PxU16*>(data);
return PxReal(ptr[0]) * (1.0f / 65535.0f) * (subgridsMaxSdfValue - subgridsMinSdfValue) + subgridsMinSdfValue;
}
case 4:
//If 4 bytes per subgrid pixel are available, then normal floats are used. No need to
//de-normalize integer values since the floats already contain real distance values
PxReal* ptr = reinterpret_cast<PxReal*>(data);
return ptr[0];
}
return 0;
}
PX_FORCE_INLINE PxU32 makeColor(PxReal v, PxReal invRange0, PxReal invRange1)
{
PxVec3 midColor(0.f, 0, 255.f);
PxVec3 lowColor(255.f, 0, 0);
PxVec3 outColor(0, 255.f, 0.f);
PxU32 color;
if (v > 0.f)
{
PxReal scale = PxPow(v * invRange0, 0.25f);
PxVec3 blendColor = midColor + (outColor - midColor) * scale;
color = MakeSolidColor(
0xff000000,
PxU32(blendColor.x),
PxU32(blendColor.y),
PxU32(blendColor.z)
);
}
else
{
PxReal scale = PxPow(v * invRange1, 0.25f);
PxVec3 blendColor = midColor + (lowColor - midColor) * scale;
color = MakeSolidColor(
0xff000000,
PxU32(blendColor.x),
PxU32(blendColor.y),
PxU32(blendColor.z)
);
}
return color;
}
//Returns true if the number of samples chosen to visualize was reduced (to speed up rendering) compared to the total number of sdf samples available
static bool visualizeSDF(PxRenderOutput& out, const Gu::SDF& sdf, const PxMat34& absPose, bool limitNumberOfVisualizedSamples = false)
{
//### DEFENSIVE coding for OM-122453 and OM-112365
if (!sdf.mSdf)
{
PxGetFoundation().error(::physx::PxErrorCode::eINTERNAL_ERROR, PX_FL, "No distance data available (null pointer) when evaluating an SDF.");
return false;
}
bool dataReductionActive = false;
PxU32 upperByteLimit = 512u * 512u * 512u * sizeof(PxDebugPoint); //2GB - sizeof(PxDebugPoint)=16bytes
bool repeat = true;
PxReal low = 0.0f, high = 0.0f;
PxU32 count = 0;
PxU32 upperLimit = limitNumberOfVisualizedSamples ? 128 : 4096;
PxReal sdfSpacing = 0.0f;
PxU32 strideX = 0;
PxU32 strideY = 0;
PxU32 strideZ = 0;
PxU32 nbX = 0, nbY = 0, nbZ = 0;
PxU32 subgridSize = 0;
PxU32 subgridStride = 1;
while (repeat)
{
repeat = false;
PxU32 nbTargetSamplesPerAxis = upperLimit;
subgridStride = 1;
if (sdf.mSubgridSize == 0)
{
subgridSize = 1;
sdfSpacing = sdf.mSpacing;
nbX = sdf.mDims.x;
nbY = sdf.mDims.y;
nbZ = sdf.mDims.z;
}
else
{
//### DEFENSIVE coding for OM-122453 and OM-112365
if (!sdf.mSubgridSdf || !sdf.mSubgridStartSlots)
{
PxGetFoundation().error(::physx::PxErrorCode::eINTERNAL_ERROR, PX_FL, "No sparse grid data available (null pointer) when evaluating an SDF.");
return false;
}
subgridSize = sdf.mSubgridSize;
sdfSpacing = subgridSize * sdf.mSpacing;
nbX = sdf.mDims.x / subgridSize + 1;
nbY = sdf.mDims.y / subgridSize + 1;
nbZ = sdf.mDims.z / subgridSize + 1;
//Limit the max number of visualized sparse grid samples
if (limitNumberOfVisualizedSamples && subgridSize > 4)
{
subgridStride = (subgridSize + 3) / 4;
dataReductionActive = true;
}
}
//KS - a bit arbitrary, but let's limit how many points we churn out
strideX = (nbX + nbTargetSamplesPerAxis - 1) / nbTargetSamplesPerAxis;
strideY = (nbY + nbTargetSamplesPerAxis - 1) / nbTargetSamplesPerAxis;
strideZ = (nbZ + nbTargetSamplesPerAxis - 1) / nbTargetSamplesPerAxis;
if (strideX != 1 || strideY != 1 || strideZ != 1)
dataReductionActive = true;
low = PX_MAX_F32;
high = -PX_MAX_F32;
count = 0;
for (PxU32 k = 0; k < nbZ; k += strideZ)
{
for (PxU32 j = 0; j < nbY; j += strideY)
{
for (PxU32 i = 0; i < nbX; i += strideX)
{
PxReal v = sdf.mSdf[k*nbX*nbY + j * nbX + i];
count++;
low = PxMin(low, v);
high = PxMax(high, v);
if (sdf.mSubgridSize > 0 && k < nbZ - 1 && j < nbY - 1 && i < nbX - 1)
{
PxU32 startId = sdf.mSubgridStartSlots[k*(nbX - 1)*(nbY - 1) + j * (nbX - 1) + i];
if (startId != 0xFFFFFFFFu)
{
PxU32 xBase, yBase, zBase;
decodeTriple(startId, xBase, yBase, zBase);
PX_ASSERT(xBase < sdf.mSdfSubgrids3DTexBlockDim.x);
PX_ASSERT(yBase < sdf.mSdfSubgrids3DTexBlockDim.y);
PX_ASSERT(zBase < sdf.mSdfSubgrids3DTexBlockDim.z);
PxU32 localCount = subgridSize / subgridStride + 1;
count += localCount * localCount * localCount;
}
}
if (count * sizeof(PxDebugPoint) > upperByteLimit)
break;
}
if (count * sizeof(PxDebugPoint) > upperByteLimit)
break;
}
if (count * sizeof(PxDebugPoint) > upperByteLimit)
break;
}
if (count * sizeof(PxDebugPoint) > upperByteLimit)
{
upperLimit /= 2;
repeat = true;
if (upperLimit == 1)
return true;
}
}
const PxReal range0 = high;
const PxReal range1 = low;
const PxReal invRange0 = 1.f / range0;
const PxReal invRange1 = 1.f / range1;
PxDebugPoint* points = out.reservePoints(count);
PxVec3 localPos = sdf.mMeshLower;
PxReal spacingX = sdfSpacing * strideX;
PxReal spacingY = sdfSpacing * strideY;
PxReal spacingZ = sdfSpacing * strideZ;
for (PxU32 k = 0; k < nbZ; k += strideZ, localPos.z += spacingZ)
{
localPos.y = sdf.mMeshLower.y;
for (PxU32 j = 0; j < nbY; j += strideY, localPos.y += spacingY)
{
localPos.x = sdf.mMeshLower.x;
for (PxU32 i = 0; i < nbX; i += strideX, localPos.x += spacingX)
{
PxU32 color;
if (sdf.mSubgridSize > 0 && k < nbZ - 1 && j < nbY - 1 && i < nbX - 1)
{
PxU32 startId = sdf.mSubgridStartSlots[k * (nbX - 1) * (nbY - 1) + j * (nbX - 1) + i];
if (startId != 0xFFFFFFFFu)
{
PxU32 xBase, yBase, zBase;
decodeTriple(startId, xBase, yBase, zBase);
xBase *= (subgridSize + 1);
yBase *= (subgridSize + 1);
zBase *= (subgridSize + 1);
//Visualize the subgrid
for (PxU32 z = 0; z <= subgridSize; z += subgridStride)
{
for (PxU32 y = 0; y <= subgridSize; y += subgridStride)
{
for (PxU32 x = 0; x <= subgridSize; x += subgridStride)
{
PxReal value = decode(&sdf.mSubgridSdf[sdf.mBytesPerSparsePixel * idx(xBase + x, yBase + y, zBase + z,
sdf.mSdfSubgrids3DTexBlockDim.x * (subgridSize + 1), sdf.mSdfSubgrids3DTexBlockDim.y * (subgridSize + 1))], sdf.mBytesPerSparsePixel, sdf.mSubgridsMinSdfValue, sdf.mSubgridsMaxSdfValue);
color = makeColor(value, invRange0, invRange1);
PxVec3 subgridLocalPos = localPos + sdf.mSpacing * PxVec3(PxReal(x), PxReal(y), PxReal(z));
*points = PxDebugPoint(absPose.transform(subgridLocalPos), color);
points++;
}
}
}
}
}
PxReal v = sdf.mSdf[k*nbX*nbY + j * nbX + i];
color = makeColor(v, invRange0, invRange1);
*points = PxDebugPoint(absPose.transform(localPos), color);
points++;
}
}
}
return dataReductionActive;
}
static PX_FORCE_INLINE void outputTriangle(PxDebugLine* segments, const PxVec3& v0, const PxVec3& v1, const PxVec3& v2, PxU32 color)
{
// PT: TODO: use SIMD
segments[0] = PxDebugLine(v0, v1, color);
segments[1] = PxDebugLine(v1, v2, color);
segments[2] = PxDebugLine(v2, v0, color);
}
static void visualizeTriangleMesh(const PxTriangleMeshGeometry& geometry, PxRenderOutput& out, const PxTransform& pose, const PxBounds3& cullbox, PxReal fscale, bool visualizeShapes, bool visualizeEdges, bool useCullBox, bool visualizeSDFs)
{
const TriangleMesh* triangleMesh = static_cast<const TriangleMesh*>(geometry.triangleMesh);
const PxMat44 midt(PxIdentity);
// PT: TODO: why do we compute it that way sometimes?
// const PxMat34 vertex2worldSkew = pose * geometry.scale;
const PxMat33Padded m33(pose.q);
const PxMat34 absPose(m33 * toMat33(geometry.scale), pose.p);
PxU32 nbTriangles = triangleMesh->getNbTrianglesFast();
const PxU32 nbVertices = triangleMesh->getNbVerticesFast();
const PxVec3* vertices = triangleMesh->getVerticesFast();
const void* indices = triangleMesh->getTrianglesFast();
const bool has16Bit = triangleMesh->has16BitIndices();
bool drawSDF = visualizeSDFs && triangleMesh->getSDF();
PxU32* results = NULL;
if (!drawSDF)
{
if (useCullBox)
{
const 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
results = PX_ALLOCATE(PxU32, nbTriangles, "tmp triangle indices");
LimitedResults limitedResults(results, nbTriangles, 0);
Midphase::intersectBoxVsMesh(worldBox, *triangleMesh, pose, geometry.scale, &limitedResults);
nbTriangles = limitedResults.mNbResults;
if (visualizeShapes)
{
const PxU32 scolor = gCollisionShapeColor;
out << midt << scolor; // PT: no need to output this for each segment!
// PT: TODO: don't render the same edge multiple times
PxDebugLine* segments = out.reserveSegments(nbTriangles * 3);
for (PxU32 i = 0; i < nbTriangles; i++)
{
PxVec3 wp[3 + 1];
getWorldTriangle(results[i], wp, vertices, indices, absPose, has16Bit);
outputTriangle(segments, wp[0], wp[1], wp[2], scolor);
segments += 3;
}
}
}
else
{
if (visualizeShapes)
{
const PxU32 scolor = gCollisionShapeColor;
out << midt << scolor; // PT: no need to output this for each segment!
const Vec4V posV = Vec4V_From_Vec3V(V3LoadU(&absPose.p.x));
PxVec3* transformed = PX_ALLOCATE(PxVec3, (nbVertices + 1), "PxVec3");
// for(PxU32 i=0;i<nbVertices;i++)
// transformed[i] = absPose.transform(vertices[i]);
for (PxU32 i = 0; i < nbVertices; i++)
{
//const Vec4V vertexV = V4LoadU(&vertices[i].x);
//const Vec4V transformedV = V4Add(multiply3x3V(vertexV, absPose), posV);
//V4StoreU(transformedV, &transformed[i].x);
transformV(&transformed[i], &vertices[i], posV, absPose);
}
const Gu::EdgeList* edgeList = triangleMesh->requestEdgeList();
if (edgeList)
{
PxU32 nbEdges = edgeList->getNbEdges();
PxDebugLine* segments = out.reserveSegments(nbEdges);
const Gu::EdgeData* edges = edgeList->getEdges();
while (nbEdges--)
{
segments->pos0 = transformed[edges->Ref0];
segments->pos1 = transformed[edges->Ref1];
segments->color0 = segments->color1 = scolor;
segments++;
edges++;
}
}
else
{
PxDebugLine* segments = out.reserveSegments(nbTriangles * 3);
for (PxU32 i = 0; i < nbTriangles; i++)
{
PxVec3 wp[3];
getTriangle(i, wp, transformed, indices, has16Bit);
outputTriangle(segments, wp[0], wp[1], wp[2], scolor);
segments += 3;
}
}
PX_FREE(transformed);
}
}
}
if(fscale!=0.0f)
{
if(geometry.scale.hasNegativeDeterminant())
fscale = -fscale;
visualizeFaceNormals(fscale, out, nbTriangles, vertices, indices, has16Bit, results, absPose, midt);
}
if(visualizeEdges)
visualizeActiveEdges(out, *triangleMesh, nbTriangles, results, absPose);
if (drawSDF)
{
const Gu::SDF& sdf = triangleMesh->getSdfDataFast();
//We have an SDF, we should debug render it...
visualizeSDF(out, sdf, absPose);
}
PX_FREE(results);
}
static void visualizeHeightField(const PxHeightFieldGeometry& hfGeometry, PxRenderOutput& out, const PxTransform& absPose, const PxBounds3& cullbox, bool useCullBox)
{
const HeightField* heightfield = static_cast<const HeightField*>(hfGeometry.heightField);
// PT: TODO: the debug viz for HFs is minimal at the moment...
const PxU32 scolor = gCollisionShapeColor;
const PxMat44 midt = PxMat44(PxIdentity);
HeightFieldUtil hfUtil(hfGeometry);
const PxU32 nbRows = heightfield->getNbRowsFast();
const PxU32 nbColumns = heightfield->getNbColumnsFast();
const PxU32 nbVerts = nbRows * nbColumns;
const PxU32 nbTriangles = 2 * nbVerts;
out << midt << scolor; // PT: no need to output the same matrix/color for each triangle
if(useCullBox)
{
const PxTransform pose0((cullbox.maximum + cullbox.minimum)*0.5f);
const PxBoxGeometry boxGeometry((cullbox.maximum - cullbox.minimum)*0.5f);
PxU32* results = PX_ALLOCATE(PxU32, nbTriangles, "tmp triangle indices");
bool overflow = false;
PxU32 nbTouchedTris = PxMeshQuery::findOverlapHeightField(boxGeometry, pose0, hfGeometry, absPose, results, nbTriangles, 0, overflow);
PxDebugLine* segments = out.reserveSegments(nbTouchedTris*3);
for(PxU32 i=0; i<nbTouchedTris; i++)
{
const PxU32 index = results[i];
PxTriangle currentTriangle;
PxMeshQuery::getTriangle(hfGeometry, absPose, index, currentTriangle);
//The check has been done in the findOverlapHeightField
//if(heightfield->isValidTriangle(index) && heightfield->getTriangleMaterial(index) != PxHeightFieldMaterial::eHOLE)
{
outputTriangle(segments, currentTriangle.verts[0], currentTriangle.verts[1], currentTriangle.verts[2], scolor);
segments+=3;
}
}
PX_FREE(results);
}
else
{
// PT: transform vertices only once
PxVec3* tmpVerts = PX_ALLOCATE(PxVec3, nbVerts, "PxVec3");
// PT: TODO: optimize the following line
for(PxU32 i=0;i<nbVerts;i++)
tmpVerts[i] = absPose.transform(hfUtil.hf2shapep(heightfield->getVertex(i)));
for(PxU32 i=0; i<nbTriangles; i++)
{
if(heightfield->isValidTriangle(i) && heightfield->getTriangleMaterial(i) != PxHeightFieldMaterial::eHOLE)
{
PxU32 vi0, vi1, vi2;
heightfield->getTriangleVertexIndices(i, vi0, vi1, vi2);
PxDebugLine* segments = out.reserveSegments(3);
outputTriangle(segments, tmpVerts[vi0], tmpVerts[vi1], tmpVerts[vi2], scolor);
}
}
PX_FREE(tmpVerts);
}
}
static void visualize(const PxGeometry& geometry, PxRenderOutput& out, const PxTransform& absPose, const PxBounds3& cullbox, const PxReal fscale, bool visualizeShapes, bool visualizeEdges, bool useCullBox, bool visualizeSDFs)
{
// triangle meshes can render active edges or face normals, but for other types we can just early out if there are no collision shapes
if(!visualizeShapes && geometry.getType() != PxGeometryType::eTRIANGLEMESH)
return;
switch(geometry.getType())
{
case PxGeometryType::eSPHERE:
visualizeSphere(static_cast<const PxSphereGeometry&>(geometry), out, absPose);
break;
case PxGeometryType::eBOX:
visualizeBox(static_cast<const PxBoxGeometry&>(geometry), out, absPose);
break;
case PxGeometryType::ePLANE:
visualizePlane(static_cast<const PxPlaneGeometry&>(geometry), out, absPose);
break;
case PxGeometryType::eCAPSULE:
visualizeCapsule(static_cast<const PxCapsuleGeometry&>(geometry), out, absPose);
break;
case PxGeometryType::eCONVEXCORE:
PX_ASSERT(static_cast<const PxConvexCoreGeometry&>(geometry).isValid());
visualizeConvexCore(static_cast<const PxConvexCoreGeometry&>(geometry), out, absPose, cullbox);
break;
case PxGeometryType::eCONVEXMESH:
visualizeConvexMesh(static_cast<const PxConvexMeshGeometry&>(geometry), out, absPose);
break;
case PxGeometryType::eTRIANGLEMESH:
visualizeTriangleMesh(static_cast<const PxTriangleMeshGeometry&>(geometry), out, absPose, cullbox, fscale, visualizeShapes, visualizeEdges, useCullBox, visualizeSDFs);
break;
case PxGeometryType::eHEIGHTFIELD:
visualizeHeightField(static_cast<const PxHeightFieldGeometry&>(geometry), out, absPose, cullbox, useCullBox);
break;
case PxGeometryType::eTETRAHEDRONMESH:
case PxGeometryType::ePARTICLESYSTEM:
// A.B. missing visualization code
break;
case PxGeometryType::eCUSTOM:
PX_ASSERT(static_cast<const PxCustomGeometry&>(geometry).isValid());
static_cast<const PxCustomGeometry&>(geometry).callbacks->visualize(geometry, out, absPose, cullbox);
break;
case PxGeometryType::eINVALID:
break;
case PxGeometryType::eGEOMETRY_COUNT:
break;
}
}
void NpShapeManager::visualize(PxRenderOutput& out, NpScene& scene, const PxRigidActor& actor, float scale) const
{
PX_ASSERT(scale!=0.0f); // Else we shouldn't have been called
const Sc::Scene& scScene = scene.getScScene();
const PxU32 nbShapes = getNbShapes();
NpShape*const* PX_RESTRICT shapes = getShapes();
const bool visualizeCompounds = (nbShapes>1) && scScene.getVisualizationParameter(PxVisualizationParameter::eCOLLISION_COMPOUNDS)!=0.0f;
// PT: moved all these out of the loop, no need to grab them once per shape
const PxBounds3& cullbox = scScene.getVisualizationCullingBox();
const bool visualizeAABBs = scScene.getVisualizationParameter(PxVisualizationParameter::eCOLLISION_AABBS)!=0.0f;
const bool visualizeShapes = scScene.getVisualizationParameter(PxVisualizationParameter::eCOLLISION_SHAPES)!=0.0f;
const bool visualizeEdges = scScene.getVisualizationParameter(PxVisualizationParameter::eCOLLISION_EDGES)!=0.0f;
const bool visualizeSDFs = scScene.getVisualizationParameter(PxVisualizationParameter::eSDF)!=0.0f;
const float fNormals = scScene.getVisualizationParameter(PxVisualizationParameter::eCOLLISION_FNORMALS);
const bool visualizeFNormals = fNormals!=0.0f;
const bool visualizeCollision = visualizeShapes || visualizeFNormals || visualizeEdges || visualizeSDFs;
const bool useCullBox = !cullbox.isEmpty();
const bool needsShapeBounds0 = visualizeCompounds || (visualizeCollision && useCullBox);
const PxReal collisionAxes = scale * scScene.getVisualizationParameter(PxVisualizationParameter::eCOLLISION_AXES);
const PxReal fscale = scale * fNormals;
const PxTransform32 actorPose(actor.getGlobalPose());
PxBounds3 compoundBounds(PxBounds3::empty());
for(PxU32 i=0;i<nbShapes;i++)
{
const NpShape& npShape = *shapes[i];
PxTransform32 absPose;
aos::transformMultiply<true, true>(absPose, actorPose, npShape.getCore().getShape2Actor());
const PxGeometry& geom = npShape.getCore().getGeometry();
const bool shapeDebugVizEnabled = npShape.getCore().getFlags() & PxShapeFlag::eVISUALIZATION;
const bool needsShapeBounds = needsShapeBounds0 || (visualizeAABBs && shapeDebugVizEnabled);
const PxBounds3 currentShapeBounds = needsShapeBounds ? computeBounds(geom, absPose) : PxBounds3::empty();
if(shapeDebugVizEnabled)
{
if(visualizeAABBs)
{
out << PxU32(PxDebugColor::eARGB_YELLOW) << PxMat44(PxIdentity);
renderOutputDebugBox(out, currentShapeBounds);
}
if(collisionAxes != 0.0f)
{
out << PxMat44(absPose);
Cm::renderOutputDebugBasis(out, PxDebugBasis(PxVec3(collisionAxes), 0xcf0000, 0x00cf00, 0x0000cf));
}
if(visualizeCollision)
{
if(!useCullBox || cullbox.intersects(currentShapeBounds))
::visualize(geom, out, absPose, cullbox, fscale, visualizeShapes, visualizeEdges, useCullBox, visualizeSDFs);
}
}
if(visualizeCompounds)
compoundBounds.include(currentShapeBounds);
}
if(visualizeCompounds && !compoundBounds.isEmpty())
{
out << gCollisionShapeColor << PxMat44(PxIdentity);
renderOutputDebugBox(out, compoundBounds);
}
}
/////
static PX_FORCE_INLINE void visualizeActor(PxRenderOutput& out, const Sc::Scene& scScene, const PxRigidActor& actor, float scale)
{
//visualize actor frames
const PxReal actorAxes = scale * scScene.getVisualizationParameter(PxVisualizationParameter::eACTOR_AXES);
if(actorAxes != 0.0f)
{
out << actor.getGlobalPose();
Cm::renderOutputDebugBasis(out, PxDebugBasis(PxVec3(actorAxes)));
}
}
void physx::visualizeRigidBody(PxRenderOutput& out, NpScene& scene, const PxRigidActor& actor, const Sc::BodyCore& core, float scale)
{
PX_ASSERT(scale!=0.0f); // Else we shouldn't have been called
PX_ASSERT(core.getActorFlags() & PxActorFlag::eVISUALIZATION); // Else we shouldn't have been called
const Sc::Scene& scScene = scene.getScScene();
visualizeActor(out, scScene, actor, scale);
const PxTransform& body2World = core.getBody2World();
const PxReal bodyAxes = scale * scScene.getVisualizationParameter(PxVisualizationParameter::eBODY_AXES);
if(bodyAxes != 0.0f)
{
out << body2World;
Cm::renderOutputDebugBasis(out, PxDebugBasis(PxVec3(bodyAxes)));
}
const PxReal linVelocity = scale * scScene.getVisualizationParameter(PxVisualizationParameter::eBODY_LIN_VELOCITY);
if(linVelocity != 0.0f)
{
out << 0xffffff << PxMat44(PxIdentity);
Cm::renderOutputDebugArrow(out, PxDebugArrow(body2World.p, core.getLinearVelocity() * linVelocity, 0.2f * linVelocity));
}
const PxReal angVelocity = scale * scScene.getVisualizationParameter(PxVisualizationParameter::eBODY_ANG_VELOCITY);
if(angVelocity != 0.0f)
{
out << 0x000000 << PxMat44(PxIdentity);
Cm::renderOutputDebugArrow(out, PxDebugArrow(body2World.p, core.getAngularVelocity() * angVelocity, 0.2f * angVelocity));
}
const PxReal massAxes = scale * scScene.getVisualizationParameter(PxVisualizationParameter::eBODY_MASS_AXES);
if(massAxes != 0.0f)
{
const PxReal sleepTime = core.getWakeCounter() / scene.getWakeCounterResetValueInternal();
PxU32 color = PxU32(0xff * (sleepTime>1.0f ? 1.0f : sleepTime));
color = core.isSleeping() ? 0xff0000 : (color<<16 | color<<8 | color);
PxVec3 dims = invertDiagInertia(core.getInverseInertia());
dims = getDimsFromBodyInertia(dims, 1.0f / core.getInverseMass());
out << color << core.getBody2World();
const PxVec3 extents = dims * 0.5f;
Cm::renderOutputDebugBox(out, PxBounds3(-extents, extents));
}
}
void NpRigidStatic::visualize(PxRenderOutput& out, NpScene& scene, float scale) const
{
PX_ASSERT(scale!=0.0f); // Else we shouldn't have been called
if(!(mCore.getActorFlags() & PxActorFlag::eVISUALIZATION))
return;
NpRigidStaticT::visualize(out, scene, scale);
visualizeActor(out, scene.getScScene(), *this, scale);
}
void NpRigidDynamic::visualize(PxRenderOutput& out, NpScene& scene, float scale) const
{
PX_ASSERT(scale!=0.0f); // Else we shouldn't have been called
if(!(mCore.getActorFlags() & PxActorFlag::eVISUALIZATION))
return;
NpRigidDynamicT::visualize(out, scene, scale);
}
void NpArticulationLink::visualize(PxRenderOutput& out, NpScene& scene, float scale) const
{
PX_ASSERT(scale!=0.0f); // Else we shouldn't have been called
if(!(mCore.getActorFlags() & PxActorFlag::eVISUALIZATION))
return;
NpArticulationLinkT::visualize(out, scene, scale);
const Sc::Scene& scScene = scene.getScScene();
const PxReal frameScale = scale * scScene.getVisualizationParameter(PxVisualizationParameter::eJOINT_LOCAL_FRAMES);
const PxReal limitScale = scale * scScene.getVisualizationParameter(PxVisualizationParameter::eJOINT_LIMITS);
if(frameScale != 0.0f || limitScale != 0.0f)
{
ConstraintImmediateVisualizer viz(frameScale, limitScale, out);
visualizeJoint(viz);
}
}
void NpArticulationLink::visualizeJoint(PxConstraintVisualizer& jointViz) const
{
const NpArticulationLink* parent = getParent();
if(parent)
{
// PT:: tag: scalar transform*transform
PxTransform cA2w = getGlobalPose().transform(mInboundJoint->getChildPose());
// PT:: tag: scalar transform*transform
PxTransform cB2w = parent->getGlobalPose().transform(mInboundJoint->getParentPose());
jointViz.visualizeJointFrames(cA2w, cB2w);
NpArticulationJointReducedCoordinate* impl = static_cast<NpArticulationJointReducedCoordinate*>(mInboundJoint);
PX_ASSERT(getArticulation().getConcreteType() == PxConcreteType::eARTICULATION_REDUCED_COORDINATE);
//(1) visualize any angular dofs/limits...
const PxMat33 cA2w_m(cA2w.q), cB2w_m(cB2w.q);
PxTransform parentFrame = cB2w;
if (cA2w.q.dot(cB2w.q) < 0)
cB2w.q = -cB2w.q;
//const PxTransform cB2cA = cA2w.transformInv(cB2w);
const PxTransform cA2cB = cB2w.transformInv(cA2w);
Sc::ArticulationJointCore& joint = impl->getCore();
if(joint.getMotion(PxArticulationAxis::eTWIST) == PxArticulationMotion::eLIMITED)
{
const PxArticulationLimit pair = joint.getLimit(PxArticulationAxis::Enum(PxArticulationAxis::eTWIST));
jointViz.visualizeAngularLimit(parentFrame, pair.low, pair.high);
}
if (joint.getMotion(PxArticulationAxis::eSWING1) == PxArticulationMotion::eLIMITED)
{
const PxArticulationLimit pair = joint.getLimit(PxArticulationAxis::Enum(PxArticulationAxis::eSWING1));
PxTransform tmp = parentFrame;
tmp.q = tmp.q * PxQuat(-PxPiDivTwo, PxVec3(0.f, 0.f, 1.f));
jointViz.visualizeAngularLimit(tmp, -pair.high, -pair.low);
}
if (joint.getMotion(PxArticulationAxis::eSWING2) == PxArticulationMotion::eLIMITED)
{
const PxArticulationLimit pair = joint.getLimit(PxArticulationAxis::Enum(PxArticulationAxis::eSWING2));
PxTransform tmp = parentFrame;
tmp.q = tmp.q * PxQuat(PxPiDivTwo, PxVec3(0.f, 1.f, 0.f));
jointViz.visualizeAngularLimit(tmp, -pair.high, -pair.low);
}
for (PxU32 i = PxArticulationAxis::eX; i <= PxArticulationAxis::eZ; ++i)
{
if (joint.getMotion(PxArticulationAxis::Enum(i)) == PxArticulationMotion::eLIMITED)
{
const PxU32 index = i - PxArticulationAxis::eX;
const PxArticulationLimit pair = joint.getLimit(PxArticulationAxis::Enum(i));
const PxReal ordinate = cA2cB.p[index];
const PxVec3& origin = cB2w.p;
const PxVec3& axis = cA2w_m[index];
const bool active = ordinate < pair.low || ordinate > pair.high;
const PxVec3 p0 = origin + axis * pair.low;
const PxVec3 p1 = origin + axis * pair.high;
jointViz.visualizeLine(p0, p1, active ? 0xff0000u : 0xffffffu);
}
}
}
}
void NpArticulationReducedCoordinate::visualize(PxRenderOutput& out, NpScene& scene, float scale) const
{
const PxU32 nbLinks = mArticulationLinks.size();
for (PxU32 i = 0; i < nbLinks; i++)
mArticulationLinks[i]->visualize(out, scene, scale);
}
#else
PX_CATCH_UNDEFINED_ENABLE_DEBUG_VISUALIZATION
#endif
void NpScene::visualize()
{
PX_PROFILE_ZONE("NpScene::visualize", getContextId());
NP_READ_CHECK(this);
mRenderBuffer.clear(); // clear last frame visualizations
#if PX_ENABLE_DEBUG_VISUALIZATION
const PxReal scale = mScene.getVisualizationParameter(PxVisualizationParameter::eSCALE);
if(scale == 0.0f)
return;
PxRenderOutput out(mRenderBuffer);
// Visualize scene axes
const PxReal worldAxes = scale * mScene.getVisualizationParameter(PxVisualizationParameter::eWORLD_AXES);
if(worldAxes != 0)
Cm::renderOutputDebugBasis(out, PxDebugBasis(PxVec3(worldAxes)));
// Visualize articulations
const PxU32 articulationCount = mArticulations.size();
for(PxU32 i=0;i<articulationCount;i++)
static_cast<const NpArticulationReducedCoordinate *>(mArticulations.getEntries()[i])->visualize(out, *this, scale);
// Visualize rigid actors
const PxU32 rigidDynamicCount = mRigidDynamics.size();
for(PxU32 i=0; i<rigidDynamicCount; i++)
mRigidDynamics[i]->visualize(out, *this, scale);
const PxU32 rigidStaticCount = mRigidStatics.size();
for(PxU32 i=0; i<rigidStaticCount; i++)
mRigidStatics[i]->visualize(out, *this, scale);
// Visualize pruning structures
const bool visStatic = mScene.getVisualizationParameter(PxVisualizationParameter::eCOLLISION_STATIC) != 0.0f;
const bool visDynamic = mScene.getVisualizationParameter(PxVisualizationParameter::eCOLLISION_DYNAMIC) != 0.0f;
//flushQueryUpdates(); // DE7834
if(visStatic)
getSQAPI().visualize(PxU32(PX_SCENE_PRUNER_STATIC), out);
if(visDynamic)
getSQAPI().visualize(PxU32(PX_SCENE_PRUNER_DYNAMIC), out);
if(visStatic || visDynamic)
getSQAPI().visualize(PxU32(PX_SCENE_COMPOUND_PRUNER), out);
if(mScene.getVisualizationParameter(PxVisualizationParameter::eMBP_REGIONS) != 0.0f)
{
out << PxTransform(PxIdentity);
const Bp::BroadPhase* bp = mScene.getAABBManager()->getBroadPhase();
const PxU32 nbRegions = bp->getNbRegions();
for(PxU32 i=0;i<nbRegions;i++)
{
PxBroadPhaseRegionInfo info;
bp->getRegions(&info, 1, i);
if(info.mActive)
out << PxU32(PxDebugColor::eARGB_YELLOW);
else
out << PxU32(PxDebugColor::eARGB_BLACK);
Cm::renderOutputDebugBox(out, info.mRegion.mBounds);
}
}
if(mScene.getVisualizationParameter(PxVisualizationParameter::eCULL_BOX)!=0.0f)
{
const PxBounds3& cullbox = mScene.getVisualizationCullingBox();
if(!cullbox.isEmpty())
{
out << PxU32(PxDebugColor::eARGB_YELLOW);
Cm::renderOutputDebugBox(out, cullbox);
}
}
#if PX_SUPPORT_GPU_PHYSX
// Visualize particle systems
{
{
PxPBDParticleSystem*const* particleSystems = mPBDParticleSystems.getEntries();
const PxU32 particleSystemCount = mPBDParticleSystems.size();
for (PxU32 i = 0; i < particleSystemCount; i++)
static_cast<NpPBDParticleSystem*>(particleSystems[i])->visualize(out, *this);
}
}
// TODO Visualize deformable surfaces
#endif
#if PX_SUPPORT_PVD
mScenePvdClient.visualize(mRenderBuffer);
#endif
#else
PX_CATCH_UNDEFINED_ENABLE_DEBUG_VISUALIZATION
#endif
}