Merge pull request #7093 from hyperlogic/tony/input-poses-in-avatar-space

Controller Pose values are relative to Avatar.

interface/src/Application.cpp

@@ -3125,13 +3125,18 @@ void Application::update(float deltaTime) {
 
     auto myAvatar = getMyAvatar();
     auto userInputMapper = DependencyManager::get<UserInputMapper>();
-    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;
             }
@@ -5025,17 +5030,11 @@ void Application::setPalmData(Hand* hand, const controller::Pose& pose, float de
     // of this palm manipulation in the Hand class itself. But unfortunately the Hand and Palm don't knbow about
     // controller::Pose. More work is needed to clean this up.
     hand->modifyPalm(whichHand, [&](PalmData& palm) {
-        auto myAvatar = DependencyManager::get<AvatarManager>()->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;

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

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() {

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

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,

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;
 };
 
-}
+}

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;
+    }
 }
 

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

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() {

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

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

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

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<Pose> _poseStates = std::vector<Pose>(toInt(Action::NUM_ACTIONS));
         std::vector<float> _lastStandardStates = std::vector<float>();
 
-        glm::mat4 _sensorToWorldMat;
-
         int recordDeviceOfType(const QString& deviceName);
         QHash<const QString&, int> _deviceCounts;
 

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

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

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

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;

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

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) {

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.

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<NeuronJoint> joints;
     {
         // lock and copy
         std::lock_guard<std::mutex> 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<NeuronPlugin::NeuronJoint>& joints, const std::vector<NeuronPlugin::NeuronJoint>& prevJoints) {
+void NeuronPlugin::InputDevice::update(float deltaTime, const controller::InputCalibrationData& inputCalibrationData, const std::vector<NeuronPlugin::NeuronJoint>& joints, const std::vector<NeuronPlugin::NeuronJoint>& prevJoints) {
     for (size_t i = 0; i < joints.size(); i++) {
         glm::vec3 linearVel, angularVel;
         glm::vec3 pos = joints[i].pos;

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<NeuronPlugin::NeuronJoint>& joints, const std::vector<NeuronPlugin::NeuronJoint>& prevJoints);
+        void update(float deltaTime, const controller::InputCalibrationData& inputCalibrationData, const std::vector<NeuronPlugin::NeuronJoint>& joints, const std::vector<NeuronPlugin::NeuronJoint>& prevJoints);
     };
 
     std::shared_ptr<InputDevice> _inputDevice { std::make_shared<InputDevice>() };

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;

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") {}

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<controller::UserInputMapper>();
         for (auto joystick : _openJoysticks) {
-            joystick->update(deltaTime, jointsCaptured);
+            joystick->update(deltaTime, inputCalibrationData, jointsCaptured);
         }
         
         PerformanceTimer perfTimer("SDL2Manager::update");

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

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<float>::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
 }
 

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;

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<controller::UserInputMapper>();
 
     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));

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;

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<controller::UserInputMapper>();
@@ -133,6 +139,12 @@ int main(int argc, char** argv) {
     timer.start(50);
 
     {
+        InputCalibrationData calibrationData = {
+            glm::mat4(),
+            glm::mat4(),
+            glm::mat4()
+        };
+
         DependencyManager::set<controller::UserInputMapper>();
         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<KeyboardMouseDevice>(inputPlugin)->getInputDevice());
             }
-            inputPlugin->pluginUpdate(0, false);
+            inputPlugin->pluginUpdate(0, calibrationData, false);
         }
         rootContext->setContextProperty("Controllers", new MyControllerScriptingInterface());
     }