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);
}
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;
}
/** 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 PhysActor_PhysX::AddTorque(vec3f torque, PhysForceMode::Enum mode)
{
if (!impl->physActor)
{
LOG("Cannot add torque to actor because physActor is null!");
return;
}
PxRigidBody* actor = nullptr;
PxVec3 pushDir;
pushDir.x = torque.x;
pushDir.y = torque.y;
pushDir.z = torque.z;
PxForceMode::Enum physXMode;
switch (mode)
{
case PhysForceMode::Acceleration:
physXMode = PxForceMode::eACCELERATION;
break;
case PhysForceMode::Force:
physXMode = PxForceMode::eFORCE;
break;
case PhysForceMode::VelocityChange:
physXMode = PxForceMode::eVELOCITY_CHANGE;
break;
case PhysForceMode::Impulse:
default:
physXMode = PxForceMode::eIMPULSE;
break;
}
if (impl->physActor->isRigidDynamic() || impl->physActor->isRigidBody())
{
actor = (PxRigidBody*)impl->physActor;
}
else
{
LOG("Cannot add force to actor because actor is not a Rigid Dynamic or Rigid Body actor.");
return;
}
if (actor)
actor->addTorque(pushDir, physXMode);
}
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);
}
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;
}
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);
}
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 SceneQueryManager::validateSimUpdates()
{
if (mPrunerExt[1].type() != PxPruningStructureType::eDYNAMIC_AABB_TREE)
return;
Sc::BodyCore*const* activeBodies = mScene.getActiveBodiesArray();
const PxU32 nbActiveBodies = mScene.getNumActiveBodies();
for (PxU32 i = 0; i < nbActiveBodies; ++i)
{
const Sc::BodyCore* bCore = activeBodies[i];
if (bCore->isFrozen())
continue;
PxRigidBody* pxBody = static_cast<PxRigidBody*>(bCore->getPxActor());
PX_ASSERT(pxBody->getConcreteType() == PxConcreteType::eRIGID_DYNAMIC || pxBody->getConcreteType() == PxConcreteType::eARTICULATION_LINK);
NpShapeManager& shapeManager = *NpActor::getShapeManager(*pxBody);
const PxU32 nbShapes = shapeManager.getNbShapes();
NpShape* const* shape = shapeManager.getShapes();
for (PxU32 j = 0; j<nbShapes; j++)
{
PrunerData prunerData = shapeManager.getPrunerData(j);
if (prunerData != INVALID_PRUNERHANDLE)
{
const PrunerHandle handle = getPrunerHandle(prunerData);
const PxBounds3 worldAABB = computeWorldAABB(shape[j]->getScbShape(), *bCore);
PxBounds3 prunerAABB = static_cast<AABBPruner*>(mPrunerExt[1].pruner())->getAABB(handle);
PX_ASSERT((worldAABB.minimum - prunerAABB.minimum).magnitudeSquared() < 0.005f*mScene.getPxScene()->getPhysics().getTolerancesScale().length);
PX_ASSERT((worldAABB.maximum - prunerAABB.maximum).magnitudeSquared() < 0.005f*mScene.getPxScene()->getPhysics().getTolerancesScale().length);
PX_UNUSED(worldAABB);
PX_UNUSED(prunerAABB);
}
}
}
}
/** Applies forces - Assumes caller has obtained writer lock */
void FPhysSubstepTask::ApplyForces_AssumesLocked(const FPhysTarget& PhysTarget, FBodyInstance* BodyInstance)
{
#if WITH_PHYSX
/** Apply Forces */
PxRigidBody* PRigidBody = BodyInstance->GetPxRigidBody_AssumesLocked();
for (int32 i = 0; i < PhysTarget.Forces.Num(); ++i)
{
const FForceTarget& ForceTarget = PhysTarget.Forces[i];
if (ForceTarget.bPosition)
{
PxRigidBodyExt::addForceAtPos(*PRigidBody, U2PVector(ForceTarget.Force), U2PVector(ForceTarget.Position), PxForceMode::eFORCE, true);
}
else
{
PRigidBody->addForce(U2PVector(ForceTarget.Force), ForceTarget.bAccelChange ? PxForceMode::eACCELERATION : PxForceMode::eFORCE, true);
}
}
#endif
}
void PhysActor_PhysX::SetAngularVelocity(vec3f velocity)
{
if (!impl->physActor)
{
LOG("Cannot set angular velocity from actor because actor ptr is null!");
return;
}
PxRigidBody* actor = nullptr;
if (impl->physActor->isRigidDynamic() || impl->physActor->isRigidBody())
actor = (PxRigidBody*)impl->physActor;
else
{
LOG("Cannot set angular velocity from actor because actor is not a Rigid Dynamic or Rigid Body actor.");
return;
}
if (!actor)
return;
actor->setAngularVelocity(PxVec3(velocity.x, velocity.y, velocity.z));
}
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);
}
static void sweepRigidBody(PxRigidBody& body, PxBatchQuery& batchQuery, bool closestObject, const PxVec3& unitDir, const PxReal distance, PxSceneQueryFilterFlags filterFlags,
bool useShapeFilterData, PxFilterData* filterDataList, PxU32 filterDataCount, void* userData, const PxSweepCache* sweepCache)
{
if (body.getNbShapes() == 0)
return;
const char* outOfMemMsg = "PxRigidBodyExt: LinearSweep: Out of memory, call failed.";
bool succeeded = true;
PX_ALLOCA(shapes, PxShape*, body.getNbShapes());
if (!shapes)
{
Ps::getFoundation().error(PxErrorCode::eOUT_OF_MEMORY, __FILE__, __LINE__, outOfMemMsg);
succeeded = false;
}
PxU32 nbShapes = body.getShapes(shapes, body.getNbShapes());
PX_ALLOCA(geoms, const PxGeometry*, nbShapes);
if (!geoms)
{
Ps::getFoundation().error(PxErrorCode::eOUT_OF_MEMORY, __FILE__, __LINE__, outOfMemMsg);
succeeded = false;
}
PX_ALLOCA(poses, PxTransform, nbShapes);
if (!poses)
{
Ps::getFoundation().error(PxErrorCode::eOUT_OF_MEMORY, __FILE__, __LINE__, outOfMemMsg);
succeeded = false;
}
PxFilterData* filterData = NULL;
PX_ALLOCA(filterDataBuffer, PxFilterData, nbShapes);
if (useShapeFilterData)
{
filterData = filterDataBuffer;
if (!filterDataBuffer)
{
Ps::getFoundation().error(PxErrorCode::eOUT_OF_MEMORY, __FILE__, __LINE__, outOfMemMsg);
succeeded = false;
}
}
else if (filterDataList)
{
if (filterDataCount == nbShapes)
{
filterData = filterDataList;
}
else
{
Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__, "PxRigidBodyExt: LinearSweep: Number of filter data entries does not match number of shapes, call failed.");
succeeded = false;
}
}
if (succeeded)
{
PxU32 geomByteSize = 0;
for(PxU32 i=0; i < nbShapes; i++)
{
poses[i] = PxShapeExt::getGlobalPose(*shapes[i]);
if (useShapeFilterData)
filterData[i] = shapes[i]->getSimulationFilterData();
// Copy the non-supported geometry types too, to make sure the closest geometry index maps to the shapes
switch(shapes[i]->getGeometryType())
{
case PxGeometryType::eSPHERE :
{
geomByteSize += sizeof(PxSphereGeometry);
}
break;
case PxGeometryType::eBOX :
{
geomByteSize += sizeof(PxBoxGeometry);
}
break;
case PxGeometryType::eCAPSULE :
{
geomByteSize += sizeof(PxCapsuleGeometry);
}
break;
case PxGeometryType::eCONVEXMESH :
{
geomByteSize += sizeof(PxConvexMeshGeometry);
}
break;
case PxGeometryType::ePLANE :
{
geomByteSize += sizeof(PxPlaneGeometry);
}
break;
case PxGeometryType::eTRIANGLEMESH :
{
geomByteSize += sizeof(PxTriangleMeshGeometry);
}
break;
case PxGeometryType::eHEIGHTFIELD :
{
geomByteSize += sizeof(PxHeightFieldGeometry);
}
break;
default:
break;
}
}
PX_ALLOCA(geomBuffer, PxU8, geomByteSize);
if (geomBuffer)
{
PxU8* currBufferPos = geomBuffer;
for(PxU32 i=0; i < nbShapes; i++)
{
// Copy the non-supported geometry types too, to make sure the closest geometry index maps to the shapes
switch(shapes[i]->getGeometryType())
{
case PxGeometryType::eSPHERE :
{
PxSphereGeometry* g = reinterpret_cast<PxSphereGeometry*>(currBufferPos);
geoms[i] = g;
shapes[i]->getSphereGeometry(*g);
currBufferPos += sizeof(PxSphereGeometry);
}
break;
case PxGeometryType::eBOX :
{
PxBoxGeometry* g = reinterpret_cast<PxBoxGeometry*>(currBufferPos);
geoms[i] = g;
shapes[i]->getBoxGeometry(*g);
currBufferPos += sizeof(PxBoxGeometry);
}
break;
case PxGeometryType::eCAPSULE :
{
PxCapsuleGeometry* g = reinterpret_cast<PxCapsuleGeometry*>(currBufferPos);
geoms[i] = g;
shapes[i]->getCapsuleGeometry(*g);
currBufferPos += sizeof(PxCapsuleGeometry);
}
break;
case PxGeometryType::eCONVEXMESH :
{
PxConvexMeshGeometry* g = reinterpret_cast<PxConvexMeshGeometry*>(currBufferPos);
geoms[i] = g;
shapes[i]->getConvexMeshGeometry(*g);
currBufferPos += sizeof(PxConvexMeshGeometry);
}
break;
case PxGeometryType::ePLANE :
{
PxPlaneGeometry* g = reinterpret_cast<PxPlaneGeometry*>(currBufferPos);
geoms[i] = g;
shapes[i]->getPlaneGeometry(*g);
currBufferPos += sizeof(PxPlaneGeometry);
}
break;
case PxGeometryType::eTRIANGLEMESH :
{
PxTriangleMeshGeometry* g = reinterpret_cast<PxTriangleMeshGeometry*>(currBufferPos);
geoms[i] = g;
shapes[i]->getTriangleMeshGeometry(*g);
currBufferPos += sizeof(PxTriangleMeshGeometry);
}
break;
case PxGeometryType::eHEIGHTFIELD :
{
PxHeightFieldGeometry* g = reinterpret_cast<PxHeightFieldGeometry*>(currBufferPos);
geoms[i] = g;
shapes[i]->getHeightFieldGeometry(*g);
currBufferPos += sizeof(PxHeightFieldGeometry);
}
break;
default:
break;
}
}
if (closestObject)
batchQuery.linearCompoundGeometrySweepSingle(geoms, poses, filterData, nbShapes, unitDir, distance, filterFlags, PxSceneQueryFlag::eIMPACT|PxSceneQueryFlag::eNORMAL|PxSceneQueryFlag::eDISTANCE|PxSceneQueryFlag::eUV, userData, sweepCache);
else
batchQuery.linearCompoundGeometrySweepMultiple(geoms, poses, filterData, nbShapes, unitDir, distance, filterFlags, PxSceneQueryFlag::eIMPACT|PxSceneQueryFlag::eNORMAL|PxSceneQueryFlag::eDISTANCE|PxSceneQueryFlag::eUV, userData, sweepCache);
}
}
}
void SceneQueryManager::processSimUpdates()
{
PX_PROFILE_ZONE("Sim.updatePruningTrees", mScene.getContextId());
{
PX_PROFILE_ZONE("SceneQuery.processActiveShapes", mScene.getContextId());
// update all active objects
BodyCore*const* activeBodies = mScene.getScScene().getActiveBodiesArray();
PxU32 nbActiveBodies = mScene.getScScene().getNumActiveBodies();
#define NB_BATCHED_OBJECTS 128
PrunerHandle batchedHandles[NB_BATCHED_OBJECTS];
PxU32 nbBatchedObjects = 0;
Pruner* pruner = mPrunerExt[PruningIndex::eDYNAMIC].pruner();
while(nbActiveBodies--)
{
// PT: TODO: don't put frozen objects in "active bodies" array? After all they
// are also not included in the 'active transforms' or 'active actors' arrays.
BodyCore* currentBody = *activeBodies++;
if(currentBody->isFrozen())
continue;
PxActorType::Enum type;
PxRigidBody* pxBody = static_cast<PxRigidBody*>(getPxActorFromBodyCore(currentBody, type));
PX_ASSERT(pxBody->getConcreteType()==PxConcreteType::eRIGID_DYNAMIC || pxBody->getConcreteType()==PxConcreteType::eARTICULATION_LINK);
NpShapeManager* shapeManager;
if(type==PxActorType::eRIGID_DYNAMIC)
{
NpRigidDynamic* rigidDynamic = static_cast<NpRigidDynamic*>(pxBody);
shapeManager = &rigidDynamic->getShapeManager();
}
else
{
NpArticulationLink* articulationLink = static_cast<NpArticulationLink*>(pxBody);
shapeManager = &articulationLink->getShapeManager();
}
const PxU32 nbShapes = shapeManager->getNbShapes();
for(PxU32 i=0; i<nbShapes; i++)
{
const PrunerData data = shapeManager->getPrunerData(i);
if(data!=SQ_INVALID_PRUNER_DATA)
{
// PT: index can't be zero here!
PX_ASSERT(getPrunerIndex(data)==PruningIndex::eDYNAMIC);
const PrunerHandle handle = getPrunerHandle(data);
if(!mPrunerExt[PruningIndex::eDYNAMIC].isDirty(handle)) // PT: if dirty, will be updated in "flushShapes"
{
batchedHandles[nbBatchedObjects] = handle;
PxBounds3* bounds;
const PrunerPayload& pp = pruner->getPayload(handle, bounds);
computeDynamicWorldAABB(*bounds, *(reinterpret_cast<Scb::Shape*>(pp.data[0])), *(reinterpret_cast<Scb::Actor*>(pp.data[1])));
nbBatchedObjects++;
if(nbBatchedObjects==NB_BATCHED_OBJECTS)
{
mPrunerExt[PruningIndex::eDYNAMIC].invalidateTimestamp();
pruner->updateObjects(batchedHandles, NULL, nbBatchedObjects);
nbBatchedObjects = 0;
}
}
}
}
}
if(nbBatchedObjects)
{
mPrunerExt[PruningIndex::eDYNAMIC].invalidateTimestamp();
pruner->updateObjects(batchedHandles, NULL, nbBatchedObjects);
}
}
// flush user modified objects
flushShapes();
for(PxU32 i=0;i<PruningIndex::eCOUNT;i++)
{
if(mPrunerExt[i].pruner() && mPrunerExt[i].type() == PxPruningStructureType::eDYNAMIC_AABB_TREE)
static_cast<AABBPruner*>(mPrunerExt[i].pruner())->buildStep();
mPrunerExt[i].pruner()->commit();
}
}
// Called every frame for vehicle
void Vehicle::update()
{
resetIfNeeded();
float handBrake = 0;
float forwardSpeed = physicsVehicle->computeForwardSpeed();
(input.handBrake) ? handBrake = 1.0f: handBrake = 0.0f;
// Force gear change if speed is 0 (works, not sure why)
if (forwardSpeed == 0)
{
if (input.forward > 0)
{
physicsVehicle->mDriveDynData.forceGearChange(PxVehicleGearsData::eFIRST);
}
else if (input.backward > 0)
{
physicsVehicle->mDriveDynData.forceGearChange(PxVehicleGearsData::eREVERSE);
}
}
// If speed is close to 0 set new target gear
else if (forwardSpeed < 5 && input.backward > 0)
{
physicsVehicle->mDriveDynData.setTargetGear(PxVehicleGearsData::eREVERSE);
}
else if (forwardSpeed > -5 && input.forward > 0)
{
physicsVehicle->mDriveDynData.setTargetGear(PxVehicleGearsData::eFIRST);
}
// Determine how to apply controller input depending on current gear
if (physicsVehicle->mDriveDynData.getCurrentGear() == PxVehicleGearsData::eREVERSE)
{
vehicleInput.setAnalogAccel(input.backward);
vehicleInput.setAnalogBrake(input.forward);
}
else
{
vehicleInput.setAnalogAccel(input.forward);
vehicleInput.setAnalogBrake(input.backward);
if(input.forward > 0)
gasSignal(this);
}
if (input.forward == 0 && input.backward == 0)
idleSignal(this);
if (isSlipping && (forwardSpeed > 5 || forwardSpeed < -5))
brakeSignal(this);
// Steer and handbrake
vehicleInput.setAnalogSteer(input.steer);
vehicleInput.setAnalogHandbrake(handBrake);
PxVehicleDrive4WSmoothAnalogRawInputsAndSetAnalogInputs(smoothingData, steerVsSpeedTable, vehicleInput, stepSizeS, isInAir, *physicsVehicle);
if (input.shootPizza)
{
if (pizzaCount > 0)
{
shootPizzaSignal(this);
pizzaCount--;
}
else
{
dryFireSignal(this);
}
input.shootPizza = false;
}
if (input.jump && (!isInAir || spaceMode))
{
PxRigidBody* rigid = (PxRigidBody*)actor;
rigid->addForce(PxVec3(0, 375, 0), PxForceMode::eACCELERATION);
input.jump = false;
}
}
//=================================================================================
// Multiple hits compound sweep
// AP: we might be able to improve the return results API but no time for it in 3.3
PxU32 PxRigidBodyExt::linearSweepMultiple(
PxRigidBody& body, PxScene& scene, const PxVec3& unitDir, const PxReal distance, PxHitFlags outputFlags,
PxSweepHit* hitBuffer, PxU32* hitShapeIndices, PxU32 hitBufferSize, PxSweepHit& block, PxI32& blockingHitShapeIndex,
bool& overflow, const PxQueryFilterData& filterData, PxQueryFilterCallback* filterCall,
const PxQueryCache* cache, const PxReal inflation)
{
overflow = false;
blockingHitShapeIndex = -1;
for (PxU32 i = 0; i < hitBufferSize; i++)
hitShapeIndices[i] = 0xFFFFffff;
PxI32 sumNbResults = 0;
PxU32 nbShapes = body.getNbShapes();
PxF32 shrunkMaxDistance = distance;
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();
PxU32 bufSizeLeft = hitBufferSize-sumNbResults;
PxSweepHit extraHit;
PxSweepBuffer buffer(bufSizeLeft == 0 ? &extraHit : hitBuffer+sumNbResults, bufSizeLeft == 0 ? 1 : hitBufferSize-sumNbResults);
scene.sweep(anyGeom.any(), pose, unitDir, shrunkMaxDistance, buffer, outputFlags, fd, filterCall, cache, inflation);
// Check and abort on overflow. Assume overflow if result count is bufSize.
PxU32 nbNewResults = buffer.getNbTouches();
overflow |= (nbNewResults >= bufSizeLeft);
if (bufSizeLeft == 0) // this is for when we used the extraHit buffer
nbNewResults = 0;
// set hitShapeIndices for each new non-blocking hit
for (PxU32 j = 0; j < nbNewResults; j++)
if (sumNbResults + PxU32(j) < hitBufferSize)
hitShapeIndices[sumNbResults+j] = i;
if (buffer.hasBlock) // there's a blocking hit in the most recent sweepMultiple results
{
// overwrite the return result blocking hit with the new blocking hit if under
if (blockingHitShapeIndex == -1 || buffer.block.distance < block.distance)
{
blockingHitShapeIndex = (PxI32)i;
block = buffer.block;
}
// Remove all the old touching hits below the new maxDist
// sumNbResults is not updated yet at this point
// and represents the count accumulated so far excluding the very last query
PxI32 nbNewResultsSigned = PxI32(nbNewResults); // need a signed version, see nbNewResultsSigned-- below
for (PxI32 j = sumNbResults-1; j >= 0; j--) // iterate over "old" hits (up to shapeIndex-1)
if (buffer.block.distance < hitBuffer[j].distance)
{
// overwrite with last "new" hit
PxI32 sourceIndex = PxI32(sumNbResults)+nbNewResultsSigned-1; PX_ASSERT(sourceIndex >= j);
hitBuffer[j] = hitBuffer[sourceIndex];
hitShapeIndices[j] = hitShapeIndices[sourceIndex];
nbNewResultsSigned--; // can get negative, that means we are shifting the last results array
}
sumNbResults += nbNewResultsSigned;
} else // if there was no new blocking hit we don't need to do anything special, simply append all results to touch array
sumNbResults += nbNewResults;
PX_ASSERT(sumNbResults >= 0 && sumNbResults <= PxI32(hitBufferSize));
}
return (PxU32)sumNbResults;
}
static bool computeMassAndInertia(bool multipleMassOrDensity, PxRigidBody& body, const PxReal* densities, const PxReal* masses, PxU32 densityOrMassCount, Ext::InertiaTensorComputer& computer)
{
PX_ASSERT(!densities || !masses);
PX_ASSERT((densities || masses) && (densityOrMassCount > 0));
Ext::InertiaTensorComputer inertiaComp(true);
Ps::InlineArray<PxShape*, 16> shapes("PxShape*"); shapes.resize(body.getNbShapes());
body.getShapes(shapes.begin(), shapes.size());
PxU32 validShapeIndex = 0;
PxReal currentMassOrDensity;
const PxReal* massOrDensityArray;
if (densities)
{
massOrDensityArray = densities;
currentMassOrDensity = densities[0];
}
else
{
massOrDensityArray = masses;
currentMassOrDensity = masses[0];
}
if (!PxIsFinite(currentMassOrDensity))
{
Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__,
"computeMassAndInertia: Provided mass or density has no valid value");
return false;
}
for(PxU32 i=0; i < shapes.size(); i++)
{
if (!(shapes[i]->getFlags() & PxShapeFlag::eSIMULATION_SHAPE))
continue;
if (multipleMassOrDensity)
{
if (validShapeIndex < densityOrMassCount)
{
currentMassOrDensity = massOrDensityArray[validShapeIndex];
if (!PxIsFinite(currentMassOrDensity))
{
Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__,
"computeMassAndInertia: Provided mass or density has no valid value");
return false;
}
}
else
{
Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__,
"computeMassAndInertia: Not enough mass/density values provided for all simulation shapes");
return false;
}
}
Ext::InertiaTensorComputer it(false);
switch(shapes[i]->getGeometryType())
{
case PxGeometryType::eSPHERE :
{
PxSphereGeometry g;
bool ok = shapes[i]->getSphereGeometry(g);
PX_ASSERT(ok);
PX_UNUSED(ok);
PxTransform temp(shapes[i]->getLocalPose());
it.setSphere(g.radius, &temp);
}
break;
case PxGeometryType::eBOX :
{
PxBoxGeometry g;
bool ok = shapes[i]->getBoxGeometry(g);
PX_ASSERT(ok);
PX_UNUSED(ok);
PxTransform temp(shapes[i]->getLocalPose());
it.setBox(g.halfExtents, &temp);
}
break;
case PxGeometryType::eCAPSULE :
{
PxCapsuleGeometry g;
bool ok = shapes[i]->getCapsuleGeometry(g);
PX_ASSERT(ok);
PX_UNUSED(ok);
PxTransform temp(shapes[i]->getLocalPose());
it.setCapsule(0, g.radius, g.halfHeight, &temp);
}
break;
case PxGeometryType::eCONVEXMESH :
{
PxConvexMeshGeometry g;
bool ok = shapes[i]->getConvexMeshGeometry(g);
PX_ASSERT(ok);
PX_UNUSED(ok);
PxConvexMesh& convMesh = *g.convexMesh;
PxReal convMass;
PxMat33 convInertia;
PxVec3 convCoM;
convMesh.getMassInformation(convMass, reinterpret_cast<PxMat33&>(convInertia), convCoM);
//scale the mass:
convMass *= (g.scale.scale.x * g.scale.scale.y * g.scale.scale.z);
convCoM = g.scale.rotation.rotateInv(g.scale.scale.multiply(g.scale.rotation.rotate(convCoM)));
convInertia = Ext::MassProps::scaleInertia(convInertia, g.scale.rotation, g.scale.scale);
it = Ext::InertiaTensorComputer(convInertia, convCoM, convMass);
it.transform(shapes[i]->getLocalPose());
}
break;
default :
{
Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__,
"computeMassAndInertia: Dynamic actor with illegal collision shapes");
return false;
}
}
if (densities)
it.scaleDensity(currentMassOrDensity);
else if (multipleMassOrDensity) // mass per shape -> need to scale density per shape
it.scaleDensity(currentMassOrDensity / it.getMass());
inertiaComp.add(it);
validShapeIndex++;
}
if (validShapeIndex && masses && (!multipleMassOrDensity)) // at least one simulation shape and single mass for all shapes -> scale density at the end
{
inertiaComp.scaleDensity(currentMassOrDensity / inertiaComp.getMass());
}
computer = inertiaComp;
return true;
}
