add deltaTime to simulation

libraries/animation/src/Flow.cpp

@@ -206,22 +206,26 @@ void FlowCollisionSystem::prepareCollisions() {
     _othersCollisions.clear();
 }
 
-void FlowNode::update(const glm::vec3& accelerationOffset) {
+void FlowNode::update(float deltaTime, const glm::vec3& accelerationOffset) {
     _acceleration = glm::vec3(0.0f, _settings._gravity, 0.0f);
     _previousVelocity = _currentVelocity;
     _currentVelocity = _currentPosition - _previousPosition;
     _previousPosition = _currentPosition;
     if (!_anchored) {
         // Add inertia
+        const float FPS = 60.0f;
+        float timeRatio = (FPS * deltaTime);
+
         auto deltaVelocity = _previousVelocity - _currentVelocity;
         auto centrifugeVector = glm::length(deltaVelocity) != 0.0f ? glm::normalize(deltaVelocity) : glm::vec3();
-        _acceleration = _acceleration + centrifugeVector * _settings._inertia * glm::length(_currentVelocity);
+        _acceleration = _acceleration + centrifugeVector * _settings._inertia * glm::length(_currentVelocity) * (1 / timeRatio);
 
         // Add offset
         _acceleration += accelerationOffset;
-        // Calculate new position
+        
-        _currentPosition = (_currentPosition + _currentVelocity * _settings._damping) +
+        glm::vec3 deltaAcceleration = timeRatio * _acceleration * glm::pow(_settings._delta * _scale, 2);
-            (_acceleration * glm::pow(_settings._delta * _scale, 2));
+        // Calculate new position        
+        _currentPosition = (_currentPosition + _currentVelocity * _settings._damping) + deltaAcceleration;
     }
     else {
         _acceleration = glm::vec3(0.0f);
@@ -289,13 +293,13 @@ void FlowJoint::setRecoveryPosition(const glm::vec3& recoveryPosition) {
     _applyRecovery = true;
 }
 
-void FlowJoint::update() {
+void FlowJoint::update(float deltaTime) {
     glm::vec3 accelerationOffset = glm::vec3(0.0f);
     if (_node._settings._stiffness > 0.0f) {
         glm::vec3 recoveryVector = _recoveryPosition - _node._currentPosition;
         accelerationOffset = recoveryVector * glm::pow(_node._settings._stiffness, 3);
     }
-    _node.update(accelerationOffset);
+    _node.update(deltaTime, accelerationOffset);
     if (_node._anchored) {
         if (!_isDummy) {
             _node._currentPosition = _updatedPosition;
@@ -368,14 +372,14 @@ void FlowThread::computeRecovery() {
     }
 };
 
-void FlowThread::update() {
+void FlowThread::update(float deltaTime) {
     if (getActive()) {
         _positions.clear();
         _radius = _jointsPointer->at(_joints[0])._node._settings._radius;
         computeRecovery();
         for (size_t i = 0; i < _joints.size(); i++) {
             auto &joint = _jointsPointer->at(_joints[i]);
-            joint.update();
+            joint.update(deltaTime);
             _positions.push_back(joint._node._currentPosition);
         }
     }
@@ -447,10 +451,6 @@ bool FlowThread::getActive() {
     return _jointsPointer->at(_joints[0])._node._active;
 };
 
-void Flow::setRig(Rig* rig) {
-    _rig = rig;
-};
-
 void Flow::calculateConstraints() {
     cleanUp();
     if (!_rig) {
@@ -593,6 +593,9 @@ void Flow::calculateConstraints() {
         }
     }
     _initialized = _jointThreads.size() > 0;
+    if (_initialized) {
+        _mtimer.restart();
+    }
 }
 
 void Flow::cleanUp() {
@@ -615,14 +618,14 @@ void Flow::setTransform(float scale, const glm::vec3& position, const glm::quat&
     _active = true;
 }
 
-void Flow::update() {
+void Flow::update(float deltaTime) {
     if (_initialized && _active) {
-        QElapsedTimer _timer;
+        QElapsedTimer timer;
-        _timer.start();
+        timer.start();
         updateJoints();
         int count = 0;
         for (auto &thread : _jointThreads) {
-            thread.update();
+            thread.update(deltaTime);
             thread.solve(USE_COLLISIONS, _collisionSystem);
             if (!updateRootFramePositions(count++)) {
                 return;
@@ -630,10 +633,12 @@ void Flow::update() {
             thread.apply();
         }
         setJoints();
-        _deltaTime += _timer.nsecsElapsed();
+        _deltaTime += timer.nsecsElapsed();
         _updates++;
         if (_deltaTime > _deltaTimeLimit) {
-            qDebug() << "Flow C++ update " << _deltaTime / _updates << " nanoSeconds";
+            qint64 currentTime = _mtimer.elapsed();
+            qDebug() << "Flow C++ update " << _deltaTime / _updates << " nanoSeconds " << (currentTime - _lastTime) / _updates << " miliseconds since last update";
+            _lastTime = currentTime;
             _deltaTime = 0;
             _updates = 0;
         }

libraries/animation/src/Flow.h

@@ -199,7 +199,7 @@ public:
     bool _colliding { false };
     bool _active { true };
 
-    void update(const glm::vec3& accelerationOffset);
+    void update(float deltaTime, const glm::vec3& accelerationOffset);
     void solve(const glm::vec3& constrainPoint, float maxDistance, const FlowCollisionResult& collision);
     void solveConstraints(const glm::vec3& constrainPoint, float maxDistance);
     void solveCollisions(const FlowCollisionResult& collision);
@@ -212,7 +212,7 @@ public:
     void setInitialData(const glm::vec3& initialPosition, const glm::vec3& initialTranslation, const glm::quat& initialRotation, const glm::vec3& parentPosition);
     void setUpdatedData(const glm::vec3& updatedPosition, const glm::vec3& updatedTranslation, const glm::quat& updatedRotation, const glm::vec3& parentPosition, const glm::quat& parentWorldRotation);
     void setRecoveryPosition(const glm::vec3& recoveryPosition);
-    void update();
+    void update(float deltaTime);
     void solve(const FlowCollisionResult& collision);
 
     int _index{ -1 };
@@ -257,7 +257,7 @@ public:
     void resetLength();
     void computeFlowThread(int rootIndex);
     void computeRecovery();
-    void update();
+    void update(float deltaTime);
     void solve(bool useCollisions, FlowCollisionSystem& collisionSystem);
     void computeJointRotations();
     void apply();
@@ -274,10 +274,9 @@ public:
 
 class Flow {
 public:
-    Flow() {};
+    Flow(Rig* rig) { _rig = rig; };
-    void setRig(Rig* rig);
     void calculateConstraints();
-    void update();
+    void update(float deltaTime);
     void setTransform(float scale, const glm::vec3& position, const glm::quat& rotation);
     const std::map<int, FlowJoint>& getJoints() const { return _flowJointData; }
     const std::vector<FlowThread>& getThreads() const { return _jointThreads; }
@@ -302,6 +301,8 @@ private:
     int _deltaTime { 0 };
     int _deltaTimeLimit { 4000000 };
     int _updates { 0 };
+    QElapsedTimer _mtimer;
+    long _lastTime { 0 };
 };
 
 #endif // hifi_Flow_h

libraries/animation/src/Rig.cpp

@@ -75,7 +75,7 @@ static const QString MAIN_STATE_MACHINE_RIGHT_FOOT_ROTATION("mainStateMachineRig
 static const QString MAIN_STATE_MACHINE_RIGHT_FOOT_POSITION("mainStateMachineRightFootPosition");
 
 
-Rig::Rig() {
+Rig::Rig() : _flow(this) {
     // Ensure thread-safe access to the rigRegistry.
     std::lock_guard<std::mutex> guard(rigRegistryMutex);
 
@@ -1217,7 +1217,7 @@ void Rig::updateAnimations(float deltaTime, const glm::mat4& rootTransform, cons
     buildAbsoluteRigPoses(_internalPoseSet._relativePoses, _internalPoseSet._absolutePoses);
     buildAbsoluteRigPoses(_networkPoseSet._relativePoses, _networkPoseSet._absolutePoses);
     // copy internal poses to external poses
-    _flow.update();
+    _flow.update(deltaTime);
     {
         QWriteLocker writeLock(&_externalPoseSetLock);
         
@@ -1873,7 +1873,6 @@ void Rig::initAnimGraph(const QUrl& url) {
                 auto roleState = roleAnimState.second;
                 overrideRoleAnimation(roleState.role, roleState.url, roleState.fps, roleState.loop, roleState.firstFrame, roleState.lastFrame);
             }
-            _flow.setRig(this);
             emit onLoadComplete();
         });
         connect(_animLoader.get(), &AnimNodeLoader::error, [url](int error, QString str) {