Mirrors/overte-JulianGro
3
0
Fork 0
mirror of https://github.com/JulianGro/overte.git synced 2025-04-07 06:02:28 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

Added snap threshold to AccelerationLimiterFilter

Browse source
This commit is contained in:
Anthony Thibault 2018-10-11 09:22:12 -07:00
parent e5c2605ac2
commit 717a5ed31b
3 changed files with 37 additions and 19 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

23
libraries/controllers/src/controllers/impl/filters/AccelerationLimiterFilter.cpp
View file

@ -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;
}
}

4
libraries/controllers/src/controllers/impl/filters/AccelerationLimiterFilter.h
View file

@ -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

29
scripts/developer/tests/filtered-puck-attach.js
View file

@ -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();