From 802b3a88c32dd5a693ebb9f720667e3731329a45 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 24 Aug 2015 15:48:00 -0700 Subject: [PATCH 1/7] more corret cmake file for sixense --- cmake/externals/sixense/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cmake/externals/sixense/CMakeLists.txt b/cmake/externals/sixense/CMakeLists.txt index c80b492509..f6646e2272 100644 --- a/cmake/externals/sixense/CMakeLists.txt +++ b/cmake/externals/sixense/CMakeLists.txt @@ -47,13 +47,13 @@ if (WIN32) elseif(APPLE) # FIXME need to account for different architectures - set(${EXTERNAL_NAME_UPPER}_LIBRARIES ${SOURCE_DIR}/lib/osx32/libopenvr_api.dylib CACHE TYPE INTERNAL) + set(${EXTERNAL_NAME_UPPER}_LIBRARIES ${SOURCE_DIR}/lib/osx_x64/release_dll/libsixense_x64.dylib CACHE TYPE INTERNAL) add_paths_to_fixup_libs(${SOURCE_DIR}/bin/osx32) elseif(NOT ANDROID) # FIXME need to account for different architectures - set(${EXTERNAL_NAME_UPPER}_LIBRARIES ${SOURCE_DIR}/lib/linux32/libopenvr_api.so CACHE TYPE INTERNAL) + set(${EXTERNAL_NAME_UPPER}_LIBRARIES ${SOURCE_DIR}/lib/linux_x64/release/libsixense_x64.so CACHE TYPE INTERNAL) add_paths_to_fixup_libs(${SOURCE_DIR}/bin/linux32) endif() From bb63267e306df77d354371f338ae4815f8e9b7a1 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 24 Aug 2015 17:58:11 -0700 Subject: [PATCH 2/7] fix hand IK rotations or all models --- interface/src/avatar/SkeletonModel.cpp | 50 +++----- libraries/animation/src/JointState.cpp | 18 +++ libraries/animation/src/JointState.h | 5 + libraries/animation/src/Rig.cpp | 4 +- .../src/input-plugins/SixenseManager.cpp | 112 +++++++++++++----- 5 files changed, 122 insertions(+), 67 deletions(-) diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index 752fb55ce6..84f787c802 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -157,11 +157,11 @@ void SkeletonModel::simulate(float deltaTime, bool fullUpdate) { setBlendshapeCoefficients(_owningAvatar->getHead()->getBlendshapeCoefficients()); Model::simulate(deltaTime, fullUpdate); - + if (!isActive() || !_owningAvatar->isMyAvatar()) { return; // only simulate for own avatar } - + MyAvatar* myAvatar = static_cast(_owningAvatar); if (myAvatar->isPlaying()) { // Don't take inputs if playing back a recording. @@ -248,40 +248,24 @@ void SkeletonModel::applyHandPosition(int jointIndex, const glm::vec3& position) rotationBetween(handRotation * glm::vec3(-sign, 0.0f, 0.0f), forearmVector), true, PALM_PRIORITY); } - void SkeletonModel::applyPalmData(int jointIndex, PalmData& palm) { if (jointIndex == -1 || jointIndex >= _rig->getJointStateCount()) { return; } const FBXGeometry& geometry = _geometry->getFBXGeometry(); - float sign = (jointIndex == geometry.rightHandJointIndex) ? 1.0f : -1.0f; int parentJointIndex = geometry.joints.at(jointIndex).parentIndex; if (parentJointIndex == -1) { return; } - - // rotate palm to align with its normal (normal points out of hand's 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::vec3 palmNormal = inverseRotation * palm.getNormal(); - glm::vec3 fingerDirection = inverseRotation * palm.getFingerDirection(); - glm::quat palmRotation = rotationBetween(geometry.palmDirection, palmNormal); - palmRotation = rotationBetween(palmRotation * glm::vec3(-sign, 0.0f, 0.0f), fingerDirection) * palmRotation; + // the palm's "raw" rotation is already in the model-frame + glm::quat palmRotation = palm.getRawRotation(); - if (Menu::getInstance()->isOptionChecked(MenuOption::AlternateIK)) { - _rig->setHandPosition(jointIndex, palmPosition, palmRotation, extractUniformScale(_scale), PALM_PRIORITY); - } else if (Menu::getInstance()->isOptionChecked(MenuOption::AlignForearmsWithWrists)) { - float forearmLength = geometry.joints.at(jointIndex).distanceToParent * extractUniformScale(_scale); - glm::vec3 forearm = palmRotation * glm::vec3(sign * forearmLength, 0.0f, 0.0f); - setJointPosition(parentJointIndex, palmPosition + forearm, - glm::quat(), false, -1, false, glm::vec3(0.0f, -1.0f, 0.0f), PALM_PRIORITY); - _rig->setJointRotationInBindFrame(parentJointIndex, palmRotation, PALM_PRIORITY); - // lock hand to forearm by slamming its rotation (in parent-frame) to identity - _rig->setJointRotationInConstrainedFrame(jointIndex, glm::quat(), PALM_PRIORITY); - } else { - inverseKinematics(jointIndex, palmPosition, palmRotation, PALM_PRIORITY); - } + inverseKinematics(jointIndex, palmPosition, palmRotation, PALM_PRIORITY); } void SkeletonModel::renderJointConstraints(gpu::Batch& batch, int jointIndex) { @@ -301,13 +285,13 @@ void SkeletonModel::renderJointConstraints(gpu::Batch& batch, int jointIndex) { _rotation : _rotation * _rig->getJointState(joint.parentIndex).getRotation(); float fanScale = directionSize * 0.75f; - + Transform transform = Transform(); transform.setTranslation(position); transform.setRotation(parentRotation); transform.setScale(fanScale); batch.setModelTransform(transform); - + const int AXIS_COUNT = 3; auto geometryCache = DependencyManager::get(); @@ -318,7 +302,7 @@ void SkeletonModel::renderJointConstraints(gpu::Batch& batch, int jointIndex) { } glm::vec3 axis; axis[i] = 1.0f; - + glm::vec3 otherAxis; if (i == 0) { otherAxis.y = 1.0f; @@ -339,18 +323,18 @@ void SkeletonModel::renderJointConstraints(gpu::Batch& batch, int jointIndex) { // better if the skeleton model cached these buffers for each of the joints they are rendering geometryCache->updateVertices(_triangleFanID, points, color); geometryCache->renderVertices(batch, gpu::TRIANGLE_FAN, _triangleFanID); - + } - + renderOrientationDirections(batch, jointIndex, position, _rotation * jointState.getRotation(), directionSize); jointIndex = joint.parentIndex; - + } while (jointIndex != -1 && geometry.joints.at(jointIndex).isFree); } -void SkeletonModel::renderOrientationDirections(gpu::Batch& batch, int jointIndex, +void SkeletonModel::renderOrientationDirections(gpu::Batch& batch, int jointIndex, glm::vec3 position, const glm::quat& orientation, float size) { - + auto geometryCache = DependencyManager::get(); if (!_jointOrientationLines.contains(jointIndex)) { @@ -486,7 +470,7 @@ void SkeletonModel::buildShapes() { if (_geometry == NULL || _rig->jointStatesEmpty()) { return; } - + const FBXGeometry& geometry = _geometry->getFBXGeometry(); if (geometry.joints.isEmpty() || geometry.rootJointIndex == -1) { // rootJointIndex == -1 if the avatar model has no skeleton @@ -551,7 +535,7 @@ void SkeletonModel::renderBoundingCollisionShapes(gpu::Batch& batch, float alpha geometryCache->renderSphere(batch, _boundingCapsuleRadius, BALL_SUBDIVISIONS, BALL_SUBDIVISIONS, glm::vec4(0.6f, 0.6f, 0.8f, alpha)); - // draw a yellow sphere at the capsule bottom point + // draw a yellow sphere at the capsule bottom point glm::vec3 bottomPoint = topPoint - glm::vec3(0.0f, -_boundingCapsuleHeight, 0.0f); glm::vec3 axis = topPoint - bottomPoint; transform.setTranslation(bottomPoint); diff --git a/libraries/animation/src/JointState.cpp b/libraries/animation/src/JointState.cpp index 78da30fdcd..edea3b462d 100644 --- a/libraries/animation/src/JointState.cpp +++ b/libraries/animation/src/JointState.cpp @@ -153,6 +153,24 @@ void JointState::setRotationInBindFrame(const glm::quat& rotation, float priorit } } +void JointState::setRotationInModelFrame(const glm::quat& rotationInModelFrame, float priority, bool constrain) { + // rotation is from bind- to model-frame + if (priority >= _animationPriority) { + glm::quat parentRotation = computeParentRotation(); + + // R = Rp * Rpre * r * Rpost + // R' = Rp * Rpre * r' * Rpost + // r' = (Rp * Rpre)^ * R' * Rpost^ + glm::quat targetRotation = glm::inverse(parentRotation * _preRotation) * rotationInModelFrame * glm::inverse(_postRotation); + if (constrain && _constraint) { + _constraint->softClamp(targetRotation, _rotationInConstrainedFrame, 0.5f); + } + _rotationInConstrainedFrame = glm::normalize(targetRotation); + _transformChanged = true; + _animationPriority = priority; + } +} + void JointState::clearTransformTranslation() { _transform[3][0] = 0.0f; _transform[3][1] = 0.0f; diff --git a/libraries/animation/src/JointState.h b/libraries/animation/src/JointState.h index 8d2c1a7cd0..4f45661eb2 100644 --- a/libraries/animation/src/JointState.h +++ b/libraries/animation/src/JointState.h @@ -82,6 +82,11 @@ public: /// NOTE: the JointState's model-frame transform/rotation are NOT updated! void setRotationInBindFrame(const glm::quat& rotation, float priority, bool constrain = false); + /// \param rotationInModelRame is in model-frame + /// computes and sets new _rotationInConstrainedFrame to match rotationInModelFrame + /// NOTE: the JointState's model-frame transform/rotation are NOT updated! + void setRotationInModelFrame(const glm::quat& rotationInModelFrame, float priority, bool constrain); + void setRotationInConstrainedFrame(glm::quat targetRotation, float priority, bool constrain = false, float mix = 1.0f); void setVisibleRotationInConstrainedFrame(const glm::quat& targetRotation); const glm::quat& getRotationInConstrainedFrame() const { return _rotationInConstrainedFrame; } diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 3e466b94d6..d265770f99 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -586,7 +586,7 @@ bool Rig::setJointPosition(int jointIndex, const glm::vec3& position, const glm: void Rig::inverseKinematics(int endIndex, glm::vec3 targetPosition, const glm::quat& targetRotation, float priority, const QVector& freeLineage, glm::mat4 parentTransform) { - // NOTE: targetRotation is from bind- to model-frame + // NOTE: targetRotation is from in model-frame if (endIndex == -1 || _jointStates.isEmpty()) { return; @@ -690,7 +690,7 @@ void Rig::inverseKinematics(int endIndex, glm::vec3 targetPosition, const glm::q } while (numIterations < MAX_ITERATION_COUNT && distanceToGo < ACCEPTABLE_IK_ERROR); // set final rotation of the end joint - endState.setRotationInBindFrame(targetRotation, priority, true); + endState.setRotationInModelFrame(targetRotation, priority, true); } bool Rig::restoreJointPosition(int jointIndex, float fraction, float priority, const QVector& freeLineage) { diff --git a/libraries/input-plugins/src/input-plugins/SixenseManager.cpp b/libraries/input-plugins/src/input-plugins/SixenseManager.cpp index 205350e751..c6cc8a404b 100644 --- a/libraries/input-plugins/src/input-plugins/SixenseManager.cpp +++ b/libraries/input-plugins/src/input-plugins/SixenseManager.cpp @@ -21,6 +21,10 @@ #include "SixenseManager.h" #include "UserActivityLogger.h" +#ifdef HAVE_SIXENSE + #include "sixense.h" +#endif + // TODO: This should not be here #include Q_DECLARE_LOGGING_CATEGORY(inputplugins) @@ -30,8 +34,6 @@ Q_LOGGING_CATEGORY(inputplugins, "hifi.inputplugins") const unsigned int LEFT_MASK = 0; const unsigned int RIGHT_MASK = 1U << 1; -#ifdef HAVE_SIXENSE - const int CALIBRATION_STATE_IDLE = 0; const int CALIBRATION_STATE_X = 1; const int CALIBRATION_STATE_Y = 2; @@ -51,8 +53,6 @@ typedef int (*SixenseTakeIntFunction)(int); typedef int (*SixenseTakeIntAndSixenseControllerData)(int, sixenseControllerData*); #endif -#endif - const QString SixenseManager::NAME = "Sixense"; const QString MENU_PARENT = "Avatar"; @@ -66,8 +66,8 @@ SixenseManager& SixenseManager::getInstance() { } SixenseManager::SixenseManager() : - InputDevice("Hydra"), -#if defined(HAVE_SIXENSE) && defined(__APPLE__) + InputDevice("Hydra"), +#ifdef __APPLE__ _sixenseLibrary(NULL), #endif _hydrasConnected(false) @@ -213,18 +213,16 @@ void SixenseManager::update(float deltaTime, bool jointsCaptured) { // NOTE: Sixense API returns pos data in millimeters but we IMMEDIATELY convert to meters. glm::vec3 position(data->pos[0], data->pos[1], data->pos[2]); position *= METERS_PER_MILLIMETER; - + // Check to see if this hand/controller is on the base const float CONTROLLER_AT_BASE_DISTANCE = 0.075f; if (glm::length(position) >= CONTROLLER_AT_BASE_DISTANCE) { handleButtonEvent(data->buttons, numActiveControllers - 1); handleAxisEvent(data->joystick_x, data->joystick_y, data->trigger, numActiveControllers - 1); - // Rotation of Palm - glm::quat rotation(data->rot_quat[3], -data->rot_quat[0], data->rot_quat[1], -data->rot_quat[2]); - rotation = glm::angleAxis(PI, glm::vec3(0.0f, 1.0f, 0.0f)) * _orbRotation * rotation; - 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(position, rotation, numActiveControllers - 1); } else { _poseStateMap.clear(); @@ -232,7 +230,7 @@ void SixenseManager::update(float deltaTime, bool jointsCaptured) { } else { _poseStateMap[(numActiveControllers - 1) == 0 ? LEFT_HAND : RIGHT_HAND] = UserInputMapper::PoseValue(); } - + // // Read controller buttons and joystick into the hand // palm->setControllerButtons(data->buttons); // palm->setTrigger(data->trigger); @@ -242,7 +240,7 @@ void SixenseManager::update(float deltaTime, bool jointsCaptured) { if (numActiveControllers == 2) { updateCalibration(controllers); } - + for (auto axisState : _axisStateMap) { if (fabsf(axisState.second) < CONTROLLER_THRESHOLD) { _axisStateMap[axisState.first] = 0.0f; @@ -436,16 +434,66 @@ void SixenseManager::handleButtonEvent(unsigned int buttons, int index) { void SixenseManager::handlePoseEvent(glm::vec3 position, glm::quat rotation, int index) { #ifdef HAVE_SIXENSE + // From ABOVE the sixense coordinate frame looks like this: + // + // | + // USB cables + // | + // .-. user + // (Orb) --neckX---- forward + // '-' | + // | | user + // neckZ y +---- right + // | (o)-----x + // | + // | + // z + // Transform the measured position into body frame. glm::vec3 neck = _neckBase; // Set y component of the "neck" to raise the measured position a little bit. neck.y = 0.5f; position = _orbRotation * (position - neck); - - // adjustment for hydra controllers fit into hands - float sign = (index == 0) ? -1.0f : 1.0f; - rotation *= glm::angleAxis(sign * PI/4.0f, glm::vec3(0.0f, 0.0f, 1.0f)); - + + // From ABOVE the hand canonical axes looks like this: + // + // | | | | y | | | | + // | | | | | | | | | + // | | | | | + // |left | / x----(+) \ |right| + // | _/ z \_ | + // | | | | + // | | | | + // + + // To convert sixense's delta-rotation into the hand's frame we will have to transform it like so: + // + // deltaHand = Qsh^ * deltaSixense * Qsh + // + // where Qsh = transform from sixense axes to hand axes. By inspection we can determine Qsh: + // + // Qsh = angleAxis(PI, zAxis) * angleAxis(-PI/2, xAxis) + // + const glm::vec3 xAxis = glm::vec3(1.0f, 0.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); + + // In addition to Qsh each hand has pre-offset introduced by the shape of the sixense controllers + // and how they fit into the hand in their relaxed state. This offset is a quarter turn about + // the sixense's z-axis, with its direction different for the two hands: + float sign = (index == 0) ? 1.0f : -1.0f; + 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); + + // The total rotation of the hand uses the formula: + // + // rotation = postOffset * Qsh^ * (measuredRotation * preOffset) * Qsh + // + rotation = postOffset * glm::inverse(sixenseToHand) * rotation * preOffset * sixenseToHand; + _poseStateMap[makeInput(JointChannel(index)).getChannel()] = UserInputMapper::PoseValue(position, rotation); #endif // HAVE_SIXENSE } @@ -453,7 +501,7 @@ void SixenseManager::handlePoseEvent(glm::vec3 position, glm::quat rotation, int void SixenseManager::registerToUserInputMapper(UserInputMapper& mapper) { // Grab the current free device ID _deviceID = mapper.getFreeDeviceID(); - + auto proxy = std::make_shared(_name); proxy->getButton = [this] (const UserInputMapper::Input& input, int timestamp) -> bool { return this->getButton(input.getChannel()); }; proxy->getAxis = [this] (const UserInputMapper::Input& input, int timestamp) -> float { return this->getAxis(input.getChannel()); }; @@ -465,25 +513,25 @@ void SixenseManager::registerToUserInputMapper(UserInputMapper& mapper) { availableInputs.append(UserInputMapper::InputPair(makeInput(BUTTON_2, 0), "Left Button 2")); availableInputs.append(UserInputMapper::InputPair(makeInput(BUTTON_3, 0), "Left Button 3")); availableInputs.append(UserInputMapper::InputPair(makeInput(BUTTON_4, 0), "Left Button 4")); - + availableInputs.append(UserInputMapper::InputPair(makeInput(BUTTON_FWD, 0), "L1")); availableInputs.append(UserInputMapper::InputPair(makeInput(BACK_TRIGGER, 0), "L2")); - + availableInputs.append(UserInputMapper::InputPair(makeInput(AXIS_Y_POS, 0), "Left Stick Up")); availableInputs.append(UserInputMapper::InputPair(makeInput(AXIS_Y_NEG, 0), "Left Stick Down")); availableInputs.append(UserInputMapper::InputPair(makeInput(AXIS_X_POS, 0), "Left Stick Right")); availableInputs.append(UserInputMapper::InputPair(makeInput(AXIS_X_NEG, 0), "Left Stick Left")); availableInputs.append(UserInputMapper::InputPair(makeInput(BUTTON_TRIGGER, 0), "Left Trigger Press")); - + availableInputs.append(UserInputMapper::InputPair(makeInput(BUTTON_0, 1), "Right Start")); availableInputs.append(UserInputMapper::InputPair(makeInput(BUTTON_1, 1), "Right Button 1")); availableInputs.append(UserInputMapper::InputPair(makeInput(BUTTON_2, 1), "Right Button 2")); availableInputs.append(UserInputMapper::InputPair(makeInput(BUTTON_3, 1), "Right Button 3")); availableInputs.append(UserInputMapper::InputPair(makeInput(BUTTON_4, 1), "Right Button 4")); - + availableInputs.append(UserInputMapper::InputPair(makeInput(BUTTON_FWD, 1), "R1")); availableInputs.append(UserInputMapper::InputPair(makeInput(BACK_TRIGGER, 1), "R2")); - + availableInputs.append(UserInputMapper::InputPair(makeInput(AXIS_Y_POS, 1), "Right Stick Up")); availableInputs.append(UserInputMapper::InputPair(makeInput(AXIS_Y_NEG, 1), "Right Stick Down")); availableInputs.append(UserInputMapper::InputPair(makeInput(AXIS_X_POS, 1), "Right Stick Right")); @@ -506,35 +554,35 @@ void SixenseManager::assignDefaultInputMapping(UserInputMapper& mapper) { const float JOYSTICK_PITCH_SPEED = 0.25f; const float BUTTON_MOVE_SPEED = 1.0f; const float BOOM_SPEED = 0.1f; - + // Left Joystick: Movement, strafing mapper.addInputChannel(UserInputMapper::LONGITUDINAL_FORWARD, makeInput(AXIS_Y_POS, 0), JOYSTICK_MOVE_SPEED); mapper.addInputChannel(UserInputMapper::LONGITUDINAL_BACKWARD, makeInput(AXIS_Y_NEG, 0), JOYSTICK_MOVE_SPEED); mapper.addInputChannel(UserInputMapper::LATERAL_RIGHT, makeInput(AXIS_X_POS, 0), JOYSTICK_MOVE_SPEED); mapper.addInputChannel(UserInputMapper::LATERAL_LEFT, makeInput(AXIS_X_NEG, 0), JOYSTICK_MOVE_SPEED); - + // Right Joystick: Camera orientation mapper.addInputChannel(UserInputMapper::YAW_RIGHT, makeInput(AXIS_X_POS, 1), JOYSTICK_YAW_SPEED); mapper.addInputChannel(UserInputMapper::YAW_LEFT, makeInput(AXIS_X_NEG, 1), JOYSTICK_YAW_SPEED); mapper.addInputChannel(UserInputMapper::PITCH_UP, makeInput(AXIS_Y_POS, 1), JOYSTICK_PITCH_SPEED); mapper.addInputChannel(UserInputMapper::PITCH_DOWN, makeInput(AXIS_Y_NEG, 1), JOYSTICK_PITCH_SPEED); - + // Buttons mapper.addInputChannel(UserInputMapper::BOOM_IN, makeInput(BUTTON_3, 0), BOOM_SPEED); mapper.addInputChannel(UserInputMapper::BOOM_OUT, makeInput(BUTTON_1, 0), BOOM_SPEED); - + mapper.addInputChannel(UserInputMapper::VERTICAL_UP, makeInput(BUTTON_3, 1), BUTTON_MOVE_SPEED); mapper.addInputChannel(UserInputMapper::VERTICAL_DOWN, makeInput(BUTTON_1, 1), BUTTON_MOVE_SPEED); - + mapper.addInputChannel(UserInputMapper::SHIFT, makeInput(BUTTON_2, 0)); mapper.addInputChannel(UserInputMapper::SHIFT, makeInput(BUTTON_2, 1)); - + mapper.addInputChannel(UserInputMapper::ACTION1, makeInput(BUTTON_4, 0)); mapper.addInputChannel(UserInputMapper::ACTION2, makeInput(BUTTON_4, 1)); - + mapper.addInputChannel(UserInputMapper::LEFT_HAND, makeInput(LEFT_HAND)); mapper.addInputChannel(UserInputMapper::RIGHT_HAND, makeInput(RIGHT_HAND)); - + mapper.addInputChannel(UserInputMapper::LEFT_HAND_CLICK, makeInput(BACK_TRIGGER, 0)); mapper.addInputChannel(UserInputMapper::RIGHT_HAND_CLICK, makeInput(BACK_TRIGGER, 1)); From 617e210fc6f2071d4cc345aafff0646eb993598a Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 27 Aug 2015 16:07:36 -0700 Subject: [PATCH 3/7] fix for vive controller to place hand for IK --- .../input-plugins/ViveControllerManager.cpp | 61 ++++++++++++++++++- 1 file changed, 58 insertions(+), 3 deletions(-) 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); From fba7478c52784be85c1fa40413be6603f264f6cc Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 27 Aug 2015 16:44:08 -0700 Subject: [PATCH 4/7] remove menu items for legacy IK options --- interface/src/Menu.cpp | 2 -- interface/src/Menu.h | 2 -- 2 files changed, 4 deletions(-) diff --git a/interface/src/Menu.cpp b/interface/src/Menu.cpp index 4696463181..18015804f8 100644 --- a/interface/src/Menu.cpp +++ b/interface/src/Menu.cpp @@ -441,8 +441,6 @@ Menu::Menu() { SLOT(toggleConnexion(bool))); MenuWrapper* handOptionsMenu = developerMenu->addMenu("Hands"); - addCheckableActionToQMenuAndActionHash(handOptionsMenu, MenuOption::AlignForearmsWithWrists, 0, false); - addCheckableActionToQMenuAndActionHash(handOptionsMenu, MenuOption::AlternateIK, 0, false); addCheckableActionToQMenuAndActionHash(handOptionsMenu, MenuOption::DisplayHands, 0, true); addCheckableActionToQMenuAndActionHash(handOptionsMenu, MenuOption::DisplayHandTargets, 0, false); addCheckableActionToQMenuAndActionHash(handOptionsMenu, MenuOption::HandMouseInput, 0, true); diff --git a/interface/src/Menu.h b/interface/src/Menu.h index 94e49abcc7..f1fbb17895 100644 --- a/interface/src/Menu.h +++ b/interface/src/Menu.h @@ -131,8 +131,6 @@ namespace MenuOption { const QString AboutApp = "About Interface"; const QString AddRemoveFriends = "Add/Remove Friends..."; const QString AddressBar = "Show Address Bar"; - const QString AlignForearmsWithWrists = "Align Forearms with Wrists"; - const QString AlternateIK = "Alternate IK"; const QString Animations = "Animations..."; const QString Atmosphere = "Atmosphere"; const QString Attachments = "Attachments..."; From bdcc68ce369484d35c2f6cc70b19c7164c0ac4f7 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 31 Aug 2015 14:30:32 -0700 Subject: [PATCH 5/7] fix bug: allow IK to iterate more than once also changed name of parentTransform to be rootTransform for more correctness --- interface/src/avatar/SkeletonModel.cpp | 6 ++-- libraries/animation/src/AvatarRig.cpp | 4 +-- libraries/animation/src/AvatarRig.h | 2 +- libraries/animation/src/EntityRig.cpp | 4 +-- libraries/animation/src/EntityRig.h | 2 +- libraries/animation/src/Rig.cpp | 50 ++++++++++++++++---------- libraries/animation/src/Rig.h | 12 +++---- 7 files changed, 47 insertions(+), 33 deletions(-) diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index 84f787c802..3558a305fd 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -41,7 +41,7 @@ SkeletonModel::~SkeletonModel() { void SkeletonModel::initJointStates(QVector states) { const FBXGeometry& geometry = _geometry->getFBXGeometry(); - glm::mat4 parentTransform = glm::scale(_scale) * glm::translate(_offset) * geometry.offset; + glm::mat4 rootTransform = glm::scale(_scale) * glm::translate(_offset) * geometry.offset; int rootJointIndex = geometry.rootJointIndex; int leftHandJointIndex = geometry.leftHandJointIndex; @@ -51,7 +51,7 @@ void SkeletonModel::initJointStates(QVector states) { int rightElbowJointIndex = rightHandJointIndex >= 0 ? geometry.joints.at(rightHandJointIndex).parentIndex : -1; int rightShoulderJointIndex = rightElbowJointIndex >= 0 ? geometry.joints.at(rightElbowJointIndex).parentIndex : -1; - _boundingRadius = _rig->initJointStates(states, parentTransform, + _boundingRadius = _rig->initJointStates(states, rootTransform, rootJointIndex, leftHandJointIndex, leftElbowJointIndex, @@ -83,7 +83,7 @@ void SkeletonModel::initJointStates(QVector states) { // of its root joint and we need that done before we try to build shapes hence we // recompute all joint transforms at this time. for (int i = 0; i < _rig->getJointStateCount(); i++) { - _rig->updateJointState(i, parentTransform); + _rig->updateJointState(i, rootTransform); } buildShapes(); diff --git a/libraries/animation/src/AvatarRig.cpp b/libraries/animation/src/AvatarRig.cpp index ebf0a8795e..0727d0956d 100644 --- a/libraries/animation/src/AvatarRig.cpp +++ b/libraries/animation/src/AvatarRig.cpp @@ -12,7 +12,7 @@ #include "AvatarRig.h" /// Updates the state of the joint at the specified index. -void AvatarRig::updateJointState(int index, glm::mat4 parentTransform) { +void AvatarRig::updateJointState(int index, glm::mat4 rootTransform) { if (index < 0 && index >= _jointStates.size()) { return; // bail } @@ -21,7 +21,7 @@ void AvatarRig::updateJointState(int index, glm::mat4 parentTransform) { // compute model transforms if (index == _rootJointIndex) { // we always zero-out the translation part of an avatar's root join-transform. - state.computeTransform(parentTransform); + state.computeTransform(rootTransform); clearJointTransformTranslation(index); } else { // guard against out-of-bounds access to _jointStates diff --git a/libraries/animation/src/AvatarRig.h b/libraries/animation/src/AvatarRig.h index 5e2153e226..f137e89939 100644 --- a/libraries/animation/src/AvatarRig.h +++ b/libraries/animation/src/AvatarRig.h @@ -21,7 +21,7 @@ class AvatarRig : public Rig { public: ~AvatarRig() {} - virtual void updateJointState(int index, glm::mat4 parentTransform); + virtual void updateJointState(int index, glm::mat4 rootTransform); virtual void setHandPosition(int jointIndex, const glm::vec3& position, const glm::quat& rotation, float scale, float priority); }; diff --git a/libraries/animation/src/EntityRig.cpp b/libraries/animation/src/EntityRig.cpp index eddbdd6464..e65407116b 100644 --- a/libraries/animation/src/EntityRig.cpp +++ b/libraries/animation/src/EntityRig.cpp @@ -12,13 +12,13 @@ #include "EntityRig.h" /// Updates the state of the joint at the specified index. -void EntityRig::updateJointState(int index, glm::mat4 parentTransform) { +void EntityRig::updateJointState(int index, glm::mat4 rootTransform) { JointState& state = _jointStates[index]; // compute model transforms int parentIndex = state.getParentIndex(); if (parentIndex == -1) { - state.computeTransform(parentTransform); + state.computeTransform(rootTransform); } else { // guard against out-of-bounds access to _jointStates if (parentIndex >= 0 && parentIndex < _jointStates.size()) { diff --git a/libraries/animation/src/EntityRig.h b/libraries/animation/src/EntityRig.h index 9b519a7bfe..b36ffb9874 100644 --- a/libraries/animation/src/EntityRig.h +++ b/libraries/animation/src/EntityRig.h @@ -21,7 +21,7 @@ class EntityRig : public Rig { public: ~EntityRig() {} - virtual void updateJointState(int index, glm::mat4 parentTransform); + virtual void updateJointState(int index, glm::mat4 rootTransform); virtual void setHandPosition(int jointIndex, const glm::vec3& position, const glm::quat& rotation, float scale, float priority) {} }; diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index d265770f99..2533c9d95c 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -185,7 +185,7 @@ void Rig::deleteAnimations() { _animationHandles.clear(); } -float Rig::initJointStates(QVector states, glm::mat4 parentTransform, +float Rig::initJointStates(QVector states, glm::mat4 rootTransform, int rootJointIndex, int leftHandJointIndex, int leftElbowJointIndex, @@ -203,7 +203,7 @@ float Rig::initJointStates(QVector states, glm::mat4 parentTransform _rightElbowJointIndex = rightElbowJointIndex; _rightShoulderJointIndex = rightShoulderJointIndex; - initJointTransforms(parentTransform); + initJointTransforms(rootTransform); int numStates = _jointStates.size(); float radius = 0.0f; @@ -233,14 +233,14 @@ int Rig::indexOfJoint(const QString& jointName) { } -void Rig::initJointTransforms(glm::mat4 parentTransform) { +void Rig::initJointTransforms(glm::mat4 rootTransform) { // compute model transforms int numStates = _jointStates.size(); for (int i = 0; i < numStates; ++i) { JointState& state = _jointStates[i]; int parentIndex = state.getParentIndex(); if (parentIndex == -1) { - state.initTransform(parentTransform); + state.initTransform(rootTransform); } else { const JointState& parentState = _jointStates.at(parentIndex); state.initTransform(parentState.getTransform()); @@ -448,7 +448,7 @@ void Rig::computeMotionAnimationState(float deltaTime, const glm::vec3& worldPos _lastPosition = worldPosition; } -void Rig::updateAnimations(float deltaTime, glm::mat4 parentTransform) { +void Rig::updateAnimations(float deltaTime, glm::mat4 rootTransform) { // First normalize the fades so that they sum to 1.0. // update the fade data in each animation (not normalized as they are an independent propert of animation) @@ -494,7 +494,7 @@ void Rig::updateAnimations(float deltaTime, glm::mat4 parentTransform) { } for (int i = 0; i < _jointStates.size(); i++) { - updateJointState(i, parentTransform); + updateJointState(i, rootTransform); } for (int i = 0; i < _jointStates.size(); i++) { _jointStates[i].resetTransformChanged(); @@ -503,7 +503,7 @@ void Rig::updateAnimations(float deltaTime, glm::mat4 parentTransform) { bool Rig::setJointPosition(int jointIndex, const glm::vec3& position, const glm::quat& rotation, bool useRotation, int lastFreeIndex, bool allIntermediatesFree, const glm::vec3& alignment, float priority, - const QVector& freeLineage, glm::mat4 parentTransform) { + const QVector& freeLineage, glm::mat4 rootTransform) { if (jointIndex == -1 || _jointStates.isEmpty()) { return false; } @@ -555,7 +555,7 @@ bool Rig::setJointPosition(int jointIndex, const glm::vec3& position, const glm: glm::vec3 positionSum; for (int k = j - 1; k > 0; k--) { int index = freeLineage.at(k); - updateJointState(index, parentTransform); + updateJointState(index, rootTransform); positionSum += extractTranslation(_jointStates.at(index).getTransform()); } glm::vec3 projectedCenterOfMass = glm::cross(jointVector, @@ -578,14 +578,14 @@ bool Rig::setJointPosition(int jointIndex, const glm::vec3& position, const glm: // now update the joint states from the top for (int j = freeLineage.size() - 1; j >= 0; j--) { - updateJointState(freeLineage.at(j), parentTransform); + updateJointState(freeLineage.at(j), rootTransform); } return true; } void Rig::inverseKinematics(int endIndex, glm::vec3 targetPosition, const glm::quat& targetRotation, float priority, - const QVector& freeLineage, glm::mat4 parentTransform) { + const QVector& freeLineage, glm::mat4 rootTransform) { // NOTE: targetRotation is from in model-frame if (endIndex == -1 || _jointStates.isEmpty()) { @@ -604,12 +604,27 @@ void Rig::inverseKinematics(int endIndex, glm::vec3 targetPosition, const glm::q const JointState& state = _jointStates.at(index); int parentIndex = state.getParentIndex(); if (parentIndex == -1) { - topParentTransform = parentTransform; + topParentTransform = rootTransform; } else { topParentTransform = _jointStates[parentIndex].getTransform(); } } + // relax toward default rotation + // NOTE: ideally this should use dt and a relaxation timescale to compute how much to relax + for (int j = 0; j < numFree; j++) { + int nextIndex = freeLineage.at(j); + JointState& nextState = _jointStates[nextIndex]; + if (! nextState.getIsFree()) { + continue; + } + + // Apply the zero rotationDelta, but use mixRotationDelta() which blends a bit of the default pose + // in the process. This provides stability to the IK solution for most models. + float mixFactor = 0.08f; + nextState.mixRotationDelta(glm::quat(), mixFactor, priority); + } + // this is a cyclic coordinate descent algorithm: see // http://www.ryanjuckett.com/programming/animation/21-cyclic-coordinate-descent-in-2d @@ -618,7 +633,7 @@ void Rig::inverseKinematics(int endIndex, glm::vec3 targetPosition, const glm::q glm::vec3 endPosition = endState.getPosition(); float distanceToGo = glm::distance(targetPosition, endPosition); - const int MAX_ITERATION_COUNT = 2; + const int MAX_ITERATION_COUNT = 3; const float ACCEPTABLE_IK_ERROR = 0.005f; // 5mm int numIterations = 0; do { @@ -655,7 +670,7 @@ void Rig::inverseKinematics(int endIndex, glm::vec3 targetPosition, const glm::q glm::cross(worldAlignment, leverArm)); float gravityAngle = glm::angle(gravityDelta); - const float MIN_GRAVITY_ANGLE = 0.1f; + const float MIN_GRAVITY_ANGLE = 0.5f; float mixFactor = 0.5f; if (gravityAngle < MIN_GRAVITY_ANGLE) { // the final rotation is a mix of the two @@ -664,11 +679,10 @@ void Rig::inverseKinematics(int endIndex, glm::vec3 targetPosition, const glm::q deltaRotation = safeMix(deltaRotation, gravityDelta, mixFactor); } - // Apply the rotation, but use mixRotationDelta() which blends a bit of the default pose - // in the process. This provides stability to the IK solution for most models. + // Apply the rotation delta. glm::quat oldNextRotation = nextState.getRotation(); - float mixFactor = 0.03f; - nextState.mixRotationDelta(deltaRotation, mixFactor, priority); + float mixFactor = 0.05f; + nextState.applyRotationDelta(deltaRotation, mixFactor, priority); // measure the result of the rotation which may have been modified by // blending and constraints @@ -687,7 +701,7 @@ void Rig::inverseKinematics(int endIndex, glm::vec3 targetPosition, const glm::q // measure our success endPosition = endState.getPosition(); distanceToGo = glm::distance(targetPosition, endPosition); - } while (numIterations < MAX_ITERATION_COUNT && distanceToGo < ACCEPTABLE_IK_ERROR); + } while (numIterations < MAX_ITERATION_COUNT && distanceToGo > ACCEPTABLE_IK_ERROR); // set final rotation of the end joint endState.setRotationInModelFrame(targetRotation, priority, true); diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index 8da20062cf..91993349fe 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -92,7 +92,7 @@ public: float priority = 1.0f, bool loop = false, bool hold = false, float firstFrame = 0.0f, float lastFrame = FLT_MAX, const QStringList& maskedJoints = QStringList(), bool startAutomatically = false); - float initJointStates(QVector states, glm::mat4 parentTransform, + float initJointStates(QVector states, glm::mat4 rootTransform, int rootJointIndex, int leftHandJointIndex, int leftElbowJointIndex, @@ -104,7 +104,7 @@ public: int getJointStateCount() const { return _jointStates.size(); } int indexOfJoint(const QString& jointName) ; - void initJointTransforms(glm::mat4 parentTransform); + void initJointTransforms(glm::mat4 rootTransform); void clearJointTransformTranslation(int jointIndex); void reset(const QVector& fbxJoints); bool getJointStateRotation(int index, glm::quat& rotation) const; @@ -135,12 +135,12 @@ public: // Start or stop animations as needed. void computeMotionAnimationState(float deltaTime, const glm::vec3& worldPosition, const glm::vec3& worldVelocity, const glm::quat& worldRotation); // Regardless of who started the animations or how many, update the joints. - void updateAnimations(float deltaTime, glm::mat4 parentTransform); + void updateAnimations(float deltaTime, glm::mat4 rootTransform); bool setJointPosition(int jointIndex, const glm::vec3& position, const glm::quat& rotation, bool useRotation, int lastFreeIndex, bool allIntermediatesFree, const glm::vec3& alignment, float priority, - const QVector& freeLineage, glm::mat4 parentTransform); + const QVector& freeLineage, glm::mat4 rootTransform); void inverseKinematics(int endIndex, glm::vec3 targetPosition, const glm::quat& targetRotation, float priority, - const QVector& freeLineage, glm::mat4 parentTransform); + const QVector& freeLineage, glm::mat4 rootTransform); bool restoreJointPosition(int jointIndex, float fraction, float priority, const QVector& freeLineage); float getLimbLength(int jointIndex, const QVector& freeLineage, const glm::vec3 scale, const QVector& fbxJoints) const; @@ -152,7 +152,7 @@ public: glm::quat getJointDefaultRotationInParentFrame(int jointIndex); void updateVisibleJointStates(); - virtual void updateJointState(int index, glm::mat4 parentTransform) = 0; + virtual void updateJointState(int index, glm::mat4 rootTransform) = 0; void setEnableRig(bool isEnabled) { _enableRig = isEnabled; } From 0e5b3573627db918af214b11d95835d7f9d43181 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 31 Aug 2015 14:32:49 -0700 Subject: [PATCH 6/7] revert unintended change to "gravity" term in IK --- libraries/animation/src/Rig.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 2533c9d95c..641235dc96 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -671,7 +671,7 @@ void Rig::inverseKinematics(int endIndex, glm::vec3 targetPosition, const glm::q float gravityAngle = glm::angle(gravityDelta); const float MIN_GRAVITY_ANGLE = 0.5f; - float mixFactor = 0.5f; + float mixFactor = 0.1f; if (gravityAngle < MIN_GRAVITY_ANGLE) { // the final rotation is a mix of the two mixFactor = 0.5f * gravityAngle / MIN_GRAVITY_ANGLE; From 47c18fa18a1906f988ed25ce708924e28fc71375 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 31 Aug 2015 14:36:42 -0700 Subject: [PATCH 7/7] reduced gravity effect in IK --- libraries/animation/src/Rig.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 641235dc96..e8c8b4f2b8 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -670,7 +670,7 @@ void Rig::inverseKinematics(int endIndex, glm::vec3 targetPosition, const glm::q glm::cross(worldAlignment, leverArm)); float gravityAngle = glm::angle(gravityDelta); - const float MIN_GRAVITY_ANGLE = 0.5f; + const float MIN_GRAVITY_ANGLE = 0.1f; float mixFactor = 0.1f; if (gravityAngle < MIN_GRAVITY_ANGLE) { // the final rotation is a mix of the two