/** Add any debug lines from the physics scene to the supplied line batcher. */
void FPhysScene::AddDebugLines(uint32 SceneType, class ULineBatchComponent* LineBatcherToUse)
{
check(SceneType < NumPhysScenes);
if (LineBatcherToUse)
{
#if WITH_PHYSX
// Render PhysX debug data
PxScene* PScene = GetPhysXScene(SceneType);
const PxRenderBuffer& DebugData = PScene->getRenderBuffer();
BatchPxRenderBufferLines(*LineBatcherToUse, DebugData);
#if WITH_APEX
// Render APEX debug data
NxApexScene* ApexScene = GetApexScene(SceneType);
const PxRenderBuffer* RenderBuffer = ApexScene->getRenderBuffer();
if (RenderBuffer != NULL)
{
BatchPxRenderBufferLines(*LineBatcherToUse, *RenderBuffer);
ApexScene->updateRenderResources();
}
#endif // WITH_APEX
#endif // WITH_PHYSX
}
}
// this code is shared between capsule vertices and sphere
bool GuContactSphereHeightFieldShared(GU_CONTACT_METHOD_ARGS, bool isCapsule)
{
#if 1
PX_UNUSED(cache);
PX_UNUSED(renderOutput);
// Get actual shape data
const PxSphereGeometry& shapeSphere = shape0.get<const PxSphereGeometry>();
const PxHeightFieldGeometryLL& hfGeom = shape1.get<const PxHeightFieldGeometryLL>();
const Gu::HeightField& hf = *static_cast<Gu::HeightField*>(hfGeom.heightField);
const Gu::HeightFieldUtil hfUtil(hfGeom, hf);
const PxReal radius = shapeSphere.radius;
const PxReal eps = PxReal(0.1) * radius;
const PxVec3 sphereInHfShape = transform1.transformInv(transform0.p);
PX_ASSERT(isCapsule || contactBuffer.count==0);
const PxReal oneOverRowScale = hfUtil.getOneOverRowScale();
const PxReal oneOverColumnScale = hfUtil.getOneOverColumnScale();
// check if the sphere is below the HF surface
if (hfUtil.isShapePointOnHeightField(sphereInHfShape.x, sphereInHfShape.z))
{
PxReal fracX, fracZ;
const PxU32 vertexIndex = hfUtil.getHeightField().computeCellCoordinates(sphereInHfShape.x * oneOverRowScale, sphereInHfShape.z * oneOverColumnScale, fracX, fracZ);
// The sphere origin projects within the bounds of the heightfield in the X-Z plane
// const PxReal sampleHeight = hfShape.getHeightAtShapePoint(sphereInHfShape.x, sphereInHfShape.z);
const PxReal sampleHeight = hfUtil.getHeightAtShapePoint2(vertexIndex, fracX, fracZ);
const PxReal deltaHeight = sphereInHfShape.y - sampleHeight;
//if (hfShape.isDeltaHeightInsideExtent(deltaHeight, eps))
if (hf.isDeltaHeightInsideExtent(deltaHeight, eps))
{
// The sphere origin is 'below' the heightfield surface
// Actually there is an epsilon involved to make sure the
// 'above' surface calculations can deliver a good normal
const PxU32 faceIndex = hfUtil.getFaceIndexAtShapePointNoTest2(vertexIndex, fracX, fracZ);
if (faceIndex != 0xffffffff)
{
//hfShape.getAbsPoseFast().M.getColumn(1, hfShapeUp);
const PxVec3 hfShapeUp = transform1.q.getBasisVector1();
if (hf.getThicknessFast() <= 0)
contactBuffer.contact(transform0.p, hfShapeUp, deltaHeight-radius, faceIndex);
else
contactBuffer.contact(transform0.p, -hfShapeUp, -deltaHeight-radius, faceIndex);
return true;
}
return false;
}
}
const PxReal epsSqr = eps * eps;
const PxReal inflatedRadius = radius + params.mContactDistance;
const PxReal inflatedRadiusSquared = inflatedRadius * inflatedRadius;
const PxVec3 sphereInHF = hfUtil.shape2hfp(sphereInHfShape);
const PxReal radiusOverRowScale = inflatedRadius * PxAbs(oneOverRowScale);
const PxReal radiusOverColumnScale = inflatedRadius * PxAbs(oneOverColumnScale);
const PxU32 minRow = hf.getMinRow(sphereInHF.x - radiusOverRowScale);
const PxU32 maxRow = hf.getMaxRow(sphereInHF.x + radiusOverRowScale);
const PxU32 minColumn = hf.getMinColumn(sphereInHF.z - radiusOverColumnScale);
const PxU32 maxColumn = hf.getMaxColumn(sphereInHF.z + radiusOverColumnScale);
// this assert is here because the following code depends on it for reasonable performance for high-count situations
PX_COMPILE_TIME_ASSERT(ContactBuffer::MAX_CONTACTS == 64);
const PxU32 nbColumns = hf.getNbColumnsFast();
#define HFU Gu::HeightFieldUtil
PxU32 numFaceContacts = 0;
for (PxU32 i = 0; i<2; i++)
{
const bool facesOnly = (i == 0);
// first we go over faces-only meaning only contacts directly in Voronoi regions of faces
// at second pass we consider edges and vertices and clamp the normals to adjacent feature's normal
// if there was a prior contact. it is equivalent to clipping the normal to it's feature's Voronoi region
for (PxU32 r = minRow; r < maxRow; r++)
{
for (PxU32 c = minColumn; c < maxColumn; c++)
{
// x--x--x
// | x |
// x x x
// | x |
// x--x--x
PxVec3 closestPoints[11];
PxU32 closestFeatures[11];
PxU32 npcp = hfUtil.findClosestPointsOnCell(
r, c, sphereInHfShape, closestPoints, closestFeatures, facesOnly, !facesOnly, true);
for(PxU32 pi = 0; pi < npcp; pi++)
{
PX_ASSERT(closestFeatures[pi] != 0xffffffff);
const PxVec3 d = sphereInHfShape - closestPoints[pi];
if (hf.isDeltaHeightOppositeExtent(d.y)) // See if we are 'above' the heightfield
{
const PxReal dMagSq = d.magnitudeSquared();
if (dMagSq > inflatedRadiusSquared)
// Too far above
continue;
PxReal dMag = -1.0f; // dMag is sqrt(sMadSq) and comes up as a byproduct of other calculations in computePointNormal
PxVec3 n; // n is in world space, rotated by transform1
PxU32 featureType = HFU::getFeatureType(closestFeatures[pi]);
if (featureType == HFU::eEDGE)
{
PxU32 edgeIndex = HFU::getFeatureIndex(closestFeatures[pi]);
PxU32 adjFaceIndices[2];
const PxU32 adjFaceCount = hf.getEdgeTriangleIndices(edgeIndex, adjFaceIndices);
PxVec3 origin;
PxVec3 direction;
const PxU32 vertexIndex = edgeIndex / 3;
const PxU32 row = vertexIndex / nbColumns;
const PxU32 col = vertexIndex % nbColumns;
hfUtil.getEdge(edgeIndex, vertexIndex, row, col, origin, direction);
n = hfUtil.computePointNormal(
hfGeom.heightFieldFlags, d, transform1, dMagSq,
closestPoints[pi].x, closestPoints[pi].z, epsSqr, dMag);
PxVec3 localN = transform1.rotateInv(n);
// clamp the edge's normal to its Voronoi region
for (PxU32 j = 0; j < adjFaceCount; j++)
{
const PxVec3 adjNormal = hfUtil.hf2shapen(hf.getTriangleNormalInternal(adjFaceIndices[j])).getNormalized();
PxU32 triCell = adjFaceIndices[j] >> 1;
PxU32 triRow = triCell/hf.getNbColumnsFast();
PxU32 triCol = triCell%hf.getNbColumnsFast();
PxVec3 tv0, tv1, tv2, tvc;
hf.getTriangleVertices(adjFaceIndices[j], triRow, triCol, tv0, tv1, tv2);
tvc = hfUtil.hf2shapep((tv0+tv1+tv2)/3.0f); // compute adjacent triangle center
PxVec3 perp = adjNormal.cross(direction).getNormalized(); // adj face normal cross edge dir
if (perp.dot(tvc-origin) < 0.0f) // make sure perp is pointing toward the center of the triangle
perp = -perp;
// perp is now a vector sticking out of the edge of the triangle (also the test edge) pointing toward the center
// perpendicular to the normal (in triangle plane)
if (perp.dot(localN) > 0.0f) // if the normal is in perp halfspace, clamp it to Voronoi region
{
n = transform1.rotate(adjNormal);
break;
}
}
} else if(featureType == HFU::eVERTEX)
{
// AP: these contacts are rare so hopefully it's ok
const PxU32 bufferCount = contactBuffer.count;
const PxU32 vertIndex = HFU::getFeatureIndex(closestFeatures[pi]);
EdgeData adjEdges[8];
const PxU32 row = vertIndex / nbColumns;
const PxU32 col = vertIndex % nbColumns;
const PxU32 numAdjEdges = ::getVertexEdgeIndices(hf, vertIndex, row, col, adjEdges);
for (PxU32 iPrevEdgeContact = numFaceContacts; iPrevEdgeContact < bufferCount; iPrevEdgeContact++)
{
if (contactBuffer.contacts[iPrevEdgeContact].forInternalUse != HFU::eEDGE)
continue; // skip non-edge contacts (can be other vertex contacts)
for (PxU32 iAdjEdge = 0; iAdjEdge < numAdjEdges; iAdjEdge++)
// does adjacent edge index for this vertex match a previously encountered edge index?
if (adjEdges[iAdjEdge].edgeIndex == contactBuffer.contacts[iPrevEdgeContact].internalFaceIndex1)
{
// if so, clamp the normal for this vertex to that edge's normal
n = contactBuffer.contacts[iPrevEdgeContact].normal;
dMag = PxSqrt(dMagSq);
break;
}
}
}
if (dMag == -1.0f)
n = hfUtil.computePointNormal(hfGeom.heightFieldFlags, d, transform1,
dMagSq, closestPoints[pi].x, closestPoints[pi].z, epsSqr, dMag);
PxVec3 p = transform0.p - n * radius;
#if DEBUG_RENDER_HFCONTACTS
printf("n=%.5f %.5f %.5f; ", n.x, n.y, n.z);
if (n.y < 0.8f)
int a = 1;
PxScene *s; PxGetPhysics().getScenes(&s, 1, 0);
Cm::RenderOutput((Cm::RenderBuffer&)s->getRenderBuffer()) << Cm::RenderOutput::LINES << PxDebugColor::eARGB_BLUE // red
<< p << (p + n * 10.0f);
#endif
// temporarily use the internalFaceIndex0 slot in the contact buffer for featureType
contactBuffer.contact(
p, n, dMag - radius, PxU16(featureType), HFU::getFeatureIndex(closestFeatures[pi]));
}
}
}
}