C++ PxTransform::getInverse方法代码示例

static PxMat44 convertViewMatrix(const PxTransform& eye)
{
	PxTransform viewMatrix = eye.getInverse();
	PxMat44 mat44 = PxMat44(viewMatrix).getTranspose();

	float m[16];
	memcpy(m, mat44.front(), sizeof m);

	PxMat44 view44;
	view44.column0.x = m[0];
	view44.column0.y = m[1];
	view44.column0.z = m[2];
	view44.column0.w = m[3];
	view44.column1.x = m[4];
	view44.column1.y = m[5];
	view44.column1.z = m[6];
	view44.column1.w = m[7];
	view44.column2.x = m[8];
	view44.column2.y = m[9];
	view44.column2.z = m[10];
	view44.column2.w = m[11];
	view44.column3.x = m[12];
	view44.column3.y = m[13];
	view44.column3.z = m[14];
	view44.column3.w = m[15];

PxMat44 tmpmat = view44.getTranspose(); view44 = tmpmat;

	return view44;
}

void physx::Pt::collideCellsWithStaticMesh(ParticleCollData* collData, const LocalCellHash& localCellHash,
                                           const GeometryUnion& meshShape, const PxTransform& world2Shape,
                                           const PxTransform& shape2World, PxReal /*cellSize*/,
                                           PxReal /*collisionRange*/, PxReal proxRadius, const PxVec3& /*packetCorner*/)
{
	PX_ASSERT(collData);
	PX_ASSERT(localCellHash.isHashValid);
	PX_ASSERT(localCellHash.numParticles <= PT_SUBPACKET_PARTICLE_LIMIT_COLLISION);
	PX_ASSERT(localCellHash.numHashEntries <= PT_LOCAL_HASH_SIZE_MESH_COLLISION);

	const PxTriangleMeshGeometryLL& meshShapeData = meshShape.get<const PxTriangleMeshGeometryLL>();

	const TriangleMesh* meshData = meshShapeData.meshData;
	PX_ASSERT(meshData);

	// mesh bounds in world space (conservative)
	const PxBounds3 shapeBounds =
	    meshData->getLocalBoundsFast().transformSafe(world2Shape.getInverse() * meshShapeData.scale);

	const bool idtScaleMesh = meshShapeData.scale.isIdentity();

	Cm::FastVertex2ShapeScaling meshScaling;
	if(!idtScaleMesh)
		meshScaling.init(meshShapeData.scale);

	// process the particle cells
	for(PxU32 c = 0; c < localCellHash.numHashEntries; c++)
	{
		const ParticleCell& cell = localCellHash.hashEntries[c];

		if(cell.numParticles == PX_INVALID_U32)
			continue;

		PxBounds3 cellBounds;

		cellBounds.setEmpty();
		PxBounds3 cellBoundsNew(PxBounds3::empty());

		PxU32* it = localCellHash.particleIndices + cell.firstParticle;
		const PxU32* end = it + cell.numParticles;
		for(; it != end; it++)
		{
			const ParticleCollData& particle = collData[*it];
			cellBounds.include(particle.oldPos);
			cellBoundsNew.include(particle.newPos);
		}
		PX_ASSERT(!cellBoundsNew.isEmpty());
		cellBoundsNew.fattenFast(proxRadius);
		cellBounds.include(cellBoundsNew);

		if(!cellBounds.intersects(shapeBounds))
			continue; // early out if (inflated) cell doesn't intersect mesh bounds

		// opcode query: cell bounds against shape bounds in unscaled mesh space
		PxcContactCellMeshCallback callback(collData, &(localCellHash.particleIndices[cell.firstParticle]),
		                                    cell.numParticles, *meshData, meshScaling, proxRadius, NULL, shape2World);
		testBoundsMesh(*meshData, world2Shape, meshScaling, idtScaleMesh, cellBounds, callback);
	}
}

