From 27e6c0f872e2665ec277ae80d8645e75b52ff3ff Mon Sep 17 00:00:00 2001
From: Anthony Thibault <ajt@hyperlogic.org>
Date: Sat, 24 Sep 2016 12:55:05 -0700
Subject: [PATCH] Added debug draw path trace for left and right feet

---
 interface/src/Menu.cpp            |  4 ++++
 interface/src/Menu.h              |  2 ++
 interface/src/avatar/MyAvatar.cpp | 27 +++++++++++++++++++++++++++
 interface/src/avatar/MyAvatar.h   | 15 +++++++++++++++
 4 files changed, 48 insertions(+)

diff --git a/interface/src/Menu.cpp b/interface/src/Menu.cpp
index 378838f0e0..7fb8096c35 100644
--- a/interface/src/Menu.cpp
+++ b/interface/src/Menu.cpp
@@ -485,6 +485,10 @@ Menu::Menu() {
     addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::RenderMyLookAtVectors, 0, false);
     addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::RenderOtherLookAtVectors, 0, false);
     addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::FixGaze, 0, false);
+    addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::DisplayLeftFootTrace, 0, false,
+        avatar, SLOT(setEnableDebugDrawLeftFootTrace(bool)));
+    addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::DisplayRightFootTrace, 0, false,
+        avatar, SLOT(setEnableDebugDrawRightFootTrace(bool)));
     addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::AnimDebugDrawDefaultPose, 0, false,
         avatar, SLOT(setEnableDebugDrawDefaultPose(bool)));
     addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::AnimDebugDrawAnimPose, 0, false,
diff --git a/interface/src/Menu.h b/interface/src/Menu.h
index 003b8b8121..a24966973f 100644
--- a/interface/src/Menu.h
+++ b/interface/src/Menu.h
@@ -88,6 +88,8 @@ namespace MenuOption {
     const QString DiskCacheEditor = "Disk Cache Editor";
     const QString DisplayCrashOptions = "Display Crash Options";
     const QString DisplayHandTargets = "Show Hand Targets";
+    const QString DisplayLeftFootTrace = "Show Left Foot Trace";
+    const QString DisplayRightFootTrace = "Show Right Foot Trace";
     const QString DisplayModelBounds = "Display Model Bounds";
     const QString DisplayModelTriangles = "Display Model Triangles";
     const QString DisplayModelElementChildProxies = "Display Model Element Children";
diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index 1a184a6d12..9f079288f4 100755
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -817,6 +817,14 @@ float loadSetting(Settings& settings, const QString& name, float defaultValue) {
     return value;
 }
 
+void MyAvatar::setEnableDebugDrawLeftFootTrace(bool isEnabled) {
+    _enableDebugDrawLeftFootTrace = isEnabled;
+}
+
+void MyAvatar::setEnableDebugDrawRightFootTrace(bool isEnabled) {
+    _enableDebugDrawRightFootTrace = isEnabled;
+}
+
 void MyAvatar::setEnableDebugDrawDefaultPose(bool isEnabled) {
     _enableDebugDrawDefaultPose = isEnabled;
 
@@ -1619,6 +1627,25 @@ void MyAvatar::postUpdate(float deltaTime) {
 
     DebugDraw::getInstance().updateMyAvatarPos(getPosition());
     DebugDraw::getInstance().updateMyAvatarRot(getOrientation());
+
+    if (_enableDebugDrawLeftFootTrace || _enableDebugDrawRightFootTrace) {
+        int boneIndex = _enableDebugDrawLeftFootTrace ? getJointIndex("LeftFoot") : getJointIndex("RightFoot");
+        const glm::vec4 RED(1.0f, 0.0f, 0.0f, 1.0f);
+        const glm::vec4 WHITE(1.0f, 1.0f, 1.0f, 1.0f);
+        const glm::vec4 TRANS(1.0f, 1.0f, 1.0f, 0.0f);
+        static bool colorBit = true;
+        colorBit = !colorBit;
+        glm::vec4 color = colorBit ? RED : WHITE;
+
+        _debugLineLoop[_debugLineLoopIndex] = DebugDrawVertex(getJointPosition(boneIndex), color);
+        _debugLineLoopIndex = (_debugLineLoopIndex + 1) % DEBUG_LINE_LOOP_SIZE;
+        _debugLineLoop[_debugLineLoopIndex] = DebugDrawVertex(getJointPosition(boneIndex), TRANS);
+        for (size_t prev = DEBUG_LINE_LOOP_SIZE - 1, next = 0; next < DEBUG_LINE_LOOP_SIZE; prev = next, next++) {
+            if (_debugLineLoop[prev].color.w > 0.0f) {
+                DebugDraw::getInstance().drawRay(_debugLineLoop[prev].pos, _debugLineLoop[next].pos, _debugLineLoop[prev].color);
+            }
+        }
+    }
 }
 
 
diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h
index d265acee93..b6b6f1373b 100644
--- a/interface/src/avatar/MyAvatar.h
+++ b/interface/src/avatar/MyAvatar.h
@@ -55,6 +55,8 @@ enum AudioListenerMode {
 };
 Q_DECLARE_METATYPE(AudioListenerMode);
 
+const size_t DEBUG_LINE_LOOP_SIZE = 1000;
+
 class MyAvatar : public Avatar {
     Q_OBJECT
     Q_PROPERTY(bool shouldRenderLocally READ getShouldRenderLocally WRITE setShouldRenderLocally)
@@ -303,6 +305,8 @@ public slots:
 
     Q_INVOKABLE void updateMotionBehaviorFromMenu();
 
+    void setEnableDebugDrawLeftFootTrace(bool isEnabled);
+    void setEnableDebugDrawRightFootTrace(bool isEnabled);
     void setEnableDebugDrawDefaultPose(bool isEnabled);
     void setEnableDebugDrawAnimPose(bool isEnabled);
     void setEnableDebugDrawPosition(bool isEnabled);
@@ -481,6 +485,8 @@ private:
     RigPointer _rig;
     bool _prevShouldDrawHead;
 
+    bool _enableDebugDrawLeftFootTrace { false };
+    bool _enableDebugDrawRightFootTrace { false };
     bool _enableDebugDrawDefaultPose { false };
     bool _enableDebugDrawAnimPose { false };
     bool _enableDebugDrawHandControllers { false };
@@ -517,6 +523,15 @@ private:
     float getEnergy();
     void setEnergy(float value);
     bool didTeleport();
+
+    struct DebugDrawVertex {
+        DebugDrawVertex() : pos(), color() {}
+        DebugDrawVertex(const glm::vec3& posIn, const glm::vec4& colorIn) : pos(posIn), color(colorIn) {}
+        glm::vec3 pos;
+        glm::vec4 color;
+    };
+    DebugDrawVertex _debugLineLoop[DEBUG_LINE_LOOP_SIZE];
+    size_t _debugLineLoopIndex { 0 };
 };
 
 QScriptValue audioListenModeToScriptValue(QScriptEngine* engine, const AudioListenerMode& audioListenerMode);