From 111ed65bf88a71ff12f298fae0ce1e7b9fb72421 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Thu, 11 Feb 2016 17:18:01 -0800 Subject: [PATCH] Controller Pose values are relative to Avatar. Pass a InputCalibrationData to each inputPlugin and inputDevice. This contains the most up sensorToWorldMatrix, avatarMat and hmdSensorMatrix. Each input plugin can use this data to transform it's poses into Avatar space before sending it up the chain. This fixes a bug in the handControllerGrab.js script that relied on the hand controller rotation/positions being in the avatar frame. --- interface/src/Application.cpp | 20 ++++---- interface/src/avatar/MyAvatar.cpp | 4 +- .../controllers/src/controllers/Actions.cpp | 2 +- .../controllers/src/controllers/Actions.h | 2 +- libraries/controllers/src/controllers/Input.h | 6 +++ .../controllers/src/controllers/InputDevice.h | 6 +-- .../controllers/src/controllers/Pose.cpp | 9 ++++ libraries/controllers/src/controllers/Pose.h | 2 + .../src/controllers/StandardController.cpp | 2 +- .../src/controllers/StandardController.h | 2 +- .../src/controllers/StateController.cpp | 2 +- .../src/controllers/StateController.h | 2 +- .../src/controllers/UserInputMapper.h | 5 -- .../src/input-plugins/KeyboardMouseDevice.cpp | 6 +-- .../src/input-plugins/KeyboardMouseDevice.h | 4 +- .../src/input-plugins/SpacemouseManager.cpp | 2 +- .../src/input-plugins/SpacemouseManager.h | 2 +- libraries/plugins/src/plugins/InputPlugin.h | 6 ++- libraries/shared/src/GLMHelpers.cpp | 11 ++++- libraries/shared/src/GLMHelpers.h | 3 +- plugins/hifiNeuron/src/NeuronPlugin.cpp | 6 +-- plugins/hifiNeuron/src/NeuronPlugin.h | 6 +-- plugins/hifiSdl2/src/Joystick.cpp | 2 +- plugins/hifiSdl2/src/Joystick.h | 2 +- plugins/hifiSdl2/src/SDL2Manager.cpp | 4 +- plugins/hifiSdl2/src/SDL2Manager.h | 2 +- plugins/hifiSixense/src/SixenseManager.cpp | 23 +++++---- plugins/hifiSixense/src/SixenseManager.h | 6 +-- plugins/openvr/src/ViveControllerManager.cpp | 49 ++++++++++--------- plugins/openvr/src/ViveControllerManager.h | 6 +-- tests/controllers/src/main.cpp | 16 +++++- 31 files changed, 131 insertions(+), 89 deletions(-) diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index c0416e75da..ded13600be 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -3070,13 +3070,18 @@ void Application::update(float deltaTime) { auto myAvatar = getMyAvatar(); auto userInputMapper = DependencyManager::get(); - userInputMapper->setSensorToWorldMat(myAvatar->getSensorToWorldMatrix()); userInputMapper->update(deltaTime); + controller::InputCalibrationData calibrationData = { + myAvatar->getSensorToWorldMatrix(), + createMatFromQuatAndPos(myAvatar->getOrientation(), myAvatar->getPosition()), + myAvatar->getHMDSensorMatrix() + }; + bool jointsCaptured = false; for (auto inputPlugin : PluginManager::getInstance()->getInputPlugins()) { if (inputPlugin->isActive()) { - inputPlugin->pluginUpdate(deltaTime, jointsCaptured); + inputPlugin->pluginUpdate(deltaTime, calibrationData, jointsCaptured); if (inputPlugin->isJointController()) { jointsCaptured = true; } @@ -4981,14 +4986,9 @@ void Application::setPalmData(Hand* hand, const controller::Pose& pose, float de auto myAvatar = DependencyManager::get()->getMyAvatar(); palm.setActive(pose.isValid()); - // transform from sensor space, to world space, to avatar model space. - glm::mat4 poseMat = createMatFromQuatAndPos(pose.getRotation(), pose.getTranslation() * myAvatar->getScale()); - glm::mat4 sensorToWorldMat = myAvatar->getSensorToWorldMatrix(); - glm::mat4 modelMat = createMatFromQuatAndPos(myAvatar->getOrientation(), myAvatar->getPosition()); - glm::mat4 objectPose = glm::inverse(modelMat) * sensorToWorldMat * poseMat; - - glm::vec3 position = extractTranslation(objectPose); - glm::quat rotation = glm::quat_cast(objectPose); + // controller pose is in Avatar frame. + glm::vec3 position = pose.getTranslation(); + glm::quat rotation = pose.getRotation(); // Compute current velocity from position change glm::vec3 rawVelocity; diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index d86af92a55..0f96f67a7d 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -1392,7 +1392,7 @@ void MyAvatar::updateOrientation(float deltaTime) { desiredFacing.y = 0.0f; // This is our reference frame, it is captured when the user begins to move. - glm::vec3 referenceFacing = transformVector(_sensorToWorldMatrix, _hoverReferenceCameraFacing); + glm::vec3 referenceFacing = transformVectorFast(_sensorToWorldMatrix, _hoverReferenceCameraFacing); referenceFacing.y = 0.0f; referenceFacing = glm::normalize(referenceFacing); glm::vec3 referenceRight(referenceFacing.z, 0.0f, -referenceFacing.x); @@ -1597,7 +1597,7 @@ void MyAvatar::updatePosition(float deltaTime) { if (!_hoverReferenceCameraFacingIsCaptured && (fabs(_driveKeys[TRANSLATE_Z]) > 0.1f || fabs(_driveKeys[TRANSLATE_X]) > 0.1f)) { _hoverReferenceCameraFacingIsCaptured = true; // transform the camera facing vector into sensor space. - _hoverReferenceCameraFacing = transformVector(glm::inverse(_sensorToWorldMatrix), getHead()->getCameraOrientation() * Vectors::UNIT_Z); + _hoverReferenceCameraFacing = transformVectorFast(glm::inverse(_sensorToWorldMatrix), getHead()->getCameraOrientation() * Vectors::UNIT_Z); } else if (_hoverReferenceCameraFacingIsCaptured && (fabs(_driveKeys[TRANSLATE_Z]) <= 0.1f && fabs(_driveKeys[TRANSLATE_X]) <= 0.1f)) { _hoverReferenceCameraFacingIsCaptured = false; } diff --git a/libraries/controllers/src/controllers/Actions.cpp b/libraries/controllers/src/controllers/Actions.cpp index 7bee5101b1..e6662caa37 100644 --- a/libraries/controllers/src/controllers/Actions.cpp +++ b/libraries/controllers/src/controllers/Actions.cpp @@ -120,7 +120,7 @@ namespace controller { return availableInputs; } - void ActionsDevice::update(float deltaTime, bool jointsCaptured) { + void ActionsDevice::update(float deltaTime, const InputCalibrationData& inpuCalibrationData, bool jointsCaptured) { } void ActionsDevice::focusOutEvent() { diff --git a/libraries/controllers/src/controllers/Actions.h b/libraries/controllers/src/controllers/Actions.h index 812d3b8df3..e77e5e5a93 100644 --- a/libraries/controllers/src/controllers/Actions.h +++ b/libraries/controllers/src/controllers/Actions.h @@ -110,7 +110,7 @@ class ActionsDevice : public QObject, public InputDevice { public: virtual EndpointPointer createEndpoint(const Input& input) const override; virtual Input::NamedVector getAvailableInputs() const override; - virtual void update(float deltaTime, bool jointsCaptured) override; + virtual void update(float deltaTime, const InputCalibrationData& inputCalibrationData, bool jointsCaptured) override; virtual void focusOutEvent() override; ActionsDevice(); diff --git a/libraries/controllers/src/controllers/Input.h b/libraries/controllers/src/controllers/Input.h index db99e820da..9c7f09d526 100644 --- a/libraries/controllers/src/controllers/Input.h +++ b/libraries/controllers/src/controllers/Input.h @@ -15,6 +15,12 @@ namespace controller { +struct InputCalibrationData { + glm::mat4 sensorToWorldMat; + glm::mat4 avatarMat; + glm::mat4 hmdSensorMat; +}; + enum class ChannelType { UNKNOWN = 0, BUTTON, diff --git a/libraries/controllers/src/controllers/InputDevice.h b/libraries/controllers/src/controllers/InputDevice.h index 3add7d236f..93247965bc 100644 --- a/libraries/controllers/src/controllers/InputDevice.h +++ b/libraries/controllers/src/controllers/InputDevice.h @@ -52,12 +52,12 @@ public: float getValue(const Input& input) const; float getValue(ChannelType channelType, uint16_t channel) const; Pose getPoseValue(uint16_t channel) const; - + const QString& getName() const { return _name; } // Update call MUST be called once per simulation loop // It takes care of updating the action states and deltas - virtual void update(float deltaTime, bool jointsCaptured) = 0; + virtual void update(float deltaTime, const InputCalibrationData& inputCalibrationData, bool jointsCaptured) = 0; virtual void focusOutEvent() = 0; @@ -101,4 +101,4 @@ private: static float _reticleMoveSpeed; }; -} \ No newline at end of file +} diff --git a/libraries/controllers/src/controllers/Pose.cpp b/libraries/controllers/src/controllers/Pose.cpp index d9641618c1..b86391bbba 100644 --- a/libraries/controllers/src/controllers/Pose.cpp +++ b/libraries/controllers/src/controllers/Pose.cpp @@ -60,5 +60,14 @@ namespace controller { } } + Pose Pose::transform(const glm::mat4& mat) const { + auto rot = glmExtractRotation(mat); + Pose pose(transformPoint(mat, translation), + rot * rotation, + transformVectorFast(mat, velocity), + rot * angularVelocity); + pose.valid = valid; + return pose; + } } diff --git a/libraries/controllers/src/controllers/Pose.h b/libraries/controllers/src/controllers/Pose.h index a8a4452758..47ba59279a 100644 --- a/libraries/controllers/src/controllers/Pose.h +++ b/libraries/controllers/src/controllers/Pose.h @@ -40,6 +40,8 @@ namespace controller { vec3 getVelocity() const { return velocity; } vec3 getAngularVelocity() const { return angularVelocity; } + Pose transform(const glm::mat4& mat) const; + static QScriptValue toScriptValue(QScriptEngine* engine, const Pose& event); static void fromScriptValue(const QScriptValue& object, Pose& event); }; diff --git a/libraries/controllers/src/controllers/StandardController.cpp b/libraries/controllers/src/controllers/StandardController.cpp index e101c5f4ff..5996cad5df 100644 --- a/libraries/controllers/src/controllers/StandardController.cpp +++ b/libraries/controllers/src/controllers/StandardController.cpp @@ -25,7 +25,7 @@ StandardController::StandardController() : InputDevice("Standard") { StandardController::~StandardController() { } -void StandardController::update(float deltaTime, bool jointsCaptured) { +void StandardController::update(float deltaTime, const controller::InputCalibrationData& inputCalibrationData, bool jointsCaptured) { } void StandardController::focusOutEvent() { diff --git a/libraries/controllers/src/controllers/StandardController.h b/libraries/controllers/src/controllers/StandardController.h index 57bd0faba5..fee608f822 100644 --- a/libraries/controllers/src/controllers/StandardController.h +++ b/libraries/controllers/src/controllers/StandardController.h @@ -28,7 +28,7 @@ public: virtual EndpointPointer createEndpoint(const Input& input) const override; virtual Input::NamedVector getAvailableInputs() const override; virtual QStringList getDefaultMappingConfigs() const override; - virtual void update(float deltaTime, bool jointsCaptured) override; + virtual void update(float deltaTime, const controller::InputCalibrationData& inputCalibrationData, bool jointsCaptured) override; virtual void focusOutEvent() override; StandardController(); diff --git a/libraries/controllers/src/controllers/StateController.cpp b/libraries/controllers/src/controllers/StateController.cpp index 6f89c6365c..1d39449e4d 100644 --- a/libraries/controllers/src/controllers/StateController.cpp +++ b/libraries/controllers/src/controllers/StateController.cpp @@ -25,7 +25,7 @@ StateController::StateController() : InputDevice("Application") { StateController::~StateController() { } -void StateController::update(float deltaTime, bool jointsCaptured) {} +void StateController::update(float deltaTime, const InputCalibrationData& inputCalibrationData, bool jointsCaptured) {} void StateController::focusOutEvent() {} diff --git a/libraries/controllers/src/controllers/StateController.h b/libraries/controllers/src/controllers/StateController.h index 12f3e8b2f1..ed36e4f838 100644 --- a/libraries/controllers/src/controllers/StateController.h +++ b/libraries/controllers/src/controllers/StateController.h @@ -28,7 +28,7 @@ public: // Device functions virtual Input::NamedVector getAvailableInputs() const override; - virtual void update(float deltaTime, bool jointsCaptured) override; + virtual void update(float deltaTime, const InputCalibrationData& inputCalibrationData, bool jointsCaptured) override; virtual void focusOutEvent() override; StateController(); diff --git a/libraries/controllers/src/controllers/UserInputMapper.h b/libraries/controllers/src/controllers/UserInputMapper.h index 98a85a2a44..1021032b40 100644 --- a/libraries/controllers/src/controllers/UserInputMapper.h +++ b/libraries/controllers/src/controllers/UserInputMapper.h @@ -97,9 +97,6 @@ namespace controller { // Update means go grab all the device input channels and update the output channel values void update(float deltaTime); - void setSensorToWorldMat(glm::mat4 sensorToWorldMat) { _sensorToWorldMat = sensorToWorldMat; } - glm::mat4 getSensorToWorldMat() { return _sensorToWorldMat; } - const DevicesMap& getDevices() { return _registeredDevices; } uint16 getStandardDeviceID() const { return STANDARD_DEVICE; } InputDevice::Pointer getStandardDevice() { return _registeredDevices[getStandardDeviceID()]; } @@ -131,8 +128,6 @@ namespace controller { std::vector _poseStates = std::vector(toInt(Action::NUM_ACTIONS)); std::vector _lastStandardStates = std::vector(); - glm::mat4 _sensorToWorldMat; - int recordDeviceOfType(const QString& deviceName); QHash _deviceCounts; diff --git a/libraries/input-plugins/src/input-plugins/KeyboardMouseDevice.cpp b/libraries/input-plugins/src/input-plugins/KeyboardMouseDevice.cpp index 1d7fbbec67..c3d2f7c51c 100755 --- a/libraries/input-plugins/src/input-plugins/KeyboardMouseDevice.cpp +++ b/libraries/input-plugins/src/input-plugins/KeyboardMouseDevice.cpp @@ -20,8 +20,8 @@ const QString KeyboardMouseDevice::NAME = "Keyboard/Mouse"; -void KeyboardMouseDevice::pluginUpdate(float deltaTime, bool jointsCaptured) { - _inputDevice->update(deltaTime, jointsCaptured); +void KeyboardMouseDevice::pluginUpdate(float deltaTime, const controller::InputCalibrationData& inputCalibrationData, bool jointsCaptured) { + _inputDevice->update(deltaTime, inputCalibrationData, jointsCaptured); // For touch event, we need to check that the last event is not too long ago // Maybe it's a Qt issue, but the touch event sequence (begin, update, end) is not always called properly @@ -36,7 +36,7 @@ void KeyboardMouseDevice::pluginUpdate(float deltaTime, bool jointsCaptured) { } } -void KeyboardMouseDevice::InputDevice::update(float deltaTime, bool jointsCaptured) { +void KeyboardMouseDevice::InputDevice::update(float deltaTime, const controller::InputCalibrationData& inputCalibrationData, bool jointsCaptured) { _axisStateMap.clear(); } diff --git a/libraries/input-plugins/src/input-plugins/KeyboardMouseDevice.h b/libraries/input-plugins/src/input-plugins/KeyboardMouseDevice.h index 6acb2bf041..55ca9a1704 100644 --- a/libraries/input-plugins/src/input-plugins/KeyboardMouseDevice.h +++ b/libraries/input-plugins/src/input-plugins/KeyboardMouseDevice.h @@ -70,7 +70,7 @@ public: virtual const QString& getName() const override { return NAME; } virtual void pluginFocusOutEvent() override { _inputDevice->focusOutEvent(); } - virtual void pluginUpdate(float deltaTime, bool jointsCaptured) override; + virtual void pluginUpdate(float deltaTime, const controller::InputCalibrationData& inputCalibrationData, bool jointsCaptured) override; void keyPressEvent(QKeyEvent* event); void keyReleaseEvent(QKeyEvent* event); @@ -97,7 +97,7 @@ protected: // Device functions virtual controller::Input::NamedVector getAvailableInputs() const override; virtual QString getDefaultMappingConfig() const override; - virtual void update(float deltaTime, bool jointsCaptured) override; + virtual void update(float deltaTime, const controller::InputCalibrationData& inputCalibrationData, bool jointsCaptured) override; virtual void focusOutEvent() override; // Let's make it easy for Qt because we assume we love Qt forever diff --git a/libraries/input-plugins/src/input-plugins/SpacemouseManager.cpp b/libraries/input-plugins/src/input-plugins/SpacemouseManager.cpp index fe90470cb4..d946990319 100644 --- a/libraries/input-plugins/src/input-plugins/SpacemouseManager.cpp +++ b/libraries/input-plugins/src/input-plugins/SpacemouseManager.cpp @@ -111,7 +111,7 @@ controller::Input::NamedPair SpacemouseDevice::makePair(SpacemouseDevice::Positi return controller::Input::NamedPair(makeInput(axis), name); } -void SpacemouseDevice::update(float deltaTime, bool jointsCaptured) { +void SpacemouseDevice::update(float deltaTime, const controller::InputCalibrationData& inputCalibrationData, bool jointsCaptured) { // the update is done in the SpacemouseManager class. // for windows in the nativeEventFilter the inputmapper is connected or registed or removed when an 3Dconnnexion device is attached or detached // for osx the api will call DeviceAddedHandler or DeviceRemoveHandler when a 3Dconnexion device is attached or detached diff --git a/libraries/input-plugins/src/input-plugins/SpacemouseManager.h b/libraries/input-plugins/src/input-plugins/SpacemouseManager.h index 6253fa7f9d..82c4fa8fb6 100644 --- a/libraries/input-plugins/src/input-plugins/SpacemouseManager.h +++ b/libraries/input-plugins/src/input-plugins/SpacemouseManager.h @@ -214,7 +214,7 @@ public: virtual controller::Input::NamedVector getAvailableInputs() const override; virtual QString getDefaultMappingConfig() const override; - virtual void update(float deltaTime, bool jointsCaptured) override; + virtual void update(float deltaTime, const controller::InputCalibrationData& inputCalibrationData, bool jointsCaptured) override; virtual void focusOutEvent() override; glm::vec3 cc_position; diff --git a/libraries/plugins/src/plugins/InputPlugin.h b/libraries/plugins/src/plugins/InputPlugin.h index e9d8ac8d86..b45fa862c1 100644 --- a/libraries/plugins/src/plugins/InputPlugin.h +++ b/libraries/plugins/src/plugins/InputPlugin.h @@ -12,12 +12,16 @@ #include "Plugin.h" +namespace controller { + struct InputCalibrationData; +} + class InputPlugin : public Plugin { public: virtual bool isJointController() const = 0; virtual void pluginFocusOutEvent() = 0; - virtual void pluginUpdate(float deltaTime, bool jointsCaptured) = 0; + virtual void pluginUpdate(float deltaTime, const controller::InputCalibrationData& inputCalibrationData, bool jointsCaptured) = 0; }; diff --git a/libraries/shared/src/GLMHelpers.cpp b/libraries/shared/src/GLMHelpers.cpp index 7b0f397b07..e9d6591cac 100644 --- a/libraries/shared/src/GLMHelpers.cpp +++ b/libraries/shared/src/GLMHelpers.cpp @@ -407,7 +407,14 @@ glm::vec3 transformPoint(const glm::mat4& m, const glm::vec3& p) { return glm::vec3(temp.x / temp.w, temp.y / temp.w, temp.z / temp.w); } -glm::vec3 transformVector(const glm::mat4& m, const glm::vec3& v) { +// does not handle non-uniform scale correctly, but it's faster then transformVectorFull +glm::vec3 transformVectorFast(const glm::mat4& m, const glm::vec3& v) { + glm::mat3 rot(m); + return rot * v; +} + +// handles non-uniform scale. +glm::vec3 transformVectorFull(const glm::mat4& m, const glm::vec3& v) { glm::mat3 rot(m); return glm::inverse(glm::transpose(rot)) * v; } @@ -437,7 +444,7 @@ glm::vec2 getFacingDir2D(const glm::quat& rot) { } glm::vec2 getFacingDir2D(const glm::mat4& m) { - glm::vec3 facing3D = transformVector(m, Vectors::UNIT_NEG_Z); + glm::vec3 facing3D = transformVectorFast(m, Vectors::UNIT_NEG_Z); glm::vec2 facing2D(facing3D.x, facing3D.z); const float ALMOST_ZERO = 0.0001f; if (glm::length(facing2D) < ALMOST_ZERO) { diff --git a/libraries/shared/src/GLMHelpers.h b/libraries/shared/src/GLMHelpers.h index 89b924051f..15f3f60cf7 100644 --- a/libraries/shared/src/GLMHelpers.h +++ b/libraries/shared/src/GLMHelpers.h @@ -215,7 +215,8 @@ glm::mat4 createMatFromQuatAndPos(const glm::quat& q, const glm::vec3& p); glm::quat cancelOutRollAndPitch(const glm::quat& q); glm::mat4 cancelOutRollAndPitch(const glm::mat4& m); glm::vec3 transformPoint(const glm::mat4& m, const glm::vec3& p); -glm::vec3 transformVector(const glm::mat4& m, const glm::vec3& v); +glm::vec3 transformVectorFast(const glm::mat4& m, const glm::vec3& v); +glm::vec3 transformVectorFull(const glm::mat4& m, const glm::vec3& v); // Calculate an orthogonal basis from a primary and secondary axis. // The uAxis, vAxis & wAxis will form an orthognal basis. diff --git a/plugins/hifiNeuron/src/NeuronPlugin.cpp b/plugins/hifiNeuron/src/NeuronPlugin.cpp index a175ce8e06..e2143dbdcf 100644 --- a/plugins/hifiNeuron/src/NeuronPlugin.cpp +++ b/plugins/hifiNeuron/src/NeuronPlugin.cpp @@ -498,14 +498,14 @@ void NeuronPlugin::deactivate() { #endif } -void NeuronPlugin::pluginUpdate(float deltaTime, bool jointsCaptured) { +void NeuronPlugin::pluginUpdate(float deltaTime, const controller::InputCalibrationData& inputCalibrationData, bool jointsCaptured) { std::vector joints; { // lock and copy std::lock_guard guard(_jointsMutex); joints = _joints; } - _inputDevice->update(deltaTime, joints, _prevJoints); + _inputDevice->update(deltaTime, inputCalibrationData, joints, _prevJoints); _prevJoints = joints; } @@ -537,7 +537,7 @@ QString NeuronPlugin::InputDevice::getDefaultMappingConfig() const { return MAPPING_JSON; } -void NeuronPlugin::InputDevice::update(float deltaTime, const std::vector& joints, const std::vector& prevJoints) { +void NeuronPlugin::InputDevice::update(float deltaTime, const controller::InputCalibrationData& inputCalibrationData, const std::vector& joints, const std::vector& prevJoints) { for (size_t i = 0; i < joints.size(); i++) { glm::vec3 linearVel, angularVel; glm::vec3 pos = joints[i].pos; diff --git a/plugins/hifiNeuron/src/NeuronPlugin.h b/plugins/hifiNeuron/src/NeuronPlugin.h index 33c6879f5b..afe8530b07 100644 --- a/plugins/hifiNeuron/src/NeuronPlugin.h +++ b/plugins/hifiNeuron/src/NeuronPlugin.h @@ -35,7 +35,7 @@ public: virtual void deactivate() override; virtual void pluginFocusOutEvent() override { _inputDevice->focusOutEvent(); } - virtual void pluginUpdate(float deltaTime, bool jointsCaptured) override; + virtual void pluginUpdate(float deltaTime, const controller::InputCalibrationData& inputCalibrationData, bool jointsCaptured) override; virtual void saveSettings() const override; virtual void loadSettings() override; @@ -56,10 +56,10 @@ protected: // Device functions virtual controller::Input::NamedVector getAvailableInputs() const override; virtual QString getDefaultMappingConfig() const override; - virtual void update(float deltaTime, bool jointsCaptured) override {}; + virtual void update(float deltaTime, const controller::InputCalibrationData& inputCalibrationData, bool jointsCaptured) override {}; virtual void focusOutEvent() override {}; - void update(float deltaTime, const std::vector& joints, const std::vector& prevJoints); + void update(float deltaTime, const controller::InputCalibrationData& inputCalibrationData, const std::vector& joints, const std::vector& prevJoints); }; std::shared_ptr _inputDevice { std::make_shared() }; diff --git a/plugins/hifiSdl2/src/Joystick.cpp b/plugins/hifiSdl2/src/Joystick.cpp index b7d69b9406..9d195fd606 100644 --- a/plugins/hifiSdl2/src/Joystick.cpp +++ b/plugins/hifiSdl2/src/Joystick.cpp @@ -39,7 +39,7 @@ void Joystick::closeJoystick() { #endif } -void Joystick::update(float deltaTime, bool jointsCaptured) { +void Joystick::update(float deltaTime, const controller::InputCalibrationData& inputCalibrationData, bool jointsCaptured) { for (auto axisState : _axisStateMap) { if (fabsf(axisState.second) < CONTROLLER_THRESHOLD) { _axisStateMap[axisState.first] = 0.0f; diff --git a/plugins/hifiSdl2/src/Joystick.h b/plugins/hifiSdl2/src/Joystick.h index fa50f8eab6..08bf27b960 100644 --- a/plugins/hifiSdl2/src/Joystick.h +++ b/plugins/hifiSdl2/src/Joystick.h @@ -39,7 +39,7 @@ public: // Device functions virtual controller::Input::NamedVector getAvailableInputs() const override; virtual QString getDefaultMappingConfig() const override; - virtual void update(float deltaTime, bool jointsCaptured) override; + virtual void update(float deltaTime, const controller::InputCalibrationData& inputCalibrationData, bool jointsCaptured) override; virtual void focusOutEvent() override; Joystick() : InputDevice("GamePad") {} diff --git a/plugins/hifiSdl2/src/SDL2Manager.cpp b/plugins/hifiSdl2/src/SDL2Manager.cpp index 7ca49bbd75..58da2c5df0 100644 --- a/plugins/hifiSdl2/src/SDL2Manager.cpp +++ b/plugins/hifiSdl2/src/SDL2Manager.cpp @@ -138,12 +138,12 @@ void SDL2Manager::pluginFocusOutEvent() { #endif } -void SDL2Manager::pluginUpdate(float deltaTime, bool jointsCaptured) { +void SDL2Manager::pluginUpdate(float deltaTime, const controller::InputCalibrationData& inputCalibrationData, bool jointsCaptured) { #ifdef HAVE_SDL2 if (_isInitialized) { auto userInputMapper = DependencyManager::get(); for (auto joystick : _openJoysticks) { - joystick->update(deltaTime, jointsCaptured); + joystick->update(deltaTime, inputCalibrationData, jointsCaptured); } PerformanceTimer perfTimer("SDL2Manager::update"); diff --git a/plugins/hifiSdl2/src/SDL2Manager.h b/plugins/hifiSdl2/src/SDL2Manager.h index 6506cdccb6..a88e41128d 100644 --- a/plugins/hifiSdl2/src/SDL2Manager.h +++ b/plugins/hifiSdl2/src/SDL2Manager.h @@ -40,7 +40,7 @@ public: virtual void deactivate() override; virtual void pluginFocusOutEvent() override; - virtual void pluginUpdate(float deltaTime, bool jointsCaptured) override; + virtual void pluginUpdate(float deltaTime, const controller::InputCalibrationData& inputCalibrationData, bool jointsCaptured) override; signals: void joystickAdded(Joystick* joystick); diff --git a/plugins/hifiSixense/src/SixenseManager.cpp b/plugins/hifiSixense/src/SixenseManager.cpp index b23982f948..aad61e4bea 100644 --- a/plugins/hifiSixense/src/SixenseManager.cpp +++ b/plugins/hifiSixense/src/SixenseManager.cpp @@ -132,16 +132,16 @@ void SixenseManager::setSixenseFilter(bool filter) { #endif } -void SixenseManager::pluginUpdate(float deltaTime, bool jointsCaptured) { +void SixenseManager::pluginUpdate(float deltaTime, const controller::InputCalibrationData& inputCalibrationData, bool jointsCaptured) { BAIL_IF_NOT_LOADED - _inputDevice->update(deltaTime, jointsCaptured); + _inputDevice->update(deltaTime, inputCalibrationData, jointsCaptured); if (_inputDevice->_requestReset) { _container->requestReset(); _inputDevice->_requestReset = false; } } -void SixenseManager::InputDevice::update(float deltaTime, bool jointsCaptured) { +void SixenseManager::InputDevice::update(float deltaTime, const controller::InputCalibrationData& inputCalibrationData, bool jointsCaptured) { BAIL_IF_NOT_LOADED #ifdef HAVE_SIXENSE _buttonPressedMap.clear(); @@ -205,7 +205,7 @@ void SixenseManager::InputDevice::update(float deltaTime, bool jointsCaptured) { if (!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); + handlePoseEvent(deltaTime, inputCalibrationData, position, rotation, left); rawPoses[i] = controller::Pose(position, rotation, Vectors::ZERO, Vectors::ZERO); } else { _poseStateMap.clear(); @@ -415,7 +415,7 @@ void SixenseManager::InputDevice::handleButtonEvent(unsigned int buttons, bool l } } -void SixenseManager::InputDevice::handlePoseEvent(float deltaTime, glm::vec3 position, glm::quat rotation, bool left) { +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; @@ -437,7 +437,7 @@ void SixenseManager::InputDevice::handlePoseEvent(float deltaTime, glm::vec3 pos auto prevPose = _poseStateMap[hand]; // Transform the measured position into body frame. - position = _avatarRotation * (position + _avatarPosition); + vec3 pos = _avatarRotation * (position + _avatarPosition); // From ABOVE the hand canonical axes look like this: // @@ -476,7 +476,7 @@ void SixenseManager::InputDevice::handlePoseEvent(float deltaTime, glm::vec3 pos // rotation = postOffset * Qsh^ * (measuredRotation * preOffset) * Qsh // // TODO: find a shortcut with fewer rotations. - rotation = _avatarRotation * postOffset * glm::inverse(sixenseToHand) * rotation * preOffset * sixenseToHand; + glm::quat rot = _avatarRotation * postOffset * glm::inverse(sixenseToHand) * rotation * preOffset * sixenseToHand; glm::vec3 velocity(0.0f); glm::vec3 angularVelocity(0.0f); @@ -484,11 +484,11 @@ void SixenseManager::InputDevice::handlePoseEvent(float deltaTime, glm::vec3 pos if (prevPose.isValid() && deltaTime > std::numeric_limits::epsilon()) { auto& samples = _collectedSamples[hand]; - velocity = (position - prevPose.getTranslation()) / deltaTime; + velocity = (pos - prevPose.getTranslation()) / deltaTime; samples.first.addSample(velocity); velocity = samples.first.average; - auto deltaRot = glm::normalize(rotation * glm::conjugate(prevPose.getRotation())); + auto deltaRot = glm::normalize(rot * glm::conjugate(prevPose.getRotation())); auto axis = glm::axis(deltaRot); auto speed = glm::angle(deltaRot) / deltaTime; assert(!glm::isnan(speed)); @@ -500,7 +500,10 @@ void SixenseManager::InputDevice::handlePoseEvent(float deltaTime, glm::vec3 pos _collectedSamples[hand].second.clear(); } - _poseStateMap[hand] = controller::Pose(position, rotation, velocity, angularVelocity); + // 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; #endif // HAVE_SIXENSE } diff --git a/plugins/hifiSixense/src/SixenseManager.h b/plugins/hifiSixense/src/SixenseManager.h index bbb9774368..fc74b83532 100644 --- a/plugins/hifiSixense/src/SixenseManager.h +++ b/plugins/hifiSixense/src/SixenseManager.h @@ -36,7 +36,7 @@ public: virtual void deactivate() override; virtual void pluginFocusOutEvent() override { _inputDevice->focusOutEvent(); } - virtual void pluginUpdate(float deltaTime, bool jointsCaptured) override; + virtual void pluginUpdate(float deltaTime, const controller::InputCalibrationData& inputCalibrationData, bool jointsCaptured) override; virtual void saveSettings() const override; virtual void loadSettings() override; @@ -66,11 +66,11 @@ private: // Device functions virtual controller::Input::NamedVector getAvailableInputs() const override; virtual QString getDefaultMappingConfig() const override; - virtual void update(float deltaTime, bool jointsCaptured) override; + virtual void update(float deltaTime, const controller::InputCalibrationData& inputCalibrationData, bool jointsCaptured) override; virtual void focusOutEvent() override; void handleButtonEvent(unsigned int buttons, bool left); - void handlePoseEvent(float deltaTime, glm::vec3 position, glm::quat rotation, bool left); + void handlePoseEvent(float deltaTime, const controller::InputCalibrationData& inputCalibrationData, const glm::vec3& position, const glm::quat& rotation, bool left); void updateCalibration(SixenseControllerData* controllers); friend class SixenseManager; diff --git a/plugins/openvr/src/ViveControllerManager.cpp b/plugins/openvr/src/ViveControllerManager.cpp index 522c7f9c3d..720a6d48c6 100644 --- a/plugins/openvr/src/ViveControllerManager.cpp +++ b/plugins/openvr/src/ViveControllerManager.cpp @@ -102,7 +102,7 @@ void ViveControllerManager::activate() { // vertexBufferPtr->getSize() - sizeof(float) * 2, // sizeof(vr::RenderModel_Vertex_t), // gpu::Element(gpu::VEC2, gpu::FLOAT, gpu::RAW))); - + gpu::Element formatGPU = gpu::Element(gpu::VEC4, gpu::NUINT8, gpu::RGBA); gpu::Element formatMip = gpu::Element(gpu::VEC4, gpu::NUINT8, gpu::RGBA); _texture = gpu::TexturePointer( @@ -205,8 +205,8 @@ void ViveControllerManager::renderHand(const controller::Pose& pose, gpu::Batch& } -void ViveControllerManager::pluginUpdate(float deltaTime, bool jointsCaptured) { - _inputDevice->update(deltaTime, jointsCaptured); +void ViveControllerManager::pluginUpdate(float deltaTime, const controller::InputCalibrationData& inputCalibrationData, bool jointsCaptured) { + _inputDevice->update(deltaTime, inputCalibrationData, jointsCaptured); auto userInputMapper = DependencyManager::get(); if (_inputDevice->_trackedControllers == 0 && _registeredWithInputMapper) { @@ -222,7 +222,7 @@ void ViveControllerManager::pluginUpdate(float deltaTime, bool jointsCaptured) { } } -void ViveControllerManager::InputDevice::update(float deltaTime, bool jointsCaptured) { +void ViveControllerManager::InputDevice::update(float deltaTime, const controller::InputCalibrationData& inputCalibrationData, bool jointsCaptured) { _poseStateMap.clear(); _buttonPressedMap.clear(); @@ -232,29 +232,29 @@ void ViveControllerManager::InputDevice::update(float deltaTime, bool jointsCapt int numTrackedControllers = 0; for (vr::TrackedDeviceIndex_t device = vr::k_unTrackedDeviceIndex_Hmd + 1; - device < vr::k_unMaxTrackedDeviceCount && numTrackedControllers < 2; ++device) { - + device < vr::k_unMaxTrackedDeviceCount && numTrackedControllers < 2; ++device) { + if (!_hmd->IsTrackedDeviceConnected(device)) { continue; } - - if(_hmd->GetTrackedDeviceClass(device) != vr::TrackedDeviceClass_Controller) { + + if (_hmd->GetTrackedDeviceClass(device) != vr::TrackedDeviceClass_Controller) { continue; } if (!_trackedDevicePose[device].bPoseIsValid) { continue; } - + numTrackedControllers++; bool left = numTrackedControllers == 2; - + const mat4& mat = _trackedDevicePoseMat4[device]; - + if (!jointsCaptured) { - handlePoseEvent(mat, numTrackedControllers - 1); + handlePoseEvent(inputCalibrationData, mat, numTrackedControllers - 1); } - + // handle inputs vr::VRControllerState_t controllerState = vr::VRControllerState_t(); if (_hmd->GetControllerState(device, &controllerState)) { @@ -271,7 +271,7 @@ void ViveControllerManager::InputDevice::update(float deltaTime, bool jointsCapt } } } - + _trackedControllers = numTrackedControllers; } @@ -314,7 +314,7 @@ void ViveControllerManager::InputDevice::handleButtonEvent(uint32_t button, bool } } -void ViveControllerManager::InputDevice::handlePoseEvent(const mat4& mat, bool left) { +void ViveControllerManager::InputDevice::handlePoseEvent(const controller::InputCalibrationData& inputCalibrationData, const mat4& mat, bool left) { // When the sensor-to-world rotation is identity the coordinate axes look like this: // // user @@ -342,11 +342,11 @@ void ViveControllerManager::InputDevice::handlePoseEvent(const mat4& mat, bool l // | | | | // - // So when the user is standing in Vive space facing the -zAxis with hands outstretched and palms down + // So when the user is standing in Vive space facing the -zAxis with hands outstretched and palms down // the rotation to align the Vive axes with those of the hands is: // // QviveToHand = halfTurnAboutY * quaterTurnAboutX - + // Due to how the Vive controllers fit into the palm there is an offset that is different for each hand. // You can think of this offset as the inverse of the measured rotation when the hands are posed, such that // the combination (measurement * offset) is identity at this orientation. @@ -384,8 +384,11 @@ void ViveControllerManager::InputDevice::handlePoseEvent(const mat4& mat, bool l position += rotation * (left ? leftTranslationOffset : rightTranslationOffset); rotation = rotation * (left ? leftRotationOffset : rightRotationOffset); - - _poseStateMap[left ? controller::LEFT_HAND : controller::RIGHT_HAND] = controller::Pose(position, rotation); + + // transform into avatar frame + glm::mat4 controllerToAvatar = glm::inverse(inputCalibrationData.avatarMat) * inputCalibrationData.sensorToWorldMat; + auto avatarPose = controller::Pose(position, rotation).transform(controllerToAvatar); + _poseStateMap[left ? controller::LEFT_HAND : controller::RIGHT_HAND] = avatarPose; } controller::Input::NamedVector ViveControllerManager::InputDevice::getAvailableInputs() const { @@ -436,7 +439,7 @@ QString ViveControllerManager::InputDevice::getDefaultMappingConfig() const { //void ViveControllerManager::assignDefaultInputMapping(UserInputMapper& mapper) { // const float JOYSTICK_MOVE_SPEED = 1.0f; -// +// // // Left Trackpad: Movement, strafing // mapper.addInputChannel(UserInputMapper::LONGITUDINAL_FORWARD, makeInput(AXIS_Y_POS, 0), makeInput(TRACKPAD_BUTTON, 0), JOYSTICK_MOVE_SPEED); // mapper.addInputChannel(UserInputMapper::LONGITUDINAL_BACKWARD, makeInput(AXIS_Y_NEG, 0), makeInput(TRACKPAD_BUTTON, 0), JOYSTICK_MOVE_SPEED); @@ -446,17 +449,17 @@ QString ViveControllerManager::InputDevice::getDefaultMappingConfig() const { // // Right Trackpad: Vertical movement, zooming // mapper.addInputChannel(UserInputMapper::VERTICAL_UP, makeInput(AXIS_Y_POS, 1), makeInput(TRACKPAD_BUTTON, 1), JOYSTICK_MOVE_SPEED); // mapper.addInputChannel(UserInputMapper::VERTICAL_DOWN, makeInput(AXIS_Y_NEG, 1), makeInput(TRACKPAD_BUTTON, 1), JOYSTICK_MOVE_SPEED); -// +// // // Buttons // mapper.addInputChannel(UserInputMapper::SHIFT, makeInput(BUTTON_A, 0)); // mapper.addInputChannel(UserInputMapper::SHIFT, makeInput(BUTTON_A, 1)); -// +// // mapper.addInputChannel(UserInputMapper::ACTION1, makeInput(GRIP_BUTTON, 0)); // mapper.addInputChannel(UserInputMapper::ACTION2, makeInput(GRIP_BUTTON, 1)); // // mapper.addInputChannel(UserInputMapper::LEFT_HAND_CLICK, makeInput(BACK_TRIGGER, 0)); // mapper.addInputChannel(UserInputMapper::RIGHT_HAND_CLICK, makeInput(BACK_TRIGGER, 1)); -// +// // // Hands // mapper.addInputChannel(UserInputMapper::LEFT_HAND, makeInput(LEFT_HAND)); // mapper.addInputChannel(UserInputMapper::RIGHT_HAND, makeInput(RIGHT_HAND)); diff --git a/plugins/openvr/src/ViveControllerManager.h b/plugins/openvr/src/ViveControllerManager.h index 077aec7fc3..282c8e41a5 100644 --- a/plugins/openvr/src/ViveControllerManager.h +++ b/plugins/openvr/src/ViveControllerManager.h @@ -41,7 +41,7 @@ public: virtual void deactivate() override; virtual void pluginFocusOutEvent() override { _inputDevice->focusOutEvent(); } - virtual void pluginUpdate(float deltaTime, bool jointsCaptured) override; + virtual void pluginUpdate(float deltaTime, const controller::InputCalibrationData& inputCalibrationData, bool jointsCaptured) override; void updateRendering(RenderArgs* args, render::ScenePointer scene, render::PendingChanges pendingChanges); @@ -55,12 +55,12 @@ private: // Device functions virtual controller::Input::NamedVector getAvailableInputs() const override; virtual QString getDefaultMappingConfig() const override; - virtual void update(float deltaTime, bool jointsCaptured) override; + virtual void update(float deltaTime, const controller::InputCalibrationData& inputCalibrationData, bool jointsCaptured) override; virtual void focusOutEvent() override; void handleButtonEvent(uint32_t button, bool pressed, bool left); void handleAxisEvent(uint32_t axis, float x, float y, bool left); - void handlePoseEvent(const mat4& mat, bool left); + void handlePoseEvent(const controller::InputCalibrationData& inputCalibrationData, const mat4& mat, bool left); int _trackedControllers { 0 }; vr::IVRSystem*& _hmd; diff --git a/tests/controllers/src/main.cpp b/tests/controllers/src/main.cpp index da233d650c..13b6b51d82 100644 --- a/tests/controllers/src/main.cpp +++ b/tests/controllers/src/main.cpp @@ -123,8 +123,14 @@ int main(int argc, char** argv) { float delta = now - last; last = now; + InputCalibrationData calibrationData = { + glm::mat4(), + glm::mat4(), + glm::mat4() + }; + foreach(auto inputPlugin, PluginManager::getInstance()->getInputPlugins()) { - inputPlugin->pluginUpdate(delta, false); + inputPlugin->pluginUpdate(delta, calibrationData, false); } auto userInputMapper = DependencyManager::get(); @@ -133,6 +139,12 @@ int main(int argc, char** argv) { timer.start(50); { + InputCalibrationData calibrationData = { + glm::mat4(), + glm::mat4(), + glm::mat4() + }; + DependencyManager::set(); foreach(auto inputPlugin, PluginManager::getInstance()->getInputPlugins()) { QString name = inputPlugin->getName(); @@ -141,7 +153,7 @@ int main(int argc, char** argv) { if (name == KeyboardMouseDevice::NAME) { userInputMapper->registerDevice(std::dynamic_pointer_cast(inputPlugin)->getInputDevice()); } - inputPlugin->pluginUpdate(0, false); + inputPlugin->pluginUpdate(0, calibrationData, false); } rootContext->setContextProperty("Controllers", new MyControllerScriptingInterface()); }