Add experiment walk animation clip generation.

- Fix intermediate bones remove;
- Add locomotion controller;
- Add head, tail node marks for animation.

ACKNOWLEDGEMENTS.html

@@ -563,3 +563,13 @@ https://www.reddit.com/r/gamedev/comments/5iuf3h/i_am_writting_a_3d_monster_mode
     Generic Spine Model with Simple Physics for Life-Like Quadrupeds and Reptiles
     http://liris.cnrs.fr/Documents/Liris-5778.pdf
 </pre>
+
+<h1>Hermite Curves</h1>
+<pre>
+    https://en.wikibooks.org/wiki/Cg_Programming/Unity/Hermite_Curves
+</pre>
+
+<h1>AppImage</h1>
+<pre>
+    https://appimage.org/
+</pre>

dust3d.pro

@@ -155,6 +155,15 @@ HEADERS += src/skinnedmesh.h
 SOURCES += src/ragdoll.cpp
 HEADERS += src/ragdoll.h
 
+SOURCES += src/pogostick.cpp
+HEADERS += src/pogostick.h
+
+SOURCES += src/ikjoint.cpp
+HEADERS += src/ikjoint.h
+
+SOURCES += src/locomotioncontroller.cpp
+HEADERS += src/locomotioncontroller.h
+
 HEADERS += src/qtlightmapper.h
 
 SOURCES += src/main.cpp

src/animationclipgenerator.cpp

@@ -4,7 +4,7 @@
 
 const std::vector<QString> AnimationClipGenerator::supportedClipNames = {
     "Idle",
-    //"Walk",
+    "Walk",
     //"Run",
     //"Attack",
     //"Hurt",
@@ -41,6 +41,7 @@ AnimationClipGenerator::~AnimationClipGenerator()
 #if USE_RAGDOLL
     delete m_ragdoll;
 #endif
+    delete m_locomotionController;
 }
 
 std::vector<std::pair<float, MeshLoader *>> AnimationClipGenerator::takeFrameMeshes()
