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

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