C++ PxRigidBody::getGlobalPose方法代码示例

void AddRadialForceToPxRigidBody_AssumesLocked(PxRigidBody& PRigidBody, const FVector& Origin, float Radius, float Strength, uint8 Falloff, bool bAccelChange)
{
#if WITH_PHYSX
	if (!(PRigidBody.getRigidBodyFlags() & PxRigidBodyFlag::eKINEMATIC))
	{
		float Mass = PRigidBody.getMass();
		PxTransform PCOMTransform = PRigidBody.getGlobalPose().transform(PRigidBody.getCMassLocalPose());
		PxVec3 PCOMPos = PCOMTransform.p; // center of mass in world space
		PxVec3 POrigin = U2PVector(Origin); // origin of radial impulse, in world space
		PxVec3 PDelta = PCOMPos - POrigin; // vector from

		float Mag = PDelta.magnitude(); // Distance from COM to origin, in Unreal scale : @todo: do we still need conversion scale?

		// If COM is outside radius, do nothing.
		if (Mag > Radius)
		{
			return;
		}

		PDelta.normalize();

		// If using linear falloff, scale with distance.
		float ForceMag = Strength;
		if (Falloff == RIF_Linear)
		{
			ForceMag *= (1.0f - (Mag / Radius));
		}

		// Apply force
		PxVec3 PImpulse = PDelta * ForceMag;
		PRigidBody.addForce(PImpulse, bAccelChange ? PxForceMode::eACCELERATION : PxForceMode::eFORCE);
	}
#endif // WITH_PHYSX
}

void PxRigidBodyExt::addForceAtLocalPos(PxRigidBody& body, const PxVec3& force, const PxVec3& pos, PxForceMode::Enum mode, bool wakeup)
{
	//transform pos to world space
	const PxVec3 globalForcePos = body.getGlobalPose().transform(pos);

	addForceAtPosInternal(body, force, globalForcePos, mode, wakeup);
}

void AddRadialImpulseToPxRigidBody_AssumesLocked(PxRigidBody& PRigidBody, const FVector& Origin, float Radius, float Strength, uint8 Falloff, bool bVelChange)
{
#if WITH_PHYSX
	if (!(PRigidBody.getRigidBodyFlags() & PxRigidBodyFlag::eKINEMATIC))
	{
		float Mass = PRigidBody.getMass();
		PxTransform PCOMTransform = PRigidBody.getGlobalPose().transform(PRigidBody.getCMassLocalPose());
		PxVec3 PCOMPos = PCOMTransform.p; // center of mass in world space
		PxVec3 POrigin = U2PVector(Origin); // origin of radial impulse, in world space
		PxVec3 PDelta = PCOMPos - POrigin; // vector from origin to COM

		float Mag = PDelta.magnitude(); // Distance from COM to origin, in Unreal scale : @todo: do we still need conversion scale?

		// If COM is outside radius, do nothing.
		if (Mag > Radius)
		{
			return;
		}

		PDelta.normalize();

		// Scale by U2PScale here, because units are velocity * mass. 
		float ImpulseMag = Strength;
		if (Falloff == RIF_Linear)
		{
			ImpulseMag *= (1.0f - (Mag / Radius));
		}

		PxVec3 PImpulse = PDelta * ImpulseMag;

		PxForceMode::Enum Mode = bVelChange ? PxForceMode::eVELOCITY_CHANGE : PxForceMode::eIMPULSE;
		PRigidBody.addForce(PImpulse, Mode);
	}
#endif // WITH_PHYSX
}

