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
124 const bool disabledPrevious = actor->getActorFlags() & physx::PxActorFlag::eDISABLE_SIMULATION;
125 const bool disabled = !dynamicRigidBody->simulationEnabled();
126 if (disabled != disabledPrevious) {
127 actor->setActorFlag(flag: physx::PxActorFlag::eDISABLE_SIMULATION, value: disabled);
128 if (!disabled && !dynamicRigidBody->isKinematic())
129 dynamicActor->wakeUp();
130 }
131
132 QPhysXActorBody::sync(deltaTime, transformCache);
133}
134
135void QPhysXDynamicBody::rebuildDirtyShapes(QPhysicsWorld *world, QPhysXWorld *physX)
136{
137 if (!shapesDirty())
138 return;
139
140 buildShapes(physX);
141
142 QDynamicRigidBody *drb = static_cast<QDynamicRigidBody *>(frontendNode);
143
144 // Density must be set after shapes so the inertia tensor is set
145 if (!drb->hasStaticShapes()) {
146 // Body with only dynamic shapes, set/calculate mass
147 QPhysicsCommand *command = nullptr;
148 switch (drb->massMode()) {
149 case QDynamicRigidBody::MassMode::DefaultDensity: {
150 command = new QPhysicsCommandSetDensity(world->defaultDensity());
151 break;
152 }
153 case QDynamicRigidBody::MassMode::CustomDensity: {
154 command = new QPhysicsCommandSetDensity(drb->density());
155 break;
156 }
157 case QDynamicRigidBody::MassMode::Mass: {
158 const float mass = qMax(a: drb->mass(), b: 0.f);
159 command = new QPhysicsCommandSetMass(mass);
160 break;
161 }
162 case QDynamicRigidBody::MassMode::MassAndInertiaTensor: {
163 const float mass = qMax(a: drb->mass(), b: 0.f);
164 command = new QPhysicsCommandSetMassAndInertiaTensor(mass, drb->inertiaTensor());
165 break;
166 }
167 case QDynamicRigidBody::MassMode::MassAndInertiaMatrix: {
168 const float mass = qMax(a: drb->mass(), b: 0.f);
169 command = new QPhysicsCommandSetMassAndInertiaMatrix(mass, drb->inertiaMatrix());
170 break;
171 }
172 }
173
174 drb->commandQueue().enqueue(t: command);
175 } else if (!drb->isKinematic()) {
176 // Body with static shapes that is not kinematic, this is disallowed
177 qWarning() << "Cannot make body containing trimesh/heightfield/plane non-kinematic, "
178 "forcing kinematic.";
179 drb->setIsKinematic(true);
180 }
181
182 auto *dynamicBody = static_cast<physx::PxRigidDynamic *>(actor);
183 dynamicBody->setRigidBodyFlag(flag: physx::PxRigidBodyFlag::eKINEMATIC, value: drb->isKinematic());
184
185 if (world->enableCCD() && !drb->isKinematic()) // CCD not supported for kinematic bodies
186 dynamicBody->setRigidBodyFlag(flag: physx::PxRigidBodyFlag::eENABLE_CCD, value: true);
187
188 setShapesDirty(false);
189}
190
191void QPhysXDynamicBody::updateDefaultDensity(float density)
192{
193 QDynamicRigidBody *rigidBody = static_cast<QDynamicRigidBody *>(frontendNode);
194 rigidBody->updateDefaultDensity(defaultDensity: density);
195}
196
197QT_END_NAMESPACE
198

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