Revision control
Copy as Markdown
// Copyright (C) 2021 The Qt Company Ltd.
// SPDX-License-Identifier: LicenseRef-Qt-Commercial OR GPL-3.0-only
// Qt-Security score:significant reason:default
#include "qphysicsworld_p.h"
#include "qphysicscommands_p.h"
#include "qphysicsutils_p.h"
#include "qdynamicrigidbody_p.h"
#include "PxPhysicsAPI.h"
#include <QtGui/qquaternion.h>
QT_BEGIN_NAMESPACE
static bool isKinematicBody(physx::PxRigidDynamic &body)
{
return static_cast<bool>(body.getRigidBodyFlags() & physx::PxRigidBodyFlag::eKINEMATIC);
}
// A body with its simulation disabled has no simulation object behind it, and the PhysX
// functions that accumulate forces and impulses dereference that object without checking, so
// they must not be called for such a body. Forces on a body that is not being simulated would
// have no effect anyway.
static bool isSimulationDisabled(physx::PxRigidBody &body)
{
return static_cast<bool>(body.getActorFlags() & physx::PxActorFlag::eDISABLE_SIMULATION);
}
static void resolveCCDFlags(physx::PxRigidBody &body, QDynamicRigidBody::CCDType ccd,
bool isKinematic, bool worldEnableCCD)
{
switch (ccd) {
case QDynamicRigidBody::CCDType::SweepBasedCCD: {
// Kinematic bodies only support speculative CCD
body.setRigidBodyFlag(physx::PxRigidBodyFlag::eENABLE_CCD, !isKinematic); // Sweep-based
body.setRigidBodyFlag(physx::PxRigidBodyFlag::eENABLE_SPECULATIVE_CCD, isKinematic);
break;
}
case QDynamicRigidBody::CCDType::SpeculativeCCD: {
body.setRigidBodyFlag(physx::PxRigidBodyFlag::eENABLE_SPECULATIVE_CCD, true);
break;
}
case QDynamicRigidBody::CCDType::None: {
if (worldEnableCCD) {
// Kinematic bodies only support speculative CCD
body.setRigidBodyFlag(physx::PxRigidBodyFlag::eENABLE_CCD, !isKinematic); // Sweep-based
body.setRigidBodyFlag(physx::PxRigidBodyFlag::eENABLE_SPECULATIVE_CCD, isKinematic);
}
break;
}
}
}
QPhysicsCommand::~QPhysicsCommand()
= default;
QPhysicsCommandApplyCentralForce::QPhysicsCommandApplyCentralForce(const QVector3D &inForce)
: QPhysicsCommand(), force(inForce)
{
}
QPhysicsCommandApplyCentralForce::~QPhysicsCommandApplyCentralForce()
= default;
void QPhysicsCommandApplyCentralForce::execute(const QDynamicRigidBody &rigidBody,
physx::PxRigidDynamic &body)
{
Q_UNUSED(rigidBody)
if (isKinematicBody(body) || isSimulationDisabled(body))
return;
body.addForce(QPhysicsUtils::toPhysXType(force));
}
QPhysicsCommandApplyForce::QPhysicsCommandApplyForce(const QVector3D &inForce,
const QVector3D &inPosition)
: QPhysicsCommand(), force(inForce), position(inPosition)
{
}
QPhysicsCommandApplyForce::~QPhysicsCommandApplyForce()
= default;
void QPhysicsCommandApplyForce::execute(const QDynamicRigidBody &rigidBody,
physx::PxRigidDynamic &body)
{
Q_UNUSED(rigidBody)
if (isKinematicBody(body) || isSimulationDisabled(body))
return;
physx::PxRigidBodyExt::addForceAtPos(body, QPhysicsUtils::toPhysXType(force),
QPhysicsUtils::toPhysXType(position));
}
QPhysicsCommandApplyTorque::QPhysicsCommandApplyTorque(const QVector3D &inTorque)
: QPhysicsCommand(), torque(inTorque)
{
}
QPhysicsCommandApplyTorque::~QPhysicsCommandApplyTorque()
= default;
void QPhysicsCommandApplyTorque::execute(const QDynamicRigidBody &rigidBody,
physx::PxRigidDynamic &body)
{
Q_UNUSED(rigidBody)
if (isKinematicBody(body) || isSimulationDisabled(body))
return;
body.addTorque(QPhysicsUtils::toPhysXType(torque));
}
QPhysicsCommandApplyCentralImpulse::QPhysicsCommandApplyCentralImpulse(const QVector3D &inImpulse)
: QPhysicsCommand(), impulse(inImpulse)
{
}
QPhysicsCommandApplyCentralImpulse::~QPhysicsCommandApplyCentralImpulse()
= default;
void QPhysicsCommandApplyCentralImpulse::execute(const QDynamicRigidBody &rigidBody,
physx::PxRigidDynamic &body)
{
Q_UNUSED(rigidBody)
if (isKinematicBody(body) || isSimulationDisabled(body))
return;
body.addForce(QPhysicsUtils::toPhysXType(impulse), physx::PxForceMode::eIMPULSE);
}
QPhysicsCommandApplyImpulse::QPhysicsCommandApplyImpulse(const QVector3D &inImpulse,
const QVector3D &inPosition)
: QPhysicsCommand(), impulse(inImpulse), position(inPosition)
{
}
QPhysicsCommandApplyImpulse::~QPhysicsCommandApplyImpulse()
= default;
void QPhysicsCommandApplyImpulse::execute(const QDynamicRigidBody &rigidBody,
physx::PxRigidDynamic &body)
{
Q_UNUSED(rigidBody)
if (isKinematicBody(body) || isSimulationDisabled(body))
return;
physx::PxRigidBodyExt::addForceAtPos(body, QPhysicsUtils::toPhysXType(impulse),
QPhysicsUtils::toPhysXType(position),
physx::PxForceMode::eIMPULSE);
}
QPhysicsCommandApplyTorqueImpulse::QPhysicsCommandApplyTorqueImpulse(const QVector3D &inImpulse)
: QPhysicsCommand(), impulse(inImpulse)
{
}
QPhysicsCommandApplyTorqueImpulse::~QPhysicsCommandApplyTorqueImpulse()
= default;
void QPhysicsCommandApplyTorqueImpulse::execute(const QDynamicRigidBody &rigidBody,
physx::PxRigidDynamic &body)
{
Q_UNUSED(rigidBody)
if (isKinematicBody(body) || isSimulationDisabled(body))
return;
body.addTorque(QPhysicsUtils::toPhysXType(impulse), physx::PxForceMode::eIMPULSE);
}
QPhysicsCommandSetAngularVelocity::QPhysicsCommandSetAngularVelocity(
const QVector3D &inAngularVelocity)
: QPhysicsCommand(), angularVelocity(inAngularVelocity)
{
}
QPhysicsCommandSetAngularVelocity::~QPhysicsCommandSetAngularVelocity()
= default;
void QPhysicsCommandSetAngularVelocity::execute(const QDynamicRigidBody &rigidBody,
physx::PxRigidDynamic &body)
{
Q_UNUSED(rigidBody)
body.setAngularVelocity(QPhysicsUtils::toPhysXType(angularVelocity));
}
QPhysicsCommandSetLinearVelocity::QPhysicsCommandSetLinearVelocity(
const QVector3D &inLinearVelocity)
: QPhysicsCommand(), linearVelocity(inLinearVelocity)
{
}
QPhysicsCommandSetLinearVelocity::~QPhysicsCommandSetLinearVelocity()
= default;
void QPhysicsCommandSetLinearVelocity::execute(const QDynamicRigidBody &rigidBody,
physx::PxRigidDynamic &body)
{
Q_UNUSED(rigidBody)
body.setLinearVelocity(QPhysicsUtils::toPhysXType(linearVelocity));
}
QPhysicsCommandSetMass::QPhysicsCommandSetMass(float inMass) : QPhysicsCommand(), mass(inMass) { }
QPhysicsCommandSetMass::~QPhysicsCommandSetMass()
= default;
void QPhysicsCommandSetMass::execute(const QDynamicRigidBody &rigidBody, physx::PxRigidDynamic &body)
{
if (rigidBody.hasStaticShapes()) {
qWarning() << "Cannot set mass or density on a body containing trimesh/heightfield/plane, "
"ignoring.";
return;
}
physx::PxRigidBodyExt::setMassAndUpdateInertia(body, mass);
}
void QPhysicsCommandSetMassAndInertiaTensor::execute(const QDynamicRigidBody &rigidBody,
physx::PxRigidDynamic &body)
{
if (rigidBody.hasStaticShapes()) {
qWarning() << "Cannot set mass or density on a body containing trimesh/heightfield/plane, "
"ignoring.";
return;
}
body.setMass(mass);
body.setCMassLocalPose(
physx::PxTransform(QPhysicsUtils::toPhysXType(rigidBody.centerOfMassPosition()),
QPhysicsUtils::toPhysXType(rigidBody.centerOfMassRotation())));
body.setMassSpaceInertiaTensor(QPhysicsUtils::toPhysXType(inertia));
}
QPhysicsCommandSetMassAndInertiaMatrix::QPhysicsCommandSetMassAndInertiaMatrix(
float inMass, const QMatrix3x3 &inInertia)
: QPhysicsCommand(), mass(inMass), inertia(inInertia)
{
}
QPhysicsCommandSetMassAndInertiaMatrix::~QPhysicsCommandSetMassAndInertiaMatrix()
= default;
void QPhysicsCommandSetMassAndInertiaMatrix::execute(const QDynamicRigidBody &rigidBody,
physx::PxRigidDynamic &body)
{
if (rigidBody.hasStaticShapes()) {
qWarning() << "Cannot set mass or density on a body containing trimesh/heightfield/plane, "
"ignoring.";
return;
}
physx::PxQuat massFrame;
physx::PxVec3 diagTensor = physx::PxDiagonalize(QPhysicsUtils::toPhysXType(inertia), massFrame);
if (!QPhysicsUtils::isFinite(diagTensor) || diagTensor.x <= 0.0f || diagTensor.y <= 0.0f
|| diagTensor.z <= 0.0f) {
qWarning() << "Invalid inertiaMatrix, does not diagonalize to a positive-definite "
"tensor, ignoring.";
return;
}
body.setCMassLocalPose(physx::PxTransform(
QPhysicsUtils::toPhysXType(rigidBody.centerOfMassPosition()), massFrame));
body.setMass(mass);
body.setMassSpaceInertiaTensor(diagTensor);
}
QPhysicsCommandSetDensity::QPhysicsCommandSetDensity(float inDensity)
: QPhysicsCommand(), density(inDensity)
{
}
QPhysicsCommandSetDensity::~QPhysicsCommandSetDensity()
= default;
void QPhysicsCommandSetDensity::execute(const QDynamicRigidBody &rigidBody,
physx::PxRigidDynamic &body)
{
if (rigidBody.hasStaticShapes()) {
qWarning() << "Cannot set mass or density on a body containing trimesh/heightfield/plane, "
"ignoring.";
return;
}
physx::PxRigidBodyExt::updateMassAndInertia(body, density);
}
QPhysicsCommandSetIsKinematic::QPhysicsCommandSetIsKinematic(bool inIsKinematic,
bool worldEnableCCD)
: QPhysicsCommand(), isKinematic(inIsKinematic), worldEnableCCD(worldEnableCCD)
{
}
QPhysicsCommandSetIsKinematic::~QPhysicsCommandSetIsKinematic()
= default;
void QPhysicsCommandSetIsKinematic::execute(const QDynamicRigidBody &rigidBody,
physx::PxRigidDynamic &body)
{
if (rigidBody.hasStaticShapes() && !isKinematic) {
qWarning() << "Cannot make a body containing trimesh/heightfield/plane non-kinematic, "
"ignoring.";
return;
}
// Clear CCD before flipping kinematic mode: PhysX rejects sweep-based CCD on
// kinematic bodies, so leaving a stale CCD flag set while eKINEMATIC changes
// would trigger a spurious warning regardless of transition direction.
body.setRigidBodyFlag(physx::PxRigidBodyFlag::eENABLE_CCD, false); // Sweep-based
body.setRigidBodyFlag(physx::PxRigidBodyFlag::eENABLE_SPECULATIVE_CCD, false);
body.setRigidBodyFlag(physx::PxRigidBodyFlag::eKINEMATIC, isKinematic);
// Sync CCD flags directly since changing kinematic mode alters supported CCD types
resolveCCDFlags(body, rigidBody.ccd(), isKinematic, worldEnableCCD);
}
QPhysicsCommandSetCCD::QPhysicsCommandSetCCD(QDynamicRigidBody::CCDType ccdType,
bool worldEnableCCD)
: QPhysicsCommand(), ccdType(ccdType), worldEnableCCD(worldEnableCCD)
{
}
QPhysicsCommandSetCCD::~QPhysicsCommandSetCCD() = default;
void QPhysicsCommandSetCCD::execute(const QDynamicRigidBody &rigidBody, physx::PxRigidDynamic &body)
{
// Clear both flags first so the writes below can never collide with a stale
// flag from the previous ccd mode (PhysX rejects raising eENABLE_CCD while
// eENABLE_SPECULATIVE_CCD, or vice versa, is still set).
body.setRigidBodyFlag(physx::PxRigidBodyFlag::eENABLE_CCD, false); // Sweep-based
body.setRigidBodyFlag(physx::PxRigidBodyFlag::eENABLE_SPECULATIVE_CCD, false);
resolveCCDFlags(body, ccdType, rigidBody.isKinematic(), worldEnableCCD);
}
QPhysicsCommandSetGravityEnabled::QPhysicsCommandSetGravityEnabled(bool inGravityEnabled)
: QPhysicsCommand(), gravityEnabled(inGravityEnabled)
{
}
QPhysicsCommandSetGravityEnabled::~QPhysicsCommandSetGravityEnabled()
= default;
void QPhysicsCommandSetGravityEnabled::execute(const QDynamicRigidBody &rigidBody,
physx::PxRigidDynamic &body)
{
Q_UNUSED(rigidBody)
body.setActorFlag(physx::PxActorFlag::eDISABLE_GRAVITY, !gravityEnabled);
}
QPhysicsCommandReset::QPhysicsCommandReset(QVector3D inPosition, QVector3D inEulerRotation)
: QPhysicsCommand(), position(inPosition), eulerRotation(inEulerRotation)
{
}
QPhysicsCommandReset::~QPhysicsCommandReset()
= default;
void QPhysicsCommandReset::execute(const QDynamicRigidBody &rigidBody, physx::PxRigidDynamic &body)
{
Q_UNUSED(rigidBody)
body.setLinearVelocity(physx::PxVec3(0, 0, 0));
body.setAngularVelocity(physx::PxVec3(0, 0, 0));
auto *parentNode = rigidBody.parentNode();
QVector3D scenePosition = parentNode ? parentNode->mapPositionToScene(position) : position;
// TODO: rotation also needs to be mapped
body.setGlobalPose(physx::PxTransform(
QPhysicsUtils::toPhysXType(scenePosition),
QPhysicsUtils::toPhysXType(QQuaternion::fromEulerAngles(eulerRotation))));
}
QPhysicsCommandSetMassAndInertiaTensor::QPhysicsCommandSetMassAndInertiaTensor(
float inMass, const QVector3D &inInertia)
: QPhysicsCommand(), mass(inMass), inertia(inInertia)
{
}
QPhysicsCommandSetMassAndInertiaTensor::~QPhysicsCommandSetMassAndInertiaTensor()
= default;
QT_END_NAMESPACE