static bool computeMassAndDiagInertia(Ext::InertiaTensorComputer& inertiaComp, 
		PxVec3& diagTensor, PxQuat& orient, PxReal& massOut, PxVec3& coM, bool lockCOM, const PxRigidBody& body, const char* errorStr)
{
	// The inertia tensor and center of mass is relative to the actor at this point. Transform to the
	// body frame directly if CoM is specified, else use computed center of mass
	if (lockCOM)
	{
		inertiaComp.translate(-coM);  // base the tensor on user's desired center of mass.
	}
	else
	{
		//get center of mass - has to be done BEFORE centering.
		coM = inertiaComp.getCenterOfMass();

		//the computed result now needs to be centered around the computed center of mass:
		inertiaComp.center();
	}
	// The inertia matrix is now based on the body's center of mass desc.massLocalPose.p
	
	massOut = inertiaComp.getMass();
	diagTensor = PxDiagonalize(inertiaComp.getInertia(), orient);

	if ((diagTensor.x > 0.0f) && (diagTensor.y > 0.0f) && (diagTensor.z > 0.0f))
		return true;
	else
	{
		Ps::getFoundation().error(PxErrorCode::eDEBUG_WARNING, __FILE__, __LINE__, 
								"%s: inertia tensor has negative components (ill-conditioned input expected). Approximation for inertia tensor will be used instead.", errorStr);

		// keep center of mass but use the AABB as a crude approximation for the inertia tensor
		PxBounds3 bounds = body.getWorldBounds();
		PxTransform pose = body.getGlobalPose();
		bounds = PxBounds3::transformFast(pose.getInverse(), bounds);
		Ext::InertiaTensorComputer it(false);
		it.setBox(bounds.getExtents());
		it.scaleDensity(massOut / it.getMass());
		PxMat33 inertia = it.getInertia();
		diagTensor = PxVec3(inertia.column0.x, inertia.column1.y, inertia.column2.z);
		orient = PxQuat(PxIdentity);

		return true;
	}
}

bool
Skin::bindToCharacter(Character &character, SampleArray<PxVec4> &positions)
{
	// currently we just bind everything to the 'thorax' (between neck and clavicles).
	// Modify this if you need to do more elaborate skin binding

	mBindPos.resize(positions.size());
	Acclaim::Bone* bone = getBoneFromName(*character.mASFData, "thorax");
	if (bone == NULL)
		return false;

	PxTransform boneTransform = computeBoneTransformRest(*bone);
	PxTransform boneTransformInv = boneTransform.getInverse();

	mBoneID = bone->mID - 1;

	for (PxU32 i = 0; i < positions.size(); i++)
	{
		mBindPos[i] = boneTransformInv.transform(
			reinterpret_cast<const PxVec3&>(positions[i]));
	}

	return true;
}

