diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 7bf18f468c..14968fceb6 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -91,8 +91,6 @@ const float MIN_SCALE_CHANGED_DELTA = 0.001f; const int MODE_READINGS_RING_BUFFER_SIZE = 500; const float CENTIMETERS_PER_METER = 100.0f; -//#define DEBUG_DRAW_HMD_MOVING_AVERAGE - MyAvatar::MyAvatar(QThread* thread) : Avatar(thread), _yawSpeed(YAW_SPEED_DEFAULT), @@ -447,9 +445,27 @@ void MyAvatar::reset(bool andRecenter, bool andReload, bool andHead) { void MyAvatar::update(float deltaTime) { // update moving average of HMD facing in xz plane. const float HMD_FACING_TIMESCALE = getRotationRecenterFilterLength(); + const float PERCENTAGE_WEIGHT_HEAD_VS_SHOULDERS_AZIMUTH = 0.0f; // 100 percent shoulders float tau = deltaTime / HMD_FACING_TIMESCALE; - _headControllerFacingMovingAverage = lerp(_headControllerFacingMovingAverage, _headControllerFacing, tau); + setHipToHandController(computeHandAzimuth()); + + // put the average hand azimuth into sensor space. + // then mix it with head facing direction to determine rotation recenter + if (getControllerPoseInAvatarFrame(controller::Action::LEFT_HAND).isValid() && getControllerPoseInAvatarFrame(controller::Action::RIGHT_HAND).isValid()) { + glm::vec3 handHipAzimuthWorldSpace = transformVectorFast(getTransform().getMatrix(), glm::vec3(_hipToHandController.x, 0.0f, _hipToHandController.y)); + glm::mat4 sensorToWorldMat = getSensorToWorldMatrix(); + glm::mat4 worldToSensorMat = glm::inverse(sensorToWorldMat); + glm::vec3 handHipAzimuthSensorSpace = transformVectorFast(worldToSensorMat, handHipAzimuthWorldSpace); + glm::vec2 normedHandHipAzimuthSensorSpace(0.0f, 1.0f); + if (glm::length(glm::vec2(handHipAzimuthSensorSpace.x, handHipAzimuthSensorSpace.z)) > 0.0f) { + normedHandHipAzimuthSensorSpace = glm::normalize(glm::vec2(handHipAzimuthSensorSpace.x, handHipAzimuthSensorSpace.z)); + } + glm::vec2 headFacingPlusHandHipAzimuthMix = lerp(normedHandHipAzimuthSensorSpace, _headControllerFacing, PERCENTAGE_WEIGHT_HEAD_VS_SHOULDERS_AZIMUTH); + _headControllerFacingMovingAverage = lerp(_headControllerFacingMovingAverage, headFacingPlusHandHipAzimuthMix, tau); + } else { + _headControllerFacingMovingAverage = lerp(_headControllerFacingMovingAverage, _headControllerFacing, tau); + } if (_smoothOrientationTimer < SMOOTH_TIME_ORIENTATION) { _rotationChanged = usecTimestampNow(); @@ -462,19 +478,23 @@ void MyAvatar::update(float deltaTime) { setCurrentStandingHeight(computeStandingHeightMode(getControllerPoseInAvatarFrame(controller::Action::HEAD))); setAverageHeadRotation(computeAverageHeadRotation(getControllerPoseInAvatarFrame(controller::Action::HEAD))); -#ifdef DEBUG_DRAW_HMD_MOVING_AVERAGE - auto sensorHeadPose = getControllerPoseInSensorFrame(controller::Action::HEAD); - glm::vec3 worldHeadPos = transformPoint(getSensorToWorldMatrix(), sensorHeadPose.getTranslation()); - glm::vec3 worldFacingAverage = transformVectorFast(getSensorToWorldMatrix(), glm::vec3(_headControllerFacingMovingAverage.x, 0.0f, _headControllerFacingMovingAverage.y)); - glm::vec3 worldFacing = transformVectorFast(getSensorToWorldMatrix(), glm::vec3(_headControllerFacing.x, 0.0f, _headControllerFacing.y)); - DebugDraw::getInstance().drawRay(worldHeadPos, worldHeadPos + worldFacing, glm::vec4(0.0f, 1.0f, 0.0f, 1.0f)); - DebugDraw::getInstance().drawRay(worldHeadPos, worldHeadPos + worldFacingAverage, glm::vec4(0.0f, 0.0f, 1.0f, 1.0f)); -#endif + if (_drawAverageFacingEnabled) { + auto sensorHeadPose = getControllerPoseInSensorFrame(controller::Action::HEAD); + glm::vec3 worldHeadPos = transformPoint(getSensorToWorldMatrix(), sensorHeadPose.getTranslation()); + glm::vec3 worldFacingAverage = transformVectorFast(getSensorToWorldMatrix(), glm::vec3(_headControllerFacingMovingAverage.x, 0.0f, _headControllerFacingMovingAverage.y)); + glm::vec3 worldFacing = transformVectorFast(getSensorToWorldMatrix(), glm::vec3(_headControllerFacing.x, 0.0f, _headControllerFacing.y)); + DebugDraw::getInstance().drawRay(worldHeadPos, worldHeadPos + worldFacing, glm::vec4(0.0f, 1.0f, 0.0f, 1.0f)); + DebugDraw::getInstance().drawRay(worldHeadPos, worldHeadPos + worldFacingAverage, glm::vec4(0.0f, 0.0f, 1.0f, 1.0f)); + + // draw hand azimuth vector + glm::vec3 handAzimuthMidpoint = transformPoint(getTransform().getMatrix(), glm::vec3(_hipToHandController.x, 0.0f, _hipToHandController.y)); + DebugDraw::getInstance().drawRay(getWorldPosition(), handAzimuthMidpoint, glm::vec4(0.0f, 1.0f, 1.0f, 1.0f)); + } if (_goToPending) { setWorldPosition(_goToPosition); setWorldOrientation(_goToOrientation); - _headControllerFacingMovingAverage = _headControllerFacing; // reset moving average + _headControllerFacingMovingAverage = _headControllerFacing; // reset moving average _goToPending = false; // updateFromHMDSensorMatrix (called from paintGL) expects that the sensorToWorldMatrix is updated for any position changes // that happen between render and Application::update (which calls updateSensorToWorldMatrix to do so). @@ -796,6 +816,47 @@ void MyAvatar::updateFromHMDSensorMatrix(const glm::mat4& hmdSensorMatrix) { } } +// Find the vector halfway between the hip to hand azimuth vectors +// This midpoint hand azimuth is in Avatar space +glm::vec2 MyAvatar::computeHandAzimuth() const { + controller::Pose leftHandPoseAvatarSpace = getLeftHandPose(); + controller::Pose rightHandPoseAvatarSpace = getRightHandPose(); + controller::Pose headPoseAvatarSpace = getControllerPoseInAvatarFrame(controller::Action::HEAD); + const float HALFWAY = 0.50f; + glm::vec2 latestHipToHandController = _hipToHandController; + + if (leftHandPoseAvatarSpace.isValid() && rightHandPoseAvatarSpace.isValid() && headPoseAvatarSpace.isValid()) { + // we need the old azimuth reading to prevent flipping the facing direction 180 + // in the case where the hands go from being slightly less than 180 apart to slightly more than 180 apart. + glm::vec2 oldAzimuthReading = _hipToHandController; + if ((glm::length(glm::vec2(rightHandPoseAvatarSpace.translation.x, rightHandPoseAvatarSpace.translation.z)) > 0.0f) && (glm::length(glm::vec2(leftHandPoseAvatarSpace.translation.x, leftHandPoseAvatarSpace.translation.z)) > 0.0f)) { + latestHipToHandController = lerp(glm::normalize(glm::vec2(rightHandPoseAvatarSpace.translation.x, rightHandPoseAvatarSpace.translation.z)), glm::normalize(glm::vec2(leftHandPoseAvatarSpace.translation.x, leftHandPoseAvatarSpace.translation.z)), HALFWAY); + } else { + latestHipToHandController = glm::vec2(0.0f, -1.0f); + } + + glm::vec3 headLookAtAvatarSpace = transformVectorFast(headPoseAvatarSpace.getMatrix(), glm::vec3(0.0f, 0.0f, 1.0f)); + glm::vec2 headAzimuthAvatarSpace = glm::vec2(headLookAtAvatarSpace.x, headLookAtAvatarSpace.z); + if (glm::length(headAzimuthAvatarSpace) > 0.0f) { + headAzimuthAvatarSpace = glm::normalize(headAzimuthAvatarSpace); + } else { + headAzimuthAvatarSpace = -latestHipToHandController; + } + + // check the angular distance from forward and back + float cosForwardAngle = glm::dot(latestHipToHandController, oldAzimuthReading); + float cosHeadShoulder = glm::dot(-latestHipToHandController, headAzimuthAvatarSpace); + // if we are now closer to the 180 flip of the previous chest forward + // then we negate our computed latestHipToHandController to keep the chest from flipping. + // also check the head to shoulder azimuth difference if we negate. + // don't negate the chest azimuth if this is greater than 100 degrees. + if ((cosForwardAngle < 0.0f) && !(cosHeadShoulder < -0.2f)) { + latestHipToHandController = -latestHipToHandController; + } + } + return latestHipToHandController; +} + void MyAvatar::updateJointFromController(controller::Action poseKey, ThreadSafeValueCache& matrixCache) { assert(QThread::currentThread() == thread()); auto userInputMapper = DependencyManager::get(); @@ -3353,6 +3414,24 @@ glm::mat4 MyAvatar::deriveBodyFromHMDSensor() const { return createMatFromQuatAndPos(headOrientationYawOnly, bodyPos); } +glm::mat4 MyAvatar::getSpine2RotationRigSpace() const { + + // static const glm::quat RIG_CHANGE_OF_BASIS = Quaternions::Y_180; + // RIG_CHANGE_OF_BASIS * AVATAR_TO_RIG_ROTATION * inverse(RIG_CHANGE_OF_BASIS) = Quaternions::Y_180; //avatar Space; + const glm::quat AVATAR_TO_RIG_ROTATION = Quaternions::Y_180; + glm::vec3 hipToHandRigSpace = AVATAR_TO_RIG_ROTATION * glm::vec3(_hipToHandController.x, 0.0f, _hipToHandController.y); + + glm::vec3 u, v, w; + if (glm::length(hipToHandRigSpace) > 0.0f) { + hipToHandRigSpace = glm::normalize(hipToHandRigSpace); + } else { + hipToHandRigSpace = glm::vec3(0.0f, 0.0f, 1.0f); + } + generateBasisVectors(glm::vec3(0.0f,1.0f,0.0f), hipToHandRigSpace, u, v, w); + glm::mat4 spine2RigSpace(glm::vec4(w, 0.0f), glm::vec4(u, 0.0f), glm::vec4(v, 0.0f), glm::vec4(glm::vec3(0.0f, 0.0f, 0.0f), 1.0f)); + return spine2RigSpace; +} + // ease in function for dampening cg movement static float slope(float num) { const float CURVE_CONSTANT = 1.0f; @@ -3936,7 +4015,7 @@ bool MyAvatar::FollowHelper::shouldActivateHorizontalCG(MyAvatar& myAvatar) cons glm::vec3 currentHeadPosition = currentHeadPose.getTranslation(); float anatomicalHeadToHipsDistance = glm::length(defaultHeadPosition - defaultHipsPosition); if (!isActive(Horizontal) && - (glm::length(currentHeadPosition - defaultHipsPosition) > (anatomicalHeadToHipsDistance + DEFAULT_AVATAR_SPINE_STRETCH_LIMIT))) { + (glm::length(currentHeadPosition - defaultHipsPosition) > (anatomicalHeadToHipsDistance + (DEFAULT_AVATAR_SPINE_STRETCH_LIMIT * anatomicalHeadToHipsDistance)))) { myAvatar.setResetMode(true); stepDetected = true; } @@ -3960,25 +4039,32 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat qApp->getCamera().getMode() != CAMERA_MODE_MIRROR) { if (!isActive(Rotation) && (shouldActivateRotation(myAvatar, desiredBodyMatrix, currentBodyMatrix) || hasDriveInput)) { activate(Rotation); - myAvatar.setHeadControllerFacingMovingAverage(myAvatar._headControllerFacing); + myAvatar.setHeadControllerFacingMovingAverage(myAvatar.getHeadControllerFacing()); } if (myAvatar.getCenterOfGravityModelEnabled()) { if (!isActive(Horizontal) && (shouldActivateHorizontalCG(myAvatar) || hasDriveInput)) { activate(Horizontal); + if (myAvatar.getEnableStepResetRotation()) { + activate(Rotation); + myAvatar.setHeadControllerFacingMovingAverage(myAvatar.getHeadControllerFacing()); + } } } else { if (!isActive(Horizontal) && (shouldActivateHorizontal(myAvatar, desiredBodyMatrix, currentBodyMatrix) || hasDriveInput)) { activate(Horizontal); + if (myAvatar.getEnableStepResetRotation()) { + activate(Rotation); + myAvatar.setHeadControllerFacingMovingAverage(myAvatar.getHeadControllerFacing()); + } } } - if (!isActive(Vertical) && (shouldActivateVertical(myAvatar, desiredBodyMatrix, currentBodyMatrix) || hasDriveInput)) { activate(Vertical); } } else { if (!isActive(Rotation) && getForceActivateRotation()) { activate(Rotation); - myAvatar.setHeadControllerFacingMovingAverage(myAvatar._headControllerFacing); + myAvatar.setHeadControllerFacingMovingAverage(myAvatar.getHeadControllerFacing()); setForceActivateRotation(false); } if (!isActive(Horizontal) && getForceActivateHorizontal()) { diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index bb0df1af62..8121b99e55 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -201,6 +201,8 @@ class MyAvatar : public Avatar { Q_PROPERTY(bool hasAudioEnabledFaceMovement READ getHasAudioEnabledFaceMovement WRITE setHasAudioEnabledFaceMovement) Q_PROPERTY(float rotationRecenterFilterLength READ getRotationRecenterFilterLength WRITE setRotationRecenterFilterLength) Q_PROPERTY(float rotationThreshold READ getRotationThreshold WRITE setRotationThreshold) + Q_PROPERTY(bool enableStepResetRotation READ getEnableStepResetRotation WRITE setEnableStepResetRotation) + Q_PROPERTY(bool enableDrawAverageFacing READ getEnableDrawAverageFacing WRITE setEnableDrawAverageFacing) //TODO: make gravity feature work Q_PROPERTY(glm::vec3 gravity READ getGravity WRITE setGravity) Q_PROPERTY(glm::vec3 leftHandPosition READ getLeftHandPosition) @@ -318,6 +320,9 @@ public: // as it moves through the world. void updateFromHMDSensorMatrix(const glm::mat4& hmdSensorMatrix); + // compute the hip to hand average azimuth. + glm::vec2 computeHandAzimuth() const; + // read the location of a hand controller and save the transform void updateJointFromController(controller::Action poseKey, ThreadSafeValueCache& matrixCache); @@ -909,6 +914,10 @@ public: virtual void rebuildCollisionShape() override; + const glm::vec2& getHipToHandController() const { return _hipToHandController; } + void setHipToHandController(glm::vec2 currentHipToHandController) { _hipToHandController = currentHipToHandController; } + const glm::vec2& getHeadControllerFacing() const { return _headControllerFacing; } + void setHeadControllerFacing(glm::vec2 currentHeadControllerFacing) { _headControllerFacing = currentHeadControllerFacing; } const glm::vec2& getHeadControllerFacingMovingAverage() const { return _headControllerFacingMovingAverage; } void setHeadControllerFacingMovingAverage(glm::vec2 currentHeadControllerFacing) { _headControllerFacingMovingAverage = currentHeadControllerFacing; } float getCurrentStandingHeight() const { return _currentStandingHeight; } @@ -1046,6 +1055,8 @@ public: // results are in sensor frame (-z forward) glm::mat4 deriveBodyFromHMDSensor() const; + glm::mat4 getSpine2RotationRigSpace() const; + glm::vec3 computeCounterBalance(); // derive avatar body position and orientation from using the current HMD Sensor location in relation to the previous @@ -1516,6 +1527,10 @@ private: float getRotationRecenterFilterLength() const { return _rotationRecenterFilterLength; } void setRotationThreshold(float angleRadians); float getRotationThreshold() const { return _rotationThreshold; } + void setEnableStepResetRotation(bool stepReset) { _stepResetRotationEnabled = stepReset; } + bool getEnableStepResetRotation() const { return _stepResetRotationEnabled; } + void setEnableDrawAverageFacing(bool drawAverage) { _drawAverageFacingEnabled = drawAverage; } + bool getEnableDrawAverageFacing() const { return _drawAverageFacingEnabled; } bool isMyAvatar() const override { return true; } virtual int parseDataFromBuffer(const QByteArray& buffer) override; virtual glm::vec3 getSkeletonPosition() const override; @@ -1638,6 +1653,8 @@ private: std::atomic _hasScriptedBlendShapes { false }; std::atomic _rotationRecenterFilterLength { 4.0f }; std::atomic _rotationThreshold { 0.5235f }; // 30 degrees in radians + std::atomic _stepResetRotationEnabled { true }; + std::atomic _drawAverageFacingEnabled { false }; // working copy -- see AvatarData for thread-safe _sensorToWorldMatrixCache, used for outward facing access glm::mat4 _sensorToWorldMatrix { glm::mat4() }; @@ -1651,6 +1668,8 @@ private: glm::vec2 _headControllerFacingMovingAverage { 0.0f, 0.0f }; // facing vector in xz plane (sensor space) glm::quat _averageHeadRotation { 0.0f, 0.0f, 0.0f, 1.0f }; + glm::vec2 _hipToHandController { 0.0f, -1.0f }; // spine2 facing vector in xz plane (avatar space) + float _currentStandingHeight { 0.0f }; bool _resetMode { true }; RingBufferHistory _recentModeReadings; diff --git a/interface/src/avatar/MySkeletonModel.cpp b/interface/src/avatar/MySkeletonModel.cpp index 0fc5e7521e..77d1a87195 100644 --- a/interface/src/avatar/MySkeletonModel.cpp +++ b/interface/src/avatar/MySkeletonModel.cpp @@ -238,6 +238,35 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { params.primaryControllerPoses[Rig::PrimaryControllerType_Hips] = sensorToRigPose * hips; params.primaryControllerFlags[Rig::PrimaryControllerType_Hips] = (uint8_t)Rig::ControllerFlags::Enabled | (uint8_t)Rig::ControllerFlags::Estimated; + // set spine2 if we have hand controllers + if (myAvatar->getControllerPoseInAvatarFrame(controller::Action::RIGHT_HAND).isValid() && + myAvatar->getControllerPoseInAvatarFrame(controller::Action::LEFT_HAND).isValid() && + !(params.primaryControllerFlags[Rig::PrimaryControllerType_Spine2] & (uint8_t)Rig::ControllerFlags::Enabled)) { + + AnimPose currentSpine2Pose; + AnimPose currentHeadPose; + AnimPose currentHipsPose; + bool spine2Exists = _rig.getAbsoluteJointPoseInRigFrame(_rig.indexOfJoint("Spine2"), currentSpine2Pose); + bool headExists = _rig.getAbsoluteJointPoseInRigFrame(_rig.indexOfJoint("Head"), currentHeadPose); + bool hipsExists = _rig.getAbsoluteJointPoseInRigFrame(_rig.indexOfJoint("Hips"), currentHipsPose); + if (spine2Exists && headExists && hipsExists) { + AnimPose rigSpaceYaw(myAvatar->getSpine2RotationRigSpace()); + glm::vec3 u, v, w; + glm::vec3 fwd = rigSpaceYaw.rot() * glm::vec3(0.0f, 0.0f, 1.0f); + glm::vec3 up = currentHeadPose.trans() - currentHipsPose.trans(); + if (glm::length(up) > 0.0f) { + up = glm::normalize(up); + } else { + up = glm::vec3(0.0f, 1.0f, 0.0f); + } + generateBasisVectors(up, fwd, u, v, w); + AnimPose newSpinePose(glm::mat4(glm::vec4(w, 0.0f), glm::vec4(u, 0.0f), glm::vec4(v, 0.0f), glm::vec4(glm::vec3(0.0f, 0.0f, 0.0f), 1.0f))); + currentSpine2Pose.rot() = newSpinePose.rot(); + params.primaryControllerPoses[Rig::PrimaryControllerType_Spine2] = currentSpine2Pose; + params.primaryControllerFlags[Rig::PrimaryControllerType_Spine2] = (uint8_t)Rig::ControllerFlags::Enabled | (uint8_t)Rig::ControllerFlags::Estimated; + } + } + } else { _prevHipsValid = false; } diff --git a/libraries/shared/src/AvatarConstants.h b/libraries/shared/src/AvatarConstants.h index 39117de7ef..c3e8a3f173 100644 --- a/libraries/shared/src/AvatarConstants.h +++ b/libraries/shared/src/AvatarConstants.h @@ -34,7 +34,7 @@ const float DEFAULT_HANDS_ANGULAR_VELOCITY_STEPPING_THRESHOLD = 3.3f; const float DEFAULT_HEAD_VELOCITY_STEPPING_THRESHOLD = 0.18f; const float DEFAULT_HEAD_PITCH_STEPPING_TOLERANCE = 7.0f; const float DEFAULT_HEAD_ROLL_STEPPING_TOLERANCE = 7.0f; -const float DEFAULT_AVATAR_SPINE_STRETCH_LIMIT = 0.07f; +const float DEFAULT_AVATAR_SPINE_STRETCH_LIMIT = 0.04f; const float DEFAULT_AVATAR_FORWARD_DAMPENING_FACTOR = 0.5f; const float DEFAULT_AVATAR_LATERAL_DAMPENING_FACTOR = 2.0f; const float DEFAULT_AVATAR_HIPS_MASS = 40.0f;