qphysxdynamicbody.cpp

Go to the documentation of this file.

// Copyright (C) 2023 The Qt Company Ltd.
// SPDX-License-Identifier: LicenseRef-Qt-Commercial OR GPL-3.0-only
// Qt-Security score:significant reason:default
 
#include "qphysxdynamicbody_p.h"
 
#include "PxPhysics.h"
#include "PxRigidDynamic.h"
 
#include "qphysicscommands_p.h"
#include "qphysicsutils_p.h"
#include "qphysicsworld_p.h"
#include "qabstractphysicsbody_p.h"
#include "qdynamicrigidbody_p.h"
#include "qstaticphysxobjects_p.h"
 
#include <QtGui/qquaternion.h>
 
QT_BEGIN_NAMESPACE
 
static void processCommandQueue(QQueue<QPhysicsCommand *> &commandQueue,
                                const QDynamicRigidBody &rigidBody, physx::PxRigidDynamic &body)
{
    for (auto command : std::as_const(commandQueue)) {
        command->execute(rigidBody, body);
        delete command;
    }
 
    commandQueue.clear();
}
 
static QMatrix4x4 calculateKinematicNodeTransform(QQuick3DNode *node,
                                                  QHash<QQuick3DNode *, QMatrix4x4> &transformCache)
{
    // already calculated transform
    if (transformCache.contains(node))
        return transformCache[node];
 
    QMatrix4x4 localTransform;
 
    // DynamicRigidBody vs StaticRigidBody use different values for calculating the local transform
    if (auto drb = qobject_cast<const QDynamicRigidBody *>(node); drb != nullptr) {
        if (!drb->isKinematic()) {
            qWarning() << "Non-kinematic body as a parent of a kinematic body is unsupported";
        }
        localTransform = QSSGRenderNode::calculateTransformMatrix(
                drb->kinematicPosition(), drb->scale(), drb->kinematicPivot(),
                drb->kinematicRotation());
    } else {
        localTransform = QSSGRenderNode::calculateTransformMatrix(node->position(), node->scale(),
                                                                  node->pivot(), node->rotation());
    }
 
    QQuick3DNode *parent = node->parentNode();
    if (!parent) // no parent, local transform is scene transform
        return localTransform;
 
    // calculate the parent scene transform and apply the nodes local transform
    QMatrix4x4 parentTransform = calculateKinematicNodeTransform(parent, transformCache);
    QMatrix4x4 sceneTransform = parentTransform * localTransform;
 
    transformCache[node] = sceneTransform;
    return sceneTransform;
}
 
static physx::PxRigidDynamicLockFlags getLockFlags(QDynamicRigidBody *body)
{
    const auto lockAngular = body->angularAxisLock();
    const auto lockLinear = body->linearAxisLock();
    const int flags = (lockAngular & QDynamicRigidBody::AxisLock::LockX
                               ? physx::PxRigidDynamicLockFlag::eLOCK_ANGULAR_X
                               : 0)
            | (lockAngular & QDynamicRigidBody::AxisLock::LockY
                       ? physx::PxRigidDynamicLockFlag::eLOCK_ANGULAR_Y
                       : 0)
            | (lockAngular & QDynamicRigidBody::AxisLock::LockZ
                       ? physx::PxRigidDynamicLockFlag::eLOCK_ANGULAR_Z
                       : 0)
            | (lockLinear & QDynamicRigidBody::AxisLock::LockX
                       ? physx::PxRigidDynamicLockFlag::eLOCK_LINEAR_X
                       : 0)
            | (lockLinear & QDynamicRigidBody::AxisLock::LockY
                       ? physx::PxRigidDynamicLockFlag::eLOCK_LINEAR_Y
                       : 0)
            | (lockLinear & QDynamicRigidBody::AxisLock::LockZ
                       ? physx::PxRigidDynamicLockFlag::eLOCK_LINEAR_Z
                       : 0);
    return static_cast<physx::PxRigidDynamicLockFlags>(flags);
}
 
static physx::PxTransform getPhysXWorldTransform(const QMatrix4x4 transform)
{
    auto rotationMatrix = transform;
    QSSGUtils::mat44::normalize(rotationMatrix);
    auto rotation =
            QQuaternion::fromRotationMatrix(QSSGUtils::mat44::getUpper3x3(rotationMatrix)).normalized();
    const QVector3D worldPosition = QSSGUtils::mat44::getPosition(transform);
    return physx::PxTransform(QPhysicsUtils::toPhysXType(worldPosition),
                              QPhysicsUtils::toPhysXType(rotation));
}
 
QPhysXDynamicBody::QPhysXDynamicBody(QDynamicRigidBody *frontEnd) : QPhysXRigidBody(frontEnd) { }
 
