Merge pull request #604 from overte-org/fix/flow

Fix issues with flow bones

libraries/animation/src/Flow.cpp

@@ -320,7 +320,8 @@ void FlowJoint::toHelperJoint(const glm::vec3& initialPosition, float length) {
     _length = length;
 }
 
-FlowThread::FlowThread(int rootIndex, std::map<int, FlowJoint>* joints) {
+FlowThread::FlowThread(int rootIndex, std::map<int, FlowJoint>* joints, float rigScale) {
+    _rigScale = rigScale;
     _jointsPointer = joints;
     computeFlowThread(rootIndex);
 }
@@ -362,10 +363,10 @@ void FlowThread::computeRecovery() {
     int parentIndex = _joints[0];
     auto parentJoint = _jointsPointer->at(parentIndex);
     _jointsPointer->at(parentIndex)._recoveryPosition = parentJoint._recoveryPosition = parentJoint._currentPosition;
-    glm::quat parentRotation = parentJoint._parentWorldRotation * parentJoint._initialRotation;
     for (size_t i = 1; i < _joints.size(); i++) {
+        glm::quat parentRotation = parentJoint._parentWorldRotation * parentJoint._initialRotation;
         auto joint = _jointsPointer->at(_joints[i]);
-        _jointsPointer->at(_joints[i])._recoveryPosition = joint._recoveryPosition = parentJoint._recoveryPosition + (parentRotation * (joint._initialTranslation * 0.01f));
+        _jointsPointer->at(_joints[i])._recoveryPosition = joint._recoveryPosition = parentJoint._recoveryPosition + (parentRotation * (joint._initialTranslation * _rigScale));
         parentJoint = joint;
     }
 };
@@ -405,7 +406,7 @@ void FlowThread::computeJointRotations() {
     auto joint0 = _jointsPointer->at(_joints[0]);
     auto joint1 = _jointsPointer->at(_joints[1]);
 
-    auto initial_pos1 = pos0 + (joint0._initialRotation * (joint1._initialTranslation * 0.01f));
+    auto initial_pos1 = pos0 + (joint0._initialRotation * (joint1._initialTranslation * _rigScale));
 
     auto vec0 = initial_pos1 - pos0;
     auto vec1 = pos1 - pos0;
@@ -417,11 +418,11 @@ void FlowThread::computeJointRotations() {
     for (size_t i = 1; i < _joints.size() - 1; i++) {
         auto nextJoint = _jointsPointer->at(_joints[i + 1]);
         for (size_t j = i; j < _joints.size(); j++) {
-            _rootFramePositions[j] = glm::inverse(joint0._currentRotation) * _rootFramePositions[j] - (joint0._initialTranslation * 0.01f);
+            _rootFramePositions[j] = glm::inverse(joint0._currentRotation) * _rootFramePositions[j] - (joint0._initialTranslation * _rigScale);
         }
         pos0 = _rootFramePositions[i];
         pos1 = _rootFramePositions[i + 1];
-        initial_pos1 = pos0 + joint1._initialRotation * (nextJoint._initialTranslation * 0.01f);
+        initial_pos1 = pos0 + joint1._initialRotation * (nextJoint._initialTranslation * _rigScale);
 
         vec0 = initial_pos1 - pos0;
         vec1 = pos1 - pos0;
@@ -444,6 +445,7 @@ void FlowThread::setScale(float scale, bool initScale) {
 }
 
 FlowThread& FlowThread::operator=(const FlowThread& otherFlowThread) {
+    _rigScale = otherFlowThread._rigScale;
     for (int jointIndex: otherFlowThread._joints) {
         auto& joint = otherFlowThread._jointsPointer->at(jointIndex);
         auto& myJoint = _jointsPointer->at(jointIndex);
@@ -554,8 +556,9 @@ void Flow::calculateConstraints(const std::shared_ptr<AnimSkeleton>& skeleton,
         }
     }
     int extraIndex = -1;
+    qDebug() << "GetScaleFactorGeometryToUnscaledRig() " << _rig->GetScaleFactorGeometryToUnscaledRig();
     for (size_t i = 0; i < roots.size(); i++) {
-        FlowThread thread = FlowThread(roots[i], &_flowJointData);
+        FlowThread thread = FlowThread(roots[i], &_flowJointData, _rig->GetScaleFactorGeometryToUnscaledRig());
         // add threads with at least 2 joints
         if (thread._joints.size() > 0) {
             if (thread._joints.size() == 1) {
@@ -570,7 +573,7 @@ void Flow::calculateConstraints(const std::shared_ptr<AnimSkeleton>& skeleton,
                 glm::vec3 translation = glm::vec3(0.0f, HELPER_JOINT_LENGTH, 0.0f);
                 newJoint.setInitialData(jointPosition + translation, 100.0f * translation , Quaternions::IDENTITY, jointPosition);
                 _flowJointData.insert(std::pair<int, FlowJoint>(extraIndex, newJoint));
-                FlowThread newThread = FlowThread(jointIndex, &_flowJointData);
+                FlowThread newThread = FlowThread(jointIndex, &_flowJointData, _rig->GetScaleFactorGeometryToUnscaledRig());
                 if (newThread._joints.size() > 1) {
                     _jointThreads.push_back(newThread);
                 }
@@ -796,6 +799,7 @@ Flow& Flow::operator=(const Flow& otherFlow) {
     _active = otherFlow.getActive();
     _scale = otherFlow.getScale();
     _isScaleSet = true;
+    _rig = otherFlow._rig;
     auto &threads = otherFlow.getThreads();
     if (threads.size() == _jointThreads.size()) {
         for (size_t i = 0; i < _jointThreads.size(); i++) {

libraries/animation/src/Flow.h

@@ -263,7 +263,7 @@ class FlowThread {
     FlowThread() {};
     FlowThread& operator=(const FlowThread& otherFlowThread);
 
-    FlowThread(int rootIndex, std::map<int, FlowJoint>* joints);
+    FlowThread(int rootIndex, std::map<int, FlowJoint>* joints, float rigScale);
 
     void resetLength();
     void computeFlowThread(int rootIndex);
@@ -278,14 +278,16 @@ class FlowThread {
     std::vector<glm::vec3> _positions;
     float _radius{ 0.0f };
     float _length{ 0.0f };
+    // 100.0f was default rig scale when it was hardcoded but it caused issues with most avatars
+    float _rigScale { 100.0f };
     std::map<int, FlowJoint>* _jointsPointer;
     std::vector<glm::vec3> _rootFramePositions;
 };
 
 class Flow : public QObject{
     Q_OBJECT
 public:
-    Flow() { }
+    Flow(Rig *rig) : _rig (rig) {};
     Flow& operator=(const Flow& otherFlow);
     bool getActive() const { return _active; }
     void setActive(bool active) { _active = active; }
@@ -323,6 +325,8 @@ class Flow : public QObject{
 
     float _scale { 1.0f };
     float _lastScale{ 1.0f };
+    // Rig to which flow system belongs, it's used for getting rig scale
+    Rig *_rig { nullptr };
     glm::vec3 _entityPosition;
     glm::quat _entityRotation;
     std::map<int, FlowJoint> _flowJointData;

libraries/animation/src/Rig.cpp

@@ -305,7 +305,7 @@ static const QString MAIN_STATE_MACHINE_RIGHT_HAND_POSITION("mainStateMachineRig
 // - isLeftThumbRaise
 // - isLeftIndexPointAndThumbRaise
 // - isLeftHandGrasp
-Rig::Rig() {
+Rig::Rig() : _internalFlow(this), _networkFlow(this) {
     // Ensure thread-safe access to the rigRegistry.
     std::lock_guard<std::mutex> guard(rigRegistryMutex);
 

libraries/animation/src/Rig.h

@@ -261,6 +261,9 @@ class Rig : public QObject {
     bool getNetworkGraphActive() const;
     void setDirectionalBlending(const QString& targetName, const glm::vec3& blendingTarget, const QString& alphaName, float alpha);
 
+    // Get the scale factor to convert distances in the geometry frame into the unscaled rig frame.
+    float GetScaleFactorGeometryToUnscaledRig() const;
+
 signals:
     void onLoadComplete();
     void onLoadFailed();
@@ -290,9 +293,6 @@ class Rig : public QObject {
     glm::vec3 deflectHandFromTorso(const glm::vec3& handPosition, const HFMJointShapeInfo& hipsShapeInfo, const HFMJointShapeInfo& spineShapeInfo,
                                    const HFMJointShapeInfo& spine1ShapeInfo, const HFMJointShapeInfo& spine2ShapeInfo) const;
 
-    // Get the scale factor to convert distances in the geometry frame into the unscaled rig frame.
-    float GetScaleFactorGeometryToUnscaledRig() const;
-
     // The ground plane Y position in geometry space.
     static constexpr float GEOMETRY_GROUND_Y = 0.0f;