//// SixenseManager.cpp// input-plugins/src/input-plugins//// Created by Andrzej Kapolka on 11/15/13.// Copyright 2013 High Fidelity, Inc.//// Distributed under the Apache License, Version 2.0.// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html// #include "SixenseManager.h" #ifdef HAVE_SIXENSE#include <sixense.h>#else#define SIXENSE_FAILURE -1#define SIXENSE_SUCCESS 0#endif #include <QCoreApplication>#include <QtCore/QSysInfo>#include <QtGlobal> #include <controllers/UserInputMapper.h>#include <GLMHelpers.h>#include <DebugDraw.h>#include <NumericalConstants.h>#include <PathUtils.h>#include <PerfStat.h>#include <ui-plugins/PluginContainer.h>#include <Preferences.h>#include <SettingHandle.h> #include <QLoggingCategory> Q_DECLARE_LOGGING_CATEGORY(inputplugins)Q_LOGGING_CATEGORY(inputplugins, "hifi.inputplugins") static const unsigned int BUTTON_0 = 1U << 0; // the skinny button between 1 and 2static const unsigned int BUTTON_1 = 1U << 5;static const unsigned int BUTTON_2 = 1U << 6;static const unsigned int BUTTON_3 = 1U << 3;static const unsigned int BUTTON_4 = 1U << 4;static const unsigned int BUTTON_FWD = 1U << 7;static const unsigned int BUTTON_TRIGGER = 1U << 8; const glm::vec3 SixenseManager::DEFAULT_AVATAR_POSITION { -0.25f, -0.35f, -0.3f }; // in hydra frameconst float SixenseManager::CONTROLLER_THRESHOLD { 0.35f }; bool SixenseManager::_isEnabled = false;bool SixenseManager::_sixenseLoaded = false; #define BAIL_IF_NOT_ENABLED \ if (!_isEnabled) { \ return; \ } #define BAIL_IF_NOT_LOADED \ if (!_sixenseLoaded) { \ return; \ } const char* SixenseManager::NAME { "Sixense" };const char* SixenseManager::SIXENSE_ID_STRING { "Sixense" }; const bool DEFAULT_ENABLED = false; const char* MENU_PARENT{ "Developer" };const char* MENU_NAME { "Sixense" };const char* MENU_PATH { "Developer" ">" "Sixense" };const char* TOGGLE_SMOOTH { "Smooth Sixense Movement" };const char* SHOW_DEBUG_RAW { "Debug Draw Raw Data" };const char* SHOW_DEBUG_CALIBRATED { "Debug Draw Calibrated Data" }; bool SixenseManager::isSupported() const {#if defined(HAVE_SIXENSE) && !defined(Q_OS_OSX) return true;#else return false;#endif} void SixenseManager::init() { loadSettings(); auto preferences = DependencyManager::get<Preferences>(); static const QString SIXENSE_PLUGIN { "Sixense Controllers" }; { auto getter = [this]()->bool { return _isEnabled; }; auto setter = [this](bool value) { _isEnabled = value; saveSettings(); }; auto preference = new CheckPreference(SIXENSE_PLUGIN, "Enabled", getter, setter); preferences->addPreference(preference); }} bool SixenseManager::activate() { InputPlugin::activate(); #ifdef HAVE_SIXENSE #if !defined(Q_OS_LINUX) _container->addMenu(MENU_PATH); _container->addMenuItem(PluginType::INPUT_PLUGIN, MENU_PATH, TOGGLE_SMOOTH, [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); #endif auto userInputMapper = DependencyManager::get<controller::UserInputMapper>(); userInputMapper->registerDevice(_inputDevice); loadSettings(); _sixenseLoaded = (sixenseInit() == SIXENSE_SUCCESS); return _sixenseLoaded;#else return false;#endif} void SixenseManager::deactivate() { BAIL_IF_NOT_LOADED InputPlugin::deactivate(); #ifdef HAVE_SIXENSE #if !defined(Q_OS_LINUX) _container->removeMenuItem(MENU_NAME, TOGGLE_SMOOTH); _container->removeMenu(MENU_PATH); #endif _inputDevice->_poseStateMap.clear(); if (_inputDevice->_deviceID != controller::Input::INVALID_DEVICE) { auto userInputMapper = DependencyManager::get<controller::UserInputMapper>(); userInputMapper->removeDevice(_inputDevice->_deviceID); } sixenseExit(); saveSettings();#endif} void SixenseManager::setSixenseFilter(bool filter) { BAIL_IF_NOT_LOADED#ifdef HAVE_SIXENSE sixenseSetFilterEnabled(filter ? 1 : 0);#endif} void SixenseManager::pluginUpdate(float deltaTime, const controller::InputCalibrationData& inputCalibrationData) { BAIL_IF_NOT_ENABLED BAIL_IF_NOT_LOADED #ifdef HAVE_SIXENSE static bool sixenseHasBeenConnected { false }; if (!sixenseHasBeenConnected && sixenseIsBaseConnected(0)) { sixenseHasBeenConnected = true; emit deviceConnected(getName()); } auto userInputMapper = DependencyManager::get<controller::UserInputMapper>(); userInputMapper->withLock([&, this]() { _inputDevice->update(deltaTime, inputCalibrationData); }); if (_inputDevice->_requestReset) { _container->requestReset(); _inputDevice->_requestReset = false; }#endif} void SixenseManager::InputDevice::update(float deltaTime, const controller::InputCalibrationData& inputCalibrationData) { BAIL_IF_NOT_LOADED#ifdef HAVE_SIXENSE _buttonPressedMap.clear(); static const float MAX_DISCONNECTED_TIME = 2.0f; static bool disconnected { false }; static float disconnectedInterval { 0.0f }; if (sixenseGetNumActiveControllers() == 0) { if (!disconnected) { disconnectedInterval += deltaTime; } if (disconnectedInterval > MAX_DISCONNECTED_TIME) { disconnected = true; _axisStateMap.clear(); _buttonPressedMap.clear(); _poseStateMap.clear(); } return; } if (disconnected) { disconnected = 0; disconnectedInterval = 0.0f; } PerformanceTimer perfTimer("sixense"); // FIXME send this message once when we've positively identified hydra hardware //UserActivityLogger::getInstance().connectedDevice("spatial_controller", "hydra"); int maxControllers = sixenseGetMaxControllers(); // 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)) { continue; } SixenseControllerData* data = controllers + numActiveControllers; ++numActiveControllers; sixenseGetNewestData(i, data); // NOTE: Sixense API returns pos data in millimeters but we IMMEDIATELY convert to meters. glm::vec3 position(data->pos[0], data->pos[1], data->pos[2]); position *= METERS_PER_MILLIMETER; bool left = i == 0; using namespace controller; // Check to see if this hand/controller is on the base const float CONTROLLER_AT_BASE_DISTANCE = 0.075f; if (glm::length(position) >= CONTROLLER_AT_BASE_DISTANCE) { handleButtonEvent(data->buttons, left); _axisStateMap[left ? LX : RX] = data->joystick_x; _axisStateMap[left ? LY : RY] = data->joystick_y; _axisStateMap[left ? LT : RT] = data->trigger; // Rotation of Palm glm::quat rotation(data->rot_quat[3], data->rot_quat[0], data->rot_quat[1], data->rot_quat[2]); handlePoseEvent(deltaTime, inputCalibrationData, position, rotation, left); rawPoses[i] = controller::Pose(position, rotation, Vectors::ZERO, Vectors::ZERO); } else { auto hand = left ? controller::StandardPoseChannel::LEFT_HAND : controller::StandardPoseChannel::RIGHT_HAND; _poseStateMap[hand] = controller::Pose(); } } if (numActiveControllers == 2) { updateCalibration(controllers); } for (auto axisState : _axisStateMap) { if (fabsf(axisState.second) < CONTROLLER_THRESHOLD) { _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) { BAIL_IF_NOT_LOADED _debugDrawRaw = flag; if (!flag) { DebugDraw::getInstance().removeMyAvatarMarker("SIXENSE_RAW_LEFT"); DebugDraw::getInstance().removeMyAvatarMarker("SIXENSE_RAW_RIGHT"); }} void SixenseManager::InputDevice::setDebugDrawCalibrated(bool flag) { BAIL_IF_NOT_LOADED _debugDrawCalibrated = flag; if (!flag) { DebugDraw::getInstance().removeMyAvatarMarker("SIXENSE_CALIBRATED_LEFT"); DebugDraw::getInstance().removeMyAvatarMarker("SIXENSE_CALIBRATED_RIGHT"); }} #ifdef HAVE_SIXENSE // the calibration sequence is:// (1) reach arm straight out to the sides (xAxis is to the left)// (2) press either BUTTON_1 or BUTTON_2 on both hands and hold for one second// (3) release both buttons//// The code will:// (4) assume that the orb is on a flat surface (yAxis is UP)// (5) compute the forward direction (zAxis = xAxis cross yAxis) static const float MINIMUM_ARM_REACH = 0.3f; // metersstatic const float MAXIMUM_NOISE_LEVEL = 0.05f; // metersstatic const quint64 LOCK_DURATION = USECS_PER_SECOND / 4; // time for lock to be acquired static bool calibrationRequested(SixenseControllerData* controllers) { return (((controllers[0].buttons == BUTTON_1) || (controllers[0].buttons == BUTTON_2)) && ((controllers[1].buttons == BUTTON_1) || (controllers[1].buttons == BUTTON_2)));} void SixenseManager::InputDevice::updateCalibration(SixenseControllerData* controllers) { BAIL_IF_NOT_LOADED const SixenseControllerData* dataLeft = controllers; const SixenseControllerData* dataRight = controllers + 1; // Calibration buttons aren't set, so check the state, and request a reset if necessary. if (!calibrationRequested(controllers)) { switch (_calibrationState) { case CALIBRATION_STATE_IDLE: return; case CALIBRATION_STATE_COMPLETE: { // compute calibration results _avatarPosition = -0.5f * (_reachLeft + _reachRight); // neck is midway between right and left hands glm::vec3 xAxis = glm::normalize(_reachRight - _reachLeft); glm::vec3 zAxis = glm::normalize(glm::cross(xAxis, Vectors::UNIT_Y)); xAxis = glm::normalize(glm::cross(Vectors::UNIT_Y, zAxis)); _avatarRotation = glm::inverse(glm::quat_cast(glm::mat3(xAxis, Vectors::UNIT_Y, zAxis))); const float Y_OFFSET_CALIBRATED_HANDS_TO_AVATAR = -0.3f; _avatarPosition.y += Y_OFFSET_CALIBRATED_HANDS_TO_AVATAR; qCDebug(inputplugins, "succeess: sixense calibration"); _requestReset = true; } break; default: qCDebug(inputplugins, "failed: sixense calibration"); break; } _calibrationState = CALIBRATION_STATE_IDLE; return; } // Calibration buttons are set, continue calibration work // NOTE: Sixense API returns pos data in millimeters but we IMMEDIATELY convert to meters. const float* pos = dataLeft->pos; glm::vec3 positionLeft(pos[0], pos[1], pos[2]); positionLeft *= METERS_PER_MILLIMETER; pos = dataRight->pos; glm::vec3 positionRight(pos[0], pos[1], pos[2]); positionRight *= METERS_PER_MILLIMETER; // Gather initial calibration data if (_calibrationState == CALIBRATION_STATE_IDLE) { float reach = glm::distance(positionLeft, positionRight); if (reach > 2.0f * MINIMUM_ARM_REACH) { qCDebug(inputplugins, "started: sixense calibration"); _averageLeft = positionLeft; _averageRight = positionRight; _reachLeft = _averageLeft; _reachRight = _averageRight; _lastDistance = reach; _lockExpiry = usecTimestampNow() + LOCK_DURATION; // move to next state _calibrationState = CALIBRATION_STATE_IN_PROGRESS; } return; } quint64 now = usecTimestampNow() + LOCK_DURATION; // these are weighted running averages _averageLeft = 0.9f * _averageLeft + 0.1f * positionLeft; _averageRight = 0.9f * _averageRight + 0.1f * positionRight; if (_calibrationState == CALIBRATION_STATE_IN_PROGRESS) { // compute new sliding average float distance = glm::distance(_averageLeft, _averageRight); if (fabsf(distance - _lastDistance) > MAXIMUM_NOISE_LEVEL) { // distance is increasing so acquire the data and push the expiry out _reachLeft = _averageLeft; _reachRight = _averageRight; _lastDistance = distance; _lockExpiry = now + LOCK_DURATION; } else if (now > _lockExpiry) { // lock has expired so clamp the data and move on _lockExpiry = now + LOCK_DURATION; _lastDistance = 0.0f; _calibrationState = CALIBRATION_STATE_COMPLETE; qCDebug(inputplugins, "success: sixense calibration: left"); } }} #endif // HAVE_SIXENSE void SixenseManager::InputDevice::focusOutEvent() { BAIL_IF_NOT_LOADED _axisStateMap.clear(); _buttonPressedMap.clear();}; void SixenseManager::InputDevice::handleButtonEvent(unsigned int buttons, bool left) { BAIL_IF_NOT_LOADED using namespace controller; if (buttons & BUTTON_0) { _buttonPressedMap.insert(left ? BACK : START); } if (buttons & BUTTON_1) { _buttonPressedMap.insert(left ? DL : X); } if (buttons & BUTTON_2) { _buttonPressedMap.insert(left ? DD : A); } if (buttons & BUTTON_3) { _buttonPressedMap.insert(left ? DR : B); } if (buttons & BUTTON_4) { _buttonPressedMap.insert(left ? DU : Y); } if (buttons & BUTTON_FWD) { _buttonPressedMap.insert(left ? LB : RB); } if (buttons & BUTTON_TRIGGER) { _buttonPressedMap.insert(left ? LS : RS); }} void SixenseManager::InputDevice::handlePoseEvent(float deltaTime, const controller::InputCalibrationData& inputCalibrationData, const glm::vec3& position, const glm::quat& rotation, bool left) { BAIL_IF_NOT_LOADED#ifdef HAVE_SIXENSE auto hand = left ? controller::StandardPoseChannel::LEFT_HAND : controller::StandardPoseChannel::RIGHT_HAND; // From ABOVE the sixense coordinate frame looks like this: // // | // USB cables // | // .-. user // (Orb) --neckX---- forward // '-' | // | | user // neckZ y +---- right // | (o)-----x // | // | // z auto prevPose = _poseStateMap[hand]; // Transform the measured position into body frame. vec3 pos = _avatarRotation * (position + _avatarPosition); // From ABOVE the hand canonical axes look like this: // // | | | | y | | | | // | | | | | | | | | // | | | | | // |left | / x----(+) \ |right| // | _/ z \_ | // | | | | // | | | | // // To convert sixense's delta-rotation into the hand's frame we will have to transform it like so: // // deltaHand = Qsh^ * deltaSixense * Qsh // // where Qsh = transform from sixense axes to hand axes. By inspection we can determine Qsh: // // Qsh = angleAxis(PI, zAxis) * angleAxis(-PI/2, xAxis) // const glm::quat sixenseToHand = glm::angleAxis(PI, Vectors::UNIT_Z) * glm::angleAxis(-PI/2.0f, Vectors::UNIT_X); // In addition to Qsh each hand has pre-offset introduced by the shape of the sixense controllers // and how they fit into the hand in their relaxed state. This offset is a quarter turn about // the sixense's z-axis, with its direction different for the two hands: float sign = left ? 1.0f : -1.0f; const glm::quat preOffset = glm::angleAxis(sign * PI / 2.0f, Vectors::UNIT_Z); // Finally, there is a post-offset (same for both hands) to get the hand's rest orientation // (fingers forward, palm down) aligned properly in the avatar's model-frame,Lines 1–500 of 608. Load more
// // SixenseManager.cpp // input-plugins/src/input-plugins // // Created by Andrzej Kapolka on 11/15/13. // Copyright 2013 High Fidelity, Inc. // // Distributed under the Apache License, Version 2.0. // See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html // #include "SixenseManager.h" #ifdef HAVE_SIXENSE #include <sixense.h> #else #define SIXENSE_FAILURE -1 #define SIXENSE_SUCCESS 0 #endif #include <QCoreApplication> #include <QtCore/QSysInfo> #include <QtGlobal> #include <controllers/UserInputMapper.h> #include <GLMHelpers.h> #include <DebugDraw.h> #include <NumericalConstants.h> #include <PathUtils.h> #include <PerfStat.h> #include <ui-plugins/PluginContainer.h> #include <Preferences.h> #include <SettingHandle.h> #include <QLoggingCategory> Q_DECLARE_LOGGING_CATEGORY(inputplugins) Q_LOGGING_CATEGORY(inputplugins, "hifi.inputplugins") static const unsigned int BUTTON_0 = 1U << 0; // the skinny button between 1 and 2 static const unsigned int BUTTON_1 = 1U << 5; static const unsigned int BUTTON_2 = 1U << 6; static const unsigned int BUTTON_3 = 1U << 3; static const unsigned int BUTTON_4 = 1U << 4; static const unsigned int BUTTON_FWD = 1U << 7; static const unsigned int BUTTON_TRIGGER = 1U << 8; const glm::vec3 SixenseManager::DEFAULT_AVATAR_POSITION { -0.25f, -0.35f, -0.3f }; // in hydra frame const float SixenseManager::CONTROLLER_THRESHOLD { 0.35f }; bool SixenseManager::_isEnabled = false; bool SixenseManager::_sixenseLoaded = false; #define BAIL_IF_NOT_ENABLED \ if (!_isEnabled) { \ return; \ } #define BAIL_IF_NOT_LOADED \ if (!_sixenseLoaded) { \ return; \ } const char* SixenseManager::NAME { "Sixense" }; const char* SixenseManager::SIXENSE_ID_STRING { "Sixense" }; const bool DEFAULT_ENABLED = false; const char* MENU_PARENT{ "Developer" }; const char* MENU_NAME { "Sixense" }; const char* MENU_PATH { "Developer" ">" "Sixense" }; const char* TOGGLE_SMOOTH { "Smooth Sixense Movement" }; const char* SHOW_DEBUG_RAW { "Debug Draw Raw Data" }; const char* SHOW_DEBUG_CALIBRATED { "Debug Draw Calibrated Data" }; bool SixenseManager::isSupported() const { #if defined(HAVE_SIXENSE) && !defined(Q_OS_OSX) return true; #else return false; #endif } void SixenseManager::init() { loadSettings(); auto preferences = DependencyManager::get<Preferences>(); static const QString SIXENSE_PLUGIN { "Sixense Controllers" }; { auto getter = [this]()->bool { return _isEnabled; }; auto setter = [this](bool value) { _isEnabled = value; saveSettings(); }; auto preference = new CheckPreference(SIXENSE_PLUGIN, "Enabled", getter, setter); preferences->addPreference(preference); } } bool SixenseManager::activate() { InputPlugin::activate(); #ifdef HAVE_SIXENSE #if !defined(Q_OS_LINUX) _container->addMenu(MENU_PATH); _container->addMenuItem(PluginType::INPUT_PLUGIN, MENU_PATH, TOGGLE_SMOOTH, [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); #endif auto userInputMapper = DependencyManager::get<controller::UserInputMapper>(); userInputMapper->registerDevice(_inputDevice); loadSettings(); _sixenseLoaded = (sixenseInit() == SIXENSE_SUCCESS); return _sixenseLoaded; #else return false; #endif } void SixenseManager::deactivate() { BAIL_IF_NOT_LOADED InputPlugin::deactivate(); #ifdef HAVE_SIXENSE #if !defined(Q_OS_LINUX) _container->removeMenuItem(MENU_NAME, TOGGLE_SMOOTH); _container->removeMenu(MENU_PATH); #endif _inputDevice->_poseStateMap.clear(); if (_inputDevice->_deviceID != controller::Input::INVALID_DEVICE) { auto userInputMapper = DependencyManager::get<controller::UserInputMapper>(); userInputMapper->removeDevice(_inputDevice->_deviceID); } sixenseExit(); saveSettings(); #endif } void SixenseManager::setSixenseFilter(bool filter) { BAIL_IF_NOT_LOADED #ifdef HAVE_SIXENSE sixenseSetFilterEnabled(filter ? 1 : 0); #endif } void SixenseManager::pluginUpdate(float deltaTime, const controller::InputCalibrationData& inputCalibrationData) { BAIL_IF_NOT_ENABLED BAIL_IF_NOT_LOADED #ifdef HAVE_SIXENSE static bool sixenseHasBeenConnected { false }; if (!sixenseHasBeenConnected && sixenseIsBaseConnected(0)) { sixenseHasBeenConnected = true; emit deviceConnected(getName()); } auto userInputMapper = DependencyManager::get<controller::UserInputMapper>(); userInputMapper->withLock([&, this]() { _inputDevice->update(deltaTime, inputCalibrationData); }); if (_inputDevice->_requestReset) { _container->requestReset(); _inputDevice->_requestReset = false; } #endif } void SixenseManager::InputDevice::update(float deltaTime, const controller::InputCalibrationData& inputCalibrationData) { BAIL_IF_NOT_LOADED #ifdef HAVE_SIXENSE _buttonPressedMap.clear(); static const float MAX_DISCONNECTED_TIME = 2.0f; static bool disconnected { false }; static float disconnectedInterval { 0.0f }; if (sixenseGetNumActiveControllers() == 0) { if (!disconnected) { disconnectedInterval += deltaTime; } if (disconnectedInterval > MAX_DISCONNECTED_TIME) { disconnected = true; _axisStateMap.clear(); _buttonPressedMap.clear(); _poseStateMap.clear(); } return; } if (disconnected) { disconnected = 0; disconnectedInterval = 0.0f; } PerformanceTimer perfTimer("sixense"); // FIXME send this message once when we've positively identified hydra hardware //UserActivityLogger::getInstance().connectedDevice("spatial_controller", "hydra"); int maxControllers = sixenseGetMaxControllers(); // 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)) { continue; } SixenseControllerData* data = controllers + numActiveControllers; ++numActiveControllers; sixenseGetNewestData(i, data); // NOTE: Sixense API returns pos data in millimeters but we IMMEDIATELY convert to meters. glm::vec3 position(data->pos[0], data->pos[1], data->pos[2]); position *= METERS_PER_MILLIMETER; bool left = i == 0; using namespace controller; // Check to see if this hand/controller is on the base const float CONTROLLER_AT_BASE_DISTANCE = 0.075f; if (glm::length(position) >= CONTROLLER_AT_BASE_DISTANCE) { handleButtonEvent(data->buttons, left); _axisStateMap[left ? LX : RX] = data->joystick_x; _axisStateMap[left ? LY : RY] = data->joystick_y; _axisStateMap[left ? LT : RT] = data->trigger; // Rotation of Palm glm::quat rotation(data->rot_quat[3], data->rot_quat[0], data->rot_quat[1], data->rot_quat[2]); handlePoseEvent(deltaTime, inputCalibrationData, position, rotation, left); rawPoses[i] = controller::Pose(position, rotation, Vectors::ZERO, Vectors::ZERO); } else { auto hand = left ? controller::StandardPoseChannel::LEFT_HAND : controller::StandardPoseChannel::RIGHT_HAND; _poseStateMap[hand] = controller::Pose(); } } if (numActiveControllers == 2) { updateCalibration(controllers); } for (auto axisState : _axisStateMap) { if (fabsf(axisState.second) < CONTROLLER_THRESHOLD) { _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) { BAIL_IF_NOT_LOADED _debugDrawRaw = flag; if (!flag) { DebugDraw::getInstance().removeMyAvatarMarker("SIXENSE_RAW_LEFT"); DebugDraw::getInstance().removeMyAvatarMarker("SIXENSE_RAW_RIGHT"); } } void SixenseManager::InputDevice::setDebugDrawCalibrated(bool flag) { BAIL_IF_NOT_LOADED _debugDrawCalibrated = flag; if (!flag) { DebugDraw::getInstance().removeMyAvatarMarker("SIXENSE_CALIBRATED_LEFT"); DebugDraw::getInstance().removeMyAvatarMarker("SIXENSE_CALIBRATED_RIGHT"); } } #ifdef HAVE_SIXENSE // the calibration sequence is: // (1) reach arm straight out to the sides (xAxis is to the left) // (2) press either BUTTON_1 or BUTTON_2 on both hands and hold for one second // (3) release both buttons // // The code will: // (4) assume that the orb is on a flat surface (yAxis is UP) // (5) compute the forward direction (zAxis = xAxis cross yAxis) static const float MINIMUM_ARM_REACH = 0.3f; // meters static const float MAXIMUM_NOISE_LEVEL = 0.05f; // meters static const quint64 LOCK_DURATION = USECS_PER_SECOND / 4; // time for lock to be acquired static bool calibrationRequested(SixenseControllerData* controllers) { return (((controllers[0].buttons == BUTTON_1) || (controllers[0].buttons == BUTTON_2)) && ((controllers[1].buttons == BUTTON_1) || (controllers[1].buttons == BUTTON_2))); } void SixenseManager::InputDevice::updateCalibration(SixenseControllerData* controllers) { BAIL_IF_NOT_LOADED const SixenseControllerData* dataLeft = controllers; const SixenseControllerData* dataRight = controllers + 1; // Calibration buttons aren't set, so check the state, and request a reset if necessary. if (!calibrationRequested(controllers)) { switch (_calibrationState) { case CALIBRATION_STATE_IDLE: return; case CALIBRATION_STATE_COMPLETE: { // compute calibration results _avatarPosition = -0.5f * (_reachLeft + _reachRight); // neck is midway between right and left hands glm::vec3 xAxis = glm::normalize(_reachRight - _reachLeft); glm::vec3 zAxis = glm::normalize(glm::cross(xAxis, Vectors::UNIT_Y)); xAxis = glm::normalize(glm::cross(Vectors::UNIT_Y, zAxis)); _avatarRotation = glm::inverse(glm::quat_cast(glm::mat3(xAxis, Vectors::UNIT_Y, zAxis))); const float Y_OFFSET_CALIBRATED_HANDS_TO_AVATAR = -0.3f; _avatarPosition.y += Y_OFFSET_CALIBRATED_HANDS_TO_AVATAR; qCDebug(inputplugins, "succeess: sixense calibration"); _requestReset = true; } break; default: qCDebug(inputplugins, "failed: sixense calibration"); break; } _calibrationState = CALIBRATION_STATE_IDLE; return; } // Calibration buttons are set, continue calibration work // NOTE: Sixense API returns pos data in millimeters but we IMMEDIATELY convert to meters. const float* pos = dataLeft->pos; glm::vec3 positionLeft(pos[0], pos[1], pos[2]); positionLeft *= METERS_PER_MILLIMETER; pos = dataRight->pos; glm::vec3 positionRight(pos[0], pos[1], pos[2]); positionRight *= METERS_PER_MILLIMETER; // Gather initial calibration data if (_calibrationState == CALIBRATION_STATE_IDLE) { float reach = glm::distance(positionLeft, positionRight); if (reach > 2.0f * MINIMUM_ARM_REACH) { qCDebug(inputplugins, "started: sixense calibration"); _averageLeft = positionLeft; _averageRight = positionRight; _reachLeft = _averageLeft; _reachRight = _averageRight; _lastDistance = reach; _lockExpiry = usecTimestampNow() + LOCK_DURATION; // move to next state _calibrationState = CALIBRATION_STATE_IN_PROGRESS; } return; } quint64 now = usecTimestampNow() + LOCK_DURATION; // these are weighted running averages _averageLeft = 0.9f * _averageLeft + 0.1f * positionLeft; _averageRight = 0.9f * _averageRight + 0.1f * positionRight; if (_calibrationState == CALIBRATION_STATE_IN_PROGRESS) { // compute new sliding average float distance = glm::distance(_averageLeft, _averageRight); if (fabsf(distance - _lastDistance) > MAXIMUM_NOISE_LEVEL) { // distance is increasing so acquire the data and push the expiry out _reachLeft = _averageLeft; _reachRight = _averageRight; _lastDistance = distance; _lockExpiry = now + LOCK_DURATION; } else if (now > _lockExpiry) { // lock has expired so clamp the data and move on _lockExpiry = now + LOCK_DURATION; _lastDistance = 0.0f; _calibrationState = CALIBRATION_STATE_COMPLETE; qCDebug(inputplugins, "success: sixense calibration: left"); } } } #endif // HAVE_SIXENSE void SixenseManager::InputDevice::focusOutEvent() { BAIL_IF_NOT_LOADED _axisStateMap.clear(); _buttonPressedMap.clear(); }; void SixenseManager::InputDevice::handleButtonEvent(unsigned int buttons, bool left) { BAIL_IF_NOT_LOADED using namespace controller; if (buttons & BUTTON_0) { _buttonPressedMap.insert(left ? BACK : START); } if (buttons & BUTTON_1) { _buttonPressedMap.insert(left ? DL : X); } if (buttons & BUTTON_2) { _buttonPressedMap.insert(left ? DD : A); } if (buttons & BUTTON_3) { _buttonPressedMap.insert(left ? DR : B); } if (buttons & BUTTON_4) { _buttonPressedMap.insert(left ? DU : Y); } if (buttons & BUTTON_FWD) { _buttonPressedMap.insert(left ? LB : RB); } if (buttons & BUTTON_TRIGGER) { _buttonPressedMap.insert(left ? LS : RS); } } void SixenseManager::InputDevice::handlePoseEvent(float deltaTime, const controller::InputCalibrationData& inputCalibrationData, const glm::vec3& position, const glm::quat& rotation, bool left) { BAIL_IF_NOT_LOADED #ifdef HAVE_SIXENSE auto hand = left ? controller::StandardPoseChannel::LEFT_HAND : controller::StandardPoseChannel::RIGHT_HAND; // From ABOVE the sixense coordinate frame looks like this: // // | // USB cables // | // .-. user // (Orb) --neckX---- forward // '-' | // | | user // neckZ y +---- right // | (o)-----x // | // | // z auto prevPose = _poseStateMap[hand]; // Transform the measured position into body frame. vec3 pos = _avatarRotation * (position + _avatarPosition); // From ABOVE the hand canonical axes look like this: // // | | | | y | | | | // | | | | | | | | | // | | | | | // |left | / x----(+) \ |right| // | _/ z \_ | // | | | | // | | | | // // To convert sixense's delta-rotation into the hand's frame we will have to transform it like so: // // deltaHand = Qsh^ * deltaSixense * Qsh // // where Qsh = transform from sixense axes to hand axes. By inspection we can determine Qsh: // // Qsh = angleAxis(PI, zAxis) * angleAxis(-PI/2, xAxis) // const glm::quat sixenseToHand = glm::angleAxis(PI, Vectors::UNIT_Z) * glm::angleAxis(-PI/2.0f, Vectors::UNIT_X); // In addition to Qsh each hand has pre-offset introduced by the shape of the sixense controllers // and how they fit into the hand in their relaxed state. This offset is a quarter turn about // the sixense's z-axis, with its direction different for the two hands: float sign = left ? 1.0f : -1.0f; const glm::quat preOffset = glm::angleAxis(sign * PI / 2.0f, Vectors::UNIT_Z); // Finally, there is a post-offset (same for both hands) to get the hand's rest orientation // (fingers forward, palm down) aligned properly in the avatar's model-frame,