start on faux avatar joint that represent hand controllers

interface/src/avatar/Avatar.cpp

@@ -59,8 +59,6 @@ const float DISPLAYNAME_ALPHA = 1.0f;
 const float DISPLAYNAME_BACKGROUND_ALPHA = 0.4f;
 const glm::vec3 HAND_TO_PALM_OFFSET(0.0f, 0.12f, 0.08f);
 
-const int SENSOR_TO_WORLD_MATRIX_INDEX = 65534;
-
 namespace render {
     template <> const ItemKey payloadGetKey(const AvatarSharedPointer& avatar) {
         return ItemKey::Builder::opaqueShape();
@@ -853,32 +851,54 @@ glm::vec3 Avatar::getDefaultJointTranslation(int index) const {
 }
 
 glm::quat Avatar::getAbsoluteJointRotationInObjectFrame(int index) const {
-    if (index == SENSOR_TO_WORLD_MATRIX_INDEX) {
+    switch(index) {
-        glm::mat4 sensorToWorldMatrix = getSensorToWorldMatrix();
+        case SENSOR_TO_WORLD_MATRIX_INDEX: {
-        bool success;
+            glm::mat4 sensorToWorldMatrix = getSensorToWorldMatrix();
-        Transform avatarTransform;
+            bool success;
-        Transform::mult(avatarTransform, getParentTransform(success), getLocalTransform());
+            Transform avatarTransform;
-        glm::mat4 invAvatarMat = avatarTransform.getInverseMatrix();
+            Transform::mult(avatarTransform, getParentTransform(success), getLocalTransform());
-        return glmExtractRotation(invAvatarMat * sensorToWorldMatrix);
+            glm::mat4 invAvatarMat = avatarTransform.getInverseMatrix();
-    } else {
+            return glmExtractRotation(invAvatarMat * sensorToWorldMatrix);
-        glm::quat rotation;
+        }
-        _skeletonModel->getAbsoluteJointRotationInRigFrame(index, rotation);
+        case CONTROLLER_LEFTHAND_INDEX: {
-        return Quaternions::Y_180 * rotation;
+            Transform controllerLeftHandTransform = Transform(getControllerLeftHandMatrix());
+            return controllerLeftHandTransform.getRotation();
+        }
+        case CONTROLLER_RIGHTHAND_INDEX: {
+            Transform controllerRightHandTransform = Transform(getControllerRightHandMatrix());
+            return controllerRightHandTransform.getRotation();
+        }
+        default: {
+            glm::quat rotation;
+            _skeletonModel->getAbsoluteJointRotationInRigFrame(index, rotation);
+            return Quaternions::Y_180 * rotation;
+        }
     }
 }
 
 glm::vec3 Avatar::getAbsoluteJointTranslationInObjectFrame(int index) const {
-    if (index == SENSOR_TO_WORLD_MATRIX_INDEX) {
+    switch(index) {
-        glm::mat4 sensorToWorldMatrix = getSensorToWorldMatrix();
+        case SENSOR_TO_WORLD_MATRIX_INDEX: {
-        bool success;
+            glm::mat4 sensorToWorldMatrix = getSensorToWorldMatrix();
-        Transform avatarTransform;
+            bool success;
-        Transform::mult(avatarTransform, getParentTransform(success), getLocalTransform());
+            Transform avatarTransform;
-        glm::mat4 invAvatarMat = avatarTransform.getInverseMatrix();
+            Transform::mult(avatarTransform, getParentTransform(success), getLocalTransform());
-        return extractTranslation(invAvatarMat * sensorToWorldMatrix);
+            glm::mat4 invAvatarMat = avatarTransform.getInverseMatrix();
-    } else {
+            return extractTranslation(invAvatarMat * sensorToWorldMatrix);
-        glm::vec3 translation;
+        }
-        _skeletonModel->getAbsoluteJointTranslationInRigFrame(index, translation);
+        case CONTROLLER_LEFTHAND_INDEX: {
-        return Quaternions::Y_180 * translation;
+            Transform controllerLeftHandTransform = Transform(getControllerLeftHandMatrix());
+            return controllerLeftHandTransform.getTranslation();
+        }
+        case CONTROLLER_RIGHTHAND_INDEX: {
+            Transform controllerRightHandTransform = Transform(getControllerRightHandMatrix());
+            return controllerRightHandTransform.getTranslation();
+        }
+        default: {
+            glm::vec3 translation;
+            _skeletonModel->getAbsoluteJointTranslationInRigFrame(index, translation);
+            return Quaternions::Y_180 * translation;
+        }
     }
 }
 
@@ -889,6 +909,10 @@ int Avatar::getJointIndex(const QString& name) const {
             Q_RETURN_ARG(int, result), Q_ARG(const QString&, name));
         return result;
     }
+    int result = getFauxJointIndex(name);
+    if (result != -1) {
+        return result;
+    }
     return _skeletonModel->isActive() ? _skeletonModel->getFBXGeometry().getJointIndex(name) : -1;
 }
 

interface/src/avatar/MyAvatar.cpp

@@ -532,6 +532,23 @@ void MyAvatar::updateFromHMDSensorMatrix(const glm::mat4& hmdSensorMatrix) {
     _hmdSensorFacing = getFacingDir2D(_hmdSensorOrientation);
 }
 
+void MyAvatar::updateJointsFromControllers() {
+    if (QThread::currentThread() != thread()) { abort(); } // XXX
+    auto userInputMapper = DependencyManager::get<UserInputMapper>();
+
+    controller::Pose leftControllerPose = userInputMapper->getPoseState(controller::Action::LEFT_HAND);
+    Transform controllerLeftHandTransform;
+    controllerLeftHandTransform.setTranslation(leftControllerPose.getTranslation());
+    controllerLeftHandTransform.setRotation(leftControllerPose.getRotation());
+    _controllerLeftHandMatrixCache.set(controllerLeftHandTransform.getMatrix());
+
+    controller::Pose rightControllerPose = userInputMapper->getPoseState(controller::Action::RIGHT_HAND);
+    Transform controllerRightHandTransform;
+    controllerRightHandTransform.setTranslation(rightControllerPose.getTranslation());
+    controllerRightHandTransform.setRotation(rightControllerPose.getRotation());
+    _controllerRightHandMatrixCache.set(controllerRightHandTransform.getMatrix());
+}
+
 // best called at end of main loop, after physics.
 // update sensor to world matrix from current body position and hmd sensor.
 // This is so the correct camera can be used for rendering.
@@ -545,10 +562,13 @@ void MyAvatar::updateSensorToWorldMatrix() {
     lateUpdatePalms();
 
     if (_enableDebugDrawSensorToWorldMatrix) {
-        DebugDraw::getInstance().addMarker("sensorToWorldMatrix", glmExtractRotation(_sensorToWorldMatrix), extractTranslation(_sensorToWorldMatrix), glm::vec4(1));
+        DebugDraw::getInstance().addMarker("sensorToWorldMatrix", glmExtractRotation(_sensorToWorldMatrix),
+                                           extractTranslation(_sensorToWorldMatrix), glm::vec4(1));
     }
 
     _sensorToWorldMatrixCache.set(_sensorToWorldMatrix);
+
+    updateJointsFromControllers();
 }
 
 //  Update avatar head rotation with sensor data

interface/src/avatar/MyAvatar.h

@@ -117,6 +117,9 @@ public:
     // as it moves through the world.
     void updateFromHMDSensorMatrix(const glm::mat4& hmdSensorMatrix);
 
+    // read the locations of hand controllers and save the values
+    void updateJointsFromControllers();
+
     // best called at end of main loop, just before rendering.
     // update sensor to world matrix from current body position and hmd sensor.
     // This is so the correct camera can be used for rendering.
@@ -410,9 +413,10 @@ private:
     bool _useSnapTurn { true };
     bool _clearOverlayWhenMoving { true };
 
-    // working copy of sensorToWorldMatrix.
+    // working copies -- see AvatarData for thread-safe _sensorToWorldMatrixCache, used for outward facing access
-    // See AvatarData for thread-safe _sensorToWorldMatrixCache, used for outward facing access
+    glm::mat4 _sensorToWorldMatrix { glm::mat4() };
-    glm::mat4 _sensorToWorldMatrix;
+    glm::mat4 _controllerRightHandMatrix { glm::mat4() };
+    glm::mat4 _controllerLeftHandMatrix { glm::mat4() };
 
     // cache of the current HMD sensor position and orientation
     // in sensor space.

libraries/avatars/src/AvatarData.cpp

@@ -915,7 +915,24 @@ void AvatarData::clearJointsData() {
     }
 }
 
+int AvatarData::getFauxJointIndex(const QString& name) const {
+    if (name == "sensorToWorld") {
+        return SENSOR_TO_WORLD_MATRIX_INDEX;
+    }
+    if (name == "Controller.Standard.LeftHand") {
+        return CONTROLLER_LEFTHAND_INDEX;
+    }
+    if (name == "Controller.Standard.RightHand") {
+        return CONTROLLER_RIGHTHAND_INDEX;
+    }
+    return -1;
+}
+
 int AvatarData::getJointIndex(const QString& name) const {
+    int result = getFauxJointIndex(name);
+    if (result != -1) {
+        return result;
+    }
     QReadLocker readLock(&_jointDataLock);
     return _jointIndices.value(name) - 1;
 }
@@ -1743,6 +1760,17 @@ glm::mat4 AvatarData::getSensorToWorldMatrix() const {
     return _sensorToWorldMatrixCache.get();
 }
 
+// thread-safe
+glm::mat4 AvatarData::getControllerLeftHandMatrix() const {
+    return _controllerLeftHandMatrixCache.get();
+}
+
+// thread-safe
+glm::mat4 AvatarData::getControllerRightHandMatrix() const {
+    return _controllerRightHandMatrixCache.get();
+}
+
+
 QScriptValue RayToAvatarIntersectionResultToScriptValue(QScriptEngine* engine, const RayToAvatarIntersectionResult& value) {
     QScriptValue obj = engine->newObject();
     obj.setProperty("intersects", value.intersects);

libraries/avatars/src/AvatarData.h

@@ -173,6 +173,8 @@ class AvatarData : public QObject, public SpatiallyNestable {
     Q_PROPERTY(QUuid sessionUUID READ getSessionUUID)
 
     Q_PROPERTY(glm::mat4 sensorToWorldMatrix READ getSensorToWorldMatrix)
+    Q_PROPERTY(glm::mat4 controllerLeftHandMatrix READ getControllerLeftHandMatrix)
+    Q_PROPERTY(glm::mat4 controllerRightHandMatrix READ getControllerRightHandMatrix)
 
 public:
 
@@ -356,6 +358,8 @@ public:
 
     // thread safe
     Q_INVOKABLE glm::mat4 getSensorToWorldMatrix() const;
+    Q_INVOKABLE glm::mat4 getControllerLeftHandMatrix() const;
+    Q_INVOKABLE glm::mat4 getControllerRightHandMatrix() const;
 
 public slots:
     void sendAvatarDataPacket();
@@ -433,6 +437,10 @@ protected:
 
     // used to transform any sensor into world space, including the _hmdSensorMat, or hand controllers.
     ThreadSafeValueCache<glm::mat4> _sensorToWorldMatrixCache { glm::mat4() };
+    ThreadSafeValueCache<glm::mat4> _controllerLeftHandMatrixCache { glm::mat4() };
+    ThreadSafeValueCache<glm::mat4> _controllerRightHandMatrixCache { glm::mat4() };
+
+    int getFauxJointIndex(const QString& name) const;
 
 private:
     friend void avatarStateFromFrame(const QByteArray& frameData, AvatarData* _avatar);
@@ -519,5 +527,10 @@ Q_DECLARE_METATYPE(RayToAvatarIntersectionResult)
 QScriptValue RayToAvatarIntersectionResultToScriptValue(QScriptEngine* engine, const RayToAvatarIntersectionResult& results);
 void RayToAvatarIntersectionResultFromScriptValue(const QScriptValue& object, RayToAvatarIntersectionResult& results);
 
+// faux joint indexes (-1 means invalid)
+const int SENSOR_TO_WORLD_MATRIX_INDEX = 65534; // -2
+const int CONTROLLER_RIGHTHAND_INDEX = 65533; // -3
+const int CONTROLLER_LEFTHAND_INDEX = 65532; // -4
+
 
 #endif // hifi_AvatarData_h

libraries/controllers/src/controllers/UserInputMapper.cpp

@@ -325,7 +325,6 @@ QString UserInputMapper::getActionName(Action action) const {
     return QString();
 }
 
-
 QVector<QString> UserInputMapper::getActionNames() const {
     Locker locker(_lock);
     QVector<QString> result;
@@ -335,6 +334,18 @@ QVector<QString> UserInputMapper::getActionNames() const {
     return result;
 }
 
+Pose UserInputMapper::getPoseState(Action action) const {
+    if (QThread::currentThread() != thread()) {
+        Pose result;
+        QMetaObject::invokeMethod(const_cast<UserInputMapper*>(this), "getPoseState", Qt::BlockingQueuedConnection,
+                                  Q_RETURN_ARG(Pose, result), Q_ARG(Action, action));
+        return result;
+    }
+
+    return _poseStates[toInt(action)];
+}
+
+
 bool UserInputMapper::triggerHapticPulse(float strength, float duration, controller::Hand hand) {
     Locker locker(_lock);
     bool toReturn = false;

libraries/controllers/src/controllers/UserInputMapper.h

@@ -81,7 +81,7 @@ namespace controller {
         QVector<Action> getAllActions() const;
         QString getActionName(Action action) const;
         float getActionState(Action action) const { return _actionStates[toInt(action)]; }
-        Pose getPoseState(Action action) const { return _poseStates[toInt(action)]; }
+        Pose getPoseState(Action action) const;
         int findAction(const QString& actionName) const;
         QVector<QString> getActionNames() const;
         Input inputFromAction(Action action) const { return getActionInputs()[toInt(action)].first; }