Add joint constraint

dust3d.pro

@@ -164,6 +164,9 @@ HEADERS += src/ikjoint.h
 SOURCES += src/locomotioncontroller.cpp
 HEADERS += src/locomotioncontroller.h
 
+SOURCES += src/jointconstraint.cpp
+HEADERS += src/jointconstraint.h
+
 HEADERS += src/qtlightmapper.h
 
 SOURCES += src/main.cpp

src/animationclipgenerator.cpp

@@ -114,11 +114,11 @@ void AnimationClipGenerator::generate()
         m_locomotionController = new LocomotionController(m_jointNodeTree);
         m_locomotionController->prepare();
         
-        float duration = 0.05;
+        float duration = 0.025;
         float nextBeginTime = 0;
         for (float amount = 0.0; amount <= 1; amount += duration) {
-            generateFrame(skinnedMesh, amount, nextBeginTime, duration);
+            generateFrame(skinnedMesh, amount, nextBeginTime, duration * 1.5);
-            nextBeginTime += duration;
+            nextBeginTime += duration * 1.5;
         }
     } else if (m_clipName == "Die") {
 #if USE_BULLET

src/ccdikresolver.cpp

@@ -3,6 +3,7 @@
 #include <QDebug>
 #include <cmath>
 #include "ccdikresolver.h"
+#include "dust3dutil.h"
 
 CCDIKSolver::CCDIKSolver() :
     m_maxRound(4),
@@ -21,10 +22,14 @@ void CCDIKSolver::setDistanceThreshod(float threshold)
     m_distanceThreshold2 = threshold * threshold;
 }
 
