From 187cd0d5cf54284c7bee392287aafcb63554b945 Mon Sep 17 00:00:00 2001
From: Atlante45 <clement.brisset@gmail.com>
Date: Thu, 16 Feb 2017 17:49:17 -0800
Subject: [PATCH] Get IK error and stand up based on it

---
 interface/src/avatar/MyAvatar.cpp                 |  4 ++++
 interface/src/avatar/MyAvatar.h                   |  2 ++
 libraries/animation/src/AnimInverseKinematics.cpp |  1 +
 libraries/animation/src/AnimInverseKinematics.h   |  4 ++++
 libraries/animation/src/Rig.cpp                   | 15 +++++++++++++++
 libraries/animation/src/Rig.h                     |  3 +++
 scripts/tutorials/entity_scripts/sit.js           |  6 +++++-
 7 files changed, 34 insertions(+), 1 deletion(-)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index 0943d0a057..9f2f3eabac 100644
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -2517,6 +2517,10 @@ bool MyAvatar::clearPinOnJoint(int index) {
     return false;
 }
 
+float MyAvatar::getIKErrorOnLastSolve() const {
+    return _rig->getIKErrorOnLastSolve();
+}
+
 // thread-safe
 void MyAvatar::addHoldAction(AvatarActionHold* holdAction) {
     std::lock_guard<std::mutex> guard(_holdActionsMutex);
diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h
index 360c6918d9..edba2929e8 100644
--- a/interface/src/avatar/MyAvatar.h
+++ b/interface/src/avatar/MyAvatar.h
@@ -219,6 +219,8 @@ public:
     Q_INVOKABLE bool pinJoint(int index, const glm::vec3& position, const glm::quat& orientation);
     Q_INVOKABLE bool clearPinOnJoint(int index);
 
+    Q_INVOKABLE float getIKErrorOnLastSolve() const;
+
     Q_INVOKABLE void useFullAvatarURL(const QUrl& fullAvatarURL, const QString& modelName = QString());
     Q_INVOKABLE QUrl getFullAvatarURLFromPreferences() const { return _fullAvatarURLFromPreferences; }
     Q_INVOKABLE QString getFullAvatarModelName() const { return _fullAvatarModelName; }
diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp
index 3dc80fc764..1cf70bcc14 100644
--- a/libraries/animation/src/AnimInverseKinematics.cpp
+++ b/libraries/animation/src/AnimInverseKinematics.cpp
@@ -189,6 +189,7 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vector<I
             }
         }
     }
+    _maxErrorOnLastSolve = maxError;
 
     // finally set the relative rotation of each tip to agree with absolute target rotation
     for (auto& target: targets) {
diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h
index 7e4a7e5473..6b8d9847f3 100644
--- a/libraries/animation/src/AnimInverseKinematics.h
+++ b/libraries/animation/src/AnimInverseKinematics.h
@@ -41,6 +41,8 @@ public:
 
     void setMaxHipsOffsetLength(float maxLength);
 
+    float getMaxErrorOnLastSolve() { return _maxErrorOnLastSolve; }
+
 protected:
     void computeTargets(const AnimVariantMap& animVars, std::vector<IKTarget>& targets, const AnimPoseVec& underPoses);
     void solveWithCyclicCoordinateDescent(const std::vector<IKTarget>& targets);
@@ -93,6 +95,8 @@ protected:
     // _maxTargetIndex is tracked to help optimize the recalculation of absolute poses
     // during the the cyclic coordinate descent algorithm
     int _maxTargetIndex { 0 };
+
+    float _maxErrorOnLastSolve { FLT_MAX };
 };
 
 #endif // hifi_AnimInverseKinematics_h
diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp
index 311623a002..c47da7c0b0 100644
--- a/libraries/animation/src/Rig.cpp
+++ b/libraries/animation/src/Rig.cpp
@@ -337,6 +337,21 @@ float Rig::getMaxHipsOffsetLength() const {
     return _maxHipsOffsetLength;
 }
 
+float Rig::getIKErrorOnLastSolve() const {
+    float result = 0.0f;
+
+    if (_animNode) {
+        _animNode->traverse([&](AnimNode::Pointer node) {
+            auto ikNode = std::dynamic_pointer_cast<AnimInverseKinematics>(node);
+            if (ikNode) {
+                result = ikNode->getMaxErrorOnLastSolve();
+            }
+            return true;
+        });
+    }
+    return result;
+}
+
 int Rig::getJointParentIndex(int childIndex) const {
     if (_animSkeleton && isIndexValid(childIndex)) {
         return _animSkeleton->getParentIndex(childIndex);
diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h
index 5d1831345b..f1c87d0d3e 100644
--- a/libraries/animation/src/Rig.h
+++ b/libraries/animation/src/Rig.h
@@ -107,6 +107,8 @@ public:
     void setMaxHipsOffsetLength(float maxLength);
     float getMaxHipsOffsetLength() const;
 
+    float getIKErrorOnLastSolve() const;
+
     int getJointParentIndex(int childIndex) const;
 
     // geometry space
@@ -321,6 +323,7 @@ protected:
 
     mutable uint32_t _jointNameWarningCount { 0 };
     float _maxHipsOffsetLength { 1.0f };
+    float _maxErrorOnLastSolve { 0.0f };
 
 private:
     QMap<int, StateHandler> _stateHandlers;
diff --git a/scripts/tutorials/entity_scripts/sit.js b/scripts/tutorials/entity_scripts/sit.js
index 1a9b867c16..6eebaf6496 100644
--- a/scripts/tutorials/entity_scripts/sit.js
+++ b/scripts/tutorials/entity_scripts/sit.js
@@ -9,6 +9,7 @@
   var RELEASE_KEYS = ['w', 'a', 's', 'd', 'UP', 'LEFT', 'DOWN', 'RIGHT'];
   var RELEASE_TIME = 500; // ms
   var RELEASE_DISTANCE = 0.2; // meters
+  var MAX_IK_ERROR = 15;
 
   this.entityID = null;
   this.timers = {};
@@ -21,7 +22,10 @@
   this.update = function(dt) {
     if (MyAvatar.getParentID() === this.entityID) {
       var properties = Entities.getEntityProperties(this.entityID, ["position"]);
-      if (Vec3.distance(MyAvatar.position, properties.position) > RELEASE_DISTANCE) {
+      var avatarDistance = Vec3.distance(MyAvatar.position, properties.position);
+      var ikError = MyAvatar.getIKErrorOnLastSolve();
+      print("IK error: " + ikError);
+      if (avatarDistance > RELEASE_DISTANCE || ikError > MAX_IK_ERROR) {
         this.sitUp(this.entityID);
       }
     }