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