Add character controller debug drawing

Change-Id: Iec8368929c93bf0f142c8f10b79c3ff814ce8c0a
Reviewed-by: Paul Olav Tvete <paul.tvete@qt.io>

5 files changed, 88 insertions, 9 deletions

diff --git a/src/quick3dphysics/physxnode/qabstractphysxnode_p.h b/src/quick3dphysics/physxnode/qabstractphysxnode_p.h
index 366e8f4..8568508 100644
--- a/src/quick3dphysics/physxnode/qabstractphysxnode_p.h
+++ b/src/quick3dphysics/physxnode/qabstractphysxnode_p.h
@@ -40,7 +40,8 @@ enum class DebugDrawBodyType {
     DynamicAwake = 1,
     DynamicSleeping = 2,
     Trigger = 3,
-    Unknown = 4
+    Character = 4,
+    Unknown = 5
 };
 
 /*
diff --git a/src/quick3dphysics/physxnode/qphysxcharactercontroller.cpp b/src/quick3dphysics/physxnode/qphysxcharactercontroller.cpp
index af05a7c..11928a5 100644
--- a/src/quick3dphysics/physxnode/qphysxcharactercontroller.cpp
+++ b/src/quick3dphysics/physxnode/qphysxcharactercontroller.cpp
@@ -185,4 +185,14 @@ void QPhysXCharacterController::createMaterial(QPhysXWorld *physX)
             physX, static_cast<QCharacterController *>(frontendNode)->physicsMaterial());
 }
 
+bool QPhysXCharacterController::debugGeometryCapability()
+{
+    return true;
+}
+
+DebugDrawBodyType QPhysXCharacterController::getDebugDrawBodyType()
+{
+    return DebugDrawBodyType::Character;
+}
+
 QT_END_NAMESPACE
diff --git a/src/quick3dphysics/physxnode/qphysxcharactercontroller_p.h b/src/quick3dphysics/physxnode/qphysxcharactercontroller_p.h
index 7e56378..f7dcc39 100644
--- a/src/quick3dphysics/physxnode/qphysxcharactercontroller_p.h
+++ b/src/quick3dphysics/physxnode/qphysxcharactercontroller_p.h
@@ -35,6 +35,8 @@ public:
     void init(QPhysicsWorld *world, QPhysXWorld *physX) override;
     void sync(float deltaTime, QHash<QQuick3DNode *, QMatrix4x4> &transformCache) override;
     void createMaterial(QPhysXWorld *physX) override;
+    bool debugGeometryCapability() override;
+    DebugDrawBodyType getDebugDrawBodyType() override;
 
 private:
     physx::PxCapsuleController *controller = nullptr;
diff --git a/src/quick3dphysics/qphysicsworld.cpp b/src/quick3dphysics/qphysicsworld.cpp
index 1a3b277..674c647 100644
--- a/src/quick3dphysics/qphysicsworld.cpp
+++ b/src/quick3dphysics/qphysicsworld.cpp
@@ -590,7 +590,7 @@ void QPhysicsWorld::setupDebugMaterials(QQuick3DNode *sceneNode)
     // These colors match the indices of DebugDrawBodyType enum
     for (auto color : { QColorConstants::Svg::chartreuse, QColorConstants::Svg::cyan,
                         QColorConstants::Svg::lightsalmon, QColorConstants::Svg::red,
-                        QColorConstants::Svg::black }) {
+                        QColorConstants::Svg::blueviolet, QColorConstants::Svg::black }) {
         auto debugMaterial = new QQuick3DDefaultMaterial();
         debugMaterial->setLineWidth(lineWidth);
         debugMaterial->setParentItem(sceneNode);
@@ -634,15 +634,12 @@ void QPhysicsWorld::updateDebugDraw()
         const auto &collisionShapes = node->frontendNode->getCollisionShapesList();
         const int materialIdx = static_cast<int>(node->getDebugDrawBodyType());
         const int length = collisionShapes.length();
-        if (node->shapes.length() < length)
-            continue; // CharacterController has shapes, but not PhysX shapes
         for (int idx = 0; idx < length; idx++) {
             const auto collisionShape = collisionShapes[idx];
 
             if (!m_forceDebugDraw && !collisionShape->enableDebugDraw())
                 continue;
 
-            const auto physXShape = node->shapes[idx];
             DebugModelHolder &holder =
                 m_collisionShapeDebugModels[std::make_pair(collisionShape, node)];
             auto &model = holder.model;
@@ -652,8 +649,6 @@ void QPhysicsWorld::updateDebugDraw()
             m_hasIndividualDebugDraw =
                     m_hasIndividualDebugDraw || collisionShape->enableDebugDraw();
 
-            auto localPose = physXShape->getLocalPose();
-
             // Create/Update debug view infrastructure
             if (!model) {
                 model = new QQuick3DModel();
@@ -664,6 +659,8 @@ void QPhysicsWorld::updateDebugDraw()
                 model->setCastsReflections(false);
             }
 
+            model->setVisible(true);
+
             { // update or set material
                 auto material = m_debugMaterials[materialIdx];
                 QQmlListReference materialsRef(model, "materials");
@@ -673,6 +670,33 @@ void QPhysicsWorld::updateDebugDraw()
                 }
             }
 
+            // Special handling of CharacterController since it has collision shapes,
+            // but not PhysX shapes
+            if (QCapsuleShape *capsuleShape = qobject_cast<QCapsuleShape *>(collisionShape); capsuleShape != nullptr) {
+                const float radius = capsuleShape->diameter() * 0.5;
+                const float halfHeight = capsuleShape->height() * 0.5;
+
+                if (!qFuzzyCompare(radius, holder.radius())
+                    || !qFuzzyCompare(halfHeight, holder.halfHeight())) {
+                    auto geom = QDebugDrawHelper::generateCapsuleGeometry(radius, halfHeight);
+                    geom->setParent(model);
+                    model->setGeometry(geom);
+                    holder.setRadius(radius);
+                    holder.setHalfHeight(halfHeight);
+                }
+
+                model->setPosition(node->frontendNode->scenePosition());
+                model->setRotation(node->frontendNode->sceneRotation()
+                                   * QQuaternion::fromEulerAngles(0, 0, 90));
+                continue;
+            }
+
+            if (node->shapes.length() < length)
+                continue;
+
+            const auto physXShape = node->shapes[idx];
+            auto localPose = physXShape->getLocalPose();
+
             switch (physXShape->getGeometryType()) {
             case physx::PxGeometryType::eBOX: {
                 physx::PxBoxGeometry boxGeometry;
@@ -833,8 +857,6 @@ void QPhysicsWorld::updateDebugDraw()
                 Q_UNREACHABLE();
             }
 
-            model->setVisible(true);
-
             auto globalPose = node->getGlobalPose();
             auto finalPose = globalPose.transform(localPose);
 
diff --git a/tests/baseline/data/DebugDrawShapes.qml b/tests/baseline/data/DebugDrawShapes.qml
new file mode 100644
index 0000000..8f7472b
--- /dev/null
+++ b/tests/baseline/data/DebugDrawShapes.qml
@@ -0,0 +1,44 @@
+import QtQuick
+import QtQuick3D
+import QtQuick3D.Physics
+
+Rectangle {
+    width: 640
+    height: 480
+    visible: true
+    PhysicsWorld {
+        scene: viewport.scene
+        forceDebugDraw: true
+        running: true
+    }
+    View3D {
+        id: viewport
+        anchors.fill: parent
+        PerspectiveCamera {
+            id: camera
+            position: Qt.vector3d(0, 50, 500)
+            eulerRotation: Qt.vector3d(0, 0, 0)
+            clipFar: 5000
+            clipNear: 1
+        }
+        DirectionalLight {}
+
+        StaticRigidBody {
+            position: Qt.vector3d(-150, 0, 0)
+            collisionShapes: BoxShape {}
+        }
+        StaticRigidBody {
+            position: Qt.vector3d(-50, 0, 0)
+            collisionShapes: SphereShape {}
+        }
+        StaticRigidBody {
+            position: Qt.vector3d(50, 0, 0)
+            collisionShapes: CapsuleShape {}
+        }
+        CharacterController {
+            position: Qt.vector3d(150, 0, 0)
+            collisionShapes: CapsuleShape {}
+        }
+    }
+}
+