From 057d61527553d3ea5056b967b2828d201ad1600f Mon Sep 17 00:00:00 2001
From: Jeremy Hu <huxingyi@msn.com>
Date: Fri, 29 Jun 2018 11:28:37 +0800
Subject: [PATCH] Fix leg start orientation

---
 src/locomotioncontroller.cpp | 32 +++++++++++++++++++++++++-------
 src/pogostick.cpp            |  4 ++--
 2 files changed, 27 insertions(+), 9 deletions(-)

diff --git a/src/locomotioncontroller.cpp b/src/locomotioncontroller.cpp
index 1530e9d4..a3efa77a 100644
--- a/src/locomotioncontroller.cpp
+++ b/src/locomotioncontroller.cpp
@@ -149,7 +149,8 @@ void LocomotionController::simulate(float amount)
     std::vector<QVector3D> spineOrientations;
     spineOrientations.resize(std::max(leftLegStartPositions.size(), rightLegStartPositions.size()), pointerFront);
     for (int i = 0; i < (int)leftLegStartPositions.size() && i < (int)rightLegStartPositions.size(); i++) {
-        spineOrientations[i] = -QVector3D::crossProduct(leftLegStartPositions[i] - rightLegStartPositions[i], pointerUp);
+        auto rotateDirection = -QVector3D::crossProduct(rightLegStartPositions[i] - leftLegStartPositions[i], pointerUp);
+        spineOrientations[i] = (rotateDirection + pointerFront + pointerFront).normalized();
     }
     
     // Calculate average pitch for each leg
@@ -164,9 +165,8 @@ void LocomotionController::simulate(float amount)
     for (int i = 0; i < (int)spinePitches.size(); i++) {
         spinePitches[i].normalize();
         if (i < (int)spineOrientations.size()) {
-            QQuaternion rotation;
-            rotation.rotationTo(pointerFront, spineOrientations[i]);
-            spinePitches[i] = rotation.rotatedVector(spinePitches[i]);
+            auto quaternion = QQuaternion::rotationTo(pointerFront, spineOrientations[i]);
+            spinePitches[i] = quaternion.rotatedVector(spinePitches[i]);
         }
     }
     
@@ -233,6 +233,17 @@ void LocomotionController::simulate(float amount)
         }
     }
     
+    // Move head
+    if (-1 != m_inputJointNodeTree.head && spineNodes.size() > 0) {
+        const auto offset = legStartOffsets[0];
+        m_outputJointNodeTree.joints()[m_inputJointNodeTree.head].position += offset;
+        std::vector<int> headChildren;
+        m_inputJointNodeTree.collectChildren(m_inputJointNodeTree.head, headChildren);
+        for (const auto &child: headChildren) {
+            m_outputJointNodeTree.joints()[child].position += offset;
+        }
+    }
+    
     // Move tail
     if (-1 != m_inputJointNodeTree.tail && spineNodes.size() > 0) {
         int lastSpineJointIndex = spineNodes[spineNodes.size() - 1];
@@ -258,7 +269,7 @@ void LocomotionController::simulate(float amount)
         int headParent = m_inputJointNodeTree.joints()[m_inputJointNodeTree.head].parentIndex;
         if (-1 != headParent) {
             makeInbetweenNodesInHermiteCurve(m_inputJointNodeTree.head,
-                m_inputJointNodeTree.joints()[m_inputJointNodeTree.head].position - m_inputJointNodeTree.joints()[headParent].position,
+                m_outputJointNodeTree.joints()[m_inputJointNodeTree.head].position - m_outputJointNodeTree.joints()[headParent].position,
                 firstSpineJointIndex,
                 spinePitches[0]);
         }
@@ -312,10 +323,17 @@ void LocomotionController::makeInbetweenNodesInHermiteCurve(int firstJointIndex,
         const QVector3D &m1 = childPitch;
         for (int i = (int)inbetweenIndicies.size() - 2; i >= 0; i--) {
             accumLength += lengths[i + 1];
-            float t = accumLength / totalLength;
+            float t = (accumLength / totalLength) * 0.8;
             QVector3D revisedPosition = pointInHermiteCurve(t, p0, m0, p1, m1);
             qDebug() << "pointInHermiteCurve(t:<<" << t << ", p0:" << p0 << ", m0:" << m0 << ", p1:" << p1 << ", m1:" << m1 << ") result:" << revisedPosition;
-            m_outputJointNodeTree.joints()[inbetweenIndicies[i]].position = revisedPosition;
+            auto &inbetweenJointPosition = m_outputJointNodeTree.joints()[inbetweenIndicies[i]].position;
+            QVector3D translation = revisedPosition - inbetweenJointPosition;
+            inbetweenJointPosition = revisedPosition;
+            std::vector<int> trivialChildren;
+            m_inputJointNodeTree.collectTrivialChildren(inbetweenIndicies[i], trivialChildren);
+            for (const auto &child: trivialChildren) {
+                m_outputJointNodeTree.joints()[child].position += translation;
+            }
         }
     }
 }
diff --git a/src/pogostick.cpp b/src/pogostick.cpp
index e6f8e056..e06f0880 100644
--- a/src/pogostick.cpp
+++ b/src/pogostick.cpp
@@ -6,7 +6,7 @@ float PogoStick::m_gravitationalAcceleration = 9.8;
 
 PogoStick::PogoStick()
 {
-    setStancePhaseDuration(0.8);
+    setStancePhaseDuration(0.95);
 }
 
 void PogoStick::setGroundLocation(float groundLocation)
@@ -99,7 +99,7 @@ void PogoStick::simulate(float time)
             qDebug() << "Propulsion Phase (up) displacement:" << displacement;
             m_pelviosLocation = m_restPelvisLocation - (m_stancePhaseHeight - displacement);
         }
-        m_footHorizontalOffset = -time * 0.25 / stancePhaseDuration;
+        m_footHorizontalOffset = -time * 0.5 / stancePhaseDuration;
     } else {
         float swingPhaseMiddle = swingPhaseBegin + (1 - swingPhaseBegin) * 0.5;
         // Swing Phase