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::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);
}
}
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::computeLinearAngularImpulse(const PxRigidBody& body, const PxTransform& globalPose, const PxVec3& point, const PxVec3& impulse, const PxReal invMassScale,
const PxReal invInertiaScale, PxVec3& linearImpulse, PxVec3& angularImpulse)
{
const PxVec3 centerOfMass = globalPose.transform(body.getCMassLocalPose().p);
linearImpulse = impulse * invMassScale;
angularImpulse = (point - centerOfMass).cross(impulse) * invInertiaScale;
}
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::computeVelocityDeltaFromImpulse(const PxRigidBody& body, const PxTransform& globalPose, const PxVec3& point, const PxVec3& impulse, const PxReal invMassScale,
const PxReal invInertiaScale, PxVec3& linearVelocityChange, PxVec3& angularVelocityChange)
{
const PxVec3 centerOfMass = globalPose.transform(body.getCMassLocalPose().p);
const PxReal invMass = body.getInvMass() * invMassScale;
const PxVec3 invInertiaMS = body.getMassSpaceInvInertiaTensor() * invInertiaScale;
PxMat33 invInertia;
transformInertiaTensor(invInertiaMS, PxMat33(globalPose.q), invInertia);
linearVelocityChange = impulse * invMass;
const PxVec3 rXI = (point - centerOfMass).cross(impulse);
angularVelocityChange = invInertia * rXI;
}
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);
}
