diff --git a/libraries/controllers/src/controllers/impl/filters/AccelerationLimiterFilter.cpp b/libraries/controllers/src/controllers/impl/filters/AccelerationLimiterFilter.cpp index 234dff1c65..677e0af75a 100644 --- a/libraries/controllers/src/controllers/impl/filters/AccelerationLimiterFilter.cpp +++ b/libraries/controllers/src/controllers/impl/filters/AccelerationLimiterFilter.cpp @@ -38,7 +38,8 @@ static glm::quat deltaRotFromAngularVel(const glm::vec3& omega, float dt) { return glm::exp((dt / 2.0f) * omegaQ); } -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, const float decLimit) { +static glm::vec3 filterTranslation(const glm::vec3& x0, const glm::vec3& x1, const glm::vec3& x2, const glm::vec3& x3, + const glm::vec3& unfilteredVelocity, float dt, const float accLimit, const float decLimit) { // measure the linear velocities of this step and the previoius step glm::vec3 v1 = (x3 - x1) / (2.0f * dt); @@ -50,8 +51,8 @@ static glm::vec3 filterTranslation(const glm::vec3& x0, const glm::vec3& x1, con // clamp the acceleration if it is over the limit float aLen = glm::length(a); - // pick limit based on if we are accelerating or decelerating. - float limit = glm::length(v1) > glm::length(v0) ? accLimit : decLimit; + // pick limit based on if we are moving faster then our target + float limit = glm::length(v1) > glm::length(unfilteredVelocity) ? accLimit : decLimit; if (aLen > limit) { // Solve for a new `v1`, such that `a` does not exceed `aLimit` @@ -71,7 +72,8 @@ 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, const float decLimit) { +static glm::quat filterRotation(const glm::quat& q0In, const glm::quat& q1In, const glm::quat& q2In, const glm::quat& q3In, + const glm::vec3& unfilteredVelocity, float dt, const float accLimit, const float decLimit) { // ensure quaternions have the same polarity glm::quat q0 = q0In; @@ -86,8 +88,8 @@ static glm::quat filterRotation(const glm::quat& q0In, const glm::quat& q1In, co const glm::vec3 a = (w1 - w0) / dt; float aLen = glm::length(a); - // pick limit based on if we are accelerating or decelerating. - float limit = glm::length(w1) > glm::length(w0) ? accLimit : decLimit; + // pick limit based on if we are moving faster then our target + float limit = glm::length(w1) > glm::length(unfilteredVelocity) ? accLimit : decLimit; // clamp the acceleration if it is over the limit if (aLen > limit) { @@ -121,9 +123,14 @@ namespace controller { const float DELTA_TIME = 0.01111111f; - sensorValue.translation = filterTranslation(_prevPos[0], _prevPos[1], _prevPos[2], sensorValue.translation, + glm::vec3 unfilteredTranslation = sensorValue.translation; + glm::vec3 unfilteredLinearVel = (unfilteredTranslation - _unfilteredPrevPos[1]) / (2.0f * DELTA_TIME); + sensorValue.translation = filterTranslation(_prevPos[0], _prevPos[1], _prevPos[2], sensorValue.translation, unfilteredLinearVel, DELTA_TIME, _translationAccelerationLimit, _translationDecelerationLimit); - sensorValue.rotation = filterRotation(_prevRot[0], _prevRot[1], _prevRot[2], sensorValue.rotation, + glm::quat unfilteredRot = sensorValue.rotation; + glm::quat unfilteredPrevRot = glm::dot(unfilteredRot, _unfilteredPrevRot[1]) < 0.0f ? -_unfilteredPrevRot[1] : _unfilteredPrevRot[1]; + glm::vec3 unfilteredAngularVel = angularVelFromDeltaRot(unfilteredRot * glm::inverse(unfilteredPrevRot), 2.0f * DELTA_TIME); + sensorValue.rotation = filterRotation(_prevRot[0], _prevRot[1], _prevRot[2], sensorValue.rotation, unfilteredAngularVel, DELTA_TIME, _rotationAccelerationLimit, _rotationDecelerationLimit); // remember previous values. @@ -134,6 +141,13 @@ namespace controller { _prevRot[1] = _prevRot[2]; _prevRot[2] = sensorValue.rotation; + _unfilteredPrevPos[0] = _unfilteredPrevPos[1]; + _unfilteredPrevPos[1] = _unfilteredPrevPos[2]; + _unfilteredPrevPos[2] = unfilteredTranslation; + _unfilteredPrevRot[0] = _unfilteredPrevRot[1]; + _unfilteredPrevRot[1] = _unfilteredPrevRot[2]; + _unfilteredPrevRot[2] = unfilteredRot; + // transform back into avatar space return sensorValue.transform(sensorToAvatarMat); } else { @@ -144,6 +158,14 @@ namespace controller { _prevRot[0] = sensorValue.rotation; _prevRot[1] = sensorValue.rotation; _prevRot[2] = sensorValue.rotation; + + _unfilteredPrevPos[0] = sensorValue.translation; + _unfilteredPrevPos[1] = sensorValue.translation; + _unfilteredPrevPos[2] = sensorValue.translation; + _unfilteredPrevRot[0] = sensorValue.rotation; + _unfilteredPrevRot[1] = sensorValue.rotation; + _unfilteredPrevRot[2] = sensorValue.rotation; + _prevValid = true; // no previous value to smooth with, so return value unchanged diff --git a/libraries/controllers/src/controllers/impl/filters/AccelerationLimiterFilter.h b/libraries/controllers/src/controllers/impl/filters/AccelerationLimiterFilter.h index 6a6c7f8c33..06eeef1579 100644 --- a/libraries/controllers/src/controllers/impl/filters/AccelerationLimiterFilter.h +++ b/libraries/controllers/src/controllers/impl/filters/AccelerationLimiterFilter.h @@ -31,6 +31,8 @@ namespace controller { mutable glm::vec3 _prevPos[3]; // sensor space mutable glm::quat _prevRot[3]; // sensor space + mutable glm::vec3 _unfilteredPrevPos[3]; // sensor space + mutable glm::quat _unfilteredPrevRot[3]; // sensor space mutable bool _prevValid { false }; }; diff --git a/scripts/developer/accelerationFilterApp.js b/scripts/developer/accelerationFilterApp.js new file mode 100644 index 0000000000..52109c0f5d --- /dev/null +++ b/scripts/developer/accelerationFilterApp.js @@ -0,0 +1,214 @@ +var LEFT_HAND_INDEX = 0; +var RIGHT_HAND_INDEX = 1; +var LEFT_FOOT_INDEX = 2; +var RIGHT_FOOT_INDEX = 3; +var HIPS_INDEX = 4; +var SPINE2_INDEX = 5; + +var mappingJson = { + name: "com.highfidelity.testing.accelerationTest", + channels: [ + { + from: "Vive.LeftHand", + to: "Standard.LeftHand", + filters: [ + { + type: "accelerationLimiter", + rotationAccelerationLimit: 2000.0, + rotationDecelerationLimit: 4000.0, + translationAccelerationLimit: 100.0, + translationDecelerationLimit: 200.0 + } + ] + }, + { + from: "Vive.RightHand", + to: "Standard.RightHand", + filters: [ + { + type: "accelerationLimiter", + rotationAccelerationLimit: 2000.0, + rotationDecelerationLimit: 4000.0, + translationAccelerationLimit: 100.0, + translationDecelerationLimit: 200.0 + } + ] + }, + { + from: "Vive.LeftFoot", + to: "Standard.LeftFoot", + filters: [ + { + type: "exponentialSmoothing", + rotation: 0.15, + translation: 0.3 + } + ] + }, + { + from: "Vive.RightFoot", + to: "Standard.RightFoot", + filters: [ + { + type: "exponentialSmoothing", + rotation: 0.15, + translation: 0.3 + } + ] + }, + { + from: "Vive.Hips", + to: "Standard.Hips", + filters: [ + { + type: "exponentialSmoothing", + rotation: 0.15, + translation: 0.3 + } + ] + }, + { + from: "Vive.Spine2", + to: "Standard.Spine2", + filters: [ + { + type: "exponentialSmoothing", + rotation: 0.15, + translation: 0.3 + } + ] + } + ] +}; + +// +// tablet app boiler plate +// + +var TABLET_BUTTON_NAME = "ACCFILT"; +var HTML_URL = "https://s3.amazonaws.com/hifi-public/tony/html/accelerationFilterApp.html"; + +var tablet = Tablet.getTablet("com.highfidelity.interface.tablet.system"); +var tabletButton = tablet.addButton({ + text: TABLET_BUTTON_NAME, + icon: "https://s3.amazonaws.com/hifi-public/tony/icons/tpose-i.svg", + activeIcon: "https://s3.amazonaws.com/hifi-public/tony/icons/tpose-a.svg" +}); + +tabletButton.clicked.connect(function () { + if (shown) { + tablet.gotoHomeScreen(); + } else { + tablet.gotoWebScreen(HTML_URL); + } +}); + +var shown = false; + +function onScreenChanged(type, url) { + if (type === "Web" && url === HTML_URL) { + tabletButton.editProperties({isActive: true}); + if (!shown) { + // hook up to event bridge + tablet.webEventReceived.connect(onWebEventReceived); + shownChanged(true); + } + shown = true; + } else { + tabletButton.editProperties({isActive: false}); + if (shown) { + // disconnect from event bridge + tablet.webEventReceived.disconnect(onWebEventReceived); + shownChanged(false); + } + shown = false; + } +} + +function getTranslationAccelerationLimit(i) { + return mappingJson.channels[i].filters[0].translationAccelerationLimit; +} +function setTranslationAccelerationLimit(i, value) { + mappingJson.channels[i].filters[0].translationAccelerationLimit = value; + mappingChanged(); +} +function getTranslationDecelerationLimit(i) { + return mappingJson.channels[i].filters[0].translationDecelerationLimit; +} +function setTranslationDecelerationLimit(i, value) { + mappingJson.channels[i].filters[0].translationDecelerationLimit = value; mappingChanged(); +} +function getRotationAccelerationLimit(i) { + return mappingJson.channels[i].filters[0].rotationAccelerationLimit; +} +function setRotationAccelerationLimit(i, value) { + mappingJson.channels[i].filters[0].rotationAccelerationLimit = value; mappingChanged(); +} +function getRotationDecelerationLimit(i) { + return mappingJson.channels[i].filters[0].rotationDecelerationLimit; +} +function setRotationDecelerationLimit(i, value) { + mappingJson.channels[i].filters[0].rotationDecelerationLimit = value; mappingChanged(); +} + +function onWebEventReceived(msg) { + if (msg.name === "init-complete") { + var values = [ + {name: "left-hand-translation-acceleration-limit", val: getTranslationAccelerationLimit(LEFT_HAND_INDEX), checked: false}, + {name: "left-hand-translation-deceleration-limit", val: getTranslationDecelerationLimit(LEFT_HAND_INDEX), checked: false}, + {name: "left-hand-rotation-acceleration-limit", val: getRotationAccelerationLimit(LEFT_HAND_INDEX), checked: false}, + {name: "left-hand-rotation-deceleration-limit", val: getRotationDecelerationLimit(LEFT_HAND_INDEX), checked: false} + ]; + tablet.emitScriptEvent(JSON.stringify(values)); + } else if (msg.name === "left-hand-translation-acceleration-limit") { + setTranslationAccelerationLimit(LEFT_HAND_INDEX, parseInt(msg.val, 10)); + } else if (msg.name === "left-hand-translation-deceleration-limit") { + setTranslationDecelerationLimit(LEFT_HAND_INDEX, parseInt(msg.val, 10)); + } else if (msg.name === "left-hand-rotation-acceleration-limit") { + setRotationAccelerationLimit(LEFT_HAND_INDEX, parseInt(msg.val, 10)); + } else if (msg.name === "left-hand-rotation-deceleration-limit") { + setRotationDecelerationLimit(LEFT_HAND_INDEX, parseInt(msg.val, 10)); + } +} + +tablet.screenChanged.connect(onScreenChanged); + +function shutdownTabletApp() { + tablet.removeButton(tabletButton); + if (shown) { + tablet.webEventReceived.disconnect(onWebEventReceived); + tablet.gotoHomeScreen(); + } + tablet.screenChanged.disconnect(onScreenChanged); +} + +// +// end tablet app boiler plate +// + +var mapping; +function mappingChanged() { + if (mapping) { + mapping.disable(); + } + mapping = Controller.parseMapping(JSON.stringify(mappingJson)); + mapping.enable(); +} + +function shownChanged(newShown) { + if (newShown) { + mappingChanged(); + } else { + mapping.disable(); + } +} + +mappingChanged(); + +Script.scriptEnding.connect(function() { + if (mapping) { + mapping.disable(); + } + tablet.removeButton(tabletButton); +}); +