void PxRigidBodyExt::computeVelocityDeltaFromImpulse(const PxRigidBody& body, const PxVec3& impulsiveForce, const PxVec3& impulsiveTorque, PxVec3& deltaLinearVelocity, PxVec3& deltaAngularVelocity)
{
	{
		const PxF32 recipMass = body.getInvMass();
		deltaLinearVelocity = impulsiveForce*recipMass;
	}

	{
		const PxTransform globalPose = body.getGlobalPose();
		const PxTransform cmLocalPose = body.getCMassLocalPose();
		const PxTransform body2World = globalPose*cmLocalPose;
		PxMat33 M(body2World.q);

		const PxVec3 recipInertiaBodySpace = body.getMassSpaceInvInertiaTensor();

		PxMat33 recipInertiaWorldSpace;
		const float	axx = recipInertiaBodySpace.x*M(0,0), axy = recipInertiaBodySpace.x*M(1,0), axz = recipInertiaBodySpace.x*M(2,0);
		const float	byx = recipInertiaBodySpace.y*M(0,1), byy = recipInertiaBodySpace.y*M(1,1), byz = recipInertiaBodySpace.y*M(2,1);
		const float	czx = recipInertiaBodySpace.z*M(0,2), czy = recipInertiaBodySpace.z*M(1,2), czz = recipInertiaBodySpace.z*M(2,2);
		recipInertiaWorldSpace(0,0) = axx*M(0,0) + byx*M(0,1) + czx*M(0,2);
		recipInertiaWorldSpace(1,1) = axy*M(1,0) + byy*M(1,1) + czy*M(1,2);
		recipInertiaWorldSpace(2,2) = axz*M(2,0) + byz*M(2,1) + czz*M(2,2);
		recipInertiaWorldSpace(0,1) = recipInertiaWorldSpace(1,0) = axx*M(1,0) + byx*M(1,1) + czx*M(1,2);
		recipInertiaWorldSpace(0,2) = recipInertiaWorldSpace(2,0) = axx*M(2,0) + byx*M(2,1) + czx*M(2,2);
		recipInertiaWorldSpace(1,2) = recipInertiaWorldSpace(2,1) = axy*M(2,0) + byy*M(2,1) + czy*M(2,2);

		deltaAngularVelocity = recipInertiaWorldSpace*(impulsiveTorque);
	}
}

PxVec3 PxRigidBodyExt::getVelocityAtOffset(const PxRigidBody& body, const PxVec3& point)
{
	const PxTransform globalPose = body.getGlobalPose();
	const PxVec3 centerOfMass    = globalPose.rotate(body.getCMassLocalPose().p);
	const PxVec3 rpoint          = point - centerOfMass;

	return getVelocityAtPosInternal(body, rpoint);
}

PxVec3 PxRigidBodyExt::getLocalVelocityAtLocalPos(const PxRigidBody& body, const PxVec3& point)
{
	const PxTransform globalPose = body.getGlobalPose();
	const PxVec3 centerOfMass    = globalPose.transform(body.getCMassLocalPose().p);
	const PxVec3 rpoint          = globalPose.transform(point) - centerOfMass;

	return getVelocityAtPosInternal(body, rpoint);
}

void PxRigidBodyExt::addLocalForceAtLocalPos(PxRigidBody& body, const PxVec3& force, const PxVec3& pos, PxForceMode::Enum mode, bool wakeup)
{
	const PxTransform globalPose = body.getGlobalPose();
	const PxVec3 globalForcePos = globalPose.transform(pos);
	const PxVec3 globalForce = globalPose.rotate(force);

	addForceAtPosInternal(body, globalForce, globalForcePos, mode, wakeup);
}

PX_INLINE void addForceAtPosInternal(PxRigidBody& body, const PxVec3& force, const PxVec3& pos, PxForceMode::Enum mode, bool wakeup)
{
	if(mode == PxForceMode::eACCELERATION || mode == PxForceMode::eVELOCITY_CHANGE)
	{
		Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__, 
			"PxRigidBodyExt::addForce methods do not support eACCELERATION or eVELOCITY_CHANGE modes");
		return;
	}

	const PxTransform globalPose = body.getGlobalPose();
	const PxVec3 centerOfMass = globalPose.transform(body.getCMassLocalPose().p);

	const PxVec3 torque = (pos - centerOfMass).cross(force);
	body.addForce(force, mode, wakeup);
	body.addTorque(torque, mode, wakeup);
}

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;
	}
}

