qphysxdynamicbody.cpp

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

Revision control