qphysicscommands.cpp

Go to the documentation of this file.

// 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