Beispiel #1
0
void Gu::BoxV::create(const Gu::CapsuleV& capsule)
{
	using namespace Ps::aos;
	const FloatV half = FloatV_From_F32(0.5f);
	const FloatV cRadius = capsule.radius;
	// Box center = center of the two LSS's endpoints
	center = capsule.getCenter();

	const Vec3V dir = capsule.computeDirection(); //capsule.p1 - capsule.p0;
	const FloatV d = V3Length(dir);
	rot.col0 = V3Scale(dir, FRecip(d));

	//Box extent
	extents = cRadius;
	V3SetX(extents, FMulAdd(d, half, cRadius));

	// Box orientation
	computeBasis2(rot.col0, rot.col1, rot.col2);
}
bool calculateMTD(const Gu::CapsuleV& capsuleV, const Ps::aos::FloatVArg inflatedRadiusV, const bool isDoubleSide, const MTDTriangle* triangles, const PxU32 nbTriangles, const PxU32 startIndex, Gu::MeshPersistentContact* manifoldContacts, 
				  PxU32& numContacts, Ps::aos::Vec3V& normal, Ps::aos::Vec3V& closestA, Ps::aos::Vec3V& closestB, PxU32& faceIndex, Ps::aos::FloatV& mtd)
{
	using namespace Ps::aos;
	const FloatV zero = FZero();
	bool hadContacts = false;
	FloatV deepestPen = mtd;

	for(PxU32 j=0; j<nbTriangles; ++j)
	{
		numContacts = 0;

		const MTDTriangle& curTri = triangles[j];
		TriangleV triangleV;
		triangleV.verts[0] = V3LoadU(curTri.verts[0]);
		triangleV.verts[1] = V3LoadU(curTri.verts[1]);
		triangleV.verts[2] = V3LoadU(curTri.verts[2]);
		const PxU8 triFlag = curTri.extraTriData;

		const Vec3V triangleNormal = triangleV.normal();
		const Vec3V v = V3Sub(capsuleV.getCenter(), triangleV.verts[0]);
		const FloatV dotV = V3Dot(triangleNormal, v);

		// Backface culling
		const bool culled = !isDoubleSide && (FAllGrtr(zero, dotV));
		if(culled)
			continue;
		
		Gu::PCMCapsuleVsMeshContactGeneration::processTriangle(triangleV, j+startIndex, capsuleV, inflatedRadiusV, triFlag, manifoldContacts, numContacts);

		if(numContacts ==0)
			continue;

		hadContacts = true;

		getMTDPerTriangle(manifoldContacts, numContacts, j + startIndex, normal, closestA, closestB, faceIndex, deepestPen);


	}

	mtd = deepestPen;
	return hadContacts;
}
bool physx::Gu::computeCapsule_HeightFieldMTD(const PxHeightFieldGeometry& heightFieldGeom, const PxTransform& pose, Gu::CapsuleV& capsuleV, const PxReal inflatedRadius, const PxReal distance, const bool isDoubleSided, const PxU32 flags, PxSweepHit& hit)
{

	using namespace Ps::aos;

	const Vec3V zeroV = V3Zero();

	Gu::MeshPersistentContact manifoldContacts[64]; 
	PxU32 numContacts = 0;
	FloatV distV = FLoad(distance);
	//inflated the capsule by 1% in case of some disgreement between sweep and mtd calculation.If sweep said initial overlap, but mtd has a positive seperation,
	//we are still be able to return a valid normal but we should zero the distance.
	const FloatV inflatedRadiusV = FLoad(inflatedRadius*1.01f);  
	
	bool foundInitial = false;
	static PxU32 iterations = 4;

	LocalContainer(tempContainer, 128);

	Gu::HeightFieldUtil hfUtil(heightFieldGeom);


	Vec3V closestA=zeroV, closestB=zeroV, normal=zeroV;
	PxU32 triangleIndex = 0xfffffff;

	Vec3V translation = zeroV;
	FloatV mtd;
	for(PxU32 i=0; i<iterations; ++i)
	{
		tempContainer.Reset();
		Gu::Capsule inflatedCapsule;
		V3StoreU(capsuleV.p0, inflatedCapsule.p0);
		V3StoreU(capsuleV.p1, inflatedCapsule.p1);
		inflatedCapsule.radius = inflatedRadius;

		Box capsuleBox;
		computeBoxAroundCapsule(inflatedCapsule, capsuleBox);
		const PxTransform capsuleBoxTransform = capsuleBox.getTransform();
		PxBounds3 bounds = PxBounds3::poseExtent(capsuleBoxTransform, capsuleBox.extents);
		midPhaseQueryHF(hfUtil, pose, bounds, tempContainer, flags);

		// Get results
		PxU32 nbTriangles = tempContainer.GetNbEntries();

		if(!nbTriangles)
			break;


		PxU32* indices = tempContainer.GetEntries();

		bool hadContacts = false;

		PxU32 nbBatches = (nbTriangles + BATCH_TRIANGLE_NUMBER - 1)/BATCH_TRIANGLE_NUMBER;
		mtd = FMax();
		MTDTriangle triangles[BATCH_TRIANGLE_NUMBER];
		for(PxU32 a = 0; a < nbBatches; ++a)
		{
			PxU32 startIndex = a * BATCH_TRIANGLE_NUMBER;
			PxU32 nbTrigs = PxMin(nbTriangles - startIndex, BATCH_TRIANGLE_NUMBER);
			for(PxU32 k=0; k<nbTrigs; k++)
			{
				//triangle vertext space
				const PxU32 triangleIndex1 = indices[startIndex+k];
				hfUtil.getTriangle(pose, triangles[k], NULL, NULL, triangleIndex1, true);
				triangles[k].extraTriData = 0x38;
			}

			//ML: mtd has back face culling, so if the capsule's center is below the triangle, we won't generate any contacts
			hadContacts = calculateMTD(capsuleV, inflatedRadiusV, isDoubleSided, triangles, nbTrigs, startIndex, manifoldContacts, numContacts, normal, closestA, closestB, triangleIndex, mtd) || hadContacts;
		}


		if(!hadContacts)
			break;

		triangleIndex = indices[triangleIndex];

		foundInitial = true;

		distV = FSub(mtd, capsuleV.radius);
		if(FAllGrtrOrEq(FZero(), distV))
		{
			//move the capsule to depenerate it
			Vec3V center = capsuleV.getCenter();
			Vec3V t = V3Scale(normal, distV);
			translation = V3Sub(translation, t);
			center = V3Sub(center, t);
			capsuleV.setCenter(center);
		}
		else
		{
			if(i == 0)
			{
				//First iteration so keep this normal
				hit.distance = 0.f;
				V3StoreU(closestA, hit.position);
				V3StoreU(normal, hit.normal);
				hit.faceIndex = triangleIndex;
				return true;
			}
			break;
		}
	}

	normal = V3Normalize(translation);
	distV = FNeg(V3Length(translation));
	if(foundInitial)
	{
		FStore(distV, &hit.distance);
		V3StoreU(closestA, hit.position);
		V3StoreU(normal, hit.normal);
		hit.faceIndex = triangleIndex;
	}

	return foundInitial;


}