bool doRaycastCCD(PxShape* shape, PxTransform& newPose, PxVec3& newShapeCenter, const PxVec3& ccdWitness, const PxVec3& ccdWitnessOffset)
{
	PxRigidDynamic* dyna = canDoCCD(shape);
	if(!dyna)
		return true;

	bool updateCCDWitness = true;

	const PxVec3 offset = newPose.p - newShapeCenter;
//printf("CCD0: %f | %f | %f\n", newShapeCenter.x, newShapeCenter.y, newShapeCenter.z);
	const PxVec3& origin = ccdWitness;
//			const PxVec3& dest = newPose.p;
	const PxVec3& dest = newShapeCenter;

	PxVec3 dir = dest - origin;
	const PxReal length = dir.magnitude();
	if(length!=0.0f)
	{
		dir /= length;

		// Compute internal radius
//		PxVec3 localCenter;
		const PxReal internalRadius = computeInternalRadius(shape, dir, /*localCenter,*/ ccdWitnessOffset);

		// Compute distance to impact
		PxRaycastHit hit;
//		if(internalRadius!=0.0f && CCDRaycast(shape->getActor().getActiveScene(), origin + localCenter, dir, length, hit))
		if(internalRadius!=0.0f && CCDRaycast(shape->getActor().getScene(), origin, dir, length, hit))
		{
#ifdef RAYCAST_CCD_PRINT_DEBUG
			static int count=0;
			printf("CCD hit %d\n", count++);
#endif
			updateCCDWitness = false;
			const PxReal radiusLimit = internalRadius * 0.75f;
			if(hit.distance>radiusLimit)
			{
//				newPose.p = origin + dir * (hit.distance - radiusLimit);
				newShapeCenter = origin + dir * (hit.distance - radiusLimit);
#ifdef RAYCAST_CCD_PRINT_DEBUG
				printf("  Path0: %f | %f\n", hit.distance, radiusLimit);
#endif
			}
			else
			{
//				newPose.p = origin;
				newShapeCenter = origin;
//				newShapeCenter = origin + hit.normal * (radiusLimit - hit.distance);
#ifdef RAYCAST_CCD_PRINT_DEBUG
				printf("  Path1: %f\n", hit.distance);
#endif
			}

			{
				newPose.p = offset + newShapeCenter;
//newPose.p.y += 10.0f;
//printf("%f | %f | %f\n", newPose.p.x, newPose.p.y, newPose.p.z);

//				dyna->setGlobalPose(newPose);

				// newPose = actorGlobalPose * shapeLocalPose
				// newPose * inverse(shapeLocalPose) = actorGlobalPose

				const PxTransform shapeLocalPose = shape->getLocalPose();
				const PxTransform inverseShapeLocalPose = shapeLocalPose.getInverse();
				PxTransform newGlobalPose = newPose * inverseShapeLocalPose;
				dyna->setGlobalPose(newGlobalPose);
//dyna->setGlobalPose(newPose);
//printf("%f | %f | %f\n", newGlobalPose.p.x, newGlobalPose.p.y, newGlobalPose.p.z);
//printf("%f | %f | %f\n", shapeLocalPose.p.x, shapeLocalPose.p.y, shapeLocalPose.p.z);

/*PX_INLINE PxTransform PxShapeExt::getGlobalPose(const PxShape& shape)
{
PxRigidActor& ra = shape.getActor();

return ra.getGlobalPose() * shape.getLocalPose();
}*/
const PxVec3 testShapeCenter = getShapeCenter(shape, ccdWitnessOffset);
float d = (testShapeCenter - newShapeCenter).magnitude();
//printf("%f\n", d);
//printf("CCD1: %f | %f | %f\n", testShapeCenter.x, testShapeCenter.y, testShapeCenter.z);

//dyna->clearForce(PxForceMode::eFORCE);
//dyna->clearForce(PxForceMode::eIMPULSE);
//dyna->setLinearVelocity(PxVec3(0));	// PT: this helps the CCT but stops small objects dead, which doesn't look great

			}
		}
	}
	return updateCCDWitness;
}

