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

@@ -3070,13 +3070,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;
             }
@@ -4981,14 +4986,9 @@ void Application::setPalmData(Hand* hand, const controller::Pose& pose, float de
         auto myAvatar = DependencyManager::get<AvatarManager>()->getMyAvatar();
         palm.setActive(pose.isValid());
 
-        // transform from sensor space, to world space, to avatar model space.
+        // controller pose is in Avatar frame.
-        glm::mat4 poseMat = createMatFromQuatAndPos(pose.getRotation(), pose.getTranslation() * myAvatar->getScale());
+        glm::vec3 position = pose.getTranslation();
-        glm::mat4 sensorToWorldMat = myAvatar->getSensorToWorldMatrix();
+        glm::quat rotation = pose.getRotation();
-        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);
 
         //  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) { 
+void KeyboardMouseDevice::pluginUpdate(float deltaTime, const controller::InputCalibrationData& inputCalibrationData, bool jointsCaptured) { 
-    _inputDevice->update(deltaTime, 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) { 
+void ViveControllerManager::pluginUpdate(float deltaTime, const controller::InputCalibrationData& inputCalibrationData, bool jointsCaptured) {
-    _inputDevice->update(deltaTime, 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());
     }