C++ Matrix34::rotate方法代码示例

bool Gu::SweepBoxTriangles(	PxU32 nbTris, const PxTriangle* triangles, bool isDoubleSided,
							const PxBoxGeometry& boxGeom, const PxTransform& boxPose, const PxVec3& dir, const PxReal length, PxVec3& _hit,
							PxVec3& _normal, float& _d, PxU32& _index, const PxU32* cachedIndex, const PxReal inflation, PxHitFlags hintFlags)
{
	PX_UNUSED(hintFlags);

	if(!nbTris)
		return false;

	const bool meshBothSides = hintFlags & PxHitFlag::eMESH_BOTH_SIDES;
	const bool doBackfaceCulling = !isDoubleSided && !meshBothSides;

	Box box;
	buildFrom1(box, boxPose.p, boxGeom.halfExtents, boxPose.q);

	PxSweepHit sweepHit;
	// Move to AABB space
	Matrix34 worldToBox;
	computeWorldToBoxMatrix(worldToBox, box);

	const PxVec3 localDir = worldToBox.rotate(dir);
	const PxVec3 localMotion = localDir * length;

	const Vec3V base0 = V3LoadU(worldToBox.base0);
	const Vec3V base1 = V3LoadU(worldToBox.base1);
	const Vec3V base2 = V3LoadU(worldToBox.base2);
	const Mat33V matV(base0, base1, base2);
	const Vec3V p	  = V3LoadU(worldToBox.base3);
	const PsMatTransformV worldToBoxV(p, matV);

	const FloatV zero = FZero();
	const Vec3V zeroV = V3Zero();
	const BoolV bTrue = BTTTT();
	const Vec3V boxExtents = V3LoadU(box.extents);
	const Vec3V boxDir = V3LoadU(localDir);
	const FloatV inflationV = FLoad(inflation);
	const Vec3V absBoxDir = V3Abs(boxDir);
	const FloatV boxRadiusV = FAdd(V3Dot(absBoxDir, boxExtents), inflationV);
	BoxV boxV(zeroV, boxExtents);

#ifdef PX_DEBUG
	PxU32 totalTestsExpected = nbTris;
	PxU32 totalTestsReal = 0;
	PX_UNUSED(totalTestsExpected);
	PX_UNUSED(totalTestsReal);
#endif

	Vec3V boxLocalMotion = V3LoadU(localMotion);
	Vec3V minClosestA = zeroV, minNormal = zeroV;
	PxU32 minTriangleIndex = 0;
	PxVec3 bestTriNormal(0.0f);
	FloatV dist = FLoad(length);

	const PsTransformV boxPos = loadTransformU(boxPose);

	bool status = false;

	const PxU32 idx = cachedIndex ? *cachedIndex : 0;

	for(PxU32 ii=0;ii<nbTris;ii++)
	{
		const PxU32 triangleIndex = getTriangleIndex(ii, idx);

		const Vec3V localV0 =  V3LoadU(triangles[triangleIndex].verts[0]);
		const Vec3V localV1 =  V3LoadU(triangles[triangleIndex].verts[1]);
		const Vec3V localV2 =  V3LoadU(triangles[triangleIndex].verts[2]);

		const Vec3V triV0 = worldToBoxV.transform(localV0);
		const Vec3V triV1 = worldToBoxV.transform(localV1);
		const Vec3V triV2 = worldToBoxV.transform(localV2);

		const Vec3V triNormal = V3Cross(V3Sub(triV2, triV1),V3Sub(triV0, triV1)); 

		if(doBackfaceCulling && FAllGrtrOrEq(V3Dot(triNormal, boxLocalMotion), zero)) // backface culling
			continue;

		const FloatV dp0 = V3Dot(triV0, boxDir);
		const FloatV dp1 = V3Dot(triV1, boxDir);
		const FloatV dp2 = V3Dot(triV2, boxDir);
		
		const FloatV dp = FMin(dp0, FMin(dp1, dp2));

		const Vec3V dpV = V3Merge(dp0, dp1, dp2);

		const FloatV temp1 = FAdd(boxRadiusV, dist);
		const BoolV con0 = FIsGrtr(dp, temp1);
		const BoolV con1 = V3IsGrtr(zeroV, dpV);

		if(BAllEq(BOr(con0, con1), bTrue))
			continue;

#ifdef PX_DEBUG
		totalTestsReal++;
#endif

		TriangleV triangleV(triV0, triV1, triV2);
		
		FloatV lambda;   
		Vec3V closestA, normal;//closestA and normal is in the local space of convex hull
		bool hit  = GJKLocalRayCast(triangleV, boxV, zero, zeroV, boxLocalMotion, lambda, normal, closestA, inflation, false); 
//.........这里部分代码省略.........

// PT: test: new version for CCT, based on code for general sweeps. Just to check it works or not with rotations
// TODO: refactor this and the similar code in sweptBox for box-vs-mesh. Not so easy though.
static bool sweepBoxVsTriangles(PxU32 nbTris, const PxTriangle* triangles, const Box& box, const PxVec3& unitDir, const PxReal distance, PxSweepHit& sweepHit,
								PxHitFlags hintFlags, bool isDoubleSided, const PxU32* cachedIndex)
{
	if(!nbTris)
		return false;

	const bool meshBothSides = hintFlags & PxHitFlag::eMESH_BOTH_SIDES;
	const bool doBackfaceCulling = !isDoubleSided && !meshBothSides;

	// Move to AABB space
	Matrix34 worldToBox;
	computeWorldToBoxMatrix(worldToBox, box);

	const PxVec3 localDir = worldToBox.rotate(unitDir);
	const PxVec3 localMotion = localDir * distance;

	bool status = false;
	sweepHit.distance = distance;	//was PX_MAX_F32, but that may trigger an assert in the caller!

	const PxVec3 oneOverMotion(
		localDir.x!=0.0f ? 1.0f/localMotion.x : 0.0f,
		localDir.y!=0.0f ? 1.0f/localMotion.y : 0.0f,
		localDir.z!=0.0f ? 1.0f/localMotion.z : 0.0f);

// PT: experimental code, don't clean up before I test it more and validate it

// Project box
/*float boxRadius0 =
			PxAbs(dir.x) * box.extents.x
		+	PxAbs(dir.y) * box.extents.y
		+	PxAbs(dir.z) * box.extents.z;*/

float boxRadius =
			PxAbs(localDir.x) * box.extents.x
		+	PxAbs(localDir.y) * box.extents.y
		+	PxAbs(localDir.z) * box.extents.z;

if(gValidateBoxRadiusComputation)	// PT: run this to check the box radius is correctly computed
{
	PxVec3 boxVertices2[8];
	box.computeBoxPoints(boxVertices2);
	float dpmin = FLT_MAX;
	float dpmax = -FLT_MAX;
	for(int i=0;i<8;i++)
	{
		const float dp = boxVertices2[i].dot(unitDir);
		if(dp<dpmin)	dpmin = dp;
		if(dp>dpmax)	dpmax = dp;
	}
	const float goodRadius = (dpmax-dpmin)/2.0f;
	PX_UNUSED(goodRadius);
}

const float dpc0 = box.center.dot(unitDir);
float localMinDist = 1.0f;
#ifdef PX_DEBUG
	PxU32 totalTestsExpected = nbTris;
	PxU32 totalTestsReal = 0;
	PX_UNUSED(totalTestsExpected);
	PX_UNUSED(totalTestsReal);
#endif

	const PxU32 idx = cachedIndex ? *cachedIndex : 0;

	PxVec3 bestTriNormal(0.0f);

	for(PxU32 ii=0;ii<nbTris;ii++)
	{
		const PxU32 triangleIndex = getTriangleIndex(ii, idx);

		const PxTriangle& tri = triangles[triangleIndex];
#ifdef _XBOX
		if(cullTriangle(tri, unitDir, boxRadius, localMinDist*distance, dpc0)==0.0f)
			continue;
#else
		if(!cullTriangle(tri, unitDir, boxRadius, localMinDist*distance, dpc0))
			continue;
#endif

#ifdef PX_DEBUG
		totalTestsReal++;
#endif
		// Move to box space
		const PxTriangle currentTriangle(
			worldToBox.transform(tri.verts[0]),
			worldToBox.transform(tri.verts[1]),
			worldToBox.transform(tri.verts[2]));

		PxF32 t = PX_MAX_F32;		// could be better!
		if(triBoxSweepTestBoxSpace(currentTriangle, box.extents, localMotion, oneOverMotion, localMinDist, t, doBackfaceCulling))
		{
			if(t <= localMinDist)
			{
				// PT: test if shapes initially overlap
				if(t==0.0f)
				{
					sweepHit.flags		= PxHitFlag::eDISTANCE|PxHitFlag::eNORMAL;
					sweepHit.distance	= 0.0f;
//.........这里部分代码省略.........