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()); }