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