From e2c9058f0a36535bc1c5c8a4523b6312cfe45d0d Mon Sep 17 00:00:00 2001
From: amantley <amantley@googlemail.com>
Date: Wed, 6 Feb 2019 18:29:33 -0800
Subject: [PATCH] first try at the new elbow code

---
 .../src/AnimPoleVectorConstraint.cpp          | 58 ++++++++++++++++++-
 .../animation/src/AnimPoleVectorConstraint.h  |  2 +
 2 files changed, 59 insertions(+), 1 deletion(-)

diff --git a/libraries/animation/src/AnimPoleVectorConstraint.cpp b/libraries/animation/src/AnimPoleVectorConstraint.cpp
index f017fe2348..7431b6620b 100644
--- a/libraries/animation/src/AnimPoleVectorConstraint.cpp
+++ b/libraries/animation/src/AnimPoleVectorConstraint.cpp
@@ -34,6 +34,53 @@ AnimPoleVectorConstraint::~AnimPoleVectorConstraint() {
 
 }
 
+float AnimPoleVectorConstraint::findThetaNewWay(const glm::vec3& hand, const glm::vec3& shoulder) const {
+    // get the default poses for the upper and lower arm
+    // then use this length to judge how far the arm is away from the shoulder.
+    // then create weights that make the elbow angle less when the x value is large in either direction.
+    // make the angle less when z is small.  
+    // lower y with x center lower angle
+    // lower y with x out higher angle
+    AnimPose shoulderPose = _skeleton->getAbsoluteDefaultPose(_skeleton->nameToJointIndex("RightShoulder"));
+    AnimPose handPose = _skeleton->getAbsoluteDefaultPose(_skeleton->nameToJointIndex("RightHand"));
+    // subtract 10 centimeters from the arm length for some reason actual arm position is clamped to length - 10cm.
+    float defaultArmLength = glm::length( handPose.trans() - shoulderPose.trans() ) - 10.0f;
+    qCDebug(animation) << "default arm length " << defaultArmLength;
+
+    // phi_0 is the lowest angle we can have
+    const float phi_0 = 15.0f;
+    // biases
+    const glm::vec3 biases(60.0f, 120.0f, -30.0f);
+    // weights
+    const glm::vec3 weights(-70.0f, 60.0f, 210.0f);
+    glm::vec3 armToHand = hand - shoulder;
+    qCDebug(animation) << "current arm length " << glm::length(armToHand);
+    float initial_valuesX = ((fabsf(armToHand[0]) / defaultArmLength) * weights[0]) + biases[0];
+    float initial_valuesY = ((armToHand[1] / defaultArmLength) * weights[1]) + biases[1];
+    float initial_valuesZ = ((armToHand[2] / defaultArmLength) * weights[2]) + biases[2];
+    if (initial_valuesX < 0.0f) {
+        initial_valuesX = 0.0f;
+    }
+    if (initial_valuesY < 0.0f) {
+        initial_valuesY = 0.0f;
+    }
+    if (initial_valuesZ < 0.0f) {
+        initial_valuesZ = 0.0f;
+    }
+
+    float theta = initial_valuesX + initial_valuesY + initial_valuesZ;
+
+    if (theta < 13.0f) {
+        theta = 13.0f;
+    }
+    if (theta > 175.0f) {
+        theta = 175.0f;
+    }
+
+    qCDebug(animation) << "relative hand" << initial_valuesX << " " << initial_valuesY << " " << initial_valuesZ << "theta value for pole vector is " << theta;
+    return theta;
+}
+
 const AnimPoseVec& AnimPoleVectorConstraint::evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimVariantMap& triggersOut) {
 
     assert(_children.size() == 1);
@@ -121,7 +168,14 @@ const AnimPoseVec& AnimPoleVectorConstraint::evaluate(const AnimVariantMap& anim
         float sideDot = glm::dot(poleVector, sideVector);
         float theta = copysignf(1.0f, sideDot) * acosf(dot);
 
-        glm::quat deltaRot = glm::angleAxis(theta, unitAxis);
+        float fred;
+        if (_skeleton->nameToJointIndex("RightHand") == _tipJointIndex) {
+            qCDebug(animation) << "theta from the old code " << theta;
+            fred = findThetaNewWay(tipPose.trans(), basePose.trans());
+        }
+
+        //glm::quat deltaRot = glm::angleAxis(theta, unitAxis);
+        glm::quat deltaRot = glm::angleAxis(((180.0f - fred)/180.0f)*PI, unitAxis);
 
         // transform result back into parent relative frame.
         glm::quat relBaseRot = glm::inverse(baseParentPose.rot()) * deltaRot * basePose.rot();
@@ -208,6 +262,8 @@ const AnimPoseVec& AnimPoleVectorConstraint::evaluate(const AnimVariantMap& anim
     return _poses;
 }
 
+
+
 // for AnimDebugDraw rendering
 const AnimPoseVec& AnimPoleVectorConstraint::getPosesInternal() const {
     return _poses;
diff --git a/libraries/animation/src/AnimPoleVectorConstraint.h b/libraries/animation/src/AnimPoleVectorConstraint.h
index 44e22671c1..df8cddec7d 100644
--- a/libraries/animation/src/AnimPoleVectorConstraint.h
+++ b/libraries/animation/src/AnimPoleVectorConstraint.h
@@ -25,6 +25,8 @@ public:
                              const QString& enabledVar, const QString& poleVectorVar);
     virtual ~AnimPoleVectorConstraint() override;
 
+    float findThetaNewWay(const glm::vec3& hand, const glm::vec3& shoulder) const;
+
     virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimVariantMap& triggersOut) override;
 
 protected: