Added items to the developer menu for debugging physics

Hooked up Bullet's internal debug draw functionality to our client.
Under the Developer > Physics Menu there are five new items:

  * Show Bullet Collision - will draw all collision shapes in wireframe. WARNING: can be slow on large scenes.
  * Show Bullet Bounding Boxes - will draw axis aligned bounding boxes around all physics shapes.
  * Show Bullet Contact Points - will draw all contact points where two or more objects are colliding.
  * Show Bullet Constraints - will render wire frame axes for each constraint connecting bodies together.
  * Show Bullet Constraint Limits - will render the joint limits for each constraint.

interface/src/Application.cpp

@@ -7828,6 +7828,26 @@ void Application::switchDisplayMode() {
     _previousHMDWornStatus = currentHMDWornStatus;
 }
 
+void Application::setShowBulletWireframe(bool value) {
+    _physicsEngine->setShowBulletWireframe(value);
+}
+
+void Application::setShowBulletAABBs(bool value) {
+    _physicsEngine->setShowBulletAABBs(value);
+}
+
+void Application::setShowBulletContactPoints(bool value) {
+    _physicsEngine->setShowBulletContactPoints(value);
+}
+
+void Application::setShowBulletConstraints(bool value) {
+    _physicsEngine->setShowBulletConstraints(value);
+}
+
+void Application::setShowBulletConstraintLimits(bool value) {
+    _physicsEngine->setShowBulletConstraintLimits(value);
+}
+
 void Application::startHMDStandBySession() {
     _autoSwitchDisplayModeSupportedHMDPlugin->startStandBySession();
 }

interface/src/Application.h

@@ -243,6 +243,7 @@ public:
 
     bool isAboutToQuit() const { return _aboutToQuit; }
     bool isPhysicsEnabled() const { return _physicsEnabled; }
+    PhysicsEnginePointer getPhysicsEngine() { return _physicsEngine; }
 
     // the isHMDMode is true whenever we use the interface from an HMD and not a standard flat display
     // rendering of several elements depend on that
@@ -453,6 +454,12 @@ private slots:
     void handleSandboxStatus(QNetworkReply* reply);
     void switchDisplayMode();
 
+    void setShowBulletWireframe(bool value);
+    void setShowBulletAABBs(bool value);
+    void setShowBulletContactPoints(bool value);
+    void setShowBulletConstraints(bool value);
+    void setShowBulletConstraintLimits(bool value);
+
 private:
     void init();
     bool handleKeyEventForFocusedEntityOrOverlay(QEvent* event);

interface/src/Menu.cpp

@@ -735,6 +735,12 @@ Menu::Menu() {
     }
     addCheckableActionToQMenuAndActionHash(physicsOptionsMenu, MenuOption::PhysicsShowHulls, 0, false, qApp->getEntities().data(), SIGNAL(setRenderDebugHulls()));
 
+    addCheckableActionToQMenuAndActionHash(physicsOptionsMenu, MenuOption::PhysicsShowBulletWireframe, 0, false, qApp, SLOT(setShowBulletWireframe(bool)));
+    addCheckableActionToQMenuAndActionHash(physicsOptionsMenu, MenuOption::PhysicsShowBulletAABBs, 0, false, qApp, SLOT(setShowBulletAABBs(bool)));
+    addCheckableActionToQMenuAndActionHash(physicsOptionsMenu, MenuOption::PhysicsShowBulletContactPoints, 0, false, qApp, SLOT(setShowBulletContactPoints(bool)));
+    addCheckableActionToQMenuAndActionHash(physicsOptionsMenu, MenuOption::PhysicsShowBulletConstraints, 0, false, qApp, SLOT(setShowBulletConstraints(bool)));
+    addCheckableActionToQMenuAndActionHash(physicsOptionsMenu, MenuOption::PhysicsShowBulletConstraintLimits, 0, false, qApp, SLOT(setShowBulletConstraintLimits(bool)));
+
     // Developer > Ask to Reset Settings
     addCheckableActionToQMenuAndActionHash(developerMenu, MenuOption::AskToResetSettings, 0, false);
 

