From 992bd5c9d20e578df833fe7ec406e00e7da0e22b Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 2 Sep 2015 11:16:36 -0700 Subject: [PATCH] fix sixense and vive rotation readings for palms --- interface/src/Application.cpp | 2 +- interface/src/avatar/SkeletonModel.cpp | 4 +--- libraries/avatars/src/HandData.h | 6 +++++- .../src/input-plugins/SixenseManager.cpp | 7 +++++-- .../src/input-plugins/ViveControllerManager.cpp | 13 ++++++------- 5 files changed, 18 insertions(+), 14 deletions(-) diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index a8781ee308..e0726311c9 100644 --- a/interface/src/Application.cpp +++ b/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) { diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index a2e5908477..6a36a41dc4 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/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); - - // the palm's "raw" rotation is already in the model-frame - glm::quat palmRotation = palm.getRawRotation(); + glm::quat palmRotation = inverseRotation * palm.getRotation(); inverseKinematics(jointIndex, palmPosition, palmRotation, PALM_PRIORITY); } diff --git a/libraries/avatars/src/HandData.h b/libraries/avatars/src/HandData.h index 6388c882c7..c87c840399 100644 --- a/libraries/avatars/src/HandData.h +++ b/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 _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; diff --git a/libraries/input-plugins/src/input-plugins/SixenseManager.cpp b/libraries/input-plugins/src/input-plugins/SixenseManager.cpp index 579fddcd63..f090db7959 100644 --- a/libraries/input-plugins/src/input-plugins/SixenseManager.cpp +++ b/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. - const glm::quat postOffset = glm::angleAxis(PI / 2.0f, xAxis); + // (fingers forward, palm down) aligned properly in the avatar's model-frame, + // 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); diff --git a/libraries/input-plugins/src/input-plugins/ViveControllerManager.cpp b/libraries/input-plugins/src/input-plugins/ViveControllerManager.cpp index 082c37a837..5410db11a4 100644 --- a/libraries/input-plugins/src/input-plugins/ViveControllerManager.cpp +++ b/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)) // - - // Finally there is another flip around the yAxis to re-align from model to Vive space, so the full equation is: + // 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);