1// Copyright (C) 2021 The Qt Company Ltd.
2// SPDX-License-Identifier: LicenseRef-Qt-Commercial OR GPL-3.0-only
3
4#include "qphysicsworld_p.h"
5#include "qphysicscommands_p.h"
6#include "qphysicsutils_p.h"
7#include "qdynamicrigidbody_p.h"
8#include "PxPhysicsAPI.h"
9
10#include <QtGui/qquaternion.h>
11
12QT_BEGIN_NAMESPACE
13
14static bool isKinematicBody(physx::PxRigidBody &body)
15{
16 return static_cast<bool>(body.getRigidBodyFlags() & physx::PxRigidBodyFlag::eKINEMATIC);
17}
18
19
20QPhysicsCommand::~QPhysicsCommand()
21 = default;
22
23QPhysicsCommandApplyCentralForce::QPhysicsCommandApplyCentralForce(const QVector3D &inForce)
24 : QPhysicsCommand(), force(inForce)
25{
26}
27
28QPhysicsCommandApplyCentralForce::~QPhysicsCommandApplyCentralForce()
29 = default;
30
31void QPhysicsCommandApplyCentralForce::execute(const QDynamicRigidBody &rigidBody,
32 physx::PxRigidBody &body)
33{
34 Q_UNUSED(rigidBody)
35 if (isKinematicBody(body))
36 return;
37 body.addForce(force: QPhysicsUtils::toPhysXType(qvec: force));
38}
39
40QPhysicsCommandApplyForce::QPhysicsCommandApplyForce(const QVector3D &inForce,
41 const QVector3D &inPosition)
42 : QPhysicsCommand(), force(inForce), position(inPosition)
43{
44}
45
46QPhysicsCommandApplyForce::~QPhysicsCommandApplyForce()
47 = default;
48
49void QPhysicsCommandApplyForce::execute(const QDynamicRigidBody &rigidBody,
50 physx::PxRigidBody &body)
51{
52 Q_UNUSED(rigidBody)
53 if (isKinematicBody(body))
54 return;
55 physx::PxRigidBodyExt::addForceAtPos(body, force: QPhysicsUtils::toPhysXType(qvec: force),
56 pos: QPhysicsUtils::toPhysXType(qvec: position));
57}
58
59QPhysicsCommandApplyTorque::QPhysicsCommandApplyTorque(const QVector3D &inTorque)
60 : QPhysicsCommand(), torque(inTorque)
61{
62}
63
64QPhysicsCommandApplyTorque::~QPhysicsCommandApplyTorque()
65 = default;
66
67void QPhysicsCommandApplyTorque::execute(const QDynamicRigidBody &rigidBody,
68 physx::PxRigidBody &body)
69{
70 Q_UNUSED(rigidBody)
71 if (isKinematicBody(body))
72 return;
73 body.addTorque(torque: QPhysicsUtils::toPhysXType(qvec: torque));
74}
75
76QPhysicsCommandApplyCentralImpulse::QPhysicsCommandApplyCentralImpulse(const QVector3D &inImpulse)
77 : QPhysicsCommand(), impulse(inImpulse)
78{
79}
80
81QPhysicsCommandApplyCentralImpulse::~QPhysicsCommandApplyCentralImpulse()
82 = default;
83
84void QPhysicsCommandApplyCentralImpulse::execute(const QDynamicRigidBody &rigidBody,
85 physx::PxRigidBody &body)
86{
87 Q_UNUSED(rigidBody)
88 if (isKinematicBody(body))
89 return;
90 body.addForce(force: QPhysicsUtils::toPhysXType(qvec: impulse), mode: physx::PxForceMode::eIMPULSE);
91}
92
93QPhysicsCommandApplyImpulse::QPhysicsCommandApplyImpulse(const QVector3D &inImpulse,
94 const QVector3D &inPosition)
95 : QPhysicsCommand(), impulse(inImpulse), position(inPosition)
96{
97}
98
99QPhysicsCommandApplyImpulse::~QPhysicsCommandApplyImpulse()
100 = default;
101
102void QPhysicsCommandApplyImpulse::execute(const QDynamicRigidBody &rigidBody,
103 physx::PxRigidBody &body)
104{
105 Q_UNUSED(rigidBody)
106 if (isKinematicBody(body))
107 return;
108 physx::PxRigidBodyExt::addForceAtPos(body, force: QPhysicsUtils::toPhysXType(qvec: impulse),
109 pos: QPhysicsUtils::toPhysXType(qvec: position),
110 mode: physx::PxForceMode::eIMPULSE);
111}
112
113QPhysicsCommandApplyTorqueImpulse::QPhysicsCommandApplyTorqueImpulse(const QVector3D &inImpulse)
114 : QPhysicsCommand(), impulse(inImpulse)
115{
116}
117
118QPhysicsCommandApplyTorqueImpulse::~QPhysicsCommandApplyTorqueImpulse()
119 = default;
120
121void QPhysicsCommandApplyTorqueImpulse::execute(const QDynamicRigidBody &rigidBody,
122 physx::PxRigidBody &body)
123{
124 Q_UNUSED(rigidBody)
125 if (isKinematicBody(body))
126 return;
127
128 body.addTorque(torque: QPhysicsUtils::toPhysXType(qvec: impulse), mode: physx::PxForceMode::eIMPULSE);
129}
130
131QPhysicsCommandSetAngularVelocity::QPhysicsCommandSetAngularVelocity(
132 const QVector3D &inAngularVelocity)
133 : QPhysicsCommand(), angularVelocity(inAngularVelocity)
134{
135}
136
137QPhysicsCommandSetAngularVelocity::~QPhysicsCommandSetAngularVelocity()
138 = default;
139
140void QPhysicsCommandSetAngularVelocity::execute(const QDynamicRigidBody &rigidBody,
141 physx::PxRigidBody &body)
142{
143 Q_UNUSED(rigidBody)
144 body.setAngularVelocity(angVel: QPhysicsUtils::toPhysXType(qvec: angularVelocity));
145}
146
147QPhysicsCommandSetLinearVelocity::QPhysicsCommandSetLinearVelocity(
148 const QVector3D &inLinearVelocity)
149 : QPhysicsCommand(), linearVelocity(inLinearVelocity)
150{
151}
152
153QPhysicsCommandSetLinearVelocity::~QPhysicsCommandSetLinearVelocity()
154 = default;
155
156void QPhysicsCommandSetLinearVelocity::execute(const QDynamicRigidBody &rigidBody,
157 physx::PxRigidBody &body)
158{
159 Q_UNUSED(rigidBody)
160 body.setLinearVelocity(linVel: QPhysicsUtils::toPhysXType(qvec: linearVelocity));
161}
162
163QPhysicsCommandSetMass::QPhysicsCommandSetMass(float inMass) : QPhysicsCommand(), mass(inMass) { }
164
165QPhysicsCommandSetMass::~QPhysicsCommandSetMass()
166 = default;
167
168void QPhysicsCommandSetMass::execute(const QDynamicRigidBody &rigidBody, physx::PxRigidBody &body)
169{
170 if (rigidBody.hasStaticShapes()) {
171 qWarning() << "Cannot set mass or density on a body containing trimesh/heightfield/plane, "
172 "ignoring.";
173 return;
174 }
175
176 physx::PxRigidBodyExt::setMassAndUpdateInertia(body, mass);
177}
178
179void QPhysicsCommandSetMassAndInertiaTensor::execute(const QDynamicRigidBody &rigidBody,
180 physx::PxRigidBody &body)
181{
182 if (rigidBody.hasStaticShapes()) {
183 qWarning() << "Cannot set mass or density on a body containing trimesh/heightfield/plane, "
184 "ignoring.";
185 return;
186 }
187
188 body.setMass(mass);
189 body.setCMassLocalPose(
190 physx::PxTransform(QPhysicsUtils::toPhysXType(qvec: rigidBody.centerOfMassPosition()),
191 QPhysicsUtils::toPhysXType(qquat: rigidBody.centerOfMassRotation())));
192 body.setMassSpaceInertiaTensor(QPhysicsUtils::toPhysXType(qvec: inertia));
193}
194
195QPhysicsCommandSetMassAndInertiaMatrix::QPhysicsCommandSetMassAndInertiaMatrix(
196 float inMass, const QMatrix3x3 &inInertia)
197 : QPhysicsCommand(), mass(inMass), inertia(inInertia)
198{
199}
200
201QPhysicsCommandSetMassAndInertiaMatrix::~QPhysicsCommandSetMassAndInertiaMatrix()
202 = default;
203
204void QPhysicsCommandSetMassAndInertiaMatrix::execute(const QDynamicRigidBody &rigidBody,
205 physx::PxRigidBody &body)
206{
207 if (rigidBody.hasStaticShapes()) {
208 qWarning() << "Cannot set mass or density on a body containing trimesh/heightfield/plane, "
209 "ignoring.";
210 return;
211 }
212
213 physx::PxQuat massFrame;
214 physx::PxVec3 diagTensor = physx::PxDiagonalize(m: QPhysicsUtils::toPhysXType(m: inertia), axes&: massFrame);
215 if ((diagTensor.x <= 0.0f) || (diagTensor.y <= 0.0f) || (diagTensor.z <= 0.0f))
216 return; // FIXME: print error?
217
218 body.setCMassLocalPose(physx::PxTransform(
219 QPhysicsUtils::toPhysXType(qvec: rigidBody.centerOfMassPosition()), massFrame));
220 body.setMass(mass);
221 body.setMassSpaceInertiaTensor(diagTensor);
222}
223
224QPhysicsCommandSetDensity::QPhysicsCommandSetDensity(float inDensity)
225 : QPhysicsCommand(), density(inDensity)
226{
227}
228
229QPhysicsCommandSetDensity::~QPhysicsCommandSetDensity()
230 = default;
231
232void QPhysicsCommandSetDensity::execute(const QDynamicRigidBody &rigidBody,
233 physx::PxRigidBody &body)
234{
235 if (rigidBody.hasStaticShapes()) {
236 qWarning() << "Cannot set mass or density on a body containing trimesh/heightfield/plane, "
237 "ignoring.";
238 return;
239 }
240
241 const float clampedDensity = qMax(a: 0.0000001, b: density);
242 if (clampedDensity != density) {
243 qWarning() << "Clamping density " << density;
244 return;
245 }
246
247 physx::PxRigidBodyExt::updateMassAndInertia(body, density: clampedDensity);
248}
249
250QPhysicsCommandSetIsKinematic::QPhysicsCommandSetIsKinematic(bool inIsKinematic)
251 : QPhysicsCommand(), isKinematic(inIsKinematic)
252{
253}
254
255QPhysicsCommandSetIsKinematic::~QPhysicsCommandSetIsKinematic()
256 = default;
257
258void QPhysicsCommandSetIsKinematic::execute(const QDynamicRigidBody &rigidBody,
259 physx::PxRigidBody &body)
260{
261 if (rigidBody.hasStaticShapes() && !isKinematic) {
262 qWarning() << "Cannot make a body containing trimesh/heightfield/plane non-kinematic, "
263 "ignoring.";
264 return;
265 }
266
267 body.setRigidBodyFlag(flag: physx::PxRigidBodyFlag::eKINEMATIC, value: isKinematic);
268}
269
270QPhysicsCommandSetGravityEnabled::QPhysicsCommandSetGravityEnabled(bool inGravityEnabled)
271 : QPhysicsCommand(), gravityEnabled(inGravityEnabled)
272{
273}
274
275QPhysicsCommandSetGravityEnabled::~QPhysicsCommandSetGravityEnabled()
276 = default;
277
278void QPhysicsCommandSetGravityEnabled::execute(const QDynamicRigidBody &rigidBody,
279 physx::PxRigidBody &body)
280{
281 Q_UNUSED(rigidBody)
282 body.setActorFlag(flag: physx::PxActorFlag::eDISABLE_GRAVITY, value: !gravityEnabled);
283}
284
285QPhysicsCommandReset::QPhysicsCommandReset(QVector3D inPosition, QVector3D inEulerRotation)
286 : QPhysicsCommand(), position(inPosition), eulerRotation(inEulerRotation)
287{
288}
289
290QPhysicsCommandReset::~QPhysicsCommandReset()
291 = default;
292
293void QPhysicsCommandReset::execute(const QDynamicRigidBody &rigidBody, physx::PxRigidBody &body)
294{
295 Q_UNUSED(rigidBody)
296 body.setLinearVelocity(linVel: physx::PxVec3(0, 0, 0));
297 body.setAngularVelocity(angVel: physx::PxVec3(0, 0, 0));
298
299 auto *parentNode = rigidBody.parentNode();
300 QVector3D scenePosition = parentNode ? parentNode->mapPositionToScene(localPosition: position) : position;
301 // TODO: rotation also needs to be mapped
302
303 body.setGlobalPose(pose: physx::PxTransform(
304 QPhysicsUtils::toPhysXType(qvec: scenePosition),
305 QPhysicsUtils::toPhysXType(qquat: QQuaternion::fromEulerAngles(angles: eulerRotation))));
306}
307
308QPhysicsCommandSetMassAndInertiaTensor::QPhysicsCommandSetMassAndInertiaTensor(
309 float inMass, const QVector3D &inInertia)
310 : QPhysicsCommand(), mass(inMass), inertia(inInertia)
311{
312}
313
314QPhysicsCommandSetMassAndInertiaTensor::~QPhysicsCommandSetMassAndInertiaTensor()
315 = default;
316
317QT_END_NAMESPACE
318

source code of qtquick3dphysics/src/quick3dphysics/qphysicscommands.cpp