qdebugdrawhelper.cpp

// Copyright (C) 2022 The Qt Company Ltd.

// SPDX-License-Identifier: LicenseRef-Qt-Commercial OR GPL-3.0-only

// Qt-Security score:significant reason:default

#include "qdebugdrawhelper_p.h"

#include "qcollisiondebugmeshbuilder_p.h"

#include "qphysicsutils_p.h"

#include <foundation/PxBounds3.h>

#include <foundation/PxVec3.h>

#include <geometry/PxConvexMesh.h>

#include <geometry/PxTriangleMesh.h>

#include <geometry/PxHeightField.h>

#include <QQuick3DGeometry>

QQuick3DGeometry *QDebugDrawHelper::generateBoxGeometry(const QVector3D &halfExtents)

    auto boxGeometry = new QQuick3DGeometry();

    boxGeometry->clear();

    boxGeometry->addAttribute(QQuick3DGeometry::Attribute::PositionSemantic, 0,

                              QQuick3DGeometry::Attribute::ComponentType::F32Type);

    boxGeometry->addAttribute(QQuick3DGeometry::Attribute::NormalSemantic, 16,

                              QQuick3DGeometry::Attribute::ComponentType::F32Type);

    boxGeometry->setStride(32);

    boxGeometry->setPrimitiveType(QQuick3DGeometry::PrimitiveType::Lines);

    boxGeometry->setBounds(-halfExtents, halfExtents);

    const float x = halfExtents.x();

    const float y = halfExtents.y();

    const float z = halfExtents.z();

    QCollisionDebugMeshBuilder builder;

    // top

    builder.addLine(QVector3D(-x, -y, z), QVector3D(-x, y, z));

    builder.addLine(QVector3D(-x, y, z), QVector3D(x, y, z));

    builder.addLine(QVector3D(x, y, z), QVector3D(x, -y, z));

    builder.addLine(QVector3D(x, -y, z), QVector3D(-x, -y, z));

    // bottom

    builder.addLine(QVector3D(-x, -y, -z), QVector3D(-x, y, -z));

    builder.addLine(QVector3D(-x, y, -z), QVector3D(x, y, -z));

    builder.addLine(QVector3D(x, y, -z), QVector3D(x, -y, -z));

    builder.addLine(QVector3D(x, -y, -z), QVector3D(-x, -y, -z));

    // front

    builder.addLine(QVector3D(x, -y, z), QVector3D(x, -y, -z));

    builder.addLine(QVector3D(-x, -y, -z), QVector3D(-x, -y, z));

    // back

    builder.addLine(QVector3D(x, y, z), QVector3D(x, y, -z));

    builder.addLine(QVector3D(-x, y, -z), QVector3D(-x, y, z));

    boxGeometry->setVertexData(builder.generateVertexArray());

    return boxGeometry;