void QPhysXDynamicBody::createActor(QPhysXWorld *physX)
{
    auto *dynamicRigidBody = static_cast<QDynamicRigidBody *>(frontendNode);
    if (!dynamicRigidBody->isKinematic()) {
        QPhysXRigidBody::createActor(physX);
        return;
    }
 
    // A kinematic body is never moved by setGlobalPose()/the plain position/rotation
    // properties; it's driven by setKinematicTarget() using kinematicPosition()/
    // kinematicRotation() (see sync() below). If the actor were created at the plain,
    // usually-unset position/rotation instead, the first simulated step would snap it
    // over to its real kinematic pose in one step, which can violently yank anything
    // jointed to it. So the initial pose must already match the kinematic one.
    QHash<QQuick3DNode *, QMatrix4x4> transformCache;
    const QMatrix4x4 transform = calculateKinematicNodeTransform(dynamicRigidBody, transformCache);
    physx::PxTransform trf = getPhysXWorldTransform(transform);
    if (!trf.isSane()) {
        qWarning() << "DynamicRigidBody: kinematic position/rotation is not finite, using "
                      "identity instead.";
        trf = physx::PxTransform(physx::PxIdentity);
    }
    auto &s_physx = StaticPhysXObjects::getReference();
    actor = s_physx.physics->createRigidDynamic(trf);
}
 
DebugDrawBodyType QPhysXDynamicBody::getDebugDrawBodyType()
{
    auto *dynamicRigidBody = static_cast<QDynamicRigidBody *>(frontendNode);
    return dynamicRigidBody->isSleeping() ? DebugDrawBodyType::DynamicSleeping
                                          : DebugDrawBodyType::DynamicAwake;
}
 
void QPhysXDynamicBody::sync(float deltaTime, QHash<QQuick3DNode *, QMatrix4x4> &transformCache)
{
    auto *dynamicRigidBody = static_cast<QDynamicRigidBody *>(frontendNode);
    // first update front end node from physx simulation
    dynamicRigidBody->updateFromPhysicsTransform(actor->getGlobalPose());
 
    // The writes in updateFromPhysicsTransform() ran the body's bindings,
    // which can delete it.
    if (!frontendNode)
        return;
 
    auto *dynamicActor = static_cast<physx::PxRigidDynamic *>(actor);
    processCommandQueue(dynamicRigidBody->commandQueue(), *dynamicRigidBody, *dynamicActor);
 
    const bool disabledPrevious = actor->getActorFlags() & physx::PxActorFlag::eDISABLE_SIMULATION;
    const bool disabled = !dynamicRigidBody->simulationEnabled();
 
    // A body whose simulation is disabled has no simulation object behind it, and
    // PxRigidDynamic::setKinematicTarget() dereferences that object without checking, so it
    // must not be called for such a body. Nothing is lost by skipping it: the target is
    // recomputed and set again on the first frame the body takes part in the simulation.
    if (dynamicRigidBody->isKinematic() && !disabled && !disabledPrevious) {
        // Since this is a kinematic body we need to calculate the transform by hand and since
        // bodies can occur in other bodies we need to calculate the tranform recursively for all
        // parents. To save some computation we cache these transforms in 'transformCache'.
        QMatrix4x4 transform = calculateKinematicNodeTransform(dynamicRigidBody, transformCache);
        const physx::PxTransform worldTransform = getPhysXWorldTransform(transform);
        if (worldTransform.isSane()) {
            dynamicActor->setKinematicTarget(worldTransform);
        } else {
            qWarning() << "DynamicRigidBody: kinematic transform is not finite, keeping "
                          "previous target.";
        }
    } else if (!dynamicRigidBody->isKinematic()) {
        dynamicActor->setRigidDynamicLockFlags(getLockFlags(dynamicRigidBody));
    }
 
    if (disabled != disabledPrevious) {
        actor->setActorFlag(physx::PxActorFlag::eDISABLE_SIMULATION, disabled);
        if (!disabled && !dynamicRigidBody->isKinematic())
            dynamicActor->wakeUp();
    }
 
    // Read once the queued commands and the simulation flag are applied, so that a velocity set
    // for this frame is reported in it, and a body disabled in it reads zero, as it also reads
    // as sleeping.
    dynamicRigidBody->updateLinearVelocity(
            QPhysicsUtils::toQtType(dynamicActor->getLinearVelocity()));
    dynamicRigidBody->updateAngularVelocity(
            QPhysicsUtils::toQtType(dynamicActor->getAngularVelocity()));
    dynamicRigidBody->setIsSleeping(dynamicActor->isSleeping());
 
    // The velocity and sleeping updates ran the body's bindings, which can delete the body.
    if (!frontendNode)
        return;
 
    QPhysXActorBody::sync(deltaTime, transformCache);
}
 