void SimulationEventCallback::destroyWallBlock(const PxRigidBody& _krRigidBody, const PxShape& _krShape)
{
	Entity* wallBlock = static_cast<Entity*>(_krRigidBody.userData);
	if (find(m_destroyedWallBlocks.begin(), m_destroyedWallBlocks.end(), wallBlock) != m_destroyedWallBlocks.end())
	{
		return;
	}

	PxBoxGeometry geometry;
	_krShape.getBoxGeometry(geometry);

	PxTransform transform = _krRigidBody.getGlobalPose();
	Matrix44 transformation = PhysXMatrix::toMatrix44(transform);

	PxVec3 angularVelocity = _krRigidBody.getAngularVelocity();
	PxVec3 linearVelocity = _krRigidBody.getLinearVelocity();

	m_destroyedWallBlocks.push_back(wallBlock);
	GazEngine::removeEntity(*wallBlock);

	float halfSize = geometry.halfExtents.x / 2.0f;

	Matrix44 fragment0Transformation = transformation;
	getTranslation3(fragment0Transformation) += Vector3(-halfSize, halfSize, 0.0f);
	Entity* pFragment0 = CreateWallBlock(fragment0Transformation, halfSize, *m_pParentNode);
	PxRigidBody* pFragment0RigidBody = pFragment0->getSingleComponent<PhysXBody>()->getActor()->isRigidBody();
	pFragment0RigidBody->setAngularVelocity(angularVelocity);
	pFragment0RigidBody->setLinearVelocity(linearVelocity);
	// The body only got a position in the constructor... lets give it a rotation too.
	PxTransform fragment0Transform = transform;
	fragment0Transform.p += PxVec3(-halfSize, halfSize, 0.0f);
	pFragment0RigidBody->setGlobalPose(fragment0Transform);

	Matrix44 fragment1Transformation = transformation;
	getTranslation3(fragment1Transformation) += Vector3(halfSize, halfSize, 0.0f);
	Entity* pFragment1 = CreateWallBlock(fragment1Transformation, halfSize, *m_pParentNode);
	PxRigidBody* pFragment1RigidBody = pFragment1->getSingleComponent<PhysXBody>()->getActor()->isRigidBody();
	pFragment1RigidBody->setAngularVelocity(angularVelocity);
	pFragment1RigidBody->setLinearVelocity(linearVelocity);
	// The body only got a position in the constructor... lets give it a rotation too.
	PxTransform fragment1Transform = transform;
	fragment1Transform.p += PxVec3(halfSize, halfSize, 0.0f);
	pFragment1RigidBody->setGlobalPose(fragment1Transform);

	Matrix44 fragment2Transformation = transformation;
	getTranslation3(fragment2Transformation) += Vector3(halfSize, -halfSize, 0.0f);
	Entity* pFragment2 = CreateWallBlock(fragment2Transformation, halfSize, *m_pParentNode);
	PxRigidBody* pFragment2RigidBody = pFragment2->getSingleComponent<PhysXBody>()->getActor()->isRigidBody();
	pFragment2RigidBody->setAngularVelocity(angularVelocity);
	pFragment2RigidBody->setLinearVelocity(linearVelocity);
	// The body only got a position in the constructor... lets give it a rotation too.
	PxTransform fragment2Transform = transform;
	fragment2Transform.p += PxVec3(halfSize, -halfSize, 0.0f);
	pFragment2RigidBody->setGlobalPose(fragment2Transform);

	Matrix44 fragment3Transformation = transformation;
	getTranslation3(fragment3Transformation) += Vector3(-halfSize, -halfSize, 0.0f);
	Entity* pFragment3 = CreateWallBlock(fragment3Transformation, halfSize, *m_pParentNode);
	PxRigidBody* pFragment3RigidBody = pFragment3->getSingleComponent<PhysXBody>()->getActor()->isRigidBody();
	pFragment3RigidBody->setAngularVelocity(angularVelocity);
	pFragment3RigidBody->setLinearVelocity(linearVelocity);
	// The body only got a position in the constructor... lets give it a rotation too.
	PxTransform fragment3Transform = transform;
	fragment3Transform.p += PxVec3(-halfSize, -halfSize, 0.0f);
	pFragment3RigidBody->setGlobalPose(fragment3Transform);
}