1// Copyright (C) 2022 The Qt Company Ltd.
2// SPDX-License-Identifier: LicenseRef-Qt-Commercial OR GPL-3.0-only
3
4#include "qdebugdrawhelper_p.h"
5
6#include "qcollisiondebugmeshbuilder_p.h"
7#include "qphysicsutils_p.h"
8
9#include <foundation/PxBounds3.h>
10#include <foundation/PxVec3.h>
11#include <geometry/PxConvexMesh.h>
12#include <geometry/PxTriangleMesh.h>
13#include <geometry/PxHeightField.h>
14
15#include <QQuick3DGeometry>
16
17QQuick3DGeometry *QDebugDrawHelper::generateBoxGeometry(const QVector3D &halfExtents)
18{
19 auto boxGeometry = new QQuick3DGeometry();
20 boxGeometry->clear();
21 boxGeometry->addAttribute(semantic: QQuick3DGeometry::Attribute::PositionSemantic, offset: 0,
22 componentType: QQuick3DGeometry::Attribute::ComponentType::F32Type);
23 boxGeometry->addAttribute(semantic: QQuick3DGeometry::Attribute::NormalSemantic, offset: 16,
24 componentType: QQuick3DGeometry::Attribute::ComponentType::F32Type);
25 boxGeometry->setStride(32);
26 boxGeometry->setPrimitiveType(QQuick3DGeometry::PrimitiveType::Lines);
27 boxGeometry->setBounds(min: -halfExtents, max: halfExtents);
28
29 const float x = halfExtents.x();
30 const float y = halfExtents.y();
31 const float z = halfExtents.z();
32
33 QCollisionDebugMeshBuilder builder;
34 // top
35 builder.addLine(start: QVector3D(-x, -y, z), end: QVector3D(-x, y, z));
36 builder.addLine(start: QVector3D(-x, y, z), end: QVector3D(x, y, z));
37 builder.addLine(start: QVector3D(x, y, z), end: QVector3D(x, -y, z));
38 builder.addLine(start: QVector3D(x, -y, z), end: QVector3D(-x, -y, z));
39
40 // bottom
41 builder.addLine(start: QVector3D(-x, -y, -z), end: QVector3D(-x, y, -z));
42 builder.addLine(start: QVector3D(-x, y, -z), end: QVector3D(x, y, -z));
43 builder.addLine(start: QVector3D(x, y, -z), end: QVector3D(x, -y, -z));
44 builder.addLine(start: QVector3D(x, -y, -z), end: QVector3D(-x, -y, -z));
45
46 // front
47 builder.addLine(start: QVector3D(x, -y, z), end: QVector3D(x, -y, -z));
48 builder.addLine(start: QVector3D(-x, -y, -z), end: QVector3D(-x, -y, z));
49
50 // back
51 builder.addLine(start: QVector3D(x, y, z), end: QVector3D(x, y, -z));
52 builder.addLine(start: QVector3D(-x, y, -z), end: QVector3D(-x, y, z));
53
54 boxGeometry->setVertexData(builder.generateVertexArray());
55
56 return boxGeometry;
57}
58
59QQuick3DGeometry *QDebugDrawHelper::generateSphereGeometry(const float radius)
60{
61 auto sphereGeometry = new QQuick3DGeometry();
62 sphereGeometry->clear();
63 sphereGeometry->addAttribute(semantic: QQuick3DGeometry::Attribute::PositionSemantic, offset: 0,
64 componentType: QQuick3DGeometry::Attribute::ComponentType::F32Type);
65 sphereGeometry->addAttribute(semantic: QQuick3DGeometry::Attribute::NormalSemantic, offset: 16,
66 componentType: QQuick3DGeometry::Attribute::ComponentType::F32Type);
67 sphereGeometry->setStride(32);
68 sphereGeometry->setPrimitiveType(QQuick3DGeometry::PrimitiveType::Lines);
69 sphereGeometry->setBounds(min: QVector3D(-radius, -radius, -radius),
70 max: QVector3D(radius, radius, radius));
71
72 // One circle around each axis
73 // So create a 2D circle first from points
74 const int circleSegments = 24;
75 constexpr double TAU = 2 * M_PI;
76 const float step = float(TAU / circleSegments);
77 QVector<QVector2D> circlePoints;
78 for (float theta = 0; theta < TAU; theta += step) {
79 const float x = radius * qCos(v: theta);
80 const float y = radius * qSin(v: theta);
81 circlePoints.append(t: QVector2D(x, y));
82 }
83
84 QCollisionDebugMeshBuilder builder;
85 // X
86 for (int i = 0; i < circlePoints.count(); ++i) {
87 const auto refPoint1 = circlePoints[i];
88 int index2 = i + 1;
89 if (index2 == circlePoints.count())
90 index2 = 0;
91 const auto refPoint2 = circlePoints[index2];
92 const auto vertex1 = QVector3D(0.0f, refPoint1.x(), refPoint1.y());
93 const auto vertex2 = QVector3D(0.0f, refPoint2.x(), refPoint2.y());
94 builder.addLine(start: vertex1, end: vertex2, normal: QVector3D(1, 0, 0));
95 }
96
97 // Y
98 for (int i = 0; i < circlePoints.count(); ++i) {
99 const auto refPoint1 = circlePoints[i];
100 int index2 = i + 1;
101 if (index2 == circlePoints.count())
102 index2 = 0;
103 const auto refPoint2 = circlePoints[index2];
104 const auto vertex1 = QVector3D(refPoint1.x(), 0.0f, refPoint1.y());
105 const auto vertex2 = QVector3D(refPoint2.x(), 0.0f, refPoint2.y());
106 builder.addLine(start: vertex1, end: vertex2, normal: QVector3D(0, 1, 0));
107 }
108
109 // Z
110 for (int i = 0; i < circlePoints.count(); ++i) {
111 const auto refPoint1 = circlePoints[i];
112 int index2 = i + 1;
113 if (index2 == circlePoints.count())
114 index2 = 0;
115 const auto refPoint2 = circlePoints[index2];
116 const auto vertex1 = QVector3D(refPoint1.x(), refPoint1.y(), 0.0f);
117 const auto vertex2 = QVector3D(refPoint2.x(), refPoint2.y(), 0.0f);
118 builder.addLine(start: vertex1, end: vertex2, normal: QVector3D(0, 0, 1));
119 }
120 sphereGeometry->setVertexData(builder.generateVertexArray());
121
122 return sphereGeometry;
123}
124
125QQuick3DGeometry *QDebugDrawHelper::generateCapsuleGeometry(const float radius,
126 const float halfHeight)
127{
128 auto capsuleGeometry = new QQuick3DGeometry();
129 capsuleGeometry->clear();
130 capsuleGeometry->addAttribute(semantic: QQuick3DGeometry::Attribute::PositionSemantic, offset: 0,
131 componentType: QQuick3DGeometry::Attribute::ComponentType::F32Type);
132 capsuleGeometry->addAttribute(semantic: QQuick3DGeometry::Attribute::NormalSemantic, offset: 16,
133 componentType: QQuick3DGeometry::Attribute::ComponentType::F32Type);
134 capsuleGeometry->setStride(32);
135 capsuleGeometry->setPrimitiveType(QQuick3DGeometry::PrimitiveType::Lines);
136 capsuleGeometry->setBounds(min: QVector3D(-(halfHeight + radius), -radius, -radius),
137 max: QVector3D(halfHeight + radius, radius, radius));
138
139 // The total height is height+2*radius, so the height is just the height
140 // between the center of each 'sphere' of the capsule caps.
141
142 // Create a 2D circle first for points
143 const int circleSegments = 32;
144 constexpr double TAU = 2 * M_PI;
145
146 Q_ASSERT(circleSegments % 4 == 0);
147 const float step = float(TAU / circleSegments);
148 QVector<QVector2D> circlePoints;
149 for (float theta = 0; theta < TAU; theta += step) {
150 const float x = radius * qCos(v: theta);
151 const float y = radius * qSin(v: theta);
152 circlePoints.append(t: QVector2D(x, y));
153 }
154
155 QCollisionDebugMeshBuilder builder;
156
157 // Top Y Cirlce (y = height * 0.5)
158 for (int i = 0; i < circlePoints.count(); ++i) {
159 const auto refPoint1 = circlePoints[i];
160 int index2 = i + 1;
161 if (index2 == circlePoints.count())
162 index2 = 0;
163 const auto refPoint2 = circlePoints[index2];
164 const auto vertex1 = QVector3D(halfHeight, refPoint1.x(), refPoint1.y());
165 const auto vertex2 = QVector3D(halfHeight, refPoint2.x(), refPoint2.y());
166 const auto normal = QVector3D(1, 0, 0);
167 builder.addLine(start: vertex1, end: vertex2, normal);
168 }
169
170 // Bottom Y Circle (y = -height * 0.5)
171 for (int i = 0; i < circlePoints.count(); ++i) {
172 const auto refPoint1 = circlePoints[i];
173 int index2 = i + 1;
174 if (index2 == circlePoints.count())
175 index2 = 0;
176 const auto refPoint2 = circlePoints[index2];
177 const auto vertex1 = QVector3D(-halfHeight, refPoint1.x(), refPoint1.y());
178 const auto vertex2 = QVector3D(-halfHeight, refPoint2.x(), refPoint2.y());
179 const auto normal = QVector3D(1, 0, 0);
180 builder.addLine(start: vertex1, end: vertex2, normal);
181 }
182
183 // Front Cylinder Line (z = radius, y = length , x = 0)
184 {
185 const auto vertex1 = QVector3D(halfHeight, 0, radius);
186 const auto vertex2 = QVector3D(-halfHeight, 0, radius);
187 const auto normal = QVector3D(0, 0, 1);
188 builder.addLine(start: vertex1, end: vertex2, normal);
189 }
190
191 // Back Cylinder Line (z = -radius, y = length, x = 0)
192 {
193 const auto vertex1 = QVector3D(halfHeight, 0, -radius);
194 const auto vertex2 = QVector3D(-halfHeight, 0, -radius);
195 const auto normal = QVector3D(0, 0, -1);
196 builder.addLine(start: vertex1, end: vertex2, normal);
197 }
198
199 // Left Cylinder Line (x = -radius, y = length, z = 0)
200 {
201 const auto vertex1 = QVector3D(halfHeight, -radius, 0);
202 const auto vertex2 = QVector3D(-halfHeight, -radius, 0);
203 const auto normal = QVector3D(0, -1, 0);
204 builder.addLine(start: vertex1, end: vertex2, normal);
205 }
206
207 // Right Cylinder Line (x = radius, y = length, z = 0)
208 {
209 const auto vertex1 = QVector3D(halfHeight, radius, 0);
210 const auto vertex2 = QVector3D(-halfHeight, radius, 0);
211 const auto normal = QVector3D(0, 1, 0);
212 builder.addLine(start: vertex1, end: vertex2, normal);
213 }
214
215 // Get half circle values
216 QVector<int> topIndexes;
217 QVector<int> bottomIndexes;
218 {
219 const int half = circlePoints.count() / 2;
220 for (int i = 0; i < half + 1; ++i)
221 topIndexes.append(t: i);
222
223 for (int i = half; i <= circlePoints.count(); ++i) {
224 int index = i;
225 if (i >= circlePoints.count())
226 index = index - circlePoints.count();
227 bottomIndexes.append(t: index);
228 }
229 }
230
231 // Z Top Half Circle
232 for (int i = 0; i < topIndexes.count(); ++i) {
233 const auto refPoint1 = circlePoints[topIndexes[i]];
234 int index2 = i + 1;
235 if (index2 == topIndexes.count())
236 break;
237 const auto refPoint2 = circlePoints[topIndexes[index2]];
238 const auto vertex1 = QVector3D(refPoint1.y() + halfHeight, refPoint1.x(), 0.0f);
239 const auto vertex2 = QVector3D(refPoint2.y() + halfHeight, refPoint2.x(), 0.0f);
240 const auto normal = QVector3D(0, 0, 1);
241 builder.addLine(start: vertex1, end: vertex2, normal);
242 }
243
244 // Z Bottom Half Circle
245 for (int i = 0; i < bottomIndexes.count(); ++i) {
246 const auto refPoint1 = circlePoints[bottomIndexes[i]];
247 int index2 = i + 1;
248 if (index2 == bottomIndexes.count())
249 break;
250 const auto refPoint2 = circlePoints[bottomIndexes[index2]];
251 const auto vertex1 = QVector3D(refPoint1.y() - halfHeight, refPoint1.x(), 0.0f);
252 const auto vertex2 = QVector3D(refPoint2.y() - halfHeight, refPoint2.x(), 0.0f);
253 const auto normal = QVector3D(0, 0, 1);
254 builder.addLine(start: vertex1, end: vertex2, normal);
255 }
256
257 // X Top Half Circle
258 for (int i = 0; i < topIndexes.count(); ++i) {
259 const auto refPoint1 = circlePoints[topIndexes[i]];
260 int index2 = i + 1;
261 if (index2 == topIndexes.count())
262 break;
263 const auto refPoint2 = circlePoints[topIndexes[index2]];
264 const auto vertex1 = QVector3D(refPoint1.y() + halfHeight, 0.0f, refPoint1.x());
265 const auto vertex2 = QVector3D(refPoint2.y() + halfHeight, 0.0f, refPoint2.x());
266 const auto normal = QVector3D(0, 1, 0);
267 builder.addLine(start: vertex1, end: vertex2, normal);
268 }
269
270 // X Bottom Half Circle
271 for (int i = 0; i < bottomIndexes.count(); ++i) {
272 const auto refPoint1 = circlePoints[bottomIndexes[i]];
273 int index2 = i + 1;
274 if (index2 == bottomIndexes.count())
275 break;
276 const auto refPoint2 = circlePoints[bottomIndexes[index2]];
277 const auto vertex1 = QVector3D(refPoint1.y() - halfHeight, 0.0f, refPoint1.x());
278 const auto vertex2 = QVector3D(refPoint2.y() - halfHeight, 0.0f, refPoint2.x());
279 const auto normal = QVector3D(0, 1, 0);
280 builder.addLine(start: vertex1, end: vertex2, normal);
281 }
282
283 capsuleGeometry->setVertexData(builder.generateVertexArray());
284 return capsuleGeometry;
285}
286
287QQuick3DGeometry *QDebugDrawHelper::generatePlaneGeometry()
288{
289 auto planeGeometry = new QQuick3DGeometry();
290 planeGeometry->clear();
291 planeGeometry->addAttribute(semantic: QQuick3DGeometry::Attribute::PositionSemantic, offset: 0,
292 componentType: QQuick3DGeometry::Attribute::ComponentType::F32Type);
293 planeGeometry->addAttribute(semantic: QQuick3DGeometry::Attribute::NormalSemantic, offset: 16,
294 componentType: QQuick3DGeometry::Attribute::ComponentType::F32Type);
295 planeGeometry->setStride(32);
296 planeGeometry->setPrimitiveType(QQuick3DGeometry::PrimitiveType::Lines);
297
298 // TODO: Some sort of debug scale? Or a level-of-detail grid thing? => QtQuick3DHelpers
299 float s = 50;
300 float h = 5; // Set to avoid flat bounding box
301 planeGeometry->setBounds(min: { -s, -s, -h }, max: { s, s, h });
302 QCollisionDebugMeshBuilder builder;
303
304 builder.addLine(start: { -s, -s, 0 }, end: { s, -s, 0 });
305 builder.addLine(start: { -s, -s, 0 }, end: { 0, 0, 0 });
306
307 builder.addLine(start: { s, -s, 0 }, end: { s, s, 0 });
308 builder.addLine(start: { s, -s, 0 }, end: { 0, 0, 0 });
309
310 builder.addLine(start: { s, s, 0 }, end: { -s, s, 0 });
311 builder.addLine(start: { s, s, 0 }, end: { 0, 0, 0 });
312
313 builder.addLine(start: { -s, s, 0 }, end: { -s, -s, 0 });
314 builder.addLine(start: { -s, s, 0 }, end: { 0, 0, 0 });
315
316 planeGeometry->setVertexData(builder.generateVertexArray());
317 return planeGeometry;
318}
319
320QQuick3DGeometry *QDebugDrawHelper::generateHeightFieldGeometry(physx::PxHeightField *heightField,
321 float heightScale, float rowScale,
322 float columnScale)
323{
324 if (!heightField || heightField->getNbRows() < 2 || heightField->getNbColumns() < 2)
325 return nullptr;
326
327 auto geometry = new QQuick3DGeometry();
328 geometry->clear();
329 geometry->addAttribute(semantic: QQuick3DGeometry::Attribute::PositionSemantic, offset: 0,
330 componentType: QQuick3DGeometry::Attribute::ComponentType::F32Type);
331 geometry->addAttribute(semantic: QQuick3DGeometry::Attribute::NormalSemantic, offset: 16,
332 componentType: QQuick3DGeometry::Attribute::ComponentType::F32Type);
333 geometry->setStride(32);
334 geometry->setPrimitiveType(QQuick3DGeometry::PrimitiveType::Lines);
335
336 QCollisionDebugMeshBuilder builder;
337
338 const int numRows = heightField->getNbRows();
339 const int numCols = heightField->getNbColumns();
340
341 const float sizeX = rowScale * (numRows - 1);
342 const float sizeZ = columnScale * (numCols - 1);
343
344 const float heightF = heightScale;
345
346 float minHeight = 0.f;
347 float maxHeight = 0.f;
348
349 auto sample = [&](int row, int col) -> QVector3D {
350 const float height = heightField->getSample(row, column: col).height * heightF;
351 maxHeight = qMax(a: maxHeight, b: height);
352 minHeight = qMin(a: minHeight, b: height);
353 return QVector3D(row * rowScale, height, col * columnScale);
354 };
355
356 for (int row = 0; row < numRows; row++) {
357 for (int col = 0; col < numCols; col++) {
358 if (row < numRows - 1)
359 builder.addLine(start: sample(row, col), end: sample(row + 1, col));
360 if (col < numCols - 1)
361 builder.addLine(start: sample(row, col), end: sample(row, col + 1));
362 }
363 }
364
365 geometry->setBounds(min: QVector3D(0, minHeight, 0), max: QVector3D(sizeX, maxHeight, sizeZ));
366 geometry->setVertexData(builder.generateVertexArray());
367 return geometry;
368}
369
370QQuick3DGeometry *QDebugDrawHelper::generateConvexMeshGeometry(physx::PxConvexMesh *convexMesh)
371{
372 if (!convexMesh)
373 return nullptr;
374
375 auto geometry = new QQuick3DGeometry();
376 geometry->clear();
377 geometry->addAttribute(semantic: QQuick3DGeometry::Attribute::PositionSemantic, offset: 0,
378 componentType: QQuick3DGeometry::Attribute::ComponentType::F32Type);
379 geometry->addAttribute(semantic: QQuick3DGeometry::Attribute::NormalSemantic, offset: 16,
380 componentType: QQuick3DGeometry::Attribute::ComponentType::F32Type);
381 geometry->setStride(32);
382 geometry->setPrimitiveType(QQuick3DGeometry::PrimitiveType::Lines);
383
384 QCollisionDebugMeshBuilder builder;
385
386 const physx::PxU32 nbPolys = convexMesh->getNbPolygons();
387 const physx::PxU8 *polygons = convexMesh->getIndexBuffer();
388 const physx::PxVec3 *verts = convexMesh->getVertices();
389 const physx::PxU32 nbVerts = convexMesh->getNbVertices();
390
391 physx::PxHullPolygon data;
392 for (physx::PxU32 i = 0; i < nbPolys; i++) {
393 convexMesh->getPolygonData(index: i, data);
394
395 Q_ASSERT(data.mNbVerts > 2);
396 const physx::PxU32 nbTris = physx::PxU32(data.mNbVerts - 2);
397 const physx::PxU8 vref0 = polygons[data.mIndexBase + 0];
398 Q_ASSERT(vref0 < nbVerts);
399
400 for (physx::PxU32 j = 0; j < nbTris; j++) {
401 const physx::PxU32 vref1 = polygons[data.mIndexBase + 0 + j + 1];
402 const physx::PxU32 vref2 = polygons[data.mIndexBase + 0 + j + 2];
403 Q_ASSERT(vref1 < nbVerts);
404 Q_ASSERT(vref2 < nbVerts);
405
406 const QVector3D p0 = QPhysicsUtils::toQtType(vec: verts[vref0]);
407 const QVector3D p1 = QPhysicsUtils::toQtType(vec: verts[vref1]);
408 const QVector3D p2 = QPhysicsUtils::toQtType(vec: verts[vref2]);
409
410 builder.addLine(start: p0, end: p1);
411 builder.addLine(start: p1, end: p2);
412 builder.addLine(start: p2, end: p0);
413 }
414 }
415
416 auto bounds = convexMesh->getLocalBounds();
417
418 geometry->setBounds(min: QPhysicsUtils::toQtType(vec: bounds.minimum),
419 max: QPhysicsUtils::toQtType(vec: bounds.maximum));
420 geometry->setVertexData(builder.generateVertexArray());
421 return geometry;
422}
423
424QQuick3DGeometry *
425QDebugDrawHelper::generateTriangleMeshGeometry(physx::PxTriangleMesh *triangleMesh)
426{
427 if (!triangleMesh)
428 return nullptr;
429
430 auto geometry = new QQuick3DGeometry();
431 geometry->clear();
432 geometry->addAttribute(semantic: QQuick3DGeometry::Attribute::PositionSemantic, offset: 0,
433 componentType: QQuick3DGeometry::Attribute::ComponentType::F32Type);
434 geometry->addAttribute(semantic: QQuick3DGeometry::Attribute::NormalSemantic, offset: 16,
435 componentType: QQuick3DGeometry::Attribute::ComponentType::F32Type);
436 geometry->setStride(32);
437 geometry->setPrimitiveType(QQuick3DGeometry::PrimitiveType::Lines);
438
439 QCollisionDebugMeshBuilder builder;
440
441 const physx::PxU32 triangleCount = triangleMesh->getNbTriangles();
442 const physx::PxU32 has16BitIndices =
443 triangleMesh->getTriangleMeshFlags() & physx::PxTriangleMeshFlag::e16_BIT_INDICES;
444 const void *indexBuffer = triangleMesh->getTriangles();
445 const physx::PxVec3 *vertexBuffer = triangleMesh->getVertices();
446 const physx::PxU32 *intIndices = reinterpret_cast<const physx::PxU32 *>(indexBuffer);
447 const physx::PxU16 *shortIndices = reinterpret_cast<const physx::PxU16 *>(indexBuffer);
448 for (physx::PxU32 i = 0; i < triangleCount; ++i) {
449 physx::PxVec3 triVert[3];
450
451 if (has16BitIndices) {
452 triVert[0] = vertexBuffer[*shortIndices++];
453 triVert[1] = vertexBuffer[*shortIndices++];
454 triVert[2] = vertexBuffer[*shortIndices++];
455 } else {
456 triVert[0] = vertexBuffer[*intIndices++];
457 triVert[1] = vertexBuffer[*intIndices++];
458 triVert[2] = vertexBuffer[*intIndices++];
459 }
460
461 const QVector3D p0 = QPhysicsUtils::toQtType(vec: triVert[0]);
462 const QVector3D p1 = QPhysicsUtils::toQtType(vec: triVert[1]);
463 const QVector3D p2 = QPhysicsUtils::toQtType(vec: triVert[2]);
464
465 builder.addLine(start: p0, end: p1);
466 builder.addLine(start: p1, end: p2);
467 builder.addLine(start: p2, end: p0);
468 }
469
470 auto bounds = triangleMesh->getLocalBounds();
471
472 geometry->setBounds(min: QPhysicsUtils::toQtType(vec: bounds.minimum),
473 max: QPhysicsUtils::toQtType(vec: bounds.maximum));
474 geometry->setVertexData(builder.generateVertexArray());
475 return geometry;
476}
477

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