Added developer option to draw IK targets in world.

(cherry picked from commit 7e7803c648)

interface/src/Menu.cpp

@@ -510,6 +510,8 @@ Menu::Menu() {
         avatar.get(), SLOT(setEnableInverseKinematics(bool)));
     addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::RenderSensorToWorldMatrix, 0, false,
         avatar.get(), SLOT(setEnableDebugDrawSensorToWorldMatrix(bool)));
+    addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::RenderIKTargets, 0, false,
+        avatar.get(), SLOT(setEnableDebugDrawIKTargets(bool)));
 
     addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::ActionMotorControl,
         Qt::CTRL | Qt::SHIFT | Qt::Key_K, true, avatar.get(), SLOT(updateMotionBehaviorFromMenu()),

interface/src/Menu.h

@@ -161,6 +161,7 @@ namespace MenuOption {
     const QString RenderResolutionThird = "1/3";
     const QString RenderResolutionQuarter = "1/4";
     const QString RenderSensorToWorldMatrix = "Show SensorToWorld Matrix";
+    const QString RenderIKTargets = "Show IK Targets";
     const QString ResetAvatarSize = "Reset Avatar Size";
     const QString ResetSensors = "Reset Sensors";
     const QString RunningScripts = "Running Scripts...";

interface/src/avatar/MyAvatar.cpp

@@ -82,6 +82,8 @@ const float MyAvatar::ZOOM_MIN = 0.5f;
 const float MyAvatar::ZOOM_MAX = 25.0f;
 const float MyAvatar::ZOOM_DEFAULT = 1.5f;
 
+extern bool HACKY_GLOBAL_ENABLE_DEBUG_DRAW_IK_TARGETS;
+
 MyAvatar::MyAvatar(RigPointer rig) :
     Avatar(rig),
     _wasPushing(false),
@@ -916,6 +918,13 @@ void MyAvatar::setEnableDebugDrawSensorToWorldMatrix(bool isEnabled) {
     }
 }
 
+void MyAvatar::setEnableDebugDrawIKTargets(bool isEnabled) {
+    _enableDebugDrawIKTargets = isEnabled;
+
+    HACKY_GLOBAL_ENABLE_DEBUG_DRAW_IK_TARGETS = isEnabled;
+}
+
+
 void MyAvatar::setEnableMeshVisible(bool isEnabled) {
     render::ScenePointer scene = qApp->getMain3DScene();
     _skeletonModel->setVisibleInScene(isEnabled, scene);

interface/src/avatar/MyAvatar.h

@@ -330,6 +330,7 @@ public slots:
     void setEnableDebugDrawPosition(bool isEnabled);
     void setEnableDebugDrawHandControllers(bool isEnabled);
     void setEnableDebugDrawSensorToWorldMatrix(bool isEnabled);
+    void setEnableDebugDrawIKTargets(bool isEnabled);
     bool getEnableMeshVisible() const { return _skeletonModel->isVisible(); }
     void setEnableMeshVisible(bool isEnabled);
     void setUseAnimPreAndPostRotations(bool isEnabled);
@@ -516,6 +517,7 @@ private:
     bool _enableDebugDrawAnimPose { false };
     bool _enableDebugDrawHandControllers { false };
     bool _enableDebugDrawSensorToWorldMatrix { false };
+    bool _enableDebugDrawIKTargets { false };
 
     AudioListenerMode _audioListenerMode;
     glm::vec3 _customListenPosition;

libraries/animation/src/AnimInverseKinematics.cpp

@@ -14,11 +14,16 @@
 #include <NumericalConstants.h>
 #include <SharedUtil.h>
 #include <shared/NsightHelpers.h>
+#include <DebugDraw.h>
+#include "Rig.h"
 
 #include "ElbowConstraint.h"
 #include "SwingTwistConstraint.h"
 #include "AnimationLogging.h"
 
+bool HACKY_GLOBAL_ENABLE_DEBUG_DRAW_IK_TARGETS = false;
+Rig* HACKY_GLOBAL_RIG_POINTER = nullptr;
+
 AnimInverseKinematics::AnimInverseKinematics(const QString& id) : AnimNode(AnimNode::Type::InverseKinematics, id) {
 }
 
@@ -439,6 +444,22 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
             computeTargets(animVars, targets, underPoses);
         }
 
+        // AJT: HACK
+        if (HACKY_GLOBAL_ENABLE_DEBUG_DRAW_IK_TARGETS && HACKY_GLOBAL_RIG_POINTER) {
+            const float CM_TO_M = 0.01f;
+            const vec4 WHITE(1.0f);
+            glm::mat4 geomToRigMat = HACKY_GLOBAL_RIG_POINTER->getGeometryToRigTransform();
+            glm::mat4 rigToAvatarMat = createMatFromQuatAndPos(Quaternions::Y_180, glm::vec3());
+
+            for (auto& target : targets) {
+                glm::mat4 geomTargetMat = createMatFromQuatAndPos(target.getRotation(), target.getTranslation());
+                glm::mat4 avatarTargetMat = rigToAvatarMat * geomToRigMat * geomTargetMat;
+
+                std::string name = "ikTarget" + std::to_string(target.getIndex());
+                DebugDraw::getInstance().addMyAvatarMarker(name, glmExtractRotation(avatarTargetMat), extractTranslation(avatarTargetMat), WHITE);
+            }
+        }
+
         if (targets.empty()) {
             // no IK targets but still need to enforce constraints
             std::map<int, RotationConstraint*>::iterator constraintItr = _constraints.begin();

libraries/animation/src/Rig.cpp

@@ -48,6 +48,8 @@ const glm::vec3 DEFAULT_LEFT_EYE_POS(0.3f, 0.9f, 0.0f);
 const glm::vec3 DEFAULT_HEAD_POS(0.0f, 0.75f, 0.0f);
 const glm::vec3 DEFAULT_NECK_POS(0.0f, 0.70f, 0.0f);
 
+extern Rig* HACKY_GLOBAL_RIG_POINTER;
+
 void Rig::overrideAnimation(const QString& url, float fps, bool loop, float firstFrame, float lastFrame) {
 
     UserAnimState::ClipNodeEnum clipNodeEnum;
@@ -951,6 +953,7 @@ void Rig::updateAnimations(float deltaTime, glm::mat4 rootTransform) {
         _animVars.setRigToGeometryTransform(_rigToGeometryTransform);
 
         // evaluate the animation
+        HACKY_GLOBAL_RIG_POINTER = this;
         AnimNode::Triggers triggersOut;
         _internalPoseSet._relativePoses = _animNode->evaluate(_animVars, deltaTime, triggersOut);
         if ((int)_internalPoseSet._relativePoses.size() != _animSkeleton->getNumJoints()) {
@@ -961,6 +964,7 @@ void Rig::updateAnimations(float deltaTime, glm::mat4 rootTransform) {
         for (auto& trigger : triggersOut) {
             _animVars.setTrigger(trigger);
         }
+        HACKY_GLOBAL_RIG_POINTER = nullptr;
     }
     applyOverridePoses();
     buildAbsoluteRigPoses(_internalPoseSet._relativePoses, _internalPoseSet._absolutePoses);