void QPhysXDynamicBody::rebuildDirtyShapes(QPhysicsWorld *world, QPhysXWorld *physX)
{
    if (!shapesDirty())
        return;
 
    QDynamicRigidBody *drb = static_cast<QDynamicRigidBody *>(frontendNode);
 
    // Before buildShapes(), since setIsKinematic() runs the body's bindings and
    // can delete it, which would leave the shapes half built.
    if (drb->hasStaticShapes() && !drb->isKinematic()) {
        // Body with static shapes that is not kinematic, this is disallowed
        qWarning() << "Cannot make body containing trimesh/heightfield/plane non-kinematic, "
                      "forcing kinematic.";
        drb->setIsKinematic(true);
        if (!frontendNode)
            return;
    }
 
    const bool isKinematic = drb->isKinematic();
    auto *dynamicBody = static_cast<physx::PxRigidDynamic *>(actor);
 
    // 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.
    dynamicBody->setRigidBodyFlag(physx::PxRigidBodyFlag::eENABLE_CCD, false); // Sweep-based
    dynamicBody->setRigidBodyFlag(physx::PxRigidBodyFlag::eENABLE_SPECULATIVE_CCD, false);
 
    // Becoming kinematic has to be told before the shapes are built: PhysX
    // refuses to attach a trimesh, heightfield or plane simulation shape to a
    // body that is not kinematic yet, and drops it without telling the caller
    // anything it checks.
    if (isKinematic)
        dynamicBody->setRigidBodyFlag(physx::PxRigidBodyFlag::eKINEMATIC, true);
 
    buildShapes(physX);
 
    // Ceasing to be kinematic has to wait until they have been, for the mirror
    // reason: PhysX refuses to take eKINEMATIC off a body while such a shape is
    // still attached, so the flag can only be dropped once buildShapes() has
    // detached the shapes it objects to.
    if (!isKinematic)
        dynamicBody->setRigidBodyFlag(physx::PxRigidBodyFlag::eKINEMATIC, false);
 
    // Density must be set after shapes so the inertia tensor is set
    if (!drb->hasStaticShapes()) {
        // Body with only dynamic shapes, set/calculate mass
        QPhysicsCommand *command = nullptr;
        switch (drb->massMode()) {
        case QDynamicRigidBody::MassMode::DefaultDensity: {
            command = new QPhysicsCommandSetDensity(world->defaultDensity());
            break;
        }
        case QDynamicRigidBody::MassMode::CustomDensity: {
            command = new QPhysicsCommandSetDensity(drb->density());
            break;
        }
        case QDynamicRigidBody::MassMode::Mass: {
            const float mass = qMax(drb->mass(), 0.f);
            command = new QPhysicsCommandSetMass(mass);
            break;
        }
        case QDynamicRigidBody::MassMode::MassAndInertiaTensor: {
            const float mass = qMax(drb->mass(), 0.f);
            command = new QPhysicsCommandSetMassAndInertiaTensor(mass, drb->inertiaTensor());
            break;
        }
        case QDynamicRigidBody::MassMode::MassAndInertiaMatrix: {
            const float mass = qMax(drb->mass(), 0.f);
            command = new QPhysicsCommandSetMassAndInertiaMatrix(mass, drb->inertiaMatrix());
            break;
        }
        }
 
        drb->commandQueue().enqueue(command);
    }
 
    QDynamicRigidBody::CCDType ccd = drb->ccd();
 
    switch (ccd) {
    case QDynamicRigidBody::CCDType::SweepBasedCCD: {
        // Kinematic bodies only support speculative CCD
        dynamicBody->setRigidBodyFlag(physx::PxRigidBodyFlag::eENABLE_CCD, // Sweep-based
                                      !isKinematic);
        dynamicBody->setRigidBodyFlag(physx::PxRigidBodyFlag::eENABLE_SPECULATIVE_CCD, isKinematic);
        break;
    }
 
    case QDynamicRigidBody::CCDType::SpeculativeCCD: {
        dynamicBody->setRigidBodyFlag(physx::PxRigidBodyFlag::eENABLE_SPECULATIVE_CCD, true);
        break;
    }
 
    case QDynamicRigidBody::CCDType::None: {
        if (world->enableCCD()) {
            // Kinematic bodies only support speculative CCD
            dynamicBody->setRigidBodyFlag(physx::PxRigidBodyFlag::eENABLE_CCD, // Sweep-based
                                          !isKinematic);
            dynamicBody->setRigidBodyFlag(physx::PxRigidBodyFlag::eENABLE_SPECULATIVE_CCD,
                                          isKinematic);
        }
        break;
    }
    }
 
    setShapesDirty(false);
}
 
void QPhysXDynamicBody::updateDefaultDensity(float density)
{
    QDynamicRigidBody *rigidBody = static_cast<QDynamicRigidBody *>(frontendNode);
    rigidBody->updateDefaultDensity(density);
}
 
QT_END_NAMESPACE