Revision control
Copy as Markdown
// 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