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