diff --git a/libraries/input-plugins/src/input-plugins/SixenseManager.cpp b/libraries/input-plugins/src/input-plugins/SixenseManager.cpp index 5dd0248224..3b08c218a2 100644 --- a/libraries/input-plugins/src/input-plugins/SixenseManager.cpp +++ b/libraries/input-plugins/src/input-plugins/SixenseManager.cpp @@ -21,6 +21,7 @@ #include #include +#include #include #include #include @@ -48,6 +49,8 @@ const QString MENU_PARENT = "Avatar"; const QString MENU_NAME = "Sixense"; const QString MENU_PATH = MENU_PARENT + ">" + MENU_NAME; const QString TOGGLE_SMOOTH = "Smooth Sixense Movement"; +const QString SHOW_DEBUG_RAW = "Debug Draw Raw Data"; +const QString SHOW_DEBUG_CALIBRATED = "Debug Draw Calibrated Data"; bool SixenseManager::isSupported() const { #ifdef HAVE_SIXENSE @@ -72,6 +75,14 @@ void SixenseManager::activate() { [this] (bool clicked) { setSixenseFilter(clicked); }, true, true); + _container->addMenuItem(PluginType::INPUT_PLUGIN, MENU_PATH, SHOW_DEBUG_RAW, + [this] (bool clicked) { _inputDevice->setDebugDrawRaw(clicked); }, + true, false); + + _container->addMenuItem(PluginType::INPUT_PLUGIN, MENU_PATH, SHOW_DEBUG_CALIBRATED, + [this] (bool clicked) { _inputDevice->setDebugDrawCalibrated(clicked); }, + true, false); + auto userInputMapper = DependencyManager::get(); userInputMapper->registerDevice(_inputDevice); @@ -149,6 +160,9 @@ void SixenseManager::InputDevice::update(float deltaTime, bool jointsCaptured) { // we only support two controllers SixenseControllerData controllers[2]; + // store the raw controller data for debug rendering + controller::Pose rawPoses[2]; + int numActiveControllers = 0; for (int i = 0; i < maxControllers && numActiveControllers < 2; i++) { if (!sixenseIsControllerEnabled(i)) { @@ -175,7 +189,7 @@ void SixenseManager::InputDevice::update(float deltaTime, bool jointsCaptured) { // Rotation of Palm glm::quat rotation(data->rot_quat[3], data->rot_quat[0], data->rot_quat[1], data->rot_quat[2]); handlePoseEvent(deltaTime, position, rotation, left); - + rawPoses[i] = controller::Pose(position, rotation, glm::vec3(0), glm::quat()); } else { _poseStateMap.clear(); _collectedSamples.clear(); @@ -197,9 +211,54 @@ void SixenseManager::InputDevice::update(float deltaTime, bool jointsCaptured) { _axisStateMap[axisState.first] = 0.0f; } } + + if (_debugDrawCalibrated) { + auto poseIter = _poseStateMap.find(controller::StandardPoseChannel::LEFT_HAND); + if (poseIter != _poseStateMap.end() && poseIter->second.isValid()) { + DebugDraw::getInstance().addMyAvatarMarker("SIXENSE_CALIBRATED_LEFT", poseIter->second.rotation, poseIter->second.translation, glm::vec4(1)); + } else { + DebugDraw::getInstance().removeMyAvatarMarker("SIXENSE_CALIBRATED_LEFT"); + } + poseIter = _poseStateMap.find(controller::StandardPoseChannel::RIGHT_HAND); + if (poseIter != _poseStateMap.end() && poseIter->second.isValid()) { + DebugDraw::getInstance().addMyAvatarMarker("SIXENSE_CALIBRATED_RIGHT", poseIter->second.rotation, poseIter->second.translation, glm::vec4(1)); + } else { + DebugDraw::getInstance().removeMyAvatarMarker("SIXENSE_CALIBRATED_RIGHT"); + } + } + + if (_debugDrawRaw) { + if (rawPoses[0].isValid()) { + DebugDraw::getInstance().addMyAvatarMarker("SIXENSE_RAW_LEFT", rawPoses[0].rotation, rawPoses[0].translation, glm::vec4(1)); + } else { + DebugDraw::getInstance().removeMyAvatarMarker("SIXENSE_RAW_LEFT"); + } + if (rawPoses[1].isValid()) { + DebugDraw::getInstance().addMyAvatarMarker("SIXENSE_RAW_RIGHT", rawPoses[1].rotation, rawPoses[1].translation, glm::vec4(1)); + } else { + DebugDraw::getInstance().removeMyAvatarMarker("SIXENSE_RAW_RIGHT"); + } + } + #endif // HAVE_SIXENSE } +void SixenseManager::InputDevice::setDebugDrawRaw(bool flag) { + _debugDrawRaw = flag; + if (!flag) { + DebugDraw::getInstance().removeMyAvatarMarker("SIXENSE_RAW_LEFT"); + DebugDraw::getInstance().removeMyAvatarMarker("SIXENSE_RAW_RIGHT"); + } +} + +void SixenseManager::InputDevice::setDebugDrawCalibrated(bool flag) { + _debugDrawCalibrated = flag; + if (!flag) { + DebugDraw::getInstance().removeMyAvatarMarker("SIXENSE_CALIBRATED_LEFT"); + DebugDraw::getInstance().removeMyAvatarMarker("SIXENSE_CALIBRATED_RIGHT"); + } +} + #ifdef HAVE_SIXENSE // the calibration sequence is: diff --git a/libraries/input-plugins/src/input-plugins/SixenseManager.h b/libraries/input-plugins/src/input-plugins/SixenseManager.h index 348a7a4590..b6bf4b24c0 100644 --- a/libraries/input-plugins/src/input-plugins/SixenseManager.h +++ b/libraries/input-plugins/src/input-plugins/SixenseManager.h @@ -60,6 +60,8 @@ private: class InputDevice : public controller::InputDevice { public: InputDevice() : controller::InputDevice("Hydra") {} + void setDebugDrawRaw(bool flag); + void setDebugDrawCalibrated(bool flag); private: // Device functions virtual controller::Input::NamedVector getAvailableInputs() const override; @@ -82,6 +84,8 @@ private: float _lastDistance; bool _requestReset { false }; + bool _debugDrawRaw { false }; + bool _debugDrawCalibrated { false }; // these are measured values used to compute the calibration results quint64 _lockExpiry; glm::vec3 _averageLeft;