SimulationEngine::enforceConstraints() takes args

...for limiting simulation costs

libraries/shared/src/SimulationEngine.cpp

@@ -11,6 +11,7 @@
 
 #include <glm/glm.hpp>
 
+#include "SharedUtil.h"
 #include "SimulationEngine.h"
 
 int MAX_DOLLS_PER_ENGINE = 32;
@@ -59,13 +60,26 @@ void SimulationEngine::removeRagDoll(RagDoll* doll) {
     }
 }
 
-float SimulationEngine::enforceConstraints() {
-    float maxMovement = 0.0f;
-    int numDolls = _dolls.size();
-    for (int i = 0; i < numDolls; ++i) {
-        maxMovement = glm::max(maxMovement, _dolls[i]->enforceConstraints());
-    }
-    return maxMovement;
+void SimulationEngine::enforceConstraints(float minError, int maxIterations, quint64 maxUsec) {
+    // enforce the constraints
+    int iterations = 0;
+    float delta = 0.0f;
+    quint64 now = usecTimestampNow();
+    quint64 startTime = now;
+    quint64 expiry = now + maxUsec;
+    float error = 0.0f;
+    do {
+        error = 0.0f;
+        int numDolls = _dolls.size();
+        for (int i = 0; i < numDolls; ++i) {
+            error = glm::max(error, _dolls[i]->enforceConstraints());
+        }
+        ++iterations;
+        now = usecTimestampNow();
+    } while (iterations < maxIterations && delta > minError && now < expiry);
+    _enforcementIterations = iterations;
+    _enforcementError = delta;
+    _enforcementTime = now - startTime;
 }
 
 int SimulationEngine::computeCollisions() {

libraries/shared/src/SimulationEngine.h

@@ -28,8 +28,15 @@ public:
 
     void removeRagDoll(RagDoll* doll);
 
+    /// \param minError constraint motion below this value is considered "close enough"
+    /// \param maxIterations max number of iterations before giving up
+    /// \param maxUsec max number of usec to spend enforcing constraints
     /// \return distance of largest movement
-    float enforceConstraints();
+    void enforceConstraints(float minError, int maxIterations, quint64 maxUsec);
+
+    int getEnforementIterations() const { return _enforcementIterations; }
+    float getEnforcementError() const { return _enforcementError; }
+    quint64 getEnforcementTime() const { return _enforcementTime; }
 
     /// \return number of collisions
     int computeCollisions();
@@ -39,6 +46,11 @@ public:
 private:
     CollisionList _collisionList;
     QVector<RagDoll*> _dolls;
+
+    // some stats for performance queries
+    int _enforcementIterations;
+    float _enforcementError;
+    quint64 _enforcementTime;
 };
 
 #endif // hifi_SimulationEngine_h