From 0fd260076bbcae43f4d7bcf11b752f85476a9d7e Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 22 Feb 2016 20:05:41 -0800 Subject: [PATCH] SixenseManager: fix velocity and angularVelocity computation Copied the delta based computation of velocity and angularVelocity that was in Application::setPalmData() and moved it into SixenseManager. This will guarantee that the velocity computation is the same as it was previously. The goal here is to NOT change the behavior of the hydra. The moving average style calculation of velocities has been removed. Removed dead code. --- interface/src/Application.cpp | 25 ------------- interface/src/avatar/Avatar.cpp | 29 --------------- interface/src/avatar/AvatarActionHold.cpp | 16 --------- plugins/hifiSixense/src/SixenseManager.cpp | 42 +++++++++------------- plugins/hifiSixense/src/SixenseManager.h | 7 ---- 5 files changed, 16 insertions(+), 103 deletions(-) diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index 5c28e31d8c..ed7aef0dab 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -5036,31 +5036,6 @@ void Application::setPalmData(Hand* hand, const controller::Pose& pose, float de // controller pose is in Avatar frame. glm::vec3 position = pose.getTranslation(); glm::quat rotation = pose.getRotation(); - - // AJT: REMOVE - /* - // Compute current velocity from position change - glm::vec3 rawVelocity; - if (deltaTime > 0.0f) { - rawVelocity = (position - palm.getRawPosition()) / deltaTime; - } else { - rawVelocity = glm::vec3(0.0f); - } - palm.setRawVelocity(rawVelocity); // meters/sec - - // Angular Velocity of Palm - glm::quat deltaRotation = rotation * glm::inverse(palm.getRawRotation()); - glm::vec3 angularVelocity(0.0f); - float rotationAngle = glm::angle(deltaRotation); - if ((rotationAngle > EPSILON) && (deltaTime > 0.0f)) { - angularVelocity = glm::normalize(glm::axis(deltaRotation)); - angularVelocity *= (rotationAngle / deltaTime); - palm.setRawAngularVelocity(angularVelocity); - } else { - palm.setRawAngularVelocity(glm::vec3(0.0f)); - } - */ - glm::vec3 rawVelocity = pose.getVelocity(); glm::vec3 angularVelocity = pose.getAngularVelocity(); diff --git a/interface/src/avatar/Avatar.cpp b/interface/src/avatar/Avatar.cpp index f62eb5a070..2b164a7bac 100644 --- a/interface/src/avatar/Avatar.cpp +++ b/interface/src/avatar/Avatar.cpp @@ -929,35 +929,6 @@ void Avatar::setAttachmentData(const QVector& attachmentData) { } _attachmentModels[i]->setURL(attachmentData[i].modelURL); } - - // AJT: TODO REMOVE - /* - // make sure we have as many models as attachments - while (_attachmentModels.size() < attachmentData.size()) { - Model* model = nullptr; - if (_unusedAttachments.size() > 0) { - model = _unusedAttachments.takeFirst(); - } else { - model = new Model(std::make_shared(), this); - } - model->init(); - _attachmentModels.append(model); - } - while (_attachmentModels.size() > attachmentData.size()) { - auto attachmentModel = _attachmentModels.back(); - _attachmentModels.pop_back(); - _attachmentsToRemove.push_back(attachmentModel); - } - */ - - /* - // update the urls - for (int i = 0; i < attachmentData.size(); i++) { - _attachmentModels[i]->setURL(attachmentData.at(i).modelURL); - _attachmentModels[i]->setSnapModelToCenter(true); - _attachmentModels[i]->setScaleToFit(true, getUniformScale() * _attachmentData.at(i).scale); - } - */ } diff --git a/interface/src/avatar/AvatarActionHold.cpp b/interface/src/avatar/AvatarActionHold.cpp index 0093c22377..87990b941e 100644 --- a/interface/src/avatar/AvatarActionHold.cpp +++ b/interface/src/avatar/AvatarActionHold.cpp @@ -243,22 +243,6 @@ void AvatarActionHold::doKinematicUpdate(float deltaTimeStep) { if (_kinematicSetVelocity) { rigidBody->setLinearVelocity(glmToBullet(_linearVelocityTarget)); rigidBody->setAngularVelocity(glmToBullet(_angularVelocityTarget)); - /* - if (_previousSet) { - // smooth velocity over 2 frames - glm::vec3 positionalDelta = _positionalTarget - _previousPositionalTarget; - glm::vec3 positionalVelocity = - (positionalDelta + _previousPositionalDelta) / (deltaTimeStep + _previousDeltaTimeStep); - rigidBody->setLinearVelocity(glmToBullet(positionalVelocity)); - - if (_hand == "right") { - qDebug() << "AJT: rb vel = " << positionalVelocity.x << positionalVelocity.y << positionalVelocity.z; - } - - _previousPositionalDelta = positionalDelta; - _previousDeltaTimeStep = deltaTimeStep; - } - */ } btTransform worldTrans = rigidBody->getWorldTransform(); diff --git a/plugins/hifiSixense/src/SixenseManager.cpp b/plugins/hifiSixense/src/SixenseManager.cpp index aad61e4bea..9fdce3add4 100644 --- a/plugins/hifiSixense/src/SixenseManager.cpp +++ b/plugins/hifiSixense/src/SixenseManager.cpp @@ -113,7 +113,6 @@ void SixenseManager::deactivate() { _container->removeMenu(MENU_PATH); _inputDevice->_poseStateMap.clear(); - _inputDevice->_collectedSamples.clear(); if (_inputDevice->_deviceID != controller::Input::INVALID_DEVICE) { auto userInputMapper = DependencyManager::get(); @@ -158,7 +157,6 @@ void SixenseManager::InputDevice::update(float deltaTime, const controller::Inpu _axisStateMap.clear(); _buttonPressedMap.clear(); _poseStateMap.clear(); - _collectedSamples.clear(); } return; } @@ -209,13 +207,10 @@ void SixenseManager::InputDevice::update(float deltaTime, const controller::Inpu rawPoses[i] = controller::Pose(position, rotation, Vectors::ZERO, Vectors::ZERO); } else { _poseStateMap.clear(); - _collectedSamples.clear(); } } else { auto hand = left ? controller::StandardPoseChannel::LEFT_HAND : controller::StandardPoseChannel::RIGHT_HAND; _poseStateMap[hand] = controller::Pose(); - _collectedSamples[hand].first.clear(); - _collectedSamples[hand].second.clear(); } } @@ -481,29 +476,24 @@ void SixenseManager::InputDevice::handlePoseEvent(float deltaTime, const control glm::vec3 velocity(0.0f); glm::vec3 angularVelocity(0.0f); - if (prevPose.isValid() && deltaTime > std::numeric_limits::epsilon()) { - auto& samples = _collectedSamples[hand]; - - velocity = (pos - prevPose.getTranslation()) / deltaTime; - samples.first.addSample(velocity); - velocity = samples.first.average; - - auto deltaRot = glm::normalize(rot * glm::conjugate(prevPose.getRotation())); - auto axis = glm::axis(deltaRot); - auto speed = glm::angle(deltaRot) / deltaTime; - assert(!glm::isnan(speed)); - angularVelocity = speed * axis; - samples.second.addSample(angularVelocity); - angularVelocity = samples.second.average; - } else if (!prevPose.isValid()) { - _collectedSamples[hand].first.clear(); - _collectedSamples[hand].second.clear(); - } + glm::mat4 controllerToAvatar = glm::inverse(inputCalibrationData.avatarMat) * inputCalibrationData.sensorToWorldMat; // transform pose into avatar frame. - glm::mat4 controllerToAvatar = glm::inverse(inputCalibrationData.avatarMat) * inputCalibrationData.sensorToWorldMat; - auto avatarPose = controller::Pose(pos, rot, velocity, angularVelocity).transform(controllerToAvatar); - _poseStateMap[hand] = avatarPose; + auto nextPose = controller::Pose(pos, rot, velocity, angularVelocity).transform(controllerToAvatar); + + if (prevPose.isValid() && (deltaTime > std::numeric_limits::epsilon())) { + nextPose.velocity = (nextPose.getTranslation() - prevPose.getTranslation()) / deltaTime; + + glm::quat deltaRotation = nextPose.getRotation() * glm::inverse(prevPose.getRotation()); + float rotationAngle = glm::angle(deltaRotation); + if (rotationAngle > EPSILON) { + nextPose.angularVelocity = glm::normalize(glm::axis(deltaRotation)); + nextPose.angularVelocity *= (rotationAngle / deltaTime); + } + + } + _poseStateMap[hand] = nextPose; + #endif // HAVE_SIXENSE } diff --git a/plugins/hifiSixense/src/SixenseManager.h b/plugins/hifiSixense/src/SixenseManager.h index fc74b83532..5106c87836 100644 --- a/plugins/hifiSixense/src/SixenseManager.h +++ b/plugins/hifiSixense/src/SixenseManager.h @@ -52,11 +52,6 @@ private: static const glm::vec3 DEFAULT_AVATAR_POSITION; static const float CONTROLLER_THRESHOLD; - template - using SampleAverage = MovingAverage; - using Samples = std::pair, SampleAverage>; - using MovingAverageMap = std::map; - class InputDevice : public controller::InputDevice { public: InputDevice() : controller::InputDevice("Hydra") {} @@ -75,8 +70,6 @@ private: friend class SixenseManager; - MovingAverageMap _collectedSamples; - int _calibrationState { CALIBRATION_STATE_IDLE }; // these are calibration results glm::vec3 _avatarPosition { DEFAULT_AVATAR_POSITION }; // in hydra-frame