Merge pull request #5701 from AndrewMeadows/einsteinium

fix sixense and vive rotation readings for palms

interface/src/Application.cpp

@@ -4919,7 +4919,7 @@ void Application::setPalmData(Hand* hand, UserInputMapper::PoseValue pose, float
 
     // Store the one fingertip in the palm structure so we can track velocity
     const float FINGER_LENGTH = 0.3f;   //  meters
-    const glm::vec3 FINGER_VECTOR(0.0f, 0.0f, FINGER_LENGTH);
+    const glm::vec3 FINGER_VECTOR(0.0f, FINGER_LENGTH, 0.0f);
     const glm::vec3 newTipPosition = position + rotation * FINGER_VECTOR;
     glm::vec3 oldTipPosition = palm->getTipRawPosition();
     if (deltaTime > 0.0f) {

interface/src/avatar/SkeletonModel.cpp

@@ -261,9 +261,7 @@ void SkeletonModel::applyPalmData(int jointIndex, PalmData& palm) {
     // the palm's position must be transformed into the model-frame
     glm::quat inverseRotation = glm::inverse(_rotation);
     glm::vec3 palmPosition = inverseRotation * (palm.getPosition() - _translation);
-
+    glm::quat palmRotation = inverseRotation * palm.getRotation();
-    // the palm's "raw" rotation is already in the model-frame
-    glm::quat palmRotation = palm.getRawRotation();
 
     inverseKinematics(jointIndex, palmPosition, palmRotation, PALM_PRIORITY);
 }

libraries/avatars/src/HandData.h

@@ -64,12 +64,13 @@ public:
     bool findSpherePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, glm::vec3& penetration, 
         const PalmData*& collidingPalm) const;
 
+    glm::quat getBaseOrientation() const;
+
     friend class AvatarData;
 protected:
     AvatarData* _owningAvatarData;
     std::vector<PalmData> _palms;
     
-    glm::quat getBaseOrientation() const;
     glm::vec3 getBasePosition() const;
     float getBaseScale() const;
     
@@ -95,6 +96,7 @@ public:
 
     void setRawRotation(const glm::quat rawRotation) { _rawRotation = rawRotation; };
     glm::quat getRawRotation() const { return _rawRotation; }
+    glm::quat getRotation() const { return _owningHandData->getBaseOrientation() * _rawRotation; }
     void setRawPosition(const glm::vec3& pos)  { _rawPosition = pos; }
     void setRawVelocity(const glm::vec3& velocity) { _rawVelocity = velocity; }
     const glm::vec3& getRawVelocity()  const { return _rawVelocity; }
@@ -147,6 +149,7 @@ public:
     glm::vec3 getNormal() const;
 
 private:
+    // unless marked otherwise, these are all in the model-frame
     glm::quat _rawRotation;
     glm::vec3 _rawPosition;
     glm::vec3 _rawVelocity;
@@ -156,6 +159,7 @@ private:
     glm::vec3 _tipPosition;
     glm::vec3 _tipVelocity;
     glm::vec3 _totalPenetration;    // accumulator for per-frame penetrations
+
     unsigned int _controllerButtons;
     unsigned int _lastControllerButtons;
     float _trigger;

libraries/input-plugins/src/input-plugins/SixenseManager.cpp

@@ -481,6 +481,7 @@ void SixenseManager::handlePoseEvent(glm::vec3 position, glm::quat rotation, int
     //     Qsh = angleAxis(PI, zAxis) * angleAxis(-PI/2, xAxis)
     //
     const glm::vec3 xAxis = glm::vec3(1.0f, 0.0f, 0.0f);
+    const glm::vec3 yAxis = glm::vec3(0.0f, 1.0f, 0.0f);
     const glm::vec3 zAxis = glm::vec3(0.0f, 0.0f, 1.0f);
     const glm::quat sixenseToHand = glm::angleAxis(PI, zAxis) * glm::angleAxis(-PI/2.0f, xAxis);
 
@@ -491,13 +492,15 @@ void SixenseManager::handlePoseEvent(glm::vec3 position, glm::quat rotation, int
     const glm::quat preOffset = glm::angleAxis(sign * PI / 2.0f, zAxis);
 
     // Finally, there is a post-offset (same for both hands) to get the hand's rest orientation
-    // (fingers forward, palm down) aligned properly in the avatar's model-frame.
+    // (fingers forward, palm down) aligned properly in the avatar's model-frame,
-    const glm::quat postOffset = glm::angleAxis(PI / 2.0f, xAxis);
+    // and then a flip about the yAxis to get into model-frame.
+    const glm::quat postOffset = glm::angleAxis(PI, yAxis) * glm::angleAxis(PI / 2.0f, xAxis);
 
     // The total rotation of the hand uses the formula:
     //
     //     rotation = postOffset * Qsh^ * (measuredRotation * preOffset) * Qsh
     //
+    // TODO: find a shortcut with fewer rotations.
     rotation = postOffset * glm::inverse(sixenseToHand) * rotation * preOffset * sixenseToHand;
 
     _poseStateMap[makeInput(JointChannel(index)).getChannel()] = UserInputMapper::PoseValue(position, rotation);

libraries/input-plugins/src/input-plugins/ViveControllerManager.cpp

@@ -362,18 +362,17 @@ void ViveControllerManager::handlePoseEvent(const mat4& mat, int index) {
     //
     //    Qoffset = glm::inverse(deltaRotation when hand is posed fingers forward, palm down)
     //
-    // An approximate offset for the Vive can be obtained by inpection:
+    // An approximate offset for the Vive can be obtained by inspection:
     //
     //    Qoffset = glm::inverse(glm::angleAxis(sign * PI/4.0f, zAxis) * glm::angleAxis(PI/2.0f, xAxis))
     //
-
+    // So the full equation is:
-    // Finally there is another flip around the yAxis to re-align from model to Vive space, so the full equation is:
     //
-    //    Q = yFlip * combinedMeasurement * viveToHand
+    //    Q = combinedMeasurement * viveToHand
     //
-    //    Q = yFlip * (deltaQ * QOffset) * (yFlip * quarterTurnAboutX)
+    //    Q = (deltaQ * QOffset) * (yFlip * quarterTurnAboutX)
     //
-    //    Q = yFlip * (deltaQ * inverse(deltaQForAlignedHand)) * (yFlip * quarterTurnAboutX)
+    //    Q = (deltaQ * inverse(deltaQForAlignedHand)) * (yFlip * quarterTurnAboutX)
    
     const glm::quat quarterX = glm::angleAxis(PI / 2.0f, glm::vec3(1.0f, 0.0f, 0.0f));
     const glm::quat yFlip = glm::angleAxis(PI, glm::vec3(0.0f, 1.0f, 0.0f));
@@ -381,7 +380,7 @@ void ViveControllerManager::handlePoseEvent(const mat4& mat, int index) {
     const glm::quat signedQuaterZ = glm::angleAxis(sign * PI / 2.0f, glm::vec3(0.0f, 0.0f, 1.0f)); 
     const glm::quat eighthX = glm::angleAxis(PI / 4.0f, glm::vec3(1.0f, 0.0f, 0.0f));
     const glm::quat offset = glm::inverse(signedQuaterZ * eighthX);
-    rotation = yFlip * rotation * offset * yFlip * quarterX;
+    rotation = rotation * offset * yFlip * quarterX;
 
     position += rotation * glm::vec3(0, 0, -CONTROLLER_LENGTH_OFFSET);