-int CCDIKSolver::addNodeInOrder(const QVector3D &position)
+int CCDIKSolver::addNodeInOrder(const QVector3D &position, const JointConstraint *constraint)
 {
     CCDIKNode node;
     node.position = position;
+    if (nullptr != constraint) {
+        node.constraint = *constraint;
+        node.hasConstraint = true;
+    }
     int nodeCount = m_nodes.size();
     m_nodes.push_back(node);
     return nodeCount;
@@ -66,6 +71,38 @@ void CCDIKSolver::iterate()
         const auto &endEffector = m_nodes[m_nodes.size() - 1];
         QVector3D from = (endEffector.position - origin.position).normalized();
         QVector3D to = (m_destination - origin.position).normalized();
+        if (origin.hasConstraint) {
+            if (origin.constraint.type() == JointConstraintType::Hinge) {
+                const auto &next = m_nodes[i + 1];
+                
+                const auto originPointerToChild = projectLineOnPlane(next.position - origin.position, origin.constraint.hingePlaneNormal()).normalized();
+                const auto originPointerToParent = i - 1 >= 0 ? projectLineOnPlane(m_nodes[i - 1].position - origin.position, origin.constraint.hingePlaneNormal()).normalized() : QVector3D(0, 1, 0);
+
+                const auto originPointerToDestination = projectLineOnPlane(m_destination - origin.position, origin.constraint.hingePlaneNormal()).normalized();
+                const auto originPointerToEndEffector = projectLineOnPlane(endEffector.position - origin.position, origin.constraint.hingePlaneNormal()).normalized();
+                
+                const auto originPointerToRotatedDestination = QQuaternion::rotationTo(originPointerToEndEffector, originPointerToChild).rotatedVector(originPointerToDestination);
+                
+                float degrees = angleInRangle360BetweenTwoVectors(originPointerToParent, originPointerToRotatedDestination, origin.constraint.hingePlaneNormal());
+                float limitToDegrees = degrees;
+                bool needLimit = false;
+                if (degrees > origin.constraint.maxHingeDegrees()) {
+                    qDebug() << "Limit degrees:" << degrees << "to max:" << origin.constraint.maxHingeDegrees();
+                    limitToDegrees = origin.constraint.maxHingeDegrees();
+                    needLimit = true;
+                } else if (degrees < origin.constraint.minHingeDegrees()) {
+                    qDebug() << "Limit degrees:" << degrees << "to min:" << origin.constraint.minHingeDegrees();
+                    limitToDegrees = origin.constraint.minHingeDegrees();
+                    needLimit = true;
+                }
+                if (needLimit) {
+                    QQuaternion rotation = QQuaternion::fromAxisAndAngle(origin.constraint.hingePlaneNormal(), limitToDegrees);
+                    auto revisedRotatedDestDirect = rotation.rotatedVector(originPointerToParent);
+                    from = originPointerToEndEffector;
+                    to = QQuaternion::rotationTo(originPointerToChild, originPointerToEndEffector).rotatedVector(revisedRotatedDestDirect);
+                }
+            }
+        }
         auto quaternion = QQuaternion::rotationTo(from, to);
         for (size_t j = i + 1; j <= m_nodes.size() - 1; j++) {
             auto &next = m_nodes[j];

src/ccdikresolver.h

@@ -3,10 +3,13 @@
 #include <vector>
 #include <QVector3D>
 #include <QQuaternion>
+#include "jointconstraint.h"
 
 struct CCDIKNode
 {
     QVector3D position;
+    JointConstraint constraint;
+    bool hasConstraint = false;
 };
 
 class CCDIKSolver
@@ -15,7 +18,7 @@ public:
     CCDIKSolver();
     void setMaxRound(int maxRound);
     void setDistanceThreshod(float threshold);
-    int addNodeInOrder(const QVector3D &position);
+    int addNodeInOrder(const QVector3D &position, const JointConstraint *constraint=nullptr);
     void solveTo(const QVector3D &position);
     const QVector3D &getNodeSolvedPosition(int index);
     int getNodeCount(void);

src/dust3dutil.cpp

@@ -35,3 +35,19 @@ QVector3D pointInHermiteCurve(float t, QVector3D p0, QVector3D m0, QVector3D p1,
         + (-2.0f * t * t * t + 3.0f * t * t) * p1
         + (t * t * t - t * t) * m1;
 }
+
+float angleInRangle360BetweenTwoVectors(QVector3D a, QVector3D b, QVector3D planeNormal)
+{
+    float degrees = acos(QVector3D::dotProduct(a, b)) * 180.0 / M_PI;
+    QVector3D direct = QVector3D::crossProduct(a, b);
+    if (QVector3D::dotProduct(direct, planeNormal) < 0)
+        return 180 + degrees;
+    return degrees;
+}
+
+QVector3D projectLineOnPlane(QVector3D line, QVector3D planeNormal)
+{
+    const auto verticalOffset = QVector3D::dotProduct(line, planeNormal) * planeNormal;
+    return line - verticalOffset;
+}
+

src/dust3dutil.h

@@ -15,5 +15,7 @@ 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);
+float angleInRangle360BetweenTwoVectors(QVector3D a, QVector3D b, QVector3D planeNormal);
+QVector3D projectLineOnPlane(QVector3D line, QVector3D planeNormal);
 
 #endif

src/ikjoint.cpp

@@ -2,7 +2,7 @@
 #include "ccdikresolver.h"
 
 void moveIkJoints(const JointNodeTree &inputJointNodeTree, JointNodeTree &outputJointNodeTree,
-    int startJointIndex, int endJointIndex, QVector3D destination)
+    int startJointIndex, int endJointIndex, QVector3D destination, std::map<int, JointConstraint> *constrants)
 {
     CCDIKSolver ikSolver;
     ikSolver.setMaxRound(10);
@@ -18,7 +18,13 @@ void moveIkJoints(const JointNodeTree &inputJointNodeTree, JointNodeTree &output
     }
     std::reverse(std::begin(ikSolvingIndicies), std::end(ikSolvingIndicies));
     for (const auto &jointIndex: ikSolvingIndicies) {
-        ikSolver.addNodeInOrder(inputJointNodeTree.joints()[jointIndex].position);
+        const JointConstraint *constraint = nullptr;
+        if (nullptr != constrants) {
+            const auto &findResult = constrants->find(jointIndex);
+            if (findResult != constrants->end())
+                constraint = &findResult->second;
+        }
+        ikSolver.addNodeInOrder(inputJointNodeTree.joints()[jointIndex].position, constraint);
     }
     
     ikSolver.solveTo(destination);

src/ikjoint.h

@@ -1,8 +1,9 @@
 #ifndef IK_JOINT_H
 #define IK_JOINT_H
 #include "jointnodetree.h"
+#include "jointconstraint.h"
 
 void moveIkJoints(const JointNodeTree &inputJointNodeTree, JointNodeTree &outputJointNodeTree,
-    int startJointIndex, int endJointIndex, QVector3D destination);
+    int startJointIndex, int endJointIndex, QVector3D destination, std::map<int, JointConstraint> *constrants=nullptr);
 
 #endif

