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

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