C++ PxRigidBody类代码示例

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

PX_INLINE PxVec3 getVelocityAtPosInternal(const PxRigidBody& body, const PxVec3& point)
{
	PxVec3 velocity = body.getLinearVelocity();
	velocity       += body.getAngularVelocity().cross(point);
	
	return velocity;
}

vec3f PhysActor_PhysX::GetAngularVelocity()
{
	if (!impl->physActor)
	{
		LOG("Cannot get angular velocity from actor because actor ptr is null!");
		return vec3f(0.0f, 0.0f, 0.0f);
	}

	PxRigidBody* actor = nullptr;

	if (impl->physActor->isRigidDynamic() || impl->physActor->isRigidBody())
		actor = (PxRigidBody*)impl->physActor;
	else
	{
		LOG("Cannot get angular velocity from actor because actor is not a Rigid Dynamic or Rigid Body actor.");
		return vec3f(0.0f, 0.0f, 0.0f);
	}

	if (!actor)
		return vec3f(0.0f, 0.0f, 0.0f);

	PxVec3 velocity = actor->getAngularVelocity();
	vec3f vConv(velocity.x, velocity.y, velocity.z);

	return vConv;
}

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
}

//=================================================================================
// Single closest hit compound sweep
bool PxRigidBodyExt::linearSweepSingle(
	PxRigidBody& body, PxScene& scene, const PxVec3& unitDir, const PxReal distance,
	PxHitFlags outputFlags, PxSweepHit& closestHit, PxU32& shapeIndex,
	const PxQueryFilterData& filterData, PxQueryFilterCallback* filterCall,
	const PxQueryCache* cache, const PxReal inflation)
{
	shapeIndex = 0xFFFFffff;
	PxReal closestDist = distance;
	PxU32 nbShapes = body.getNbShapes();
	for(PxU32 i=0; i < nbShapes; i++)
	{
		PxShape* shape = NULL;
		body.getShapes(&shape, 1, i);
		PX_ASSERT(shape != NULL);
		PxTransform pose = PxShapeExt::getGlobalPose(*shape, body);
		PxQueryFilterData fd;
		fd.flags = filterData.flags;
		PxU32 or4 = (filterData.data.word0 | filterData.data.word1 | filterData.data.word2 | filterData.data.word3);
		fd.data = or4 ? filterData.data : shape->getSimulationFilterData();
		PxGeometryHolder anyGeom = shape->getGeometry();

		PxSweepBuffer subHit; // touching hits are not allowed to be returned from the filters
		scene.sweep(anyGeom.any(), pose, unitDir, distance, subHit, outputFlags, fd, filterCall, cache, inflation);
		if (subHit.hasBlock && subHit.block.distance < closestDist)
		{
			closestDist = subHit.block.distance;
			closestHit = subHit.block;
			shapeIndex = i;
		}
	}

	return (shapeIndex != 0xFFFFffff);
}

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
}

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

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

/** Applies torques - Assumes caller has obtained writer lock */
void FPhysSubstepTask::ApplyTorques_AssumesLocked(const FPhysTarget& PhysTarget, FBodyInstance* BodyInstance)
{
#if WITH_PHYSX
	/** Apply Torques */
	PxRigidBody* PRigidBody = BodyInstance->GetPxRigidBody_AssumesLocked();

	for (int32 i = 0; i < PhysTarget.Torques.Num(); ++i)
	{
		const FTorqueTarget& TorqueTarget = PhysTarget.Torques[i];
		PRigidBody->addTorque(U2PVector(TorqueTarget.Torque), TorqueTarget.bAccelChange ? PxForceMode::eACCELERATION : PxForceMode::eFORCE, true);
	}
#endif
}

void FPhysSubstepTask::SubstepInterpolation(float InAlpha, float DeltaTime)
{
#if WITH_PHYSX
#if WITH_APEX
	SCOPED_APEX_SCENE_WRITE_LOCK(PAScene);
	PxScene * PScene = PAScene->getPhysXScene();
#else
	PxScene * PScene = PAScene;
	SCOPED_SCENE_WRITE_LOCK(PScene);
#endif
	
	/** Note: We lock the entire scene before iterating. The assumption is that removing an FBodyInstance from the map will also be wrapped by this lock */
	
	
	PhysTargetMap& Targets = PhysTargetBuffers[!External];

	for (PhysTargetMap::TIterator Itr = Targets.CreateIterator(); Itr; ++Itr)
	{
		FPhysTarget & PhysTarget = Itr.Value();
		FBodyInstance * BodyInstance = Itr.Key();
		PxRigidBody* PRigidBody = BodyInstance->GetPxRigidBody_AssumesLocked();

		if (PRigidBody == NULL)
		{
			continue;
		}

		//We should only be iterating over actors that belong to this scene
		check(PRigidBody->getScene() == PScene);

		if (!IsKinematicHelper(PRigidBody))
		{
			ApplyCustomPhysics(PhysTarget, BodyInstance, DeltaTime);
			ApplyForces_AssumesLocked(PhysTarget, BodyInstance);
			ApplyTorques_AssumesLocked(PhysTarget, BodyInstance);
			ApplyRadialForces_AssumesLocked(PhysTarget, BodyInstance);
		}else
		{
			InterpolateKinematicActor_AssumesLocked(PhysTarget, BodyInstance, InAlpha);
		}
	}

	/** Final substep */
	if (InAlpha >= 1.f)
	{
		Targets.Empty(Targets.Num());
	}
#endif
}

