Beispiel #1
0
/** 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]));	
					}
				}
			}
		}