interface/src/Menu.h

@@ -143,6 +143,11 @@ namespace MenuOption {
     const QString PhysicsShowHulls = "Draw Collision Shapes";
     const QString PhysicsShowOwned = "Highlight Simulation Ownership";
     const QString VerboseLogging = "Verbose Logging";
+    const QString PhysicsShowBulletWireframe = "Show Bullet Collision";
+    const QString PhysicsShowBulletAABBs = "Show Bullet Bounding Boxes";
+    const QString PhysicsShowBulletContactPoints = "Show Bullet Contact Points";
+    const QString PhysicsShowBulletConstraints = "Show Bullet Constraints";
+    const QString PhysicsShowBulletConstraintLimits = "Show Bullet Constraint Limits";
     const QString PipelineWarnings = "Log Render Pipeline Warnings";
     const QString Preferences = "General...";
     const QString Quit =  "Quit";

libraries/physics/src/PhysicsDebugDraw.cpp

@@ -0,0 +1,54 @@
+//
+//  PhysicsDebugDraw.cpp
+//  libraries/physics/src
+//
+//  Created by Anthony Thibault 2018-4-18
+//  Copyright 2018 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+#include "PhysicsDebugDraw.h"
+#include "BulletUtil.h"
+#include "PhysicsLogging.h"
+
+#include <DebugDraw.h>
+#include <GLMHelpers.h>
+
+void PhysicsDebugDraw::drawLine(const btVector3& from, const btVector3& to, const btVector3& color) {
+    DebugDraw::getInstance().drawRay(bulletToGLM(from), bulletToGLM(to), glm::vec4(color.getX(), color.getY(), color.getZ(), 1.0f));
+}
+
+void PhysicsDebugDraw::drawContactPoint(const btVector3& PointOnB, const btVector3& normalOnB, btScalar distance,
+                                        int lifeTime, const btVector3& color) {
+
+    glm::vec3 normal, tangent, biNormal;
+    generateBasisVectors(bulletToGLM(normalOnB), Vectors::UNIT_X, normal, tangent, biNormal);
+    btVector3 u = glmToBullet(normal);
+    btVector3 v = glmToBullet(tangent);
+    btVector3 w = glmToBullet(biNormal);
+
+    // x marks the spot, green is along the normal.
+    const float CONTACT_POINT_RADIUS = 0.1f;
+    const btVector3 GREEN(0.0f, 1.0f, 0.0f);
+    const btVector3 WHITE(1.0f, 1.0f, 1.0f);
+    drawLine(PointOnB - u * CONTACT_POINT_RADIUS, PointOnB + u * CONTACT_POINT_RADIUS, GREEN);
+    drawLine(PointOnB - v * CONTACT_POINT_RADIUS, PointOnB + v * CONTACT_POINT_RADIUS, WHITE);
+    drawLine(PointOnB - w * CONTACT_POINT_RADIUS, PointOnB + w * CONTACT_POINT_RADIUS, WHITE);
+}
+
+void PhysicsDebugDraw::reportErrorWarning(const char* warningString) {
+    qCWarning(physics) << "BULLET:" << warningString;
+}
+
+void PhysicsDebugDraw::draw3dText(const btVector3& location, const char* textString) {
+}
+
+void PhysicsDebugDraw::setDebugMode(int debugMode) {
+    _debugDrawMode = debugMode;
+}
+
+int PhysicsDebugDraw::getDebugMode() const {
+    return _debugDrawMode;
+}

libraries/physics/src/PhysicsDebugDraw.h

@@ -0,0 +1,34 @@
+//
+//  PhysicsDebugDraw.h
+//  libraries/physics/src
+//
+//  Created by Anthony Thibault 2018-4-18
+//  Copyright 2018 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+//  http://bulletphysics.org/Bullet/BulletFull/classbtIDebugDraw.html
+
+#ifndef hifi_PhysicsDebugDraw_h
+#define hifi_PhysicsDebugDraw_h
+
+#include <stdint.h>
+#include <LinearMath/btIDebugDraw.h>
+
+class PhysicsDebugDraw : public btIDebugDraw {
+public:
+    using btIDebugDraw::drawLine;
+    virtual void drawLine(const btVector3& from, const btVector3& to, const btVector3& color) override;
+    virtual void drawContactPoint(const btVector3& PointOnB, const btVector3& normalOnB, btScalar distance,
+                                  int lifeTime, const btVector3& color) override;
+    virtual void reportErrorWarning(const char* warningString) override;
+    virtual void draw3dText(const btVector3& location, const char* textString) override;
+    virtual void setDebugMode(int debugMode) override;
+    virtual int getDebugMode() const override;
+
+protected:
+    uint32_t _debugDrawMode;
+};
+
+#endif // hifi_PhysicsDebugDraw_h

libraries/physics/src/PhysicsEngine.cpp

@@ -22,6 +22,7 @@
 #include "ObjectMotionState.h"
 #include "PhysicsEngine.h"
 #include "PhysicsHelpers.h"
+#include "PhysicsDebugDraw.h"
 #include "ThreadSafeDynamicsWorld.h"
 #include "PhysicsLogging.h"
 
@@ -49,6 +50,10 @@ void PhysicsEngine::init() {
         _broadphaseFilter = new btDbvtBroadphase();
         _constraintSolver = new btSequentialImpulseConstraintSolver;
         _dynamicsWorld = new ThreadSafeDynamicsWorld(_collisionDispatcher, _broadphaseFilter, _constraintSolver, _collisionConfig);
+        _physicsDebugDraw.reset(new PhysicsDebugDraw());
+
+        // hook up debug draw renderer
+        _dynamicsWorld->setDebugDrawer(_physicsDebugDraw.get());
 
         _ghostPairCallback = new btGhostPairCallback();
         _dynamicsWorld->getPairCache()->setInternalGhostPairCallback(_ghostPairCallback);
@@ -333,6 +338,10 @@ void PhysicsEngine::stepSimulation() {
 
         _hasOutgoingChanges = true;
     }
+
+    if (_physicsDebugDraw->getDebugMode()) {
+        _dynamicsWorld->debugDrawWorld();
+    }
 }
 
 class CProfileOperator {
@@ -785,3 +794,49 @@ void PhysicsEngine::forEachDynamic(std::function<void(EntityDynamicPointer)> act
         }
     }
 }
