WIP: debug render joint constraints.

interface/src/avatar/MyAvatar.cpp

@@ -1703,6 +1703,7 @@ void MyAvatar::postUpdate(float deltaTime) {
     }
 
     // AJT: REMOVE.
+    /*
     {
         auto ikNode = _rig->getAnimInverseKinematicsNode();
         if (ikNode) {
@@ -1714,7 +1715,7 @@ void MyAvatar::postUpdate(float deltaTime) {
             int hipsIndex = _rig->indexOfJoint("Hips");
             for (size_t i = 0; i < limitCenterPoses.size(); i++) {
                 if (i == hipsIndex) {
-                    //limitCenterPoses[i].trans() = glm::vec3(); // zero the hips
+                    limitCenterPoses[i].trans() = glm::vec3(); // zero the hips
                 }
                 // convert from cm to m
                 limitCenterPoses[i].trans() = 0.01f * limitCenterPoses[i].trans();
@@ -1723,6 +1724,7 @@ void MyAvatar::postUpdate(float deltaTime) {
             AnimDebugDraw::getInstance().addAbsolutePoses("myAvatarLimitCenterPoses", _rig->getAnimSkeleton(), limitCenterPoses, xform, glm::vec4(1));
         }
     }
+    */
 
     if (_enableDebugDrawDefaultPose || _enableDebugDrawAnimPose) {
 

libraries/animation/src/AnimContext.cpp

@@ -10,7 +10,9 @@
 
 #include "AnimContext.h"
 
-AnimContext::AnimContext(bool enableDebugDrawIKTargets, const glm::mat4& geometryToRigMatrix) :
+AnimContext::AnimContext(bool enableDebugDrawIKTargets, const glm::mat4& geometryToRigMatrix, const glm::mat4& rigToWorldMatrix) :
     _enableDebugDrawIKTargets(enableDebugDrawIKTargets),
-    _geometryToRigMatrix(geometryToRigMatrix) {
+    _geometryToRigMatrix(geometryToRigMatrix),
+    _rigToWorldMatrix(rigToWorldMatrix)
+{
 }

libraries/animation/src/AnimContext.h

@@ -16,15 +16,17 @@
 
 class AnimContext {
 public:
-    AnimContext(bool enableDebugDrawIKTargets, const glm::mat4& geometryToRigMatrix);
+    AnimContext(bool enableDebugDrawIKTargets, const glm::mat4& geometryToRigMatrix, const glm::mat4& rigToWorldMatrix);
 
     bool getEnableDebugDrawIKTargets() const { return _enableDebugDrawIKTargets; }
     const glm::mat4& getGeometryToRigMatrix() const { return _geometryToRigMatrix; }
+    const glm::mat4& getRigToWorldMatrix() const { return _rigToWorldMatrix; }
 
 protected:
 
     bool _enableDebugDrawIKTargets { false };
     glm::mat4 _geometryToRigMatrix;
+    glm::mat4 _rigToWorldMatrix;
 };
 
 #endif  // hifi_AnimContext_h

libraries/animation/src/AnimInverseKinematics.cpp

@@ -399,6 +399,8 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar
 //virtual
 const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) {
 
+    debugDrawConstraints(context);
+
     const float MAX_OVERLAY_DT = 1.0f / 30.0f; // what to clamp delta-time to in AnimInverseKinematics::overlay
     if (dt > MAX_OVERLAY_DT) {
         dt = MAX_OVERLAY_DT;
@@ -604,9 +606,9 @@ void AnimInverseKinematics::clearIKJointLimitHistory() {
     }
 }
 
-RotationConstraint* AnimInverseKinematics::getConstraint(int index) {
+RotationConstraint* AnimInverseKinematics::getConstraint(int index) const {
     RotationConstraint* constraint = nullptr;
-    std::map<int, RotationConstraint*>::iterator constraintItr = _constraints.find(index);
+    std::map<int, RotationConstraint*>::const_iterator constraintItr = _constraints.find(index);
     if (constraintItr != _constraints.end()) {
         constraint = constraintItr->second;
     }
@@ -1003,3 +1005,117 @@ void AnimInverseKinematics::setSkeletonInternal(AnimSkeleton::ConstPointer skele
         _hipsParentIndex = -1;
     }
 }
+
+void AnimInverseKinematics::debugDrawConstraints(const AnimContext& context) const {
+
+    if (_skeleton) {
+        const vec4 RED(1.0f, 0.0f, 0.0f, 1.0f);
+        const vec4 GREEN(0.0f, 1.0f, 0.0f, 1.0f);
+        const vec4 BLUE(0.0f, 0.0f, 1.0f, 1.0f);
+        const vec4 PURPLE(0.5f, 0.0f, 1.0f, 1.0f);
+        const vec4 CYAN(0.0f, 1.0f, 1.0f, 1.0f);
+        const vec4 GRAY(0.2f, 0.2f, 0.2f, 1.0f);
+        const vec4 MAGENTA(1.0f, 0.0f, 1.0f, 1.0f);
+        const float AXIS_LENGTH = 2.0f; // cm
+        const float TWIST_LENGTH = 4.0f; // cm
+        const float HINGE_LENGTH = 6.0f; // cm
+        const float SWING_LENGTH = 5.0f; // cm
+        AnimPoseVec absPoses = /*_limitCenterPoses;*/ _skeleton->getRelativeDefaultPoses();
+        _skeleton->convertRelativePosesToAbsolute(absPoses);
+
+        mat4 geomToWorldMatrix = context.getRigToWorldMatrix() * context.getGeometryToRigMatrix();
+        for (int i = 0; i < absPoses.size(); i++) {
+            // transform local axes into world space.
+            auto pose = absPoses[i];
+            glm::vec3 xAxis = transformVectorFast(geomToWorldMatrix, pose.rot() * Vectors::UNIT_X);
+            glm::vec3 yAxis = transformVectorFast(geomToWorldMatrix, pose.rot() * Vectors::UNIT_Y);
+            glm::vec3 zAxis = transformVectorFast(geomToWorldMatrix, pose.rot() * Vectors::UNIT_Z);
+            glm::vec3 pos = transformPoint(geomToWorldMatrix, pose.trans());
+            DebugDraw::getInstance().drawRay(pos, pos + AXIS_LENGTH * xAxis, RED);
+            DebugDraw::getInstance().drawRay(pos, pos + AXIS_LENGTH * yAxis, GREEN);
+            DebugDraw::getInstance().drawRay(pos, pos + AXIS_LENGTH * zAxis, BLUE);
+
+            // draw line to parent
+            int parentIndex = _skeleton->getParentIndex(i);
+            if (parentIndex != -1) {
+                glm::vec3 parentPos = transformPoint(geomToWorldMatrix, absPoses[parentIndex].trans());
+                DebugDraw::getInstance().drawRay(pos, parentPos, GRAY);
+            }
+
+            glm::quat parentAbsRot;
+            if (parentIndex != -1) {
+                parentAbsRot = absPoses[parentIndex].rot();
+            }
+
+            const RotationConstraint* constraint = getConstraint(i);
+            if (constraint) {
+                glm::quat refRot = constraint->getReferenceRotation();
+                const ElbowConstraint* elbowConstraint = dynamic_cast<const ElbowConstraint*>(constraint);
+                if (elbowConstraint) {
+                    glm::vec3 hingeAxis = transformVectorFast(geomToWorldMatrix, parentAbsRot * refRot * elbowConstraint->getHingeAxis());
+                    DebugDraw::getInstance().drawRay(pos, pos + HINGE_LENGTH * hingeAxis, MAGENTA);
+
+                    // draw elbow constraints
+                    glm::quat minRot = glm::angleAxis(elbowConstraint->getMinAngle(), elbowConstraint->getHingeAxis());
+                    glm::quat maxRot = glm::angleAxis(elbowConstraint->getMaxAngle(), elbowConstraint->getHingeAxis());
+
+                    glm::vec3 minYAxis = transformVectorFast(geomToWorldMatrix, parentAbsRot * minRot * refRot * Vectors::UNIT_Y);
+                    glm::vec3 maxYAxis = transformVectorFast(geomToWorldMatrix, parentAbsRot * maxRot * refRot * Vectors::UNIT_Y);
+
+                    const int NUM_SWING_STEPS = 10;
+                    for (int i = 0; i < NUM_SWING_STEPS + 1; i++) {
+                        glm::quat rot = glm::normalize(glm::lerp(minRot, maxRot, i * (1.0f / NUM_SWING_STEPS)));
+                        glm::vec3 axis = transformVectorFast(geomToWorldMatrix, parentAbsRot * rot * refRot * Vectors::UNIT_Y);
+                        DebugDraw::getInstance().drawRay(pos, pos + TWIST_LENGTH * axis, CYAN);
+                    }
+
+                } else {
+                    const SwingTwistConstraint* swingTwistConstraint = dynamic_cast<const SwingTwistConstraint*>(constraint);
+                    if (swingTwistConstraint) {
+                        // twist constraints
+
+                        glm::vec3 hingeAxis = transformVectorFast(geomToWorldMatrix, parentAbsRot * refRot * Vectors::UNIT_Y);
+                        DebugDraw::getInstance().drawRay(pos, pos + HINGE_LENGTH * hingeAxis, MAGENTA);
+
+                        glm::quat minRot = glm::angleAxis(swingTwistConstraint->getMinTwist(), Vectors::UNIT_Y);
+                        glm::quat maxRot = glm::angleAxis(swingTwistConstraint->getMaxTwist(), Vectors::UNIT_Y);
+
+                        glm::vec3 minTwistYAxis = transformVectorFast(geomToWorldMatrix, parentAbsRot * minRot * refRot * Vectors::UNIT_X);
+                        glm::vec3 maxTwistYAxis = transformVectorFast(geomToWorldMatrix, parentAbsRot * maxRot * refRot * Vectors::UNIT_X);
+
+                        const int NUM_SWING_STEPS = 10;
+                        for (int i = 0; i < NUM_SWING_STEPS + 1; i++) {
+                            glm::quat rot = glm::normalize(glm::lerp(minRot, maxRot, i * (1.0f / NUM_SWING_STEPS)));
+                            glm::vec3 axis = transformVectorFast(geomToWorldMatrix, parentAbsRot * rot * refRot * Vectors::UNIT_X);
+                            DebugDraw::getInstance().drawRay(pos, pos + TWIST_LENGTH * axis, CYAN);
+                        }
+
+                        // draw swing constraints.
+                        glm::vec3 previousSwingTip;
+                        const size_t NUM_MIN_DOTS = swingTwistConstraint->getMinDots().size();
+                        const float D_THETA = TWO_PI / NUM_MIN_DOTS;
+                        float theta = 0.0f;
+                        for (size_t i = 0; i < NUM_MIN_DOTS; i++, theta += D_THETA) {
+                            // compute swing rotation from theta and phi angles.
+                            float phi = acos(swingTwistConstraint->getMinDots()[i]);
+                            float cos_phi = swingTwistConstraint->getMinDots()[i];
+                            float sin_phi = sinf(phi);
+                            glm::vec3 swungAxis(sin_phi * cosf(theta), cos_phi, sin_phi * sinf(theta));
+                            glm::vec3 worldSwungAxis = transformVectorFast(geomToWorldMatrix, parentAbsRot * refRot * swungAxis);
+
+                            glm::vec3 swingTip = pos + SWING_LENGTH * worldSwungAxis;
+                            DebugDraw::getInstance().drawRay(pos, swingTip, PURPLE);
+
+                            if (previousSwingTipValid) {
+                                DebugDraw::getInstance().drawRay(previousSwingTip, swingTip, PURPLE);
+                            }
+                            previousSwingTip = swingTip;
+                            previousSwingTipValid = true;
+                        }
+                    }
+                }
+                pose.rot() = constraint->computeCenterRotation();
+            }
+        }
+    }
+}

libraries/animation/src/AnimInverseKinematics.h

@@ -51,11 +51,12 @@ protected:
     void solveWithCyclicCoordinateDescent(const std::vector<IKTarget>& targets);
     int solveTargetWithCCD(const IKTarget& target, AnimPoseVec& absolutePoses);
     virtual void setSkeletonInternal(AnimSkeleton::ConstPointer skeleton) override;
+    void debugDrawConstraints(const AnimContext& context) const;
 
     // for AnimDebugDraw rendering
     virtual const AnimPoseVec& getPosesInternal() const override { return _relativePoses; }
 
-    RotationConstraint* getConstraint(int index);
+    RotationConstraint* getConstraint(int index) const;
     void clearConstraints();
     void initConstraints();
     void initLimitCenterPoses();

libraries/animation/src/AnimPose.cpp

@@ -39,7 +39,7 @@ glm::vec3 AnimPose::xformPoint(const glm::vec3& rhs) const {
     return *this * rhs;
 }
 
-// really slow
+// really slow, but accurate for transforms with non-uniform scale
 glm::vec3 AnimPose::xformVector(const glm::vec3& rhs) const {
     glm::vec3 xAxis = _rot * glm::vec3(_scale.x, 0.0f, 0.0f);
     glm::vec3 yAxis = _rot * glm::vec3(0.0f, _scale.y, 0.0f);
@@ -49,6 +49,11 @@ glm::vec3 AnimPose::xformVector(const glm::vec3& rhs) const {
     return transInvMat * rhs;
 }
 
+// faster, but does not handle non-uniform scale correctly.
+glm::vec3 AnimPose::xformVectorFast(const glm::vec3& rhs) const {
+    return _rot * (_scale * rhs);
+}
+
 AnimPose AnimPose::operator*(const AnimPose& rhs) const {
     glm::mat4 result;
     glm_mat4u_mul(*this, rhs, result);

libraries/animation/src/AnimPose.h

@@ -25,7 +25,8 @@ public:
     static const AnimPose identity;
 
     glm::vec3 xformPoint(const glm::vec3& rhs) const;
-    glm::vec3 xformVector(const glm::vec3& rhs) const;  // really slow
+    glm::vec3 xformVector(const glm::vec3& rhs) const;  // really slow, but accurate for transforms with non-uniform scale
+    glm::vec3 xformVectorFast(const glm::vec3& rhs) const;  // faster, but does not handle non-uniform scale correctly.
 
     glm::vec3 operator*(const glm::vec3& rhs) const; // same as xformPoint
     AnimPose operator*(const AnimPose& rhs) const;

libraries/animation/src/ElbowConstraint.h

@@ -19,6 +19,11 @@ public:
     void setAngleLimits(float minAngle, float maxAngle);
     virtual bool apply(glm::quat& rotation) const override;
     virtual glm::quat computeCenterRotation() const override;
+
+    glm::vec3 getHingeAxis() const { return _axis; }
+    float getMinAngle() const { return _minAngle; }
+    float getMaxAngle() const { return _maxAngle; }
+
 protected:
     glm::vec3 _axis;
     glm::vec3 _perpAxis;

libraries/animation/src/Rig.cpp

@@ -941,7 +941,7 @@ void Rig::updateAnimationStateHandlers() { // called on avatar update thread (wh
     }
 }
 
-void Rig::updateAnimations(float deltaTime, glm::mat4 rootTransform) {
+void Rig::updateAnimations(float deltaTime, glm::mat4 rootTransform, glm::mat4 rigToWorldTransform) {
 
     PROFILE_RANGE_EX(simulation_animation_detail, __FUNCTION__, 0xffff00ff, 0);
     PerformanceTimer perfTimer("updateAnimations");
@@ -954,7 +954,7 @@ void Rig::updateAnimations(float deltaTime, glm::mat4 rootTransform) {
         updateAnimationStateHandlers();
         _animVars.setRigToGeometryTransform(_rigToGeometryTransform);
 
-        AnimContext context(_enableDebugDrawIKTargets, getGeometryToRigTransform());
+        AnimContext context(_enableDebugDrawIKTargets, getGeometryToRigTransform(), rigToWorldTransform);
 
         // evaluate the animation
         AnimNode::Triggers triggersOut;
@@ -1445,7 +1445,7 @@ void Rig::computeAvatarBoundingCapsule(
 
     // call overlay twice: once to verify AnimPoseVec joints and again to do the IK
     AnimNode::Triggers triggersOut;
-    AnimContext context(false, glm::mat4());
+    AnimContext context(false, glm::mat4(), glm::mat4());
     float dt = 1.0f; // the value of this does not matter
     ikNode.overlay(animVars, context, dt, triggersOut, _animSkeleton->getRelativeBindPoses());
     AnimPoseVec finalPoses =  ikNode.overlay(animVars, context, dt, triggersOut, _animSkeleton->getRelativeBindPoses());

libraries/animation/src/Rig.h

@@ -162,7 +162,7 @@ public:
     void computeMotionAnimationState(float deltaTime, const glm::vec3& worldPosition, const glm::vec3& worldVelocity, const glm::quat& worldRotation, CharacterControllerState ccState);
 
     // Regardless of who started the animations or how many, update the joints.
-    void updateAnimations(float deltaTime, glm::mat4 rootTransform);
+    void updateAnimations(float deltaTime, glm::mat4 rootTransform, glm::mat4 rigToWorldTransform);
 
     // legacy
     void inverseKinematics(int endIndex, glm::vec3 targetPosition, const glm::quat& targetRotation, float priority,

libraries/animation/src/SwingTwistConstraint.cpp

@@ -435,13 +435,13 @@ void SwingTwistConstraint::clearHistory() {
 glm::quat SwingTwistConstraint::computeCenterRotation() const {
     const size_t NUM_MIN_DOTS = getMinDots().size();
     const size_t NUM_LIMITS = 2 * NUM_MIN_DOTS;
-    std::vector<glm::quat> limits;
+    std::vector<glm::quat> swingLimits;
-    limits.reserve(NUM_LIMITS);
+    swingLimits.reserve(NUM_LIMITS);
-    glm::quat minTwistRot;
+
-    glm::quat maxTwistRot;
+    glm::quat twistLimits[2];
     if (_minTwist != _maxTwist) {
-        minTwistRot = glm::angleAxis(_minTwist, _referenceRotation * Vectors::UNIT_Y);
+        twistLimits[0] = glm::angleAxis(_minTwist, _referenceRotation * Vectors::UNIT_Y);
-        minTwistRot = glm::angleAxis(_maxTwist, _referenceRotation * Vectors::UNIT_Y);
+        twistLimits[1] = glm::angleAxis(_maxTwist, _referenceRotation * Vectors::UNIT_Y);
     }
     const float D_THETA = TWO_PI / NUM_MIN_DOTS;
     float theta = 0.0f;
@@ -451,9 +451,11 @@ glm::quat SwingTwistConstraint::computeCenterRotation() const {
         float cos_phi = getMinDots()[i];
         float sin_phi = sinf(phi);
         glm::vec3 swungAxis(sin_phi * cosf(theta), cos_phi, sin_phi * sinf(theta));
-        glm::quat swing = glm::angleAxis(phi, glm::cross(Vectors::UNIT_Y, swungAxis));
+        glm::quat swing = glm::angleAxis(phi, glm::normalize(glm::cross(Vectors::UNIT_Y, swungAxis)));
-        limits.push_back(swing * minTwistRot);
+        swingLimits.push_back(swing);
-        limits.push_back(swing * maxTwistRot);
     }
-    return averageQuats(limits.size(), &limits[0]);
+    glm::quat limits[2];
+    limits[0] = averageQuats(swingLimits.size(), &swingLimits[0]);
+    limits[1] = averageQuats(2, twistLimits);
+    return averageQuats(2, limits);
 }

libraries/animation/src/SwingTwistConstraint.h

@@ -101,6 +101,9 @@ public:
 
     virtual glm::quat computeCenterRotation() const override;
 
+    const float getMinTwist() const { return _minTwist; }
+    const float getMaxTwist() const { return _maxTwist; }
+
 private:
     float handleTwistBoundaryConditions(float twistAngle) const;
 

libraries/render-utils/src/Model.cpp

@@ -1046,7 +1046,8 @@ void Model::simulate(float deltaTime, bool fullUpdate) {
 //virtual
 void Model::updateRig(float deltaTime, glm::mat4 parentTransform) {
     _needsUpdateClusterMatrices = true;
-    _rig->updateAnimations(deltaTime, parentTransform);
+    glm::mat4 rigToWorldTransform = createMatFromQuatAndPos(getRotation(), getTranslation());
+    _rig->updateAnimations(deltaTime, parentTransform, rigToWorldTransform);
 }
 
 void Model::computeMeshPartLocalBounds() {