Added snap threshold to AccelerationLimiterFilter

libraries/controllers/src/controllers/impl/filters/AccelerationLimiterFilter.cpp

@@ -18,9 +18,9 @@
 #include <StreamUtils.h>
 
 static const QString JSON_ROTATION_ACCELERATION_LIMIT = QStringLiteral("rotationAccelerationLimit");
-static const QString JSON_ROTATION_DECELERATION_LIMIT = QStringLiteral("rotationDecelerationLimit");
 static const QString JSON_TRANSLATION_ACCELERATION_LIMIT = QStringLiteral("translationAccelerationLimit");
-static const QString JSON_TRANSLATION_DECELERATION_LIMIT = QStringLiteral("translationDecelerationLimit");
+static const QString JSON_TRANSLATION_SNAP_THRESHOLD = QStringLiteral("translationSnapThreshold");
+static const QString JSON_ROTATION_SNAP_THRESHOLD = QStringLiteral("rotationSnapThreshold");
 
 static glm::vec3 angularVelFromDeltaRot(const glm::quat& deltaQ, float dt) {
     // Measure the angular velocity of a delta rotation quaternion by using quaternion logarithm.
@@ -39,7 +39,7 @@ static glm::quat deltaRotFromAngularVel(const glm::vec3& omega, float dt) {
 }
 
 static glm::vec3 filterTranslation(const glm::vec3& x0, const glm::vec3& x1, const glm::vec3& x2, const glm::vec3& x3,
-                                   float dt, const float accLimit) {
+                                   float dt, float accLimit, float snapThreshold) {
 
     // measure the linear velocities of this step and the previoius step
     glm::vec3 v1 = (x3 - x1) / (2.0f * dt);
@@ -52,7 +52,8 @@ static glm::vec3 filterTranslation(const glm::vec3& x0, const glm::vec3& x1, con
     float aLen = glm::length(a);
 
     // pick limit based on if we are moving faster then our target
-    if (aLen > accLimit) {
+    float distToTarget = glm::length(x3 - x2);
+    if (aLen > accLimit && distToTarget > snapThreshold) {
         // Solve for a new `v1`, such that `a` does not exceed `aLimit`
         // This combines two steps:
         // 1) Computing a limited accelration in the direction of `a`, but with a magnitute of `aLimit`:
@@ -71,7 +72,7 @@ static glm::vec3 filterTranslation(const glm::vec3& x0, const glm::vec3& x1, con
 }
 
 static glm::quat filterRotation(const glm::quat& q0In, const glm::quat& q1In, const glm::quat& q2In, const glm::quat& q3In,
-                                float dt, const float accLimit) {
+                                float dt, float accLimit, float snapThreshold) {
 
     // ensure quaternions have the same polarity
     glm::quat q0 = q0In;
@@ -87,7 +88,8 @@ static glm::quat filterRotation(const glm::quat& q0In, const glm::quat& q1In, co
     float aLen = glm::length(a);
 
     // clamp the acceleration if it is over the limit
-    if (aLen > accLimit) {
+    float angleToTarget = glm::angle(q3 * glm::inverse(q2));
+    if (aLen > accLimit && angleToTarget > snapThreshold) {
         // solve for a new w1, such that a does not exceed the accLimit
         w1 = a * ((accLimit * dt) / aLen) + w0;
 
@@ -120,10 +122,10 @@ namespace controller {
 
                 glm::vec3 unfilteredTranslation = sensorValue.translation;
                 sensorValue.translation = filterTranslation(_prevPos[0], _prevPos[1], _prevPos[2], sensorValue.translation,
-                                                            DELTA_TIME, _translationAccelerationLimit);
+                                                            DELTA_TIME, _translationAccelerationLimit, _translationSnapThreshold);
                 glm::quat unfilteredRot = sensorValue.rotation;
                 sensorValue.rotation = filterRotation(_prevRot[0], _prevRot[1], _prevRot[2], sensorValue.rotation,
-                                                      DELTA_TIME, _rotationAccelerationLimit);
+                                                      DELTA_TIME, _rotationAccelerationLimit, _rotationSnapThreshold);
 
                 // remember previous values.
                 _prevPos[0] = _prevPos[1];
@@ -175,9 +177,12 @@ namespace controller {
     bool AccelerationLimiterFilter::parseParameters(const QJsonValue& parameters) {
         if (parameters.isObject()) {
             auto obj = parameters.toObject();
-            if (obj.contains(JSON_ROTATION_ACCELERATION_LIMIT) && obj.contains(JSON_TRANSLATION_ACCELERATION_LIMIT)) {
+            if (obj.contains(JSON_ROTATION_ACCELERATION_LIMIT) && obj.contains(JSON_TRANSLATION_ACCELERATION_LIMIT) &&
+                obj.contains(JSON_ROTATION_SNAP_THRESHOLD) && obj.contains(JSON_TRANSLATION_SNAP_THRESHOLD)) {
                 _rotationAccelerationLimit = (float)obj[JSON_ROTATION_ACCELERATION_LIMIT].toDouble();
                 _translationAccelerationLimit = (float)obj[JSON_TRANSLATION_ACCELERATION_LIMIT].toDouble();
+                _rotationSnapThreshold = (float)obj[JSON_ROTATION_SNAP_THRESHOLD].toDouble();
+                _translationSnapThreshold = (float)obj[JSON_TRANSLATION_SNAP_THRESHOLD].toDouble();
                 return true;
             }
         }

libraries/controllers/src/controllers/impl/filters/AccelerationLimiterFilter.h

@@ -25,9 +25,9 @@ namespace controller {
 
     private:
         float _rotationAccelerationLimit { FLT_MAX };
-        float _rotationDecelerationLimit { FLT_MAX };
         float _translationAccelerationLimit { FLT_MAX };
-        float _translationDecelerationLimit { FLT_MAX };
+        float _rotationSnapThreshold { 0.0f };
+        float _translationSnapThreshold { 0.0f };
 
         mutable glm::vec3 _prevPos[3];  // sensor space
         mutable glm::quat _prevRot[3];  // sensor space

scripts/developer/tests/filtered-puck-attach.js

@@ -101,6 +101,8 @@ var DEFAULT_ROTATION_SMOOTHING_CONSTANT = 1.0; // no smoothing
 var DEFAULT_TRANSLATION_SMOOTHING_CONSTANT = 1.0; // no smoothing
 var DEFAULT_TRANSLATION_ACCELERATION_LIMIT = 1000; // only extreme accelerations are smoothed
 var DEFAULT_ROTATION_ACCELERATION_LIMIT = 10000; // only extreme accelerations are smoothed
+var DEFAULT_TRANSLATION_SNAP_THRESHOLD = 0; // no snapping
+var DEFAULT_ROTATION_SNAP_THRESHOLD = 0; // no snapping
 
 function buildMappingJson() {
     var obj = {name: "com.highfidelity.testing.filteredPuckAttach", channels: []};
@@ -113,7 +115,9 @@ function buildMappingJson() {
                 {
                     type: "accelerationLimiter",
                     translationAccelerationLimit: DEFAULT_TRANSLATION_ACCELERATION_LIMIT,
-                    rotationAccelerationLimit: DEFAULT_ROTATION_ACCELERATION_LIMIT
+                    rotationAccelerationLimit: DEFAULT_ROTATION_ACCELERATION_LIMIT,
+                    translationSnapThreshold: DEFAULT_TRANSLATION_SNAP_THRESHOLD,
+                    rotationSnapThreshold: DEFAULT_ROTATION_SNAP_THRESHOLD,
                 },
                 {
                     type: "exponentialSmoothing",
@@ -154,7 +158,14 @@ function setTranslationAccelerationLimit(value) {
 }
 
 function setTranslationSnapThreshold(value) {
-    // TODO: convert from mm
+    // convert from mm
+    var MM_PER_M = 1000;
+    var meters = value / MM_PER_M;
+    var i;
+    for (i = 0; i < NUM_TRACKED_OBJECTS; i++) {
+        mappingJson.channels[i].filters[0].translationSnapThreshold = meters;
+    }
+    mappingChanged();
 }
 
 function setRotationAccelerationLimit(value) {
@@ -166,21 +177,25 @@ function setRotationAccelerationLimit(value) {
 }
 
 function setRotationSnapThreshold(value) {
-    // TODO: convert from degrees
+    // convert from degrees
+    var PI_IN_DEGREES = 180;
+    var radians = value * (Math.pi / PI_IN_DEGREES);
+    var i;
+    for (i = 0; i < NUM_TRACKED_OBJECTS; i++) {
+        mappingJson.channels[i].filters[0].translationSnapThreshold = radians;
+    }
+    mappingChanged();
 }
 
 function setTranslationSmoothingConstant(value) {
-    print("AJT: setting translation smoothing constant = " + value);
     var i;
     for (i = 0; i < NUM_TRACKED_OBJECTS; i++) {
         mappingJson.channels[i].filters[1].translation = value;
     }
     mappingChanged();
-    print("AJT: done, value = " + value);
 }
 
 function setRotationSmoothingConstant(value) {
-    print("AJT: setRotationSmoothingConstant =" + value);
     var i;
     for (i = 0; i < NUM_TRACKED_OBJECTS; i++) {
         mappingJson.channels[i].filters[1].rotation = value;
@@ -366,8 +381,6 @@ function onWebEventReceived(msg) {
         return;
     }
 
-    print("AJT: onWebEventReceived = " + msg);
-
     switch (obj.cmd) {
     case "ready":
         sendAvailableTrackedObjects();