+
+void PhysicsEngine::setShowBulletWireframe(bool value) {
+    int mode = _physicsDebugDraw->getDebugMode();
+    if (value) {
+        _physicsDebugDraw->setDebugMode(mode | btIDebugDraw::DBG_DrawWireframe);
+    } else {
+        _physicsDebugDraw->setDebugMode(mode & ~btIDebugDraw::DBG_DrawWireframe);
+    }
+}
+
+void PhysicsEngine::setShowBulletAABBs(bool value) {
+    int mode = _physicsDebugDraw->getDebugMode();
+    if (value) {
+        _physicsDebugDraw->setDebugMode(mode | btIDebugDraw::DBG_DrawAabb);
+    } else {
+        _physicsDebugDraw->setDebugMode(mode & ~btIDebugDraw::DBG_DrawAabb);
+    }
+}
+
+void PhysicsEngine::setShowBulletContactPoints(bool value) {
+    int mode = _physicsDebugDraw->getDebugMode();
+    if (value) {
+        _physicsDebugDraw->setDebugMode(mode | btIDebugDraw::DBG_DrawContactPoints);
+    } else {
+        _physicsDebugDraw->setDebugMode(mode & ~btIDebugDraw::DBG_DrawContactPoints);
+    }
+}
+
+void PhysicsEngine::setShowBulletConstraints(bool value) {
+    int mode = _physicsDebugDraw->getDebugMode();
+    if (value) {
+        _physicsDebugDraw->setDebugMode(mode | btIDebugDraw::DBG_DrawConstraints);
+    } else {
+        _physicsDebugDraw->setDebugMode(mode & ~btIDebugDraw::DBG_DrawConstraints);
+    }
+}
+
+void PhysicsEngine::setShowBulletConstraintLimits(bool value) {
+    int mode = _physicsDebugDraw->getDebugMode();
+    if (value) {
+        _physicsDebugDraw->setDebugMode(mode | btIDebugDraw::DBG_DrawConstraintLimits);
+    } else {
+        _physicsDebugDraw->setDebugMode(mode & ~btIDebugDraw::DBG_DrawConstraintLimits);
+    }
+}
+