@@ -57,11 +58,17 @@ void AnimationClipGenerator::generateFrame(SkinnedMesh &skinnedMesh, float amoun
     
     rigController->resetFrame();
     
-    if (m_clipName == "Idle")
-        rigController->idle(amount);
-    
     RigFrame frame(jointNodeTree->joints().size());
     
+    if (m_clipName == "Idle") {
+        rigController->idle(amount);
+        rigController->saveFrame(frame);
+    } else if (m_clipName == "Walk") {
+        m_locomotionController->simulate(amount);
+        JointNodeTree outputJointNodeTree = m_locomotionController->outputJointNodeTreeOnlyPositions();
+        outputJointNodeTree.recalculateMatricesAfterPositionUpdated();
+        m_jointNodeTree.diff(outputJointNodeTree, frame);
+    } else if (m_clipName == "Die") {
 #if USE_BULLET
         if (nullptr == m_ragdoll) {
             rigController->saveFrame(frame);
@@ -69,9 +76,8 @@ void AnimationClipGenerator::generateFrame(SkinnedMesh &skinnedMesh, float amoun
             m_ragdoll->stepSimulation(duration);
             m_ragdoll->saveFrame(frame);
         }
-#else
-    rigController->saveFrame(frame);
 #endif
+    }
     
     if (m_wantMesh) {
         skinnedMesh.applyRigFrameToMesh(frame);
@@ -91,8 +97,31 @@ void AnimationClipGenerator::generate()
     RigController *rigController = skinnedMesh.rigController();
     rigController->resetFrame();
     
+    qDebug() << "animation clip name:" << m_clipName;
+    
+    if (m_clipName == "Idle") {
+        float duration = 0.1;
+        float nextBeginTime = 0;
+        for (float amount = 0.0; amount <= 0.05; amount += 0.01) {
+            generateFrame(skinnedMesh, amount, nextBeginTime, duration);
+            nextBeginTime += duration;
+        }
+        for (float amount = 0.05; amount >= 0.0; amount -= 0.01) {
+            generateFrame(skinnedMesh, amount, nextBeginTime, duration);
+            nextBeginTime += duration;
+        }
+    } else if (m_clipName == "Walk") {
+        m_locomotionController = new LocomotionController(m_jointNodeTree);
+        m_locomotionController->prepare();
+        
+        float duration = 0.05;
+        float nextBeginTime = 0;
+        for (float amount = 0.0; amount <= 1; amount += duration) {
+            generateFrame(skinnedMesh, amount, nextBeginTime, duration);
+            nextBeginTime += duration;
+        }
+    } else if (m_clipName == "Die") {
 #if USE_BULLET
-    if (m_clipName == "Die") {
         delete m_ragdoll;
         float averageLegHeight = rigController->averageLegHeight();
         rigController->liftLeftLegs(QVector3D(0, averageLegHeight * 1.2, 0));
@@ -104,27 +133,14 @@ void AnimationClipGenerator::generate()
         m_ragdoll = new Ragdoll(m_jointNodeTree, ragdollJointNodeTree);
         m_ragdoll->setGroundPlaneY(rigController->minY());
         m_ragdoll->prepare();
-    }
-#endif
         
-    if (m_clipName == "Die") {
         float duration = 1.0 / 120;
         float nextBeginTime = 0;
         for (float amount = 0.0; amount <= 3; amount += duration) {
             generateFrame(skinnedMesh, amount, nextBeginTime, duration);
             nextBeginTime += duration;
         }
-    } else {
+#endif
-        float duration = 0.1;
-        float nextBeginTime = 0;
-        for (float amount = 0.0; amount <= 0.05; amount += 0.01) {
-            generateFrame(skinnedMesh, amount, nextBeginTime, duration);
-            nextBeginTime += duration;
-        }
-        for (float amount = 0.05; amount >= 0.0; amount -= 0.01) {
-            generateFrame(skinnedMesh, amount, nextBeginTime, duration);
-            nextBeginTime += duration;
-        }
     }
 }
 

src/animationclipgenerator.h

@@ -8,6 +8,7 @@
 #include "skinnedmesh.h"
 #include "jointnodetree.h"
 #include "ragdoll.h"
+#include "locomotioncontroller.h"
 
 class AnimationClipGenerator : public QObject
 {
@@ -39,6 +40,7 @@ private:
 #if USE_BULLET
     Ragdoll *m_ragdoll = nullptr;
 #endif
+    LocomotionController *m_locomotionController = nullptr;
 };
 
 #endif

src/animationpanelwidget.cpp

@@ -20,7 +20,7 @@ AnimationPanelWidget::AnimationPanelWidget(SkeletonDocument *document, QWidget *
     buttonsLayout->addSpacing(10);
     
     for (const auto &clipName: AnimationClipGenerator::supportedClipNames) {
-        QPushButton *clipButton = new QPushButton(QObject::tr(qPrintable(clipName)));
+        QPushButton *clipButton = new QPushButton(QObject::tr(qPrintable(clipName + " (Experiment)")));
         connect(clipButton, &QPushButton::clicked, [=] {
             generateClip(clipName);
         });

src/ccdikresolver.cpp

@@ -32,13 +32,13 @@ int CCDIKSolver::addNodeInOrder(const QVector3D &position)
 
 void CCDIKSolver::solveTo(const QVector3D &position)
 {
-    qDebug() << "solveTo:" << position;
+    //qDebug() << "solveTo:" << position;
     m_destination = position;
     float lastDistance2 = 0;
     for (int i = 0; i < m_maxRound; i++) {
         const auto &endEffector = m_nodes[m_nodes.size() - 1];
         float distance2 = (endEffector.position - m_destination).lengthSquared();
-        qDebug() << "Round:" << i << " distance2:" << distance2;
+        //qDebug() << "Round:" << i << " distance2:" << distance2;
         if (distance2 <= m_distanceThreshold2)
             break;
         if (lastDistance2 > 0 && abs(distance2 - lastDistance2) <= m_distanceCeaseThreshold2)

src/dust3dutil.cpp

@@ -26,3 +26,12 @@ void qNormalizeAngle(int &angle)
     while (angle > 360 * 16)
         angle -= 360 * 16;
 }
+
+// https://en.wikibooks.org/wiki/Cg_Programming/Unity/Hermite_Curves
+QVector3D pointInHermiteCurve(float t, QVector3D p0, QVector3D m0, QVector3D p1, QVector3D m1)
+{
+    return (2.0f * t * t * t - 3.0f * t * t + 1.0f) * p0
+        + (t * t * t - 2.0f * t * t + t) * m0
+        + (-2.0f * t * t * t + 3.0f * t * t) * p1
+        + (t * t * t - t * t) * m1;
+}

src/dust3dutil.h

@@ -14,5 +14,6 @@ QString valueOfKeyInMapOrEmpty(const std::map<QString, QString> &map, const QStr
 bool isTrueValueString(const QString &str);
 bool isFloatEqual(float a, float b);
 void qNormalizeAngle(int &angle);
+QVector3D pointInHermiteCurve(float t, QVector3D p0, QVector3D m0, QVector3D p1, QVector3D m1);
 
 #endif

src/ikjoint.cpp

@@ -0,0 +1,33 @@
+#include "ikjoint.h"
+#include "ccdikresolver.h"
+
+void moveIkJoints(const JointNodeTree &inputJointNodeTree, JointNodeTree &outputJointNodeTree,
+    int startJointIndex, int endJointIndex, QVector3D destination)
+{
+    CCDIKSolver ikSolver;
+    ikSolver.setMaxRound(10);
+    int loopIndex = endJointIndex;
+    
+    std::vector<int> ikSolvingIndicies;
+    while (-1 != loopIndex) {
+        const auto &joint = inputJointNodeTree.joints()[loopIndex];
+        ikSolvingIndicies.push_back(loopIndex);
+        if (loopIndex == startJointIndex)
+            break;
+        loopIndex = joint.parentIndex;
+    }
+    std::reverse(std::begin(ikSolvingIndicies), std::end(ikSolvingIndicies));
+    for (const auto &jointIndex: ikSolvingIndicies) {
+        ikSolver.addNodeInOrder(inputJointNodeTree.joints()[jointIndex].position);
+    }
+    
+    ikSolver.solveTo(destination);
+    int nodeCount = ikSolver.getNodeCount();
+    Q_ASSERT(nodeCount == (int)ikSolvingIndicies.size());
+    
+    for (int i = 0; i < nodeCount; i++) {
+        int jointIndex = ikSolvingIndicies[i];
+        const QVector3D &newPosition = ikSolver.getNodeSolvedPosition(i);
+        outputJointNodeTree.joints()[jointIndex].position = newPosition;
+    }
+}

src/ikjoint.h

@@ -0,0 +1,8 @@
+#ifndef IK_JOINT_H
+#define IK_JOINT_H
+#include "jointnodetree.h"
+
+void moveIkJoints(const JointNodeTree &inputJointNodeTree, JointNodeTree &outputJointNodeTree,
+    int startJointIndex, int endJointIndex, QVector3D destination);
+
+#endif

src/intermediateboneremover.cpp

@@ -127,11 +127,9 @@ void IntermediateBoneRemover::solveTrivialFromPairAndSaveTraceTo(std::pair<int,
     if (solvedSet.find(node) != solvedSet.end())
         return;
     solvedSet.insert(node);
-    if (m_startNodeSet.find(node) != m_startNodeSet.end()) {
-        return;
-    }
     if (m_trivialNodeSet.find(node) == m_trivialNodeSet.end()) {
-        if (!history.empty() && m_trivialNodeSet.find(history[history.size() - 1]) != m_trivialNodeSet.end()) {
+        if (!history.empty() && m_trivialNodeSet.find(history[history.size() - 1]) != m_trivialNodeSet.end() &&
+                m_intermediateNodes.find(history[history.size() - 1]) == m_intermediateNodes.end()) {
             for (int i = history.size() - 1; i >= 0; i--) {
                 if (m_trivialNodeSet.find(history[i]) == m_trivialNodeSet.end()) {
                     bool addChildren = false;

src/jointnodetree.cpp

@@ -9,6 +9,8 @@ JointNodeTree::JointNodeTree(MeshResultContext &resultContext)
         return;
     }
     
+    isVerticalSpine = resultContext.isVerticalSpine();
+    
     JointInfo rootCenterJoint;
     {
         rootCenterJoint.jointIndex = m_tracedJoints.size();
@@ -30,6 +32,101 @@ JointNodeTree::JointNodeTree(MeshResultContext &resultContext)
     traceBoneFromJoint(resultContext, std::make_pair(rootNode->bmeshId, rootNode->nodeId), visitedNodes, connections, rootCenterJoint.jointIndex);
     
     calculateMatricesByPosition();
+    
+    collectParts();
+}
+
+const std::vector<std::pair<int, int>> &JointNodeTree::legs() const
+{
+    return m_legs;
+}
+
+const std::vector<int> &JointNodeTree::spine() const
+{
+    return m_spine;
+}
+
+const std::vector<std::pair<int, int>> &JointNodeTree::leftLegs() const
+{
+    return m_leftLegs;
+}
+
+const std::vector<std::pair<int, int>> &JointNodeTree::rightLegs() const
+{
+    return m_rightLegs;
+}
+
+void JointNodeTree::collectParts()
+{
+    m_legs.clear();
+    m_spine.clear();
+    for (const auto &node: joints()) {
+        if (node.boneMark == SkeletonBoneMark::Spine) {
+            m_spine.push_back(node.jointIndex);
+        }
+        if (node.boneMark == SkeletonBoneMark::Head) {
+            if (-1 != head) {
+                qDebug() << "Get multiple head markers";
+            }
+            head = node.jointIndex;
+        }
+        if (node.boneMark == SkeletonBoneMark::Tail) {
+            if (-1 != tail) {
+                qDebug() << "Get multiple tail markers";
+            }
+            tail = node.jointIndex;
+        }
+        if (node.boneMark == SkeletonBoneMark::LegStart && node.children.size() == 1) {
+            const auto legStart = node.jointIndex;
+            const JointInfo *loopNode = &joints()[node.children[0]];
+            while (loopNode->boneMark != SkeletonBoneMark::LegEnd &&
+                    loopNode->children.size() == 1) {
+                loopNode = &joints()[loopNode->children[0]];
+            }
+            if (loopNode->boneMark == SkeletonBoneMark::LegEnd) {
+                const auto legEnd = loopNode->jointIndex;
+                addLeg(legStart, legEnd);
+            } else {
+                qDebug() << "Find leg" << node.partId << "'s end failed";
+            }
+        }
+    }
+    sortLegs(m_leftLegs);
+    sortLegs(m_rightLegs);
+    sortSpine(m_spine);
+}
+
+void JointNodeTree::sortLegs(std::vector<std::pair<int, int>> &legs)
+{
+    const auto &that = this;
+    std::sort(legs.begin(), legs.end(), [&that](const std::pair<int, int> &a, const std::pair<int, int> &b) -> bool {
+        const auto &firstLegEnd = that->joints()[a.second];
+        const auto &secondLegEnd = that->joints()[b.second];
+        return firstLegEnd.position.z() > secondLegEnd.position.z();
+    });
+}
+
+void JointNodeTree::sortSpine(std::vector<int> &spine)
+{
+    const auto &that = this;
+    std::sort(spine.begin(), spine.end(), [&that](const int &a, const int &b) -> bool {
+        const auto &firstNode = that->joints()[a];
+        const auto &secondNode = that->joints()[b];
+        if (that->isVerticalSpine)
+            return firstNode.position.y() > secondNode.position.y();
+        else
+            return firstNode.position.z() > secondNode.position.z();
+    });
+}
+
+void JointNodeTree::addLeg(int legStart, int legEnd)
+{
+    const auto &legEndJoint = joints()[legEnd];
+    m_legs.push_back(std::make_pair(legStart, legEnd));
+    if (legEndJoint.position.x() > 0)
+        m_leftLegs.push_back(std::make_pair(legStart, legEnd));
+    else
+        m_rightLegs.push_back(std::make_pair(legStart, legEnd));
 }
 
 void JointNodeTree::traceBoneFromJoint(MeshResultContext &resultContext, std::pair<int, int> node, std::set<std::pair<int, int>> &visitedNodes, std::set<std::pair<std::pair<int, int>, std::pair<int, int>>> &connections, int parentIndex)
@@ -65,6 +162,7 @@ void JointNodeTree::traceBoneFromJoint(MeshResultContext &resultContext, std::pa
         joint.boneMark = toNode->second->boneMark;
         joint.scale = QVector3D(1.0, 1.0, 1.0);
         joint.radius = toNode->second->radius;
+        joint.parentIndex = parentIndex;
         
         m_tracedNodeToJointIndexMap[std::make_pair(it.first, it.second)] = joint.jointIndex;
         
@@ -184,3 +282,99 @@ void JointNodeTree::recalculateMatricesAfterTransformUpdated()
     calculateMatricesByTransform();
 }
 
+void JointNodeTree::diff(const JointNodeTree &another, RigFrame &rigFrame)
+{
+    if (rigFrame.translations.size() != joints().size())
+        rigFrame = RigFrame(joints().size());
+    for (const auto &first: joints()) {
+        const auto &second = another.joints()[first.jointIndex];
+        if (!qFuzzyCompare(first.translation, second.translation)) {
+            rigFrame.updateTranslation(first.jointIndex, second.translation);
+        }
+        if (!qFuzzyCompare(first.scale, second.scale)) {
+            rigFrame.updateScale(first.jointIndex, second.scale);
+        }
+        if (!qFuzzyCompare(first.rotation, second.rotation)) {
+            rigFrame.updateRotation(first.jointIndex, second.rotation);
+        }
+    }
+}
+
+int JointNodeTree::findHubJoint(int jointIndex, std::vector<int> *tracePath) const
+{
+    int loopJointIndex = joints()[jointIndex].parentIndex;
+    while (-1 != loopJointIndex) {
+        if (nullptr != tracePath)
+            tracePath->push_back(loopJointIndex);
+        const auto &joint = joints()[loopJointIndex];
+        if (joint.children.size() > 1 || -1 == joint.parentIndex)
+            return joint.jointIndex;
+        loopJointIndex = joint.parentIndex;
+    }
+    return -1;
+}
+
+int JointNodeTree::findParent(int jointIndex, int parentIndex, std::vector<int> *tracePath) const
+{
+    int loopJointIndex = joints()[jointIndex].parentIndex;
+    while (-1 != loopJointIndex) {
+        if (nullptr != tracePath)
+            tracePath->push_back(loopJointIndex);
+        const auto &joint = joints()[loopJointIndex];
+        if (joint.jointIndex == parentIndex)
+            return joint.jointIndex;
+        loopJointIndex = joint.parentIndex;
+    }
+    return -1;
+}
+
+int JointNodeTree::findSpineFromChild(int jointIndex)
+{
+    int loopJointIndex = joints()[jointIndex].parentIndex;
+    while (-1 != loopJointIndex) {
+        const auto &joint = joints()[loopJointIndex];
+        if (joint.boneMark == SkeletonBoneMark::Spine)
+            return joint.jointIndex;
+        loopJointIndex = joint.parentIndex;
+    }
+    return -1;
+}
+
+void JointNodeTree::collectChildren(int jointIndex, std::vector<int> &children) const
+{
+    for (const auto &child: joints()[jointIndex].children) {
+        children.push_back(child);
+    }
+    for (const auto &child: joints()[jointIndex].children) {
+        collectChildren(child, children);
+    }
+}
+
+void JointNodeTree::collectTrivialChildren(int jointIndex, std::vector<int> &children) const
+{
+    for (const auto &child: joints()[jointIndex].children) {
+        if (joints()[child].boneMark != SkeletonBoneMark::None) {
+            continue;
+        }
+        std::vector<int> subChildren;
+        bool hasNoTrivialNode = false;
+        collectTrivialChildrenStopEarlyOnNoTrivial(child, subChildren, hasNoTrivialNode);
+        if (hasNoTrivialNode)
+            continue;
+        children.push_back(child);
+        children.insert(children.end(), subChildren.begin(), subChildren.end());
+    }
+}
+
+void JointNodeTree::collectTrivialChildrenStopEarlyOnNoTrivial(int jointIndex, std::vector<int> &children, bool &hasNoTrivialNode) const
+{
+    for (const auto &child: joints()[jointIndex].children) {
+        if (joints()[child].boneMark != SkeletonBoneMark::None) {
+            hasNoTrivialNode = true;
+            return;
+        }
+        children.push_back(child);
+        collectTrivialChildrenStopEarlyOnNoTrivial(child, children, hasNoTrivialNode);
+    }
+}
+

src/jointnodetree.h

@@ -5,6 +5,7 @@
 #include <vector>
 #include "meshresultcontext.h"
 #include "skeletonbonemark.h"
+#include "rigframe.h"
 
 struct JointInfo
 {
@@ -33,6 +34,19 @@ public:
     int nodeToJointIndex(int partId, int nodeId) const;
     void recalculateMatricesAfterPositionUpdated();
     void recalculateMatricesAfterTransformUpdated();
+    const std::vector<std::pair<int, int>> &legs() const;
+    const std::vector<int> &spine() const;
+    const std::vector<std::pair<int, int>> &leftLegs() const;
+    const std::vector<std::pair<int, int>> &rightLegs() const;
+    void diff(const JointNodeTree &another, RigFrame &rigFrame);
+    int findHubJoint(int jointIndex, std::vector<int> *tracePath=nullptr) const;
+    void collectChildren(int jointIndex, std::vector<int> &children) const;
+    void collectTrivialChildren(int jointIndex, std::vector<int> &children) const;
+    int findParent(int jointIndex, int parentIndex, std::vector<int> *tracePath=nullptr) const;
+    int findSpineFromChild(int jointIndex);
+    bool isVerticalSpine = false;
+    int head = -1;
+    int tail = -1;
 private:
     void calculateMatricesByTransform();
     void calculateMatricesByPosition();
@@ -40,6 +54,16 @@ private:
     std::vector<JointInfo> m_tracedJoints;
     std::map<std::pair<int, int>, int> m_tracedNodeToJointIndexMap;
     void traceBoneFromJoint(MeshResultContext &resultContext, std::pair<int, int> node, std::set<std::pair<int, int>> &visitedNodes, std::set<std::pair<std::pair<int, int>, std::pair<int, int>>> &connections, int parentIndex);
+    void collectParts();
+    void addLeg(int legStart, int legEnd);
+    void sortLegs(std::vector<std::pair<int, int>> &legs);
+    void sortSpine(std::vector<int> &spine);
+    void collectTrivialChildrenStopEarlyOnNoTrivial(int jointIndex, std::vector<int> &children, bool &hasNoTrivialNode) const;
+private:
+    std::vector<std::pair<int, int>> m_legs;
+    std::vector<std::pair<int, int>> m_leftLegs;
+    std::vector<std::pair<int, int>> m_rightLegs;
+    std::vector<int> m_spine;
 };
 
 #endif

src/locomotioncontroller.cpp

@@ -0,0 +1,326 @@
+#include <cmath>
+#include "locomotioncontroller.h"
+#include "ikjoint.h"
+#include "dust3dutil.h"
+
+LocomotionController::LocomotionController(const JointNodeTree &jointNodeTree) :
+    m_inputJointNodeTree(jointNodeTree),
+    m_outputJointNodeTree(jointNodeTree)
+{
+}
+
+LocomotionController::~LocomotionController()
+{
+    release();
+}
+
+void LocomotionController::release()
+{
+    m_leftPogoSticks.clear();
+    m_rightPogoSticks.clear();
+}
+
+void LocomotionController::prepare()
+{
+    release();
+    
+    for (const auto &leg: m_inputJointNodeTree.leftLegs()) {
+        const auto &legStart = m_inputJointNodeTree.joints()[leg.first];
+        const auto &legEnd = m_inputJointNodeTree.joints()[leg.second];
+        
+        std::vector<int> childrenOfLegEnd;
+        m_inputJointNodeTree.collectChildren(leg.second, childrenOfLegEnd);
+        m_childrenOfLeftLegEnds.push_back(childrenOfLegEnd);
+        
+        PogoStick pogoStick;
+        pogoStick.setGroundLocation(legEnd.position.y());
+        pogoStick.setRestPelvisLocation(legStart.position.y());
+        m_leftPogoSticks.push_back(pogoStick);
+    }
+    
+    for (const auto &leg: m_inputJointNodeTree.rightLegs()) {
+        const auto &legStart = m_inputJointNodeTree.joints()[leg.first];
+        const auto &legEnd = m_inputJointNodeTree.joints()[leg.second];
+        
+        std::vector<int> childrenOfLegEnd;
+        m_inputJointNodeTree.collectChildren(leg.second, childrenOfLegEnd);
+        m_childrenOfRightLegEnds.push_back(childrenOfLegEnd);
+        
+        PogoStick pogoStick;
+        pogoStick.setGroundLocation(legEnd.position.y());
+        pogoStick.setRestPelvisLocation(legStart.position.y());
+        m_rightPogoSticks.push_back(pogoStick);
+    }
+}
+
+void LocomotionController::simulateLeg(PogoStick *pogoStick, const std::vector<int> &childrenOfLegEnd, std::pair<int, int> leg, float amount, QVector3D *footDirection, QVector3D *finalLegStartPosition, float *finalLegStartOffsetY)
+{
+    float time = amount;
+    
+    pogoStick->simulate(time);
+    
+    const auto &legStart = m_outputJointNodeTree.joints()[leg.first];
+    const auto &legEnd = m_outputJointNodeTree.joints()[leg.second];
+    
+    float targetLegStartY = legStart.position.y() + (pogoStick->currentPelvisLocation() - legStart.position.y()) * 0.2;
+    float targetLegEndY = pogoStick->currentFootLocation();
+    float targetLegStartZ = legStart.position.z() + pogoStick->currentFootHorizontalOffset() * 0.05;
+    float targetLegEndZ = legEnd.position.z() + pogoStick->currentFootHorizontalOffset();
+    
+    QVector3D targetLegStartPosition = QVector3D(legStart.position.x(), targetLegStartY, targetLegStartZ);
+    QVector3D targetLegEndPosition = QVector3D(legEnd.position.x(), targetLegEndY, targetLegEndZ);
+    QVector3D initialLegStartPosition = legStart.position;
+    QVector3D initialLegEndPosition = legEnd.position;
+    
+    std::vector<int> tracePath;
+    int legParentIndex = m_inputJointNodeTree.findHubJoint(leg.first, &tracePath);
+    if (-1 == legParentIndex) {
+        m_outputJointNodeTree.joints()[leg.first].position = targetLegStartPosition;
+    } else {
+        moveIkJoints(m_outputJointNodeTree, m_outputJointNodeTree, legParentIndex, leg.first, targetLegStartPosition);
+    }
+    
+    moveIkJoints(m_outputJointNodeTree, m_outputJointNodeTree, leg.first, leg.second, targetLegEndPosition);
+    
+    QVector3D finalLegEndTranslation = legEnd.position - initialLegEndPosition;
+
+    for (const auto &child: childrenOfLegEnd) {
+        m_outputJointNodeTree.joints()[child].position += finalLegEndTranslation;
+    }
+    
+    if (m_outputJointNodeTree.joints()[leg.first].children.size() > 0) {
+        int theNodeIndexDirectBelowLegStart = m_outputJointNodeTree.joints()[leg.first].children[0];
+        if (footDirection)
+            *footDirection = m_outputJointNodeTree.joints()[theNodeIndexDirectBelowLegStart].position - legStart.position;
+    } else {
+        if (footDirection)
+            *footDirection = legEnd.position - legStart.position;
+    }
+    
+    if (finalLegStartPosition)
+        *finalLegStartPosition = legEnd.position;
+    if (finalLegStartOffsetY)
+        *finalLegStartOffsetY = legStart.position.y() - initialLegStartPosition.y();
+}
+
+void LocomotionController::simulate(float amount)
+{
+    const auto pointerFront = QVector3D(0, 0, 1);
+    const auto pointerOutFromCanvas = QVector3D(1, 0, 0);
+    const auto pointerUp = QVector3D(0, 1, 0);
+    float offset = 0.3;
+    float delays[2] = {0};
+    m_outputJointNodeTree = m_inputJointNodeTree;
+    std::vector<QVector3D> leftPitches;
+    std::vector<QVector3D> rightPitches;
+    std::vector<QVector3D> leftLegStartPositions;
+    std::vector<QVector3D> rightLegStartPositions;
+    std::vector<float> leftLegStartOffsetYs;
+    std::vector<float> rightLegStartOffsetYs;
+    leftPitches.resize(m_leftPogoSticks.size());
+    leftLegStartPositions.resize(m_leftPogoSticks.size());
+    leftLegStartOffsetYs.resize(m_leftPogoSticks.size());
+    for (int i = 0; i < (int)m_leftPogoSticks.size(); i++) {
+        const auto &leg = m_inputJointNodeTree.leftLegs()[i];
+        auto pogoStick = &m_leftPogoSticks[i];
+        QVector3D footDirection;
+        simulateLeg(pogoStick, m_childrenOfLeftLegEnds[i], leg, amount + delays[i % 2], &footDirection, &leftLegStartPositions[i], &leftLegStartOffsetYs[i]);
+        leftPitches[i] = -QVector3D::crossProduct(-footDirection, pointerOutFromCanvas);
+        delays[i % 2] += offset;
+    }
+    delays[0] = 0.5;
+    delays[1] = 0.5;
+    rightPitches.resize(m_rightPogoSticks.size());
+    rightLegStartPositions.resize(m_rightPogoSticks.size());
+    rightLegStartOffsetYs.resize(m_rightPogoSticks.size());
+    for (int i = 0; i < (int)m_rightPogoSticks.size(); i++) {
+        const auto &leg = m_inputJointNodeTree.rightLegs()[i];
+        auto pogoStick = &m_rightPogoSticks[i];
+        QVector3D footDirection;
+        simulateLeg(pogoStick, m_childrenOfRightLegEnds[i], leg, amount + delays[i % 2], &footDirection, &rightLegStartPositions[i], &rightLegStartOffsetYs[i]);
+        rightPitches[i] = -QVector3D::crossProduct(-footDirection, pointerOutFromCanvas);
+        delays[i % 2] += offset;
+    }
+    
+    if (m_inputJointNodeTree.spine().empty())
+        return;
+    
+    // Calculate orientation for each leg pair
+    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);
+    }
+    
+    // Calculate average pitch for each leg
+    std::vector<QVector3D> spinePitches;
+    spinePitches.resize(std::max(leftPitches.size(), rightPitches.size()));
+    for (int i = 0; i < (int)m_leftPogoSticks.size(); i++) {
+        spinePitches[i] = leftPitches[i];
+    }
+    for (int i = 0; i < (int)m_rightPogoSticks.size(); i++) {
+        spinePitches[i] += rightPitches[i];
+    }
+    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]);
+        }
+    }
+    
+    // Find spine node for each leg pair
+    std::vector<int> spineNodes;
+    spineNodes.resize(spinePitches.size(), -1);
+    for (int i = 0; i < (int)spinePitches.size(); i++) {
+        int leftSpine = -1;
+        if (i < (int)m_inputJointNodeTree.leftLegs().size()) {
+            leftSpine = m_inputJointNodeTree.findSpineFromChild(m_inputJointNodeTree.leftLegs()[i].first);
+        }
+        int rightSpine = -1;
+        if (i < (int)m_inputJointNodeTree.rightLegs().size()) {
+            rightSpine = m_inputJointNodeTree.findSpineFromChild(m_inputJointNodeTree.rightLegs()[i].first);
+        }
+        if (-1 == leftSpine && -1 == rightSpine)
+            continue;
+        if (leftSpine == rightSpine) {
+            spineNodes[i] = leftSpine;
+            continue;
+        }
+        if (-1 == rightSpine) {
+            spineNodes[i] = leftSpine;
+            continue;
+        }
+        if (-1 == leftSpine) {
+            spineNodes[i] = rightSpine;
+            continue;
+        }
+        qDebug() << "leftSpine:" << leftSpine << " not equal rightSpine:" << rightSpine;
+        if (m_inputJointNodeTree.joints()[leftSpine].position.z() > m_inputJointNodeTree.joints()[rightSpine].position.z()) {
+            spineNodes[i] = leftSpine;
+            continue;
+        }
+        spineNodes[i] = rightSpine;
+    }
+    
+    // Merge leg start offsets
+    std::vector<QVector3D> legStartOffsets;
+    legStartOffsets.resize(std::max(leftLegStartOffsetYs.size(), rightLegStartOffsetYs.size()));
+    for (int i = 0; i < (int)spineNodes.size(); i++) {
+        float offsetY = 0;
+        if (i < (int)leftLegStartOffsetYs.size()) {
+            offsetY += leftLegStartOffsetYs[i];
+        }
+        if (i < (int)rightLegStartOffsetYs.size()) {
+            offsetY += rightLegStartOffsetYs[i];
+        }
+        offsetY /= 2;
+        offsetY *= 0.5;
+        legStartOffsets[i] = QVector3D(0, offsetY, 0);
+    }
+    
+    // Move spine nodes
+    for (int i = 0; i < (int)spineNodes.size(); i++) {
+        if (i < (int)legStartOffsets.size()) {
+            QVector3D offset = legStartOffsets[i];
+            m_outputJointNodeTree.joints()[spineNodes[i]].position += offset;
+            std::vector<int> children;
+            m_inputJointNodeTree.collectTrivialChildren(spineNodes[i], children);
+            for (const auto &child: children) {
+                m_outputJointNodeTree.joints()[child].position += offset;
+            }
+        }
+    }
+    
+    // Move tail
+    if (-1 != m_inputJointNodeTree.tail && spineNodes.size() > 0) {
+        int lastSpineJointIndex = spineNodes[spineNodes.size() - 1];
+        std::vector<int> spineIndicies;
+        if (lastSpineJointIndex == m_inputJointNodeTree.findParent(m_inputJointNodeTree.tail,
+                lastSpineJointIndex, &spineIndicies)) {
+            std::vector<int> tailChildren;
+            m_inputJointNodeTree.collectChildren(m_inputJointNodeTree.tail, tailChildren);
+            const auto offset = legStartOffsets[spineNodes.size() - 1];
+            for (int i = (int)spineIndicies.size() - 2; i >= 0; i--) {
+                m_outputJointNodeTree.joints()[spineIndicies[i]].position += offset;
+            }
+            m_outputJointNodeTree.joints()[m_inputJointNodeTree.tail].position += offset;
+            for (const auto &child: tailChildren) {
+                m_outputJointNodeTree.joints()[child].position += offset;
+            }
+        }
+    }
+    
+    // Adjust neck
+    if (-1 != m_inputJointNodeTree.head && spineNodes.size() > 0 && spinePitches.size() > 0) {
+        int firstSpineJointIndex = spineNodes[0];
+        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,
+                firstSpineJointIndex,
+                spinePitches[0]);
+        }
+    }
+    
+    // Adjust spine inbetween legs
+    for (int i = 1; i < (int)spineNodes.size() && i < (int)spinePitches.size(); i++) {
+        makeInbetweenNodesInHermiteCurve(spineNodes[i - 1], spinePitches[i - 1],
+            spineNodes[i], spinePitches[i]);
+    }
+}
+
+void LocomotionController::makeInbetweenNodesInHermiteCurve(int firstJointIndex, QVector3D firstPitch, int secondJointIndex, QVector3D secondPitch)
+{
+    int child = firstJointIndex;
+    int parent = secondJointIndex;
+    QVector3D childPitch = firstPitch;
+    QVector3D parentPitch = secondPitch;
+    if (child < parent) {
+        child = secondJointIndex;
+        parent = firstJointIndex;
+        childPitch = secondPitch;
+        parentPitch = firstPitch;
+    }
+    
+    std::vector<int> inbetweenIndicies;
+    if (parent != m_inputJointNodeTree.findParent(child,
+            parent, &inbetweenIndicies)) {
+        return;
+    }
+    if (inbetweenIndicies.size() - 1 <= 0)
+        return;
+    
+    // Calculate total length
+    float length = m_inputJointNodeTree.joints()[inbetweenIndicies[0]].position.distanceToPoint(m_inputJointNodeTree.joints()[child].position);
+    std::vector<float> lengths;
+    float totalLength = length;
+    lengths.push_back(length);
+    for (int i = 1; i <= (int)inbetweenIndicies.size() - 1; i++) {
+        length = m_inputJointNodeTree.joints()[inbetweenIndicies[i - 1]].position.distanceToPoint(m_inputJointNodeTree.joints()[inbetweenIndicies[i]].position);
+        lengths.push_back(length);
+        totalLength += length;
+    }
+    if (qFuzzyIsNull(totalLength)) {
+        qDebug() << "Inbetween nodes is too short";
+    } else {
+        float accumLength = 0;
+        const QVector3D &p0 = m_inputJointNodeTree.joints()[parent].position;
+        const QVector3D &m0 = parentPitch;
+        const QVector3D &p1 = m_inputJointNodeTree.joints()[child].position;
+        const QVector3D &m1 = childPitch;
+        for (int i = (int)inbetweenIndicies.size() - 2; i >= 0; i--) {
+            accumLength += lengths[i + 1];
+            float t = accumLength / totalLength;
+            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;
+        }
+    }
+}
+
+const JointNodeTree &LocomotionController::outputJointNodeTreeOnlyPositions() const
+{
+    return m_outputJointNodeTree;
+}

src/locomotioncontroller.h

@@ -0,0 +1,28 @@
+#ifndef LOCOMOTION_CONTROLLER_H
+#define LOCOMOTION_CONTROLLER_H
+#include "jointnodetree.h"
+#include "pogostick.h"
+
+class LocomotionController
+{
+public:
+    LocomotionController(const JointNodeTree &jointNodeTree);
+    ~LocomotionController();
+    void prepare();
+    void release();
+    void simulate(float amount);
+    const JointNodeTree &outputJointNodeTreeOnlyPositions() const;
+private:
+    void simulateLeg(PogoStick *pogoStick, const std::vector<int> &childrenOfLegEnd, std::pair<int, int> leg, float amount,
+        QVector3D *footDirection, QVector3D *finalLegStartPosition, float *finalLegStartOffsetY);
+    void makeInbetweenNodesInHermiteCurve(int firstJointIndex, QVector3D firstPitch, int secondJointIndex, QVector3D secondPitch);
+private:
+    JointNodeTree m_inputJointNodeTree;
+    JointNodeTree m_outputJointNodeTree;
+    std::vector<PogoStick> m_leftPogoSticks;
+    std::vector<PogoStick> m_rightPogoSticks;
+    std::vector<std::vector<int>> m_childrenOfLeftLegEnds;
+    std::vector<std::vector<int>> m_childrenOfRightLegEnds;
+};
+
+#endif

src/meshresultcontext.cpp

@@ -2,6 +2,7 @@
 #include <vector>
 #include <QDebug>
 #include <set>
+#include <cmath>
 #include "theme.h"
 #include "meshresultcontext.h"
 #include "thekla_atlas.h"
@@ -35,7 +36,9 @@ MeshResultContext::MeshResultContext() :
     m_centerBmeshNode(nullptr),
     m_resultPartsResolved(false),
     m_resultTriangleUvsResolved(false),
-    m_resultRearrangedVerticesResolved(false)
+    m_resultRearrangedVerticesResolved(false),
+    m_isVerticalSpine(false),
+    m_spineDirectionResolved(false)
 {
 }
 
@@ -95,6 +98,15 @@ const BmeshNode *MeshResultContext::centerBmeshNode()
     return m_centerBmeshNode;
 }
 
+bool MeshResultContext::isVerticalSpine()
+{
+    if (!m_spineDirectionResolved) {
+        calculateSpineDirection(m_isVerticalSpine);
+        m_spineDirectionResolved = true;
+    }
+    return m_isVerticalSpine;
+}
+
 void MeshResultContext::resolveBmeshConnectivity()
 {
     if (!m_bmeshConnectivityResolved) {
@@ -416,6 +428,34 @@ void MeshResultContext::calculateBmeshNodeMap(std::map<std::pair<int, int>, Bmes
     }
 }
 
+void MeshResultContext::calculateSpineDirection(bool &isVerticalSpine)
+{
+    float minZ = 10000;
+    float maxZ = -10000;
+    float minY = 10000;
+    float maxY = -10000;
+    int spineNodeNum = 0;
+    for (auto i = 0u; i < bmeshNodes.size(); i++) {
+        BmeshNode *bmeshNode = &bmeshNodes[i];
+        if (bmeshNode->boneMark != SkeletonBoneMark::Spine)
+            continue;
+        if (bmeshNode->origin.y() < minY)
+            minY = bmeshNode->origin.y();
+        if (bmeshNode->origin.y() > maxY)
+            maxY = bmeshNode->origin.y();
+        if (bmeshNode->origin.z() < minZ)
+            minZ = bmeshNode->origin.z();
+        if (bmeshNode->origin.z() > maxZ)
+            maxZ = bmeshNode->origin.z();
+        spineNodeNum++;
+    }
+    if (spineNodeNum < 2)
+        isVerticalSpine = true;
+    else
+        isVerticalSpine = fabs(maxY - minY) > fabs(maxZ - minZ);
+    qDebug() << "calculateSpineDirection isVerticalSpine:" << isVerticalSpine;
+}
+
 struct BmeshNodeDistWithWorldCenter
 {
     BmeshNode *bmeshNode;
@@ -424,6 +464,29 @@ struct BmeshNodeDistWithWorldCenter
 
 BmeshNode *MeshResultContext::calculateCenterBmeshNode()
 {
+    // If there are marked spine node, we pick the last spine node as center
+    bool pickVerticalLast = isVerticalSpine();
+    float minLocation = 10000;
+    BmeshNode *lastSpineNode = nullptr;
+    for (auto i = 0u; i < bmeshNodes.size(); i++) {
+        BmeshNode *bmeshNode = &bmeshNodes[i];
+        if (bmeshNode->boneMark != SkeletonBoneMark::Spine)
+            continue;
+        if (pickVerticalLast) {
+            if (bmeshNode->origin.y() < minLocation) {
+                minLocation = bmeshNode->origin.y();
+                lastSpineNode = bmeshNode;
+            }
+        } else {
+            if (bmeshNode->origin.z() < minLocation) {
+                minLocation = bmeshNode->origin.z();
+                lastSpineNode = bmeshNode;
+            }
+        }
+    }
+    if (nullptr != lastSpineNode)
+        return lastSpineNode;
+    
     // Sort all the nodes by distance with world center, excluding leg start nodes
     std::vector<BmeshNodeDistWithWorldCenter> nodesOrderByDistWithWorldCenter;
     for (auto i = 0u; i < bmeshNodes.size(); i++) {

src/meshresultcontext.h

@@ -120,6 +120,7 @@ public:
     const std::vector<ResultRearrangedTriangle> &rearrangedTriangles();
     const std::map<int, std::pair<int, int>> &vertexSourceMap();
     void removeIntermediateBones();
+    bool isVerticalSpine();
 private:
     bool m_triangleSourceResolved;
     bool m_triangleColorResolved;
@@ -134,6 +135,8 @@ private:
     bool m_resultPartsResolved;
     bool m_resultTriangleUvsResolved;
     bool m_resultRearrangedVerticesResolved;
+    bool m_isVerticalSpine;
+    bool m_spineDirectionResolved;
 private:
     std::vector<std::pair<int, int>> m_triangleSourceNodes;
     std::vector<QColor> m_triangleColors;
@@ -164,6 +167,7 @@ private:
     void calculateResultTriangleUvs(std::vector<ResultTriangleUv> &uvs, std::set<int> &seamVertices);
     void calculateResultRearrangedVertices(std::vector<ResultRearrangedVertex> &rearrangedVertices, std::vector<ResultRearrangedTriangle> &rearrangedTriangles);
     void calculateConnectionPossibleRscore(BmeshConnectionPossible &possible);
+    void calculateSpineDirection(bool &isVerticalSpine);
 };
 
 #endif

src/pogostick.cpp

@@ -0,0 +1,121 @@
+#include <cmath>
+#include <QDebug>
+#include "pogostick.h"
+
+float PogoStick::m_gravitationalAcceleration = 9.8;
+
+PogoStick::PogoStick()
+{
+    setStancePhaseDuration(0.8);
+}
+
+void PogoStick::setGroundLocation(float groundLocation)
+{
+    m_groundLocation = groundLocation;
+    needCalculateParameters = true;
+}
+
+void PogoStick::setRestPelvisLocation(float restPelvisLocation)
+{
+    m_restPelvisLocation = restPelvisLocation;
+    needCalculateParameters = true;
+}
+
+void PogoStick::setStancePhaseDuration(float duration)
+{
+    m_halfStancePhaseDuration = duration * 0.5;
+    m_halfSwingPhaseDuration = (1 - duration) * 0.5;
+    needCalculateParameters = true;
+}
+
+void PogoStick::setAcceleration(float acceleration)
+{
+    m_accelerationExcludedGravity = acceleration - m_gravitationalAcceleration;
+    needCalculateParameters = true;
+}
+
+void PogoStick::calculateParametersForSimulation()
+{
+    m_restBodyHeight = abs(m_restPelvisLocation - m_groundLocation);
+    
+    float maxAcceleration = m_restBodyHeight / (0.5 * (m_halfStancePhaseDuration * m_halfStancePhaseDuration));
+    if (m_accelerationExcludedGravity > maxAcceleration) {
+        qDebug() << "Reset acceleration from" << m_accelerationExcludedGravity << "to maxAcceleration:" << maxAcceleration;
+        m_accelerationExcludedGravity = maxAcceleration;
+    }
+    
+    m_stancePhaseHeight = calculateDisplacement(0, m_accelerationExcludedGravity, m_halfStancePhaseDuration);
+    m_velocity = m_accelerationExcludedGravity * m_halfStancePhaseDuration;
+    m_airAcceleration = m_velocity / m_halfSwingPhaseDuration;
+    m_swingPhaseHeight = calculateDisplacement(0, m_airAcceleration, m_halfSwingPhaseDuration);
+}
+
+float PogoStick::calculateDisplacement(float initialVelocity, float acceleration, float time)
+{
+    return 0.5 * acceleration * (time * time) + initialVelocity * time;
+}
+
+float PogoStick::currentPelvisLocation()
+{
+    return m_pelviosLocation;
+}
+
+float PogoStick::currentFootLocation()
+{
+    if (PogoStickPhase::Reception == m_currentPhase || PogoStickPhase::Propulsion == m_currentPhase)
+        return m_groundLocation;
+    return (m_pelviosLocation - m_restBodyHeight);
+}
+
+float PogoStick::currentFootHorizontalOffset()
+{
+    return m_footHorizontalOffset;
+}
+
+void PogoStick::simulate(float time)
+{
+    if (time > 1)
+        time -= 1;
+
+    if (needCalculateParameters) {
+        needCalculateParameters = false;
+        calculateParametersForSimulation();
+    }
+    
+    float stancePhaseDuration = m_halfStancePhaseDuration + m_halfStancePhaseDuration;
+    float swingPhaseBegin = stancePhaseDuration;
+    float swingPhaseDuration = 1 - stancePhaseDuration;
+    if (time <= stancePhaseDuration) {
+        if (time <= m_halfStancePhaseDuration) {
+            // Reception Phase (down)
+            m_currentPhase = PogoStickPhase::Reception;
+            float displacement = calculateDisplacement(m_velocity, m_accelerationExcludedGravity, time);
+            qDebug() << "Reception Phase (down) displacement:" << displacement;
+            m_pelviosLocation = m_restPelvisLocation - displacement;
+        } else {
+            // Propulsion Phase (up)
+            m_currentPhase = PogoStickPhase::Propulsion;
+            float displacement = calculateDisplacement(0, m_accelerationExcludedGravity, time - m_halfStancePhaseDuration);
+            qDebug() << "Propulsion Phase (up) displacement:" << displacement;
+            m_pelviosLocation = m_restPelvisLocation - (m_stancePhaseHeight - displacement);
+        }
+        m_footHorizontalOffset = -time * 0.25 / stancePhaseDuration;
+    } else {
+        float swingPhaseMiddle = swingPhaseBegin + (1 - swingPhaseBegin) * 0.5;
+        // Swing Phase
+        if (time < swingPhaseMiddle) {
+            // up
+            m_currentPhase = PogoStickPhase::SwingUp;
+            float displacement = calculateDisplacement(m_velocity, -m_airAcceleration, time - swingPhaseBegin);
+            qDebug() << "up displacement:" << displacement;
+            m_pelviosLocation = m_restPelvisLocation + displacement;
+        } else {
+            // down
+            m_currentPhase = PogoStickPhase::SwingDown;
+            float displacement = calculateDisplacement(0, m_airAcceleration, time - swingPhaseMiddle);
+            qDebug() << "down displacement:" << displacement;
+            m_pelviosLocation = m_restPelvisLocation + (m_swingPhaseHeight - displacement);
+        }
+        m_footHorizontalOffset = (time - swingPhaseBegin) * 0.5 / swingPhaseDuration;
+    }
+}

src/pogostick.h

@@ -0,0 +1,46 @@
+#ifndef POGO_STICK_H
+#define POGO_STICK_H
+#include <QVector3D>
+
+enum class PogoStickPhase
+{
+    Reception,
+    Propulsion,
+    SwingUp,
+    SwingDown
+};
+
+class PogoStick
+{
+public:
+    PogoStick();
+    void setGroundLocation(float groundLocation);
+    void setRestPelvisLocation(float restPelvisLocation);
+    void setStancePhaseDuration(float duration);
+    void setAcceleration(float acceleration);
+    void simulate(float time);
+    float currentPelvisLocation();
+    float currentFootLocation();
+    float currentFootHorizontalOffset();
+private:
+    void calculateParametersForSimulation();
+    float calculateDisplacement(float initialVelocity, float acceleration, float time);
+private:
+    float m_restBodyHeight = 0;
+    float m_halfStancePhaseDuration = 0;
+    float m_halfSwingPhaseDuration = 0;
+    static float m_gravitationalAcceleration;
+    float m_airAcceleration = 0;
+    float m_accelerationExcludedGravity = 20;
+    float m_stancePhaseHeight = 0;
+    float m_swingPhaseHeight = 0;
+    float m_velocity = 0;
+    float m_pelviosLocation = 0;
+    float m_groundLocation = 0;
+    float m_restPelvisLocation = 0;
+    float m_footHorizontalOffset = 0;
+    PogoStickPhase m_currentPhase;
+    bool needCalculateParameters = true;
+};
+
+#endif

src/skeletonbonemark.h

@@ -6,10 +6,12 @@
 enum class SkeletonBoneMark
 {
     None = 0,
+    Head,
     LegStart,
     LegJoint,
     LegEnd,
     Spine,
+    Tail,
     Max
 };
 #define SKELETON_BONE_MARK_TYPE_NUM    ((int)SkeletonBoneMark::Max - 1)
@@ -19,6 +21,8 @@ QColor SkeletonBoneMarkToColor(SkeletonBoneMark mark);
 QColor SkeletonBoneMarkToColor(SkeletonBoneMark mark)               \
 {                                                                   \
     switch (mark) {                                                 \
+        case SkeletonBoneMark::Head:                                \
+            return QColor(0x00, 0x00, 0xff);                        \
         case SkeletonBoneMark::LegStart:                            \
             return QColor(0xf4, 0xcd, 0x56);                        \
         case SkeletonBoneMark::LegJoint:                            \
@@ -27,6 +31,8 @@ QColor SkeletonBoneMarkToColor(SkeletonBoneMark mark)               \
             return QColor(0xd0, 0x8c, 0xe0);                        \
         case SkeletonBoneMark::Spine:                               \
             return QColor(0xfc, 0x63, 0x60);                        \
+        case SkeletonBoneMark::Tail:                                \
+            return QColor(0x00, 0xff, 0x00);                        \
         case SkeletonBoneMark::None:                                \
             return Qt::transparent;                                 \
         default:                                                    \
@@ -38,6 +44,8 @@ const char *SkeletonBoneMarkToString(SkeletonBoneMark mark);
 const char *SkeletonBoneMarkToString(SkeletonBoneMark mark)         \
 {                                                                   \
     switch (mark) {                                                 \
+        case SkeletonBoneMark::Head:                                \
+            return "Head";                                          \
         case SkeletonBoneMark::LegStart:                            \
             return "LegStart";                                      \
         case SkeletonBoneMark::LegJoint:                            \
@@ -46,6 +54,8 @@ const char *SkeletonBoneMarkToString(SkeletonBoneMark mark)         \
             return "LegEnd";                                        \
         case SkeletonBoneMark::Spine:                               \
             return "Spine";                                         \
+        case SkeletonBoneMark::Tail:                                \
+            return "Tail";                                          \
         case SkeletonBoneMark::None:                                \
             return "None";                                          \
         default:                                                    \
@@ -57,6 +67,8 @@ SkeletonBoneMark SkeletonBoneMarkFromString(const char *markString);
 SkeletonBoneMark SkeletonBoneMarkFromString(const char *markString) \
 {                                                                   \
     QString mark = markString;                                      \
+    if (mark == "Head")                                             \
+        return SkeletonBoneMark::Head;                              \
     if (mark == "LegStart")                                         \
         return SkeletonBoneMark::LegStart;                          \
     if (mark == "LegJoint")                                         \
@@ -65,6 +77,8 @@ SkeletonBoneMark SkeletonBoneMarkFromString(const char *markString) \
         return SkeletonBoneMark::LegEnd;                            \
     if (mark == "Spine")                                            \
         return SkeletonBoneMark::Spine;                             \
+    if (mark == "Tail")                                             \
+        return SkeletonBoneMark::Tail;                              \
     if (mark == "None")                                             \
         return SkeletonBoneMark::None;                              \
     return SkeletonBoneMark::None;                                  \
@@ -74,6 +88,8 @@ const char *SkeletonBoneMarkToDispName(SkeletonBoneMark mark);
 const char *SkeletonBoneMarkToDispName(SkeletonBoneMark mark)       \
 {                                                                   \
     switch (mark) {                                                 \
+        case SkeletonBoneMark::Head:                                \
+            return "Head";                                          \
         case SkeletonBoneMark::LegStart:                            \
             return "Leg (Start)";                                   \
         case SkeletonBoneMark::LegJoint:                            \
@@ -82,6 +98,8 @@ const char *SkeletonBoneMarkToDispName(SkeletonBoneMark mark)       \
             return "Leg (End)";                                     \
         case SkeletonBoneMark::Spine:                               \
             return "Spine";                                         \
+        case SkeletonBoneMark::Tail:                                \
+            return "Tail";                                          \
         case SkeletonBoneMark::None:                                \
             return "";                                              \
         default:                                                    \

src/skeletondocumentwindow.cpp

@@ -905,6 +905,8 @@ void SkeletonDocumentWindow::exportModelResult()
         return;
     }
     QApplication::setOverrideCursor(Qt::WaitCursor);
+    // Must update mesh before export, because there may be an animation clip playing which breaks the original mesh.
+    m_modelRenderWidget->updateMesh(m_document->takeResultMesh());
     m_modelRenderWidget->exportMeshAsObj(filename);
     QApplication::restoreOverrideCursor();
 }