diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 04a6d58aae..c79f589b15 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -3797,7 +3797,7 @@ void MyAvatar::lateUpdatePalms() { } -static const float FOLLOW_TIME = 0.5f; +static const float FOLLOW_TIME = 0.1f; MyAvatar::FollowHelper::FollowHelper() { deactivate(); @@ -3902,8 +3902,13 @@ bool MyAvatar::FollowHelper::shouldActivateHorizontalCG(MyAvatar& myAvatar) cons glm::vec3 defaultHeadPosition = myAvatar.getAbsoluteDefaultJointTranslationInObjectFrame(myAvatar.getJointIndex("Head")); glm::vec3 currentHeadPosition = currentHeadPose.getTranslation(); float anatomicalHeadToHipsDistance = glm::length(defaultHeadPosition - defaultHipsPosition); + //if (!isActive(Horizontal) && + // (glm::length(currentHeadPosition - defaultHipsPosition) > (anatomicalHeadToHipsDistance + DEFAULT_AVATAR_SPINE_STRETCH_LIMIT))) { + // myAvatar.setResetMode(true); + // stepDetected = true; + //} 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.getAvatarScale())))) { myAvatar.setResetMode(true); stepDetected = true; } @@ -3925,10 +3930,15 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat if (myAvatar.getHMDLeanRecenterEnabled() && qApp->getCamera().getMode() != CAMERA_MODE_MIRROR) { - if (!isActive(Rotation) && (shouldActivateRotation(myAvatar, desiredBodyMatrix, currentBodyMatrix) || hasDriveInput)) { + if (!isActive(Rotation) && getForceActivateRotation()) { activate(Rotation); myAvatar.setHeadControllerFacingMovingAverage(myAvatar._headControllerFacing); + setForceActivateRotation(false); } + //if (!isActive(Rotation) && (shouldActivateRotation(myAvatar, desiredBodyMatrix, currentBodyMatrix) || hasDriveInput)) { + // activate(Rotation); + // myAvatar.setHeadControllerFacingMovingAverage(myAvatar._headControllerFacing); + //} if (myAvatar.getCenterOfGravityModelEnabled()) { if (!isActive(Horizontal) && (shouldActivateHorizontalCG(myAvatar) || hasDriveInput)) { activate(Horizontal); @@ -3937,11 +3947,6 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat myAvatar.setHeadControllerFacingMovingAverage(myAvatar._headControllerFacing); } } - if (!isActive(Rotation) && getForceActivateRotation()) { - activate(Rotation); - myAvatar.setHeadControllerFacingMovingAverage(myAvatar._headControllerFacing); - setForceActivateRotation(false); - } } else { if (!isActive(Horizontal) && (shouldActivateHorizontal(myAvatar, desiredBodyMatrix, currentBodyMatrix) || hasDriveInput)) { activate(Horizontal); diff --git a/libraries/shared/src/AvatarConstants.h b/libraries/shared/src/AvatarConstants.h index 39117de7ef..b97f586d32 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.05f; 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; diff --git a/scripts/developer/rotateApp.js b/scripts/developer/rotateApp.js index 7794232196..4d18910335 100644 --- a/scripts/developer/rotateApp.js +++ b/scripts/developer/rotateApp.js @@ -259,7 +259,7 @@ function limitAngle(angle) { return (angle + 180) % 360 - 180; } -function computeHandAzimuths() { +function computeHandAzimuths(timeElapsed) { var leftHand = currentStateReadings.lhandPose.translation; var rightHand = currentStateReadings.rhandPose.translation; var head = currentStateReadings.headPose.translation; @@ -272,8 +272,9 @@ function computeHandAzimuths() { var avatarZAxis = { x: 0.0, y: 0.0, z: 1.0 }; var hipToLHand = Quat.lookAtSimple({ x: 0, y: 0, z: 0 }, lHandMinusHead); var hipToRHand = Quat.lookAtSimple({ x: 0, y: 0, z: 0 }, rHandMinusHead); - hipToLHandAverage = Quat.slerp(hipToLHandAverage, hipToLHand, AVERAGING_RATE); - hipToRHandAverage = Quat.slerp(hipToRHandAverage, hipToRHand, AVERAGING_RATE); + var tau = timeElapsed / filterLengthProperty.value; + hipToLHandAverage = Quat.slerp(hipToLHandAverage, hipToLHand, tau); + hipToRHandAverage = Quat.slerp(hipToRHandAverage, hipToRHand, tau); // var angleToLeft = limitAngle(Quat.safeEulerAngles(hipToLHandAverage).y); // var angleToRight = limitAngle(Quat.safeEulerAngles(hipToRHandAverage).y); @@ -283,11 +284,7 @@ function computeHandAzimuths() { // limit the angle because we are flipped by 180, fix this tomorrow. // print(leftRightMidpointAverage/180.0); // print("threshold value " + angleThresholdProperty.value); - // get it into radians too!! - if ((Math.abs(leftRightMidpointAverage/180.0) * Math.PI) > angleThresholdProperty.value) { - print("recenter the feet under the head"); - MyAvatar.triggerRotationRecenter(); - } + var raySpineRotation = Quat.fromVec3Degrees({ x: 0, y: leftRightMidpointAverage, z: 0 }); var zAxisSpineRotation = Vec3.multiplyQbyV(raySpineRotation, { x: 0, y: 0, z: -1 }); @@ -295,6 +292,22 @@ function computeHandAzimuths() { DebugDraw.drawRay(MyAvatar.position, Vec3.sum(MyAvatar.position, zAxisWorldSpace), { x: 1, y: 0, z: 0, w: 1 }); //print(leftRightMidpoint); + + var headPoseRigSpace = Quat.multiply(CHANGE_OF_BASIS_ROTATION, currentStateReadings.head.rotation); + headPoseAverageOrientation = Quat.slerp(headPoseAverageOrientation, headPoseRigSpace, tau); + var headPoseAverageEulers = Quat.safeEulerAngles(headPoseAverageOrientation); + + // get it into radians too!! + // average head and hands 50/50 + print("hands azimuth " + leftRightMidpointAverage + " head azimuth " + headPoseAverageEulers.y) + var headPlusHands = (leftRightMidpointAverage + headPoseAverageEulers.y) / 2.0; + if ((Math.abs(headPlusHands / 180.0) * Math.PI) > angleThresholdProperty.value) { + print("recenter the feet under the head"); + MyAvatar.triggerRotationRecenter(); + hipToLHandAverage = { x: 0, y: 0, z: 0, w: 1 }; + hipToRHandAverage = { x: 0, y: 0, z: 0, w: 1 }; + headPoseAverageOrientation = { x: 0, y: 0, z: 0, w: 1 }; + } return Quat.fromVec3Degrees({ x: 0, y: leftRightMidpoint, z: 0 }); @@ -309,7 +322,9 @@ function update(dt) { //print(JSON.stringify(currentStateReadings.head)); - var latestSpineRotation = computeHandAzimuths(); + + + var latestSpineRotation = computeHandAzimuths(dt); spine2Rotation = latestSpineRotation; var spine2Pos = MyAvatar.getAbsoluteJointTranslationInObjectFrame(MyAvatar.getJointIndex("Spine2"));