libraries/physics/src/PhysicsEngine.h

@@ -30,6 +30,7 @@
 const float HALF_SIMULATION_EXTENT = 512.0f; // meters
 
 class CharacterController;
+class PhysicsDebugDraw;
 
 // simple class for keeping track of contacts
 class ContactKey {
@@ -96,6 +97,12 @@ public:
     void removeDynamic(const QUuid dynamicID);
     void forEachDynamic(std::function<void(EntityDynamicPointer)> actor);
 
+    void setShowBulletWireframe(bool value);
+    void setShowBulletAABBs(bool value);
+    void setShowBulletContactPoints(bool value);
+    void setShowBulletConstraints(bool value);
+    void setShowBulletConstraintLimits(bool value);
+
 private:
     QList<EntityDynamicPointer> removeDynamicsForBody(btRigidBody* body);
     void addObjectToDynamicsWorld(ObjectMotionState* motionState);
@@ -114,6 +121,7 @@ private:
     btSequentialImpulseConstraintSolver* _constraintSolver = NULL;
     ThreadSafeDynamicsWorld* _dynamicsWorld = NULL;
     btGhostPairCallback* _ghostPairCallback = NULL;
+    std::unique_ptr<PhysicsDebugDraw> _physicsDebugDraw;
 
     ContactMap _contactMap;
     CollisionEvents _collisionEvents;

libraries/render-utils/src/AnimDebugDraw.cpp

@@ -52,9 +52,9 @@ public:
 
         batch.setInputFormat(_vertexFormat);
         batch.setInputBuffer(0, _vertexBuffer, 0, sizeof(Vertex));
-        batch.setIndexBuffer(gpu::UINT16, _indexBuffer, 0);
+        batch.setIndexBuffer(gpu::UINT32, _indexBuffer, 0);
 
-        auto numIndices = _indexBuffer->getSize() / sizeof(uint16_t);
+        auto numIndices = _indexBuffer->getSize() / sizeof(uint32_t);
         batch.drawIndexed(gpu::LINES, (int)numIndices);
     }
 
@@ -135,9 +135,9 @@ AnimDebugDraw::AnimDebugDraw() :
         AnimDebugDrawData::Vertex { glm::vec3(1.0, 1.0f, 1.0f), toRGBA(0, 0, 255, 255) },
         AnimDebugDrawData::Vertex { glm::vec3(1.0, 1.0f, 2.0f), toRGBA(0, 0, 255, 255) },
     });
-    static std::vector<uint16_t> indices({ 0, 1, 2, 3, 4, 5 });
+    static std::vector<uint32_t> indices({ 0, 1, 2, 3, 4, 5 });
     _animDebugDrawData->_vertexBuffer->setSubData<AnimDebugDrawData::Vertex>(0, vertices);
-    _animDebugDrawData->_indexBuffer->setSubData<uint16_t>(0, indices);
+    _animDebugDrawData->_indexBuffer->setSubData<uint32_t>(0, indices);
 }
 
 AnimDebugDraw::~AnimDebugDraw() {
@@ -425,9 +425,9 @@ void AnimDebugDraw::update() {
 
         data._isVisible = (numVerts > 0);
 
-        data._indexBuffer->resize(sizeof(uint16_t) * numVerts);
+        data._indexBuffer->resize(sizeof(uint32_t) * numVerts);
         for (int i = 0; i < numVerts; i++) {
-            data._indexBuffer->setSubData<uint16_t>(i, (uint16_t)i);;
+            data._indexBuffer->setSubData<uint32_t>(i, (uint32_t)i);;
         }
     });
     scene->enqueueTransaction(transaction);