static bool setMassAndUpdateInertia(bool multipleMassOrDensity, PxRigidBody& body, const PxReal* masses, PxU32 massCount, const PxVec3* massLocalPose, bool includeNonSimShapes)
{
	bool success;

	// default values in case there were no shapes
	PxReal massOut = 1.0f;
	PxVec3 diagTensor(1.0f,1.0f,1.0f);
	PxQuat orient = PxQuat(PxIdentity);
	bool lockCom = massLocalPose != NULL;
	PxVec3 com = lockCom ? *massLocalPose : PxVec3(0);
	const char* errorStr = "PxRigidBodyExt::setMassAndUpdateInertia";

	if(masses && massCount)
	{
		Ext::InertiaTensorComputer inertiaComp(true);
		if(computeMassAndInertia(multipleMassOrDensity, body, NULL, masses, massCount, includeNonSimShapes, inertiaComp))
		{
			success = true;

			if (inertiaComp.getMass()!=0 && !computeMassAndDiagInertia(inertiaComp, diagTensor, orient, massOut, com, lockCom, body, errorStr))
				success = false;  // computeMassAndDiagInertia() failed (mass zero?)

			if (massCount == 1)
				massOut = masses[0]; // to cover special case where body has no simulation shape
		}
		else
		{
			Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__, 
				"%s: Mass and inertia computation failed, setting mass to 1 and inertia to (1,1,1)", errorStr);

			success = false;
		}
	}
	else
	{
		Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__, 
			"%s: No mass specified, setting mass to 1 and inertia to (1,1,1)", errorStr);
		success = false;
	}

	PX_ASSERT(orient.isFinite());
	PX_ASSERT(diagTensor.isFinite());

	body.setMass(massOut);
	body.setMassSpaceInertiaTensor(diagTensor);
	body.setCMassLocalPose(PxTransform(com, orient));

	return success;
}

Vec3 PhysXPhysics::VGetVelocity(ActorId actorId)
{
	PxRigidBody* pBody = FindRigidBody(actorId);
	BE_ASSERT(pBody);
	if (!pBody)
	{
		return Vec3::g_InvalidVec3;
	}
	PxVec3 pxVec = pBody->getLinearVelocity();

	Vec3 velocity;
	PxVecToVec3(pxVec, &velocity);

	return velocity;
}

static bool updateMassAndInertia(bool multipleMassOrDensity, PxRigidBody& body, const PxReal* densities, PxU32 densityCount, const PxVec3* massLocalPose, bool includeNonSimShapes)
{
	bool success;

	// default values in case there were no shapes
	PxReal massOut = 1.0f;
	PxVec3 diagTensor(1.f,1.f,1.f);
	PxQuat orient = PxQuat(PxIdentity);
	bool lockCom = massLocalPose != NULL;
	PxVec3 com = lockCom ? *massLocalPose : PxVec3(0);
	const char* errorStr = "PxRigidBodyExt::updateMassAndInertia";

	if (densities && densityCount)
	{
		Ext::InertiaTensorComputer inertiaComp(true);
		if(computeMassAndInertia(multipleMassOrDensity, body, densities, NULL, densityCount, includeNonSimShapes, inertiaComp))
		{
			if(inertiaComp.getMass()!=0 && computeMassAndDiagInertia(inertiaComp, diagTensor, orient, massOut, com, lockCom, body, errorStr))
				success = true;
			else
				success = false;  // body with no shapes provided or computeMassAndDiagInertia() failed
		}
		else
		{
			Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__, 
				"%s: Mass and inertia computation failed, setting mass to 1 and inertia to (1,1,1)", errorStr);

			success = false;
		}
	}
	else
	{
		Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__, 
			"%s: No density specified, setting mass to 1 and inertia to (1,1,1)", errorStr);

		success = false;
	}

	PX_ASSERT(orient.isFinite());
	PX_ASSERT(diagTensor.isFinite());
	PX_ASSERT(PxIsFinite(massOut));

	body.setMass(massOut);
	body.setMassSpaceInertiaTensor(diagTensor);
	body.setCMassLocalPose(PxTransform(com, orient));

	return success;
}

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

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