From 5001453ce3d3eac39ddaad4189cb89eeee739d12 Mon Sep 17 00:00:00 2001 From: Jeremy Hu Date: Sat, 30 Jun 2018 18:46:23 +0800 Subject: [PATCH] Add joint constraint --- dust3d.pro | 3 ++ src/animationclipgenerator.cpp | 6 ++-- src/ccdikresolver.cpp | 39 +++++++++++++++++++- src/ccdikresolver.h | 5 ++- src/dust3dutil.cpp | 16 +++++++++ src/dust3dutil.h | 2 ++ src/ikjoint.cpp | 10 ++++-- src/ikjoint.h | 3 +- src/jointconstraint.cpp | 33 +++++++++++++++++ src/jointconstraint.h | 29 +++++++++++++++ src/jointnodetree.cpp | 20 +++++++++++ src/jointnodetree.h | 4 +++ src/locomotioncontroller.cpp | 66 ++++++++++++++++++++++++++++------ src/locomotioncontroller.h | 3 +- src/pogostick.cpp | 2 +- 15 files changed, 221 insertions(+), 20 deletions(-) create mode 100644 src/jointconstraint.cpp create mode 100644 src/jointconstraint.h diff --git a/dust3d.pro b/dust3d.pro index ad6ced1c..6b82aa68 100644 --- a/dust3d.pro +++ b/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 diff --git a/src/animationclipgenerator.cpp b/src/animationclipgenerator.cpp index ec310e56..d5fb1ef5 100644 --- a/src/animationclipgenerator.cpp +++ b/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); - nextBeginTime += duration; + generateFrame(skinnedMesh, amount, nextBeginTime, duration * 1.5); + nextBeginTime += duration * 1.5; } } else if (m_clipName == "Die") { #if USE_BULLET diff --git a/src/ccdikresolver.cpp b/src/ccdikresolver.cpp index 82ee5d63..f8e365e2 100644 --- a/src/ccdikresolver.cpp +++ b/src/ccdikresolver.cpp @@ -3,6 +3,7 @@ #include #include #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]; diff --git a/src/ccdikresolver.h b/src/ccdikresolver.h index 0485aafb..99396e07 100644 --- a/src/ccdikresolver.h +++ b/src/ccdikresolver.h @@ -3,10 +3,13 @@ #include #include #include +#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); diff --git a/src/dust3dutil.cpp b/src/dust3dutil.cpp index 05051c84..f76ee467 100644 --- a/src/dust3dutil.cpp +++ b/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; +} + diff --git a/src/dust3dutil.h b/src/dust3dutil.h index bbec7009..1920c13f 100644 --- a/src/dust3dutil.h +++ b/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 diff --git a/src/ikjoint.cpp b/src/ikjoint.cpp index 3ac15a60..71fdc967 100644 --- a/src/ikjoint.cpp +++ b/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 *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); diff --git a/src/ikjoint.h b/src/ikjoint.h index 585f3b88..84794a41 100644 --- a/src/ikjoint.h +++ b/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 *constrants=nullptr); #endif diff --git a/src/jointconstraint.cpp b/src/jointconstraint.cpp new file mode 100644 index 00000000..af38abd9 --- /dev/null +++ b/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; +} diff --git a/src/jointconstraint.h b/src/jointconstraint.h new file mode 100644 index 00000000..f6a55caa --- /dev/null +++ b/src/jointconstraint.h @@ -0,0 +1,29 @@ +#ifndef JOINT_CONSTRAINT_H +#define JOINT_CONSTRAINT_H +#include + +// 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 diff --git a/src/jointnodetree.cpp b/src/jointnodetree.cpp index d4dd5b55..c24ad765 100644 --- a/src/jointnodetree.cpp +++ b/src/jointnodetree.cpp @@ -59,7 +59,10 @@ const std::vector> &JointNodeTree::rightLegs() const void JointNodeTree::collectParts() { m_legs.clear(); + m_leftLegs.clear(); + m_rightLegs.clear(); m_spine.clear(); + std::map> 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> &JointNodeTree::leftLegJoints() const +{ + return m_leftLegJoints; +} + +const std::vector> &JointNodeTree::rightLegJoints() const +{ + return m_rightLegJoints; +} + void JointNodeTree::sortLegs(std::vector> &legs) { const auto &that = this; diff --git a/src/jointnodetree.h b/src/jointnodetree.h index e3f84e81..4643e700 100644 --- a/src/jointnodetree.h +++ b/src/jointnodetree.h @@ -38,6 +38,8 @@ public: const std::vector &spine() const; const std::vector> &leftLegs() const; const std::vector> &rightLegs() const; + const std::vector> &leftLegJoints() const; + const std::vector> &rightLegJoints() const; void diff(const JointNodeTree &another, RigFrame &rigFrame); int findHubJoint(int jointIndex, std::vector *tracePath=nullptr) const; void collectChildren(int jointIndex, std::vector &children) const; @@ -63,6 +65,8 @@ private: std::vector> m_legs; std::vector> m_leftLegs; std::vector> m_rightLegs; + std::vector> m_leftLegJoints; + std::vector> m_rightLegJoints; std::vector m_spine; }; diff --git a/src/locomotioncontroller.cpp b/src/locomotioncontroller.cpp index a3efa77a..0b2032d2 100644 --- a/src/locomotioncontroller.cpp +++ b/src/locomotioncontroller.cpp @@ -53,7 +53,7 @@ void LocomotionController::prepare() } } -void LocomotionController::simulateLeg(PogoStick *pogoStick, const std::vector &childrenOfLegEnd, std::pair leg, float amount, QVector3D *footDirection, QVector3D *finalLegStartPosition, float *finalLegStartOffsetY) +void LocomotionController::simulateLeg(PogoStick *pogoStick, const std::vector &childrenOfLegEnd, std::pair leg, std::map *constrants, float amount, QVector3D *footDirection, QVector3D *finalLegStartPosition, float *finalLegStartOffsetY) { float time = amount; @@ -62,7 +62,7 @@ void LocomotionController::simulateLeg(PogoStick *pogoStick, const std::vectorcurrentPelvisLocation() - 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 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 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[1] = 0.5; + delays[0] = offsets[0] - 0.1; + 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 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()) diff --git a/src/locomotioncontroller.h b/src/locomotioncontroller.h index 35bc433f..23c2a457 100644 --- a/src/locomotioncontroller.h +++ b/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 &childrenOfLegEnd, std::pair leg, float amount, + void simulateLeg(PogoStick *pogoStick, const std::vector &childrenOfLegEnd, std::pair leg, std::map *constrants, float amount, QVector3D *footDirection, QVector3D *finalLegStartPosition, float *finalLegStartOffsetY); void makeInbetweenNodesInHermiteCurve(int firstJointIndex, QVector3D firstPitch, int secondJointIndex, QVector3D secondPitch); private: diff --git a/src/pogostick.cpp b/src/pogostick.cpp index e06f0880..369646d2 100644 --- a/src/pogostick.cpp +++ b/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)