src/jointconstraint.cpp

@@ -0,0 +1,33 @@
+#include "jointconstraint.h"
+
+JointConstraint::JointConstraint(JointConstraintType type) :
+    m_type(type)
+{
+}
+
+void JointConstraint::setHingeLimit(float minDegrees, float maxDegrees, const QVector3D &planeNormal)
+{
+    m_minHingeDegrees = minDegrees;
+    m_maxHingeDegrees = maxDegrees;
+    m_hingePlaneNormal = planeNormal;
+}
+
+JointConstraintType JointConstraint::type() const
+{
+    return m_type;
+}
+
+float JointConstraint::minHingeDegrees() const
+{
+    return m_minHingeDegrees;
+}
+
+float JointConstraint::maxHingeDegrees() const
+{
+    return m_maxHingeDegrees;
+}
+
+const QVector3D &JointConstraint::hingePlaneNormal() const
+{
+    return m_hingePlaneNormal;
+}

src/jointconstraint.h

@@ -0,0 +1,29 @@
+#ifndef JOINT_CONSTRAINT_H
+#define JOINT_CONSTRAINT_H
+#include <QVector3D>
+
+// http://bulletphysics.org/mediawiki-1.5.8/index.php/Constraints
+
+enum class JointConstraintType
+{
+    Hinge,
+    ConeTwist
+};
+
+class JointConstraint
+{
+public:
+    JointConstraint(JointConstraintType type=JointConstraintType::Hinge);
+    void setHingeLimit(float minDegrees, float maxDegrees, const QVector3D &planeNormal);
+    JointConstraintType type() const;
+    float minHingeDegrees() const;
+    float maxHingeDegrees() const;
+    const const QVector3D &hingePlaneNormal() const;
+private:
+    JointConstraintType m_type;
+    float m_minHingeDegrees = 0;
+    float m_maxHingeDegrees = 0;
+    QVector3D m_hingePlaneNormal;
+};
+
+#endif

src/jointnodetree.cpp

@@ -59,7 +59,10 @@ const std::vector<std::pair<int, int>> &JointNodeTree::rightLegs() const
 void JointNodeTree::collectParts()
 {
     m_legs.clear();
+    m_leftLegs.clear();
+    m_rightLegs.clear();
     m_spine.clear();
+    std::map<int, std::vector<int>> legJointsMap;
     for (const auto &node: joints()) {
         if (node.boneMark == SkeletonBoneMark::Spine) {
             m_spine.push_back(node.jointIndex);
@@ -81,6 +84,7 @@ void JointNodeTree::collectParts()
             const JointInfo *loopNode = &joints()[node.children[0]];
             while (loopNode->boneMark != SkeletonBoneMark::LegEnd &&
                     loopNode->children.size() == 1) {
+                legJointsMap[legStart].push_back(loopNode->jointIndex);
                 loopNode = &joints()[loopNode->children[0]];
             }
             if (loopNode->boneMark == SkeletonBoneMark::LegEnd) {
@@ -93,9 +97,25 @@ void JointNodeTree::collectParts()
     }
     sortLegs(m_leftLegs);
     sortLegs(m_rightLegs);
+    for (const auto &leg: m_leftLegs) {
+        m_leftLegJoints.push_back(legJointsMap[leg.first]);
+    }
+    for (const auto &leg: m_rightLegs) {
+        m_rightLegJoints.push_back(legJointsMap[leg.first]);
+    }
     sortSpine(m_spine);
 }
 
+const std::vector<std::vector<int>> &JointNodeTree::leftLegJoints() const
+{
+    return m_leftLegJoints;
+}
+
+const std::vector<std::vector<int>> &JointNodeTree::rightLegJoints() const
+{
+    return m_rightLegJoints;
+}
+
 void JointNodeTree::sortLegs(std::vector<std::pair<int, int>> &legs)
 {
     const auto &that = this;

src/jointnodetree.h

@@ -38,6 +38,8 @@ public:
     const std::vector<int> &spine() const;
     const std::vector<std::pair<int, int>> &leftLegs() const;
     const std::vector<std::pair<int, int>> &rightLegs() const;
+    const std::vector<std::vector<int>> &leftLegJoints() const;
+    const std::vector<std::vector<int>> &rightLegJoints() 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;
@@ -63,6 +65,8 @@ 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<std::vector<int>> m_leftLegJoints;
+    std::vector<std::vector<int>> m_rightLegJoints;
     std::vector<int> m_spine;
 };
 

src/locomotioncontroller.cpp

@@ -53,7 +53,7 @@ void LocomotionController::prepare()
     }
 }
 