QQuick3DGeometry *QDebugDrawHelper::generateSphereGeometry(const float radius)

    auto sphereGeometry = new QQuick3DGeometry();

    sphereGeometry->clear();

    sphereGeometry->addAttribute(QQuick3DGeometry::Attribute::PositionSemantic, 0,

                                 QQuick3DGeometry::Attribute::ComponentType::F32Type);

    sphereGeometry->addAttribute(QQuick3DGeometry::Attribute::NormalSemantic, 16,

                                 QQuick3DGeometry::Attribute::ComponentType::F32Type);

    sphereGeometry->setStride(32);

    sphereGeometry->setPrimitiveType(QQuick3DGeometry::PrimitiveType::Lines);

    sphereGeometry->setBounds(QVector3D(-radius, -radius, -radius),

                              QVector3D(radius, radius, radius));

    // One circle around each axis

    // So create a 2D circle first from points

    const int circleSegments = 24;

    constexpr double TAU = 2 * M_PI;

    const float step = float(TAU / circleSegments);

    QVector<QVector2D> circlePoints;

    for (float theta = 0; theta < TAU; theta += step) {

        const float x = radius * qCos(theta);

        const float y = radius * qSin(theta);

        circlePoints.append(QVector2D(x, y));

    QCollisionDebugMeshBuilder builder;

    // X

    for (int i = 0; i < circlePoints.count(); ++i) {

        const auto refPoint1 = circlePoints[i];

        int index2 = i + 1;

        if (index2 == circlePoints.count())

            index2 = 0;

        const auto refPoint2 = circlePoints[index2];

        const auto vertex1 = QVector3D(0.0f, refPoint1.x(), refPoint1.y());

        const auto vertex2 = QVector3D(0.0f, refPoint2.x(), refPoint2.y());

        builder.addLine(vertex1, vertex2, QVector3D(1, 0, 0));

    // Y

    for (int i = 0; i < circlePoints.count(); ++i) {

        const auto refPoint1 = circlePoints[i];

        int index2 = i + 1;

        if (index2 == circlePoints.count())

            index2 = 0;

        const auto refPoint2 = circlePoints[index2];

        const auto vertex1 = QVector3D(refPoint1.x(), 0.0f, refPoint1.y());

        const auto vertex2 = QVector3D(refPoint2.x(), 0.0f, refPoint2.y());

        builder.addLine(vertex1, vertex2, QVector3D(0, 1, 0));

    // Z

    for (int i = 0; i < circlePoints.count(); ++i) {

        const auto refPoint1 = circlePoints[i];

        int index2 = i + 1;

        if (index2 == circlePoints.count())

            index2 = 0;

        const auto refPoint2 = circlePoints[index2];

        const auto vertex1 = QVector3D(refPoint1.x(), refPoint1.y(), 0.0f);

        const auto vertex2 = QVector3D(refPoint2.x(), refPoint2.y(), 0.0f);

        builder.addLine(vertex1, vertex2, QVector3D(0, 0, 1));

    sphereGeometry->setVertexData(builder.generateVertexArray());

    return sphereGeometry;

QQuick3DGeometry *QDebugDrawHelper::generateCapsuleGeometry(const float radius,

                                                            const float halfHeight)

    auto capsuleGeometry = new QQuick3DGeometry();

    capsuleGeometry->clear();

    capsuleGeometry->addAttribute(QQuick3DGeometry::Attribute::PositionSemantic, 0,

                                  QQuick3DGeometry::Attribute::ComponentType::F32Type);

    capsuleGeometry->addAttribute(QQuick3DGeometry::Attribute::NormalSemantic, 16,

                                  QQuick3DGeometry::Attribute::ComponentType::F32Type);

    capsuleGeometry->setStride(32);

    capsuleGeometry->setPrimitiveType(QQuick3DGeometry::PrimitiveType::Lines);

    capsuleGeometry->setBounds(QVector3D(-(halfHeight + radius), -radius, -radius),

                               QVector3D(halfHeight + radius, radius, radius));

    // The total height is height+2*radius, so the height is just the height

    // between the center of each 'sphere' of the capsule caps.

    // Create a 2D circle first for points

    const int circleSegments = 32;

    constexpr double TAU = 2 * M_PI;

    Q_ASSERT(circleSegments % 4 == 0);

    const float step = float(TAU / circleSegments);

    QVector<QVector2D> circlePoints;

    for (float theta = 0; theta < TAU; theta += step) {

        const float x = radius * qCos(theta);

        const float y = radius * qSin(theta);

        circlePoints.append(QVector2D(x, y));

    QCollisionDebugMeshBuilder builder;

    // Top Y Cirlce (y = height * 0.5)

    for (int i = 0; i < circlePoints.count(); ++i) {

        const auto refPoint1 = circlePoints[i];

        int index2 = i + 1;

        if (index2 == circlePoints.count())

            index2 = 0;

        const auto refPoint2 = circlePoints[index2];

        const auto vertex1 = QVector3D(halfHeight, refPoint1.x(), refPoint1.y());

        const auto vertex2 = QVector3D(halfHeight, refPoint2.x(), refPoint2.y());

        const auto normal = QVector3D(1, 0, 0);

        builder.addLine(vertex1, vertex2, normal);

    // Bottom Y Circle (y = -height * 0.5)

    for (int i = 0; i < circlePoints.count(); ++i) {

        const auto refPoint1 = circlePoints[i];

        int index2 = i + 1;

        if (index2 == circlePoints.count())

            index2 = 0;

        const auto refPoint2 = circlePoints[index2];

        const auto vertex1 = QVector3D(-halfHeight, refPoint1.x(), refPoint1.y());

        const auto vertex2 = QVector3D(-halfHeight, refPoint2.x(), refPoint2.y());

        const auto normal = QVector3D(1, 0, 0);

        builder.addLine(vertex1, vertex2, normal);

    // Front Cylinder Line (z = radius, y = length , x = 0)

        const auto vertex1 = QVector3D(halfHeight, 0, radius);

        const auto vertex2 = QVector3D(-halfHeight, 0, radius);

        const auto normal = QVector3D(0, 0, 1);

        builder.addLine(vertex1, vertex2, normal);

    // Back Cylinder Line (z = -radius, y = length, x = 0)

        const auto vertex1 = QVector3D(halfHeight, 0, -radius);

        const auto vertex2 = QVector3D(-halfHeight, 0, -radius);

        const auto normal = QVector3D(0, 0, -1);

        builder.addLine(vertex1, vertex2, normal);

    // Left Cylinder Line (x = -radius, y = length, z = 0)

        const auto vertex1 = QVector3D(halfHeight, -radius, 0);

        const auto vertex2 = QVector3D(-halfHeight, -radius, 0);

        const auto normal = QVector3D(0, -1, 0);

        builder.addLine(vertex1, vertex2, normal);

    // Right Cylinder Line (x = radius, y = length, z = 0)

        const auto vertex1 = QVector3D(halfHeight, radius, 0);

        const auto vertex2 = QVector3D(-halfHeight, radius, 0);

        const auto normal = QVector3D(0, 1, 0);

        builder.addLine(vertex1, vertex2, normal);

    // Get half circle values

    QVector<int> topIndexes;

    QVector<int> bottomIndexes;

        const int half = circlePoints.count() / 2;

        for (int i = 0; i < half + 1; ++i)

            topIndexes.append(i);

        for (int i = half; i <= circlePoints.count(); ++i) {

            int index = i;

            if (i >= circlePoints.count())

                index = index - circlePoints.count();

            bottomIndexes.append(index);

    // Z Top Half Circle

    for (int i = 0; i < topIndexes.count(); ++i) {

        const auto refPoint1 = circlePoints[topIndexes[i]];

        int index2 = i + 1;

        if (index2 == topIndexes.count())

            break;

        const auto refPoint2 = circlePoints[topIndexes[index2]];

        const auto vertex1 = QVector3D(refPoint1.y() + halfHeight, refPoint1.x(), 0.0f);

        const auto vertex2 = QVector3D(refPoint2.y() + halfHeight, refPoint2.x(), 0.0f);

        const auto normal = QVector3D(0, 0, 1);

        builder.addLine(vertex1, vertex2, normal);

    // Z Bottom Half Circle

    for (int i = 0; i < bottomIndexes.count(); ++i) {

        const auto refPoint1 = circlePoints[bottomIndexes[i]];

        int index2 = i + 1;

        if (index2 == bottomIndexes.count())

            break;

        const auto refPoint2 = circlePoints[bottomIndexes[index2]];

        const auto vertex1 = QVector3D(refPoint1.y() - halfHeight, refPoint1.x(), 0.0f);

        const auto vertex2 = QVector3D(refPoint2.y() - halfHeight, refPoint2.x(), 0.0f);

        const auto normal = QVector3D(0, 0, 1);

        builder.addLine(vertex1, vertex2, normal);

    // X Top Half Circle

    for (int i = 0; i < topIndexes.count(); ++i) {

        const auto refPoint1 = circlePoints[topIndexes[i]];

        int index2 = i + 1;

        if (index2 == topIndexes.count())

            break;

        const auto refPoint2 = circlePoints[topIndexes[index2]];

        const auto vertex1 = QVector3D(refPoint1.y() + halfHeight, 0.0f, refPoint1.x());

        const auto vertex2 = QVector3D(refPoint2.y() + halfHeight, 0.0f, refPoint2.x());

        const auto normal = QVector3D(0, 1, 0);

        builder.addLine(vertex1, vertex2, normal);

    // X Bottom Half Circle

    for (int i = 0; i < bottomIndexes.count(); ++i) {

        const auto refPoint1 = circlePoints[bottomIndexes[i]];

        int index2 = i + 1;

        if (index2 == bottomIndexes.count())

            break;

        const auto refPoint2 = circlePoints[bottomIndexes[index2]];

        const auto vertex1 = QVector3D(refPoint1.y() - halfHeight, 0.0f, refPoint1.x());

        const auto vertex2 = QVector3D(refPoint2.y() - halfHeight, 0.0f, refPoint2.x());

        const auto normal = QVector3D(0, 1, 0);

        builder.addLine(vertex1, vertex2, normal);

    capsuleGeometry->setVertexData(builder.generateVertexArray());

    return capsuleGeometry;

QQuick3DGeometry *QDebugDrawHelper::generatePlaneGeometry()

    auto planeGeometry = new QQuick3DGeometry();

    planeGeometry->clear();

    planeGeometry->addAttribute(QQuick3DGeometry::Attribute::PositionSemantic, 0,

                                QQuick3DGeometry::Attribute::ComponentType::F32Type);

    planeGeometry->addAttribute(QQuick3DGeometry::Attribute::NormalSemantic, 16,

                                QQuick3DGeometry::Attribute::ComponentType::F32Type);

    planeGeometry->setStride(32);

    planeGeometry->setPrimitiveType(QQuick3DGeometry::PrimitiveType::Lines);

    // TODO: Some sort of debug scale? Or a level-of-detail grid thing? => QtQuick3DHelpers

    float s = 50;

    float h = 5; // Set to avoid flat bounding box

    planeGeometry->setBounds({ -s, -s, -h }, { s, s, h });

    QCollisionDebugMeshBuilder builder;

    builder.addLine({ -s, -s, 0 }, { s, -s, 0 });

    builder.addLine({ -s, -s, 0 }, { 0, 0, 0 });

    builder.addLine({ s, -s, 0 }, { s, s, 0 });

    builder.addLine({ s, -s, 0 }, { 0, 0, 0 });

    builder.addLine({ s, s, 0 }, { -s, s, 0 });

    builder.addLine({ s, s, 0 }, { 0, 0, 0 });

    builder.addLine({ -s, s, 0 }, { -s, -s, 0 });

    builder.addLine({ -s, s, 0 }, { 0, 0, 0 });

    planeGeometry->setVertexData(builder.generateVertexArray());

    return planeGeometry;

QQuick3DGeometry *QDebugDrawHelper::generateHeightFieldGeometry(physx::PxHeightField *heightField,

                                                                float heightScale, float rowScale,

                                                                float columnScale)

    if (!heightField || heightField->getNbRows() < 2 || heightField->getNbColumns() < 2)

        return nullptr;

    auto geometry = new QQuick3DGeometry();

    geometry->clear();

    geometry->addAttribute(QQuick3DGeometry::Attribute::PositionSemantic, 0,

                           QQuick3DGeometry::Attribute::ComponentType::F32Type);

    geometry->addAttribute(QQuick3DGeometry::Attribute::NormalSemantic, 16,

                           QQuick3DGeometry::Attribute::ComponentType::F32Type);

    geometry->setStride(32);

    geometry->setPrimitiveType(QQuick3DGeometry::PrimitiveType::Lines);

    QCollisionDebugMeshBuilder builder;

    const int numRows = heightField->getNbRows();

    const int numCols = heightField->getNbColumns();

    const float sizeX = rowScale * (numRows - 1);

    const float sizeZ = columnScale * (numCols - 1);

    const float heightF = heightScale;

    float minHeight = 0.f;

    float maxHeight = 0.f;

    auto sample = [&](int row, int col) -> QVector3D {

        const float height = heightField->getSample(row, col).height * heightF;

        maxHeight = qMax(maxHeight, height);

        minHeight = qMin(minHeight, height);

        return QVector3D(row * rowScale, height, col * columnScale);

};

    for (int row = 0; row < numRows; row++) {

        for (int col = 0; col < numCols; col++) {

            if (row < numRows - 1)

                builder.addLine(sample(row, col), sample(row + 1, col));

            if (col < numCols - 1)

                builder.addLine(sample(row, col), sample(row, col + 1));

    geometry->setBounds(QVector3D(0, minHeight, 0), QVector3D(sizeX, maxHeight, sizeZ));

    geometry->setVertexData(builder.generateVertexArray());

    return geometry;

QQuick3DGeometry *QDebugDrawHelper::generateConvexMeshGeometry(physx::PxConvexMesh *convexMesh)

    if (!convexMesh)

        return nullptr;

    auto geometry = new QQuick3DGeometry();

    geometry->clear();

    geometry->addAttribute(QQuick3DGeometry::Attribute::PositionSemantic, 0,

                           QQuick3DGeometry::Attribute::ComponentType::F32Type);

    geometry->addAttribute(QQuick3DGeometry::Attribute::NormalSemantic, 16,

                           QQuick3DGeometry::Attribute::ComponentType::F32Type);

    geometry->setStride(32);

    geometry->setPrimitiveType(QQuick3DGeometry::PrimitiveType::Lines);

    QCollisionDebugMeshBuilder builder;

    const physx::PxU32 nbPolys = convexMesh->getNbPolygons();

    const physx::PxU8 *polygons = convexMesh->getIndexBuffer();

    const physx::PxVec3 *verts = convexMesh->getVertices();

    const physx::PxU32 nbVerts = convexMesh->getNbVertices();

    physx::PxHullPolygon data;

    for (physx::PxU32 i = 0; i < nbPolys; i++) {

        convexMesh->getPolygonData(i, data);

        Q_ASSERT(data.mNbVerts > 2);

        const physx::PxU32 nbTris = physx::PxU32(data.mNbVerts - 2);

        const physx::PxU8 vref0 = polygons[data.mIndexBase + 0];

        Q_ASSERT(vref0 < nbVerts);

        for (physx::PxU32 j = 0; j < nbTris; j++) {

            const physx::PxU32 vref1 = polygons[data.mIndexBase + 0 + j + 1];

            const physx::PxU32 vref2 = polygons[data.mIndexBase + 0 + j + 2];

            Q_ASSERT(vref1 < nbVerts);

            Q_ASSERT(vref2 < nbVerts);

            const QVector3D p0 = QPhysicsUtils::toQtType(verts[vref0]);

            const QVector3D p1 = QPhysicsUtils::toQtType(verts[vref1]);

            const QVector3D p2 = QPhysicsUtils::toQtType(verts[vref2]);

            builder.addLine(p0, p1);

            builder.addLine(p1, p2);

            builder.addLine(p2, p0);

    auto bounds = convexMesh->getLocalBounds();

    geometry->setBounds(QPhysicsUtils::toQtType(bounds.minimum),

                        QPhysicsUtils::toQtType(bounds.maximum));

    geometry->setVertexData(builder.generateVertexArray());

    return geometry;

QQuick3DGeometry *

QDebugDrawHelper::generateTriangleMeshGeometry(physx::PxTriangleMesh *triangleMesh)

    if (!triangleMesh)

        return nullptr;

    auto geometry = new QQuick3DGeometry();

    geometry->clear();

    geometry->addAttribute(QQuick3DGeometry::Attribute::PositionSemantic, 0,

                           QQuick3DGeometry::Attribute::ComponentType::F32Type);

    geometry->addAttribute(QQuick3DGeometry::Attribute::NormalSemantic, 16,

                           QQuick3DGeometry::Attribute::ComponentType::F32Type);

    geometry->setStride(32);

    geometry->setPrimitiveType(QQuick3DGeometry::PrimitiveType::Lines);

    QCollisionDebugMeshBuilder builder;

    const physx::PxU32 triangleCount = triangleMesh->getNbTriangles();

    const physx::PxU32 has16BitIndices =

            triangleMesh->getTriangleMeshFlags() & physx::PxTriangleMeshFlag::e16_BIT_INDICES;

    const void *indexBuffer = triangleMesh->getTriangles();

    const physx::PxVec3 *vertexBuffer = triangleMesh->getVertices();

    const physx::PxU32 *intIndices = reinterpret_cast<const physx::PxU32 *>(indexBuffer);

    const physx::PxU16 *shortIndices = reinterpret_cast<const physx::PxU16 *>(indexBuffer);

    for (physx::PxU32 i = 0; i < triangleCount; ++i) {

        physx::PxVec3 triVert[3];

        if (has16BitIndices) {

            triVert[0] = vertexBuffer[*shortIndices++];

            triVert[1] = vertexBuffer[*shortIndices++];

            triVert[2] = vertexBuffer[*shortIndices++];

        } else {

            triVert[0] = vertexBuffer[*intIndices++];

            triVert[1] = vertexBuffer[*intIndices++];

            triVert[2] = vertexBuffer[*intIndices++];

        const QVector3D p0 = QPhysicsUtils::toQtType(triVert[0]);

        const QVector3D p1 = QPhysicsUtils::toQtType(triVert[1]);

        const QVector3D p2 = QPhysicsUtils::toQtType(triVert[2]);

        builder.addLine(p0, p1);

        builder.addLine(p1, p2);

        builder.addLine(p2, p0);

    auto bounds = triangleMesh->getLocalBounds();

    geometry->setBounds(QPhysicsUtils::toQtType(bounds.minimum),

                        QPhysicsUtils::toQtType(bounds.maximum));

    geometry->setVertexData(builder.generateVertexArray());

    return geometry;

Revision control

Copy as Markdown