1// Copyright (C) 2023 The Qt Company Ltd.
2// SPDX-License-Identifier: LicenseRef-Qt-Commercial OR GPL-3.0-only
3
4#include "qphysxdynamicbody_p.h"
5
6#include "PxRigidDynamic.h"
7
8#include "qphysicscommands_p.h"
9#include "qphysicsutils_p.h"
10#include "qphysicsworld_p.h"
11#include "qabstractphysicsbody_p.h"
12#include "qdynamicrigidbody_p.h"
13
14QT_BEGIN_NAMESPACE
15
16static void processCommandQueue(QQueue<QPhysicsCommand *> &commandQueue,
17 const QDynamicRigidBody &rigidBody, physx::PxRigidBody &body)
18{
19 for (auto command : commandQueue) {
20 command->execute(rigidBody, body);
21 delete command;
22 }
23
24 commandQueue.clear();
25}
26
27static QMatrix4x4 calculateKinematicNodeTransform(QQuick3DNode *node,
28 QHash<QQuick3DNode *, QMatrix4x4> &transformCache)
29{
30 // already calculated transform
31 if (transformCache.contains(key: node))
32 return transformCache[node];
33
34 QMatrix4x4 localTransform;
35
36 // DynamicRigidBody vs StaticRigidBody use different values for calculating the local transform
37 if (auto drb = qobject_cast<const QDynamicRigidBody *>(object: node); drb != nullptr) {
38 if (!drb->isKinematic()) {
39 qWarning() << "Non-kinematic body as a parent of a kinematic body is unsupported";
40 }
41 localTransform = QSSGRenderNode::calculateTransformMatrix(
42 position: drb->kinematicPosition(), scale: drb->scale(), pivot: drb->kinematicPivot(),
43 rotation: drb->kinematicRotation());
44 } else {
45 localTransform = QSSGRenderNode::calculateTransformMatrix(position: node->position(), scale: node->scale(),
46 pivot: node->pivot(), rotation: node->rotation());
47 }
48
49 QQuick3DNode *parent = node->parentNode();
50 if (!parent) // no parent, local transform is scene transform
51 return localTransform;
52
53 // calculate the parent scene transform and apply the nodes local transform
54 QMatrix4x4 parentTransform = calculateKinematicNodeTransform(node: parent, transformCache);
55 QMatrix4x4 sceneTransform = parentTransform * localTransform;
56
57 transformCache[node] = sceneTransform;
58 return sceneTransform;
59}
60
61static physx::PxRigidDynamicLockFlags getLockFlags(QDynamicRigidBody *body)
62{
63 const auto lockAngular = body->angularAxisLock();
64 const auto lockLinear = body->linearAxisLock();
65 const int flags = (lockAngular & QDynamicRigidBody::AxisLock::LockX
66 ? physx::PxRigidDynamicLockFlag::eLOCK_ANGULAR_X
67 : 0)
68 | (lockAngular & QDynamicRigidBody::AxisLock::LockY
69 ? physx::PxRigidDynamicLockFlag::eLOCK_ANGULAR_Y
70 : 0)
71 | (lockAngular & QDynamicRigidBody::AxisLock::LockZ
72 ? physx::PxRigidDynamicLockFlag::eLOCK_ANGULAR_Z
73 : 0)
74 | (lockLinear & QDynamicRigidBody::AxisLock::LockX
75 ? physx::PxRigidDynamicLockFlag::eLOCK_LINEAR_X
76 : 0)
77 | (lockLinear & QDynamicRigidBody::AxisLock::LockY
78 ? physx::PxRigidDynamicLockFlag::eLOCK_LINEAR_Y
79 : 0)
80 | (lockLinear & QDynamicRigidBody::AxisLock::LockZ
81 ? physx::PxRigidDynamicLockFlag::eLOCK_LINEAR_Z
82 : 0);
83 return static_cast<physx::PxRigidDynamicLockFlags>(flags);
84}
85
86static physx::PxTransform getPhysXWorldTransform(const QMatrix4x4 transform)
87{
88 auto rotationMatrix = transform;
89 QSSGUtils::mat44::normalize(m&: rotationMatrix);
90 auto rotation =
91 QQuaternion::fromRotationMatrix(rot3x3: QSSGUtils::mat44::getUpper3x3(m: rotationMatrix)).normalized();
92 const QVector3D worldPosition = QSSGUtils::mat44::getPosition(m: transform);
93 return physx::PxTransform(QPhysicsUtils::toPhysXType(qvec: worldPosition),
94 QPhysicsUtils::toPhysXType(qquat: rotation));
95}
96
97QPhysXDynamicBody::QPhysXDynamicBody(QDynamicRigidBody *frontEnd) : QPhysXRigidBody(frontEnd) { }
98
99DebugDrawBodyType QPhysXDynamicBody::getDebugDrawBodyType()
100{
101 auto dynamicActor = static_cast<physx::PxRigidDynamic *>(actor);
102 return dynamicActor->isSleeping() ? DebugDrawBodyType::DynamicSleeping
103 : DebugDrawBodyType::DynamicAwake;
104}
105
106void QPhysXDynamicBody::sync(float deltaTime, QHash<QQuick3DNode *, QMatrix4x4> &transformCache)
107{
108 auto *dynamicRigidBody = static_cast<QDynamicRigidBody *>(frontendNode);
109 // first update front end node from physx simulation
110 dynamicRigidBody->updateFromPhysicsTransform(transform: actor->getGlobalPose());
111
112 auto *dynamicActor = static_cast<physx::PxRigidDynamic *>(actor);
113 processCommandQueue(commandQueue&: dynamicRigidBody->commandQueue(), rigidBody: *dynamicRigidBody, body&: *dynamicActor);
114 if (dynamicRigidBody->isKinematic()) {
115 // Since this is a kinematic body we need to calculate the transform by hand and since
116 // bodies can occur in other bodies we need to calculate the tranform recursively for all
117 // parents. To save some computation we cache these transforms in 'transformCache'.
118 QMatrix4x4 transform = calculateKinematicNodeTransform(node: dynamicRigidBody, transformCache);
119 dynamicActor->setKinematicTarget(getPhysXWorldTransform(transform));
120 } else {
121 dynamicActor->setRigidDynamicLockFlags(getLockFlags(body: dynamicRigidBody));
122 }
123 QPhysXActorBody::sync(deltaTime, transformCache);
124}
125
126void QPhysXDynamicBody::rebuildDirtyShapes(QPhysicsWorld *world, QPhysXWorld *physX)
127{
128 if (!shapesDirty())
129 return;
130
131 buildShapes(physX);
132
133 QDynamicRigidBody *drb = static_cast<QDynamicRigidBody *>(frontendNode);
134
135 // Density must be set after shapes so the inertia tensor is set
136 if (!drb->hasStaticShapes()) {
137 // Body with only dynamic shapes, set/calculate mass
138 QPhysicsCommand *command = nullptr;
139 switch (drb->massMode()) {
140 case QDynamicRigidBody::MassMode::DefaultDensity: {
141 command = new QPhysicsCommandSetDensity(world->defaultDensity());
142 break;
143 }
144 case QDynamicRigidBody::MassMode::CustomDensity: {
145 command = new QPhysicsCommandSetDensity(drb->density());
146 break;
147 }
148 case QDynamicRigidBody::MassMode::Mass: {
149 const float mass = qMax(a: drb->mass(), b: 0.f);
150 command = new QPhysicsCommandSetMass(mass);
151 break;
152 }
153 case QDynamicRigidBody::MassMode::MassAndInertiaTensor: {
154 const float mass = qMax(a: drb->mass(), b: 0.f);
155 command = new QPhysicsCommandSetMassAndInertiaTensor(mass, drb->inertiaTensor());
156 break;
157 }
158 case QDynamicRigidBody::MassMode::MassAndInertiaMatrix: {
159 const float mass = qMax(a: drb->mass(), b: 0.f);
160 command = new QPhysicsCommandSetMassAndInertiaMatrix(mass, drb->inertiaMatrix());
161 break;
162 }
163 }
164
165 drb->commandQueue().enqueue(t: command);
166 } else if (!drb->isKinematic()) {
167 // Body with static shapes that is not kinematic, this is disallowed
168 qWarning() << "Cannot make body containing trimesh/heightfield/plane non-kinematic, "
169 "forcing kinematic.";
170 drb->setIsKinematic(true);
171 }
172
173 auto *dynamicBody = static_cast<physx::PxRigidDynamic *>(actor);
174 dynamicBody->setRigidBodyFlag(flag: physx::PxRigidBodyFlag::eKINEMATIC, value: drb->isKinematic());
175
176 if (world->enableCCD() && !drb->isKinematic()) // CCD not supported for kinematic bodies
177 dynamicBody->setRigidBodyFlag(flag: physx::PxRigidBodyFlag::eENABLE_CCD, value: true);
178
179 setShapesDirty(false);
180}
181
182void QPhysXDynamicBody::updateDefaultDensity(float density)
183{
184 QDynamicRigidBody *rigidBody = static_cast<QDynamicRigidBody *>(frontendNode);
185 rigidBody->updateDefaultDensity(defaultDensity: density);
186}
187
188QT_END_NAMESPACE
189

source code of qtquick3dphysics/src/quick3dphysics/physxnode/qphysxdynamicbody.cpp