-void LocomotionController::simulateLeg(PogoStick *pogoStick, const std::vector<int> &childrenOfLegEnd, std::pair<int, int> leg, float amount, QVector3D *footDirection, QVector3D *finalLegStartPosition, float *finalLegStartOffsetY)
+void LocomotionController::simulateLeg(PogoStick *pogoStick, const std::vector<int> &childrenOfLegEnd, std::pair<int, int> leg, std::map<int, JointConstraint> *constrants, float amount, QVector3D *footDirection, QVector3D *finalLegStartPosition, float *finalLegStartOffsetY)
 {
     float time = amount;
     
@@ -62,7 +62,7 @@ void LocomotionController::simulateLeg(PogoStick *pogoStick, const std::vector<i
     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 targetLegStartY = legStart.position.y() + (pogoStick->currentPelvisLocation() - legStart.position.y()) * 0.5;
     float targetLegEndY = pogoStick->currentFootLocation();
     float targetLegStartZ = legStart.position.z() + pogoStick->currentFootHorizontalOffset() * 0.05;
     float targetLegEndZ = legEnd.position.z() + pogoStick->currentFootHorizontalOffset();
@@ -80,7 +80,7 @@ void LocomotionController::simulateLeg(PogoStick *pogoStick, const std::vector<i
         moveIkJoints(m_outputJointNodeTree, m_outputJointNodeTree, legParentIndex, leg.first, targetLegStartPosition);
     }
     
-    moveIkJoints(m_outputJointNodeTree, m_outputJointNodeTree, leg.first, leg.second, targetLegEndPosition);
+    moveIkJoints(m_outputJointNodeTree, m_outputJointNodeTree, leg.first, leg.second, targetLegEndPosition, constrants);
     
     QVector3D finalLegEndTranslation = legEnd.position - initialLegEndPosition;
 
@@ -108,7 +108,7 @@ 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 offsets[2] = {0.5, 0.48};
     float delays[2] = {0};
     m_outputJointNodeTree = m_inputJointNodeTree;
     std::vector<QVector3D> leftPitches;
@@ -124,12 +124,36 @@ void LocomotionController::simulate(float amount)
         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]);
+        std::map<int, JointConstraint> constrants;
+        /*
+        if (2 == m_inputJointNodeTree.leftLegJoints()[i].size()) {
+            int firstJointIndex = m_inputJointNodeTree.leftLegJoints()[i][0];
+            int secondJointIndex = m_inputJointNodeTree.leftLegJoints()[i][1];
+            JointConstraint legStartConstraint;
+            JointConstraint firstConstraint;
+            JointConstraint secondConstraint;
+            const auto pointerOutFromCanvas = QVector3D(1, 0, 0);
+            if (0 == i) {
+                legStartConstraint.setHingeLimit(195, 195, pointerOutFromCanvas);
+                firstConstraint.setHingeLimit(105, 210, pointerOutFromCanvas);
+                secondConstraint.setHingeLimit(165, 225, pointerOutFromCanvas);
+            } else {
+                legStartConstraint.setHingeLimit(185, 185, pointerOutFromCanvas);
+                firstConstraint.setHingeLimit(120, 240, pointerOutFromCanvas);
+                secondConstraint.setHingeLimit(145, 150, pointerOutFromCanvas);
+            }
+            constrants[leg.first] = legStartConstraint;
+            constrants[firstJointIndex] = firstConstraint;
+            constrants[secondJointIndex] = secondConstraint;
+        }*/
+        simulateLeg(pogoStick, m_childrenOfLeftLegEnds[i], leg, &constrants, amount + delays[i % 2], &footDirection, &leftLegStartPositions[i], &leftLegStartOffsetYs[i]);
         leftPitches[i] = -QVector3D::crossProduct(-footDirection, pointerOutFromCanvas);
-        delays[i % 2] += offset;
+        delays[i % 2] += offsets[i % 2];
     }
-    delays[0] = 0.5;
+    delays[0] = offsets[0] - 0.1;
-    delays[1] = 0.5;
+    delays[1] = offsets[1] - 0.1;
+    //delays[0] = 0;
+    //delays[1] = 0;
     rightPitches.resize(m_rightPogoSticks.size());
     rightLegStartPositions.resize(m_rightPogoSticks.size());
     rightLegStartOffsetYs.resize(m_rightPogoSticks.size());
@@ -137,9 +161,31 @@ void LocomotionController::simulate(float amount)
         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]);
+        std::map<int, JointConstraint> constrants;
+        /*
+        if (2 == m_inputJointNodeTree.rightLegJoints()[i].size()) {
+            int firstJointIndex = m_inputJointNodeTree.rightLegJoints()[i][0];
+            int secondJointIndex = m_inputJointNodeTree.rightLegJoints()[i][1];
+            JointConstraint legStartConstraint;
+            JointConstraint firstConstraint;
+            JointConstraint secondConstraint;
+            const auto pointerOutFromCanvas = QVector3D(1, 0, 0);
+            if (0 == i) {
+                legStartConstraint.setHingeLimit(195, 195, pointerOutFromCanvas);
+                firstConstraint.setHingeLimit(105, 210, pointerOutFromCanvas);
+                secondConstraint.setHingeLimit(165, 225, pointerOutFromCanvas);
+            } else {
+                legStartConstraint.setHingeLimit(185, 185, pointerOutFromCanvas);
+                firstConstraint.setHingeLimit(120, 240, pointerOutFromCanvas);
+                secondConstraint.setHingeLimit(145, 150, pointerOutFromCanvas);
+            }
+            constrants[leg.first] = legStartConstraint;
+            constrants[firstJointIndex] = firstConstraint;
+            constrants[secondJointIndex] = secondConstraint;
+        }*/
+        simulateLeg(pogoStick, m_childrenOfRightLegEnds[i], leg, &constrants, amount + delays[i % 2], &footDirection, &rightLegStartPositions[i], &rightLegStartOffsetYs[i]);
         rightPitches[i] = -QVector3D::crossProduct(-footDirection, pointerOutFromCanvas);
-        delays[i % 2] += offset;
+        delays[i % 2] += offsets[i % 2];
     }
     
     if (m_inputJointNodeTree.spine().empty())

src/locomotioncontroller.h

@@ -2,6 +2,7 @@
 #define LOCOMOTION_CONTROLLER_H
 #include "jointnodetree.h"
 #include "pogostick.h"
+#include "jointconstraint.h"
 
 class LocomotionController
 {
@@ -13,7 +14,7 @@ public:
     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,
+    void simulateLeg(PogoStick *pogoStick, const std::vector<int> &childrenOfLegEnd, std::pair<int, int> leg, std::map<int, JointConstraint> *constrants, float amount,
         QVector3D *footDirection, QVector3D *finalLegStartPosition, float *finalLegStartOffsetY);
     void makeInbetweenNodesInHermiteCurve(int firstJointIndex, QVector3D firstPitch, int secondJointIndex, QVector3D secondPitch);
 private:

src/pogostick.cpp

@@ -6,7 +6,7 @@ float PogoStick::m_gravitationalAcceleration = 9.8;
 
 PogoStick::PogoStick()
 {
-    setStancePhaseDuration(0.95);
+    setStancePhaseDuration(0.75);
 }
 
 void PogoStick::setGroundLocation(float groundLocation)