diff --git a/libraries/input-plugins/src/input-plugins/ViveControllerManager.cpp b/libraries/input-plugins/src/input-plugins/ViveControllerManager.cpp index 3aef37e502..082c37a837 100644 --- a/libraries/input-plugins/src/input-plugins/ViveControllerManager.cpp +++ b/libraries/input-plugins/src/input-plugins/ViveControllerManager.cpp @@ -324,9 +324,64 @@ void ViveControllerManager::handlePoseEvent(const mat4& mat, int index) { glm::vec3 position = extractTranslation(mat); glm::quat rotation = glm::quat_cast(mat); - // Flip the rotation appropriately for each hand - int sign = index == LEFT_HAND ? 1 : -1; - rotation = rotation * glm::angleAxis(PI, glm::vec3(1.0f, 0.0f, 0.0f)) * glm::angleAxis(sign * PI_OVER_TWO, glm::vec3(0.0f, 0.0f, 1.0f)); + // When the sensor-to-world rotation is identity the coordinate axes look like this: + // + // user + // forward + // z + // | + // y| user + // y o----x right + // o-----x user + // | up + // | + // z + // + // Vive + // + + // From ABOVE the hand canonical axes looks like this: + // + // | | | | y | | | | + // | | | | | | | | | + // | | | | | + // |left | / x---- + \ |right| + // | _/ z \_ | + // | | | | + // | | | | + // + + // 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. + // + // Qoffset = glm::inverse(deltaRotation when hand is posed fingers forward, palm down) + // + // An approximate offset for the Vive can be obtained by inpection: + // + // 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: + // + // Q = yFlip * combinedMeasurement * viveToHand + // + // Q = yFlip * (deltaQ * QOffset) * (yFlip * quarterTurnAboutX) + // + // Q = yFlip * (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)); + float sign = (index == LEFT_HAND) ? -1.0f : 1.0f; + 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; position += rotation * glm::vec3(0, 0, -CONTROLLER_LENGTH_OFFSET);