bool physx::Gu::computeConvex_HeightFieldMTD(const PxHeightFieldGeometry& heightFieldGeom, const PxTransform& pose, const PxConvexMeshGeometry& convexGeom, const PxTransform& convexPose, const PxReal inflation,
						  const PxReal distance, const bool isDoubleSided, const PxU32 flags, PxSweepHit& hit)
{
	using namespace Ps::aos;

	const Gu::HeightFieldUtil hfUtil(heightFieldGeom);

	
	const Vec3V zeroV = V3Zero();
	Gu::MeshPersistentContact manifoldContacts[64]; 
	PxU32 numContacts = 0;
	FloatV distV = FLoad(distance);
	
	
	bool foundInitial = false;
	static PxU32 iterations = 2;

	Gu::ConvexMesh* cm = static_cast<Gu::ConvexMesh*>(convexGeom.convexMesh);

	Gu::ConvexHullData* hullData = &cm->getHull();

	const bool idtScaleConvex = convexGeom.scale.isIdentity();
	
	 Cm::FastVertex2ShapeScaling convexScaling;
	if(!idtScaleConvex)
			convexScaling.init(convexGeom.scale);

	const PxVec3 _shapeSpaceCenterOfMass = convexScaling * hullData->mCenterOfMass;
	const Vec3V shapeSpaceCenterOfMass = V3LoadU(_shapeSpaceCenterOfMass);

	const QuatV q0 = QuatVLoadU(&convexPose.q.x);
	const Vec3V p0 = V3LoadU(&convexPose.p.x);
	PsTransformV convexTransformV(p0, q0);

	const Vec3V vScale = V3LoadU(convexGeom.scale.scale);
	const QuatV vQuat = QuatVLoadU(&convexGeom.scale.rotation.x);
	Gu::ConvexHullV convexHull(hullData, zeroV, vScale, vQuat);
	PX_ALIGN(16, PxU8 convexBuff[sizeof(SupportLocalImpl<ConvexHullV>)]);

	const FloatV convexMargin = Gu::CalculatePCMConvexMargin(hullData, vScale);
	const FloatV inflationV = FAdd(FLoad(inflation), convexMargin);
	PxReal boundInflation;
	FStore(inflationV, &boundInflation);


	LocalContainer(tempContainer, 128);


	Vec3V closestA = zeroV, closestB = zeroV, normal = zeroV;
	Vec3V worldNormal=zeroV, worldContactA=zeroV;//, worldContactB=zeroV;
	PxU32 triangleIndex = 0xfffffff;

	Vec3V translation = zeroV;

	Gu::PolygonalData polyData;
	getPCMConvexData(convexHull, idtScaleConvex, polyData);

	FloatV mtd;
	Vec3V center = p0;
	PxTransform tempConvexPose = convexPose;
	Cm::Matrix34 meshToWorldSkew(pose);
	
	
	for(PxU32 i=0; i<iterations; ++i)
	{

		tempContainer.Reset();

		//ML:: construct convex hull data
	
		V3StoreU(center, tempConvexPose.p);
		convexTransformV.p = center;

		SupportLocal* convexMap = (idtScaleConvex ? (SupportLocal*)PX_PLACEMENT_NEW(convexBuff, SupportLocalImpl<ConvexHullNoScaleV>)((ConvexHullNoScaleV&)convexHull, convexTransformV, convexHull.vertex2Shape, convexHull.shape2Vertex, idtScaleConvex) : 
		(SupportLocal*)PX_PLACEMENT_NEW(convexBuff, SupportLocalImpl<ConvexHullV>)(convexHull, convexTransformV, convexHull.vertex2Shape, convexHull.shape2Vertex, idtScaleConvex));

		convexMap->setShapeSpaceCenterofMass(shapeSpaceCenterOfMass);
		
		Gu::Box hullOBB;
		Gu::computeOBBAroundConvex(hullOBB, convexGeom, cm, tempConvexPose);
		hullOBB.extents.x += boundInflation;
		hullOBB.extents.y += boundInflation;
		hullOBB.extents.z += boundInflation;

		PxBounds3 bounds = PxBounds3::basisExtent(hullOBB.center, hullOBB.rot, hullOBB.extents);

		midPhaseQueryHF(hfUtil, pose, bounds, tempContainer, flags);

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

		if(!nbTriangles)
			break;
	
		// Move to AABB space
		Cm::Matrix34 worldToConvex(tempConvexPose.getInverse());
		const Cm::Matrix34 meshToConvex = worldToConvex*meshToWorldSkew;

		Ps::aos::Mat33V rot(V3LoadU(meshToConvex.base0), V3LoadU(meshToConvex.base1), V3LoadU(meshToConvex.base2));
		Ps::aos::PsMatTransformV meshToConvexV(V3LoadU(meshToConvex.base3), rot);
//.........这里部分代码省略.........