From ff0d938d3793e80d9e5c699115981ea96af453ba Mon Sep 17 00:00:00 2001 From: amantley Date: Mon, 17 Sep 2018 16:53:15 -0700 Subject: [PATCH 01/27] initial changes to stop the squatty potty from happening when users are sitting down in vr mode --- interface/resources/qml/AnimStats.qml | 2 +- interface/src/avatar/MyAvatar.cpp | 84 +++++++++++++++++++++--- interface/src/avatar/MyAvatar.h | 6 +- interface/src/avatar/MySkeletonModel.cpp | 2 +- 4 files changed, 81 insertions(+), 13 deletions(-) diff --git a/interface/resources/qml/AnimStats.qml b/interface/resources/qml/AnimStats.qml index 35ed3799a6..fd0a280dad 100644 --- a/interface/resources/qml/AnimStats.qml +++ b/interface/resources/qml/AnimStats.qml @@ -53,7 +53,7 @@ Item { ListView { width: firstCol.width height: root.animStateMachines.length * 15 - visible: root.animStateMchines.length > 0; + visible: root.animStateMachines.length > 0; model: root.animStateMachines delegate: StatText { text: { diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index c47cfdb383..b626fb6c2a 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -3589,9 +3589,11 @@ glm::vec3 MyAvatar::computeCounterBalance() { // if the height is higher than default hips, clamp to default hips counterBalancedCg.y = tposeHips.y + 0.05f; } else if (counterBalancedCg.y < sitSquatThreshold) { - //do a height reset + // do a height reset setResetMode(true); _follow.activate(FollowHelper::Vertical); + // disable cg behaviour in this case. + _isInSittingState = true; } return counterBalancedCg; } @@ -3832,6 +3834,10 @@ bool MyAvatar::getIsInWalkingState() const { return _isInWalkingState; } +bool MyAvatar::getIsInSittingState() const { + return _isInSittingState; +} + float MyAvatar::getWalkSpeed() const { return _walkSpeed.get() * _walkSpeedScalar; } @@ -3852,6 +3858,10 @@ void MyAvatar::setIsInWalkingState(bool isWalking) { _isInWalkingState = isWalking; } +void MyAvatar::setIsInSittingState(bool isSitting) { + _isInSittingState = isSitting; +} + void MyAvatar::setWalkSpeed(float value) { _walkSpeed.set(value); } @@ -4029,6 +4039,33 @@ bool MyAvatar::FollowHelper::shouldActivateHorizontal(const MyAvatar& myAvatar, return fabs(lateralLeanAmount) > MAX_LATERAL_LEAN; } +bool MyAvatar::FollowHelper::shouldActivateHorizontalSitting(MyAvatar& myAvatar) const { + + // get the current readings + controller::Pose currentHeadPose = myAvatar.getControllerPoseInAvatarFrame(controller::Action::HEAD); + controller::Pose currentLeftHandPose = myAvatar.getControllerPoseInAvatarFrame(controller::Action::LEFT_HAND); + controller::Pose currentRightHandPose = myAvatar.getControllerPoseInAvatarFrame(controller::Action::RIGHT_HAND); + + bool stepDetected = false; + float myScale = myAvatar.getAvatarScale(); + + if (!withinBaseOfSupport(currentHeadPose)) { + // a step is detected + stepDetected = true; + } else { + glm::vec3 defaultHipsPosition = myAvatar.getAbsoluteDefaultJointTranslationInObjectFrame(myAvatar.getJointIndex("Hips")); + 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 * anatomicalHeadToHipsDistance)))) { + myAvatar.setResetMode(true); + stepDetected = true; + } + } + return stepDetected; +} + bool MyAvatar::FollowHelper::shouldActivateHorizontalCG(MyAvatar& myAvatar) const { // get the current readings @@ -4072,33 +4109,60 @@ bool MyAvatar::FollowHelper::shouldActivateHorizontalCG(MyAvatar& myAvatar) cons return stepDetected; } -bool MyAvatar::FollowHelper::shouldActivateVertical(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) const { +bool MyAvatar::FollowHelper::shouldActivateVertical(MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) const { const float CYLINDER_TOP = 0.1f; const float CYLINDER_BOTTOM = -1.5f; + const float SITTING_BOTTOM = -0.02f; glm::vec3 offset = extractTranslation(desiredBodyMatrix) - extractTranslation(currentBodyMatrix); - return (offset.y > CYLINDER_TOP) || (offset.y < CYLINDER_BOTTOM); + if (myAvatar.getIsInSittingState()) { + if (offset.y < SITTING_BOTTOM) { + // we recenter when sitting. + return true; + } else if (offset.y > CYLINDER_TOP) { + // if we recenter upwards then no longer in sitting state + myAvatar.setIsInSittingState(false); + return true; + } else { + return false; + } + } else { + return (offset.y > CYLINDER_TOP) || (offset.y < CYLINDER_BOTTOM); + } } void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix, bool hasDriveInput) { - if (myAvatar.getHMDLeanRecenterEnabled() && - qApp->getCamera().getMode() != CAMERA_MODE_MIRROR) { + qCDebug(interfaceapp) << "in sitting state: " << myAvatar.getIsInSittingState(); + + if (myAvatar.getHMDLeanRecenterEnabled() && qApp->getCamera().getMode() != CAMERA_MODE_MIRROR) { if (!isActive(Rotation) && (shouldActivateRotation(myAvatar, desiredBodyMatrix, currentBodyMatrix) || hasDriveInput)) { activate(Rotation); myAvatar.setHeadControllerFacingMovingAverage(myAvatar.getHeadControllerFacing()); } if (myAvatar.getCenterOfGravityModelEnabled()) { - if (!isActive(Horizontal) && (shouldActivateHorizontalCG(myAvatar) || hasDriveInput)) { - activate(Horizontal); - if (myAvatar.getEnableStepResetRotation()) { - activate(Rotation); - myAvatar.setHeadControllerFacingMovingAverage(myAvatar.getHeadControllerFacing()); + if (!(myAvatar.getIsInSittingState())) { + if (!isActive(Horizontal) && (shouldActivateHorizontalCG(myAvatar) || hasDriveInput)) { + activate(Horizontal); + if (myAvatar.getEnableStepResetRotation()) { + activate(Rotation); + myAvatar.setHeadControllerFacingMovingAverage(myAvatar.getHeadControllerFacing()); + } + } + } else { + // you are in the sitting state with cg model enabled + if (!isActive(Horizontal) && (shouldActivateHorizontalSitting(myAvatar) || hasDriveInput)) { + activate(Horizontal); + if (myAvatar.getEnableStepResetRotation()) { + activate(Rotation); + myAvatar.setHeadControllerFacingMovingAverage(myAvatar.getHeadControllerFacing()); + } } } } else { + // center of gravity model is not enabled if (!isActive(Horizontal) && (shouldActivateHorizontal(myAvatar, desiredBodyMatrix, currentBodyMatrix) || hasDriveInput)) { activate(Horizontal); if (myAvatar.getEnableStepResetRotation()) { diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 139f1f6ea2..a7fdf964d0 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -1094,6 +1094,8 @@ public: void setIsInWalkingState(bool isWalking); bool getIsInWalkingState() const; + void setIsInSittingState(bool isSitting); + bool getIsInSittingState() const; void setWalkSpeed(float value); float getWalkSpeed() const; void setWalkBackwardSpeed(float value); @@ -1708,9 +1710,10 @@ private: float getMaxTimeRemaining() const; void decrementTimeRemaining(float dt); bool shouldActivateRotation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) const; - bool shouldActivateVertical(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) const; + bool shouldActivateVertical(MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) const; bool shouldActivateHorizontal(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) const; bool shouldActivateHorizontalCG(MyAvatar& myAvatar) const; + bool shouldActivateHorizontalSitting(MyAvatar& myAvatar) const; void prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat4& bodySensorMatrix, const glm::mat4& currentBodyMatrix, bool hasDriveInput); glm::mat4 postPhysicsUpdate(const MyAvatar& myAvatar, const glm::mat4& currentBodyMatrix); bool getForceActivateRotation() const; @@ -1800,6 +1803,7 @@ private: ThreadSafeValueCache _sprintSpeed { AVATAR_SPRINT_SPEED_SCALAR }; float _walkSpeedScalar { AVATAR_WALK_SPEED_SCALAR }; bool _isInWalkingState { false }; + bool _isInSittingState{ false }; // load avatar scripts once when rig is ready bool _shouldLoadScripts { false }; diff --git a/interface/src/avatar/MySkeletonModel.cpp b/interface/src/avatar/MySkeletonModel.cpp index 3084542472..42ec582c47 100644 --- a/interface/src/avatar/MySkeletonModel.cpp +++ b/interface/src/avatar/MySkeletonModel.cpp @@ -46,7 +46,7 @@ static AnimPose computeHipsInSensorFrame(MyAvatar* myAvatar, bool isFlying) { } glm::mat4 hipsMat; - if (myAvatar->getCenterOfGravityModelEnabled() && !isFlying && !(myAvatar->getIsInWalkingState())) { + if (myAvatar->getCenterOfGravityModelEnabled() && !isFlying && !(myAvatar->getIsInWalkingState()) && !(myAvatar->getIsInSittingState())) { // then we use center of gravity model hipsMat = myAvatar->deriveBodyUsingCgModel(); } else { From 14fb7e1d44f8e52fbc91f8810f48d78b776b3005 Mon Sep 17 00:00:00 2001 From: amantley Date: Mon, 17 Sep 2018 17:53:08 -0700 Subject: [PATCH 02/27] removed redundant code --- interface/src/avatar/MyAvatar.cpp | 52 ++++++------------------------- 1 file changed, 9 insertions(+), 43 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index b626fb6c2a..d4168616c3 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -4039,33 +4039,6 @@ bool MyAvatar::FollowHelper::shouldActivateHorizontal(const MyAvatar& myAvatar, return fabs(lateralLeanAmount) > MAX_LATERAL_LEAN; } -bool MyAvatar::FollowHelper::shouldActivateHorizontalSitting(MyAvatar& myAvatar) const { - - // get the current readings - controller::Pose currentHeadPose = myAvatar.getControllerPoseInAvatarFrame(controller::Action::HEAD); - controller::Pose currentLeftHandPose = myAvatar.getControllerPoseInAvatarFrame(controller::Action::LEFT_HAND); - controller::Pose currentRightHandPose = myAvatar.getControllerPoseInAvatarFrame(controller::Action::RIGHT_HAND); - - bool stepDetected = false; - float myScale = myAvatar.getAvatarScale(); - - if (!withinBaseOfSupport(currentHeadPose)) { - // a step is detected - stepDetected = true; - } else { - glm::vec3 defaultHipsPosition = myAvatar.getAbsoluteDefaultJointTranslationInObjectFrame(myAvatar.getJointIndex("Hips")); - 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 * anatomicalHeadToHipsDistance)))) { - myAvatar.setResetMode(true); - stepDetected = true; - } - } - return stepDetected; -} - bool MyAvatar::FollowHelper::shouldActivateHorizontalCG(MyAvatar& myAvatar) const { // get the current readings @@ -4078,6 +4051,10 @@ bool MyAvatar::FollowHelper::shouldActivateHorizontalCG(MyAvatar& myAvatar) cons if (myAvatar.getIsInWalkingState()) { stepDetected = true; + } else if (myAvatar.getIsInSittingState()) { + if (!withinBaseOfSupport(currentHeadPose)) { + stepDetected = true; + } } else { if (!withinBaseOfSupport(currentHeadPose) && headAngularVelocityBelowThreshold(currentHeadPose) && @@ -4143,22 +4120,11 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat myAvatar.setHeadControllerFacingMovingAverage(myAvatar.getHeadControllerFacing()); } if (myAvatar.getCenterOfGravityModelEnabled()) { - if (!(myAvatar.getIsInSittingState())) { - if (!isActive(Horizontal) && (shouldActivateHorizontalCG(myAvatar) || hasDriveInput)) { - activate(Horizontal); - if (myAvatar.getEnableStepResetRotation()) { - activate(Rotation); - myAvatar.setHeadControllerFacingMovingAverage(myAvatar.getHeadControllerFacing()); - } - } - } else { - // you are in the sitting state with cg model enabled - if (!isActive(Horizontal) && (shouldActivateHorizontalSitting(myAvatar) || hasDriveInput)) { - activate(Horizontal); - if (myAvatar.getEnableStepResetRotation()) { - activate(Rotation); - myAvatar.setHeadControllerFacingMovingAverage(myAvatar.getHeadControllerFacing()); - } + if (!isActive(Horizontal) && (shouldActivateHorizontalCG(myAvatar) || hasDriveInput)) { + activate(Horizontal); + if (myAvatar.getEnableStepResetRotation()) { + activate(Rotation); + myAvatar.setHeadControllerFacingMovingAverage(myAvatar.getHeadControllerFacing()); } } } else { From 96da9b7e54ddaec7e976527059e878d0e3e27b89 Mon Sep 17 00:00:00 2001 From: amantley Date: Tue, 18 Sep 2018 10:31:19 -0700 Subject: [PATCH 03/27] cleaned up code for squatty fix --- interface/resources/qml/AnimStats.qml | 2 +- interface/src/avatar/MyAvatar.cpp | 6 +++--- interface/src/avatar/MyAvatar.h | 3 +-- 3 files changed, 5 insertions(+), 6 deletions(-) diff --git a/interface/resources/qml/AnimStats.qml b/interface/resources/qml/AnimStats.qml index fd0a280dad..35ed3799a6 100644 --- a/interface/resources/qml/AnimStats.qml +++ b/interface/resources/qml/AnimStats.qml @@ -53,7 +53,7 @@ Item { ListView { width: firstCol.width height: root.animStateMachines.length * 15 - visible: root.animStateMachines.length > 0; + visible: root.animStateMchines.length > 0; model: root.animStateMachines delegate: StatText { text: { diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index d4168616c3..8eceb19e09 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -4074,6 +4074,7 @@ bool MyAvatar::FollowHelper::shouldActivateHorizontalCG(MyAvatar& myAvatar) cons glm::vec3 currentHeadPosition = currentHeadPose.getTranslation(); float anatomicalHeadToHipsDistance = glm::length(defaultHeadPosition - defaultHipsPosition); if (!isActive(Horizontal) && + (!isActive(Vertical)) && (glm::length(currentHeadPosition - defaultHipsPosition) > (anatomicalHeadToHipsDistance + (DEFAULT_AVATAR_SPINE_STRETCH_LIMIT * anatomicalHeadToHipsDistance)))) { myAvatar.setResetMode(true); stepDetected = true; @@ -4112,9 +4113,8 @@ bool MyAvatar::FollowHelper::shouldActivateVertical(MyAvatar& myAvatar, const gl void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix, bool hasDriveInput) { - qCDebug(interfaceapp) << "in sitting state: " << myAvatar.getIsInSittingState(); - - if (myAvatar.getHMDLeanRecenterEnabled() && qApp->getCamera().getMode() != CAMERA_MODE_MIRROR) { + if (myAvatar.getHMDLeanRecenterEnabled() && + qApp->getCamera().getMode() != CAMERA_MODE_MIRROR) { if (!isActive(Rotation) && (shouldActivateRotation(myAvatar, desiredBodyMatrix, currentBodyMatrix) || hasDriveInput)) { activate(Rotation); myAvatar.setHeadControllerFacingMovingAverage(myAvatar.getHeadControllerFacing()); diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index a7fdf964d0..6f3858c5bf 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -1713,7 +1713,6 @@ private: bool shouldActivateVertical(MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) const; bool shouldActivateHorizontal(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) const; bool shouldActivateHorizontalCG(MyAvatar& myAvatar) const; - bool shouldActivateHorizontalSitting(MyAvatar& myAvatar) const; void prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat4& bodySensorMatrix, const glm::mat4& currentBodyMatrix, bool hasDriveInput); glm::mat4 postPhysicsUpdate(const MyAvatar& myAvatar, const glm::mat4& currentBodyMatrix); bool getForceActivateRotation() const; @@ -1803,7 +1802,7 @@ private: ThreadSafeValueCache _sprintSpeed { AVATAR_SPRINT_SPEED_SCALAR }; float _walkSpeedScalar { AVATAR_WALK_SPEED_SCALAR }; bool _isInWalkingState { false }; - bool _isInSittingState{ false }; + bool _isInSittingState { false }; // load avatar scripts once when rig is ready bool _shouldLoadScripts { false }; From 42e248ef84ca8207ca92885f04768935e8457062 Mon Sep 17 00:00:00 2001 From: amantley Date: Tue, 18 Sep 2018 17:37:44 -0700 Subject: [PATCH 04/27] maded more changes to support sitting state to do: add option for stuck pose reset --- interface/src/avatar/MyAvatar.cpp | 24 +++++++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 8eceb19e09..bf684ccd9a 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -453,6 +453,16 @@ void MyAvatar::update(float deltaTime) { float tau = deltaTime / HMD_FACING_TIMESCALE; setHipToHandController(computeHandAzimuth()); + // qCDebug(interfaceapp) << "sitting state is " << getIsInSittingState(); + // debug setting for sitting state if you start in the chair. + // if the head is close to the floor in sensor space + // and the up of the head is close to sensor up then try switching us to sitting state. + auto sensorHeadPoseDebug = getControllerPoseInSensorFrame(controller::Action::HEAD); + glm::vec3 upHead = transformVectorFast(sensorHeadPoseDebug.getMatrix(), glm::vec3(0.0f, 1.0f, 0.0f)); + float acosHead = glm::dot(upHead, glm::vec3(0.0f, 1.0f, 0.0f)); + if ((acosHead > 0.98f) && !getIsInSittingState() && (sensorHeadPoseDebug.getTranslation().y < -0.5f)) { + qCDebug(interfaceapp) << "we are going to sitting state because it looks like we should" << sensorHeadPoseDebug.getTranslation().y; + } // 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()) { @@ -3593,7 +3603,7 @@ glm::vec3 MyAvatar::computeCounterBalance() { setResetMode(true); _follow.activate(FollowHelper::Vertical); // disable cg behaviour in this case. - _isInSittingState = true; + setIsInSittingState(true); } return counterBalancedCg; } @@ -4049,6 +4059,12 @@ bool MyAvatar::FollowHelper::shouldActivateHorizontalCG(MyAvatar& myAvatar) cons bool stepDetected = false; float myScale = myAvatar.getAvatarScale(); + + // debug head hips angle + //glm::vec3 hipsPos = myAvatar.getAbsoluteDefaultJointTranslationInObjectFrame(myAvatar.getJointIndex("Hips")); + //glm::vec3 headHipsBody = currentHeadPose.getTranslation() - hipsPos; + //qCDebug(interfaceapp) << "head in sensor space " << withinBaseOfSupport(currentHeadPose); + if (myAvatar.getIsInWalkingState()) { stepDetected = true; } else if (myAvatar.getIsInSittingState()) { @@ -4094,6 +4110,12 @@ bool MyAvatar::FollowHelper::shouldActivateVertical(MyAvatar& myAvatar, const gl glm::vec3 offset = extractTranslation(desiredBodyMatrix) - extractTranslation(currentBodyMatrix); + auto sensorHeadPose = myAvatar.getControllerPoseInSensorFrame(controller::Action::HEAD); + glm::vec3 headWorldSpace = myAvatar.getHead()->getPosition(); + glm::mat4 worldToSensorMatrix = glm::inverse(myAvatar.getSensorToWorldMatrix()); + glm::vec3 headSensorSpace = transformVectorFast(myAvatar.getSensorToWorldMatrix(), headWorldSpace); + //qCDebug(interfaceapp) << "sensor space position " << extractTranslation(currentBodyMatrix) << " head position sensor " << sensorHeadPose.getTranslation(); + if (myAvatar.getIsInSittingState()) { if (offset.y < SITTING_BOTTOM) { // we recenter when sitting. From ad6bbc7ff6954461c35da6ddbdef7b71bf7c5c55 Mon Sep 17 00:00:00 2001 From: amantley Date: Tue, 25 Sep 2018 11:22:44 -0700 Subject: [PATCH 05/27] latest squatty changes --- interface/src/avatar/MyAvatar.cpp | 25 +++++++++++++++++-------- interface/src/avatar/MyAvatar.h | 1 + plugins/oculus/src/OculusHelpers.cpp | 6 ++++++ 3 files changed, 24 insertions(+), 8 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index bf684ccd9a..c52e029d94 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -463,6 +463,7 @@ void MyAvatar::update(float deltaTime) { if ((acosHead > 0.98f) && !getIsInSittingState() && (sensorHeadPoseDebug.getTranslation().y < -0.5f)) { qCDebug(interfaceapp) << "we are going to sitting state because it looks like we should" << sensorHeadPoseDebug.getTranslation().y; } + // 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()) { @@ -3601,9 +3602,9 @@ glm::vec3 MyAvatar::computeCounterBalance() { } else if (counterBalancedCg.y < sitSquatThreshold) { // do a height reset setResetMode(true); - _follow.activate(FollowHelper::Vertical); + //_follow.activate(FollowHelper::Vertical); // disable cg behaviour in this case. - setIsInSittingState(true); + //setIsInSittingState(true); } return counterBalancedCg; } @@ -4059,12 +4060,6 @@ bool MyAvatar::FollowHelper::shouldActivateHorizontalCG(MyAvatar& myAvatar) cons bool stepDetected = false; float myScale = myAvatar.getAvatarScale(); - - // debug head hips angle - //glm::vec3 hipsPos = myAvatar.getAbsoluteDefaultJointTranslationInObjectFrame(myAvatar.getJointIndex("Hips")); - //glm::vec3 headHipsBody = currentHeadPose.getTranslation() - hipsPos; - //qCDebug(interfaceapp) << "head in sensor space " << withinBaseOfSupport(currentHeadPose); - if (myAvatar.getIsInWalkingState()) { stepDetected = true; } else if (myAvatar.getIsInSittingState()) { @@ -4137,6 +4132,20 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat if (myAvatar.getHMDLeanRecenterEnabled() && qApp->getCamera().getMode() != CAMERA_MODE_MIRROR) { + + // debug head hips angle + glm::vec3 headDefaultPos = myAvatar.getAbsoluteDefaultJointTranslationInObjectFrame(myAvatar.getJointIndex("Head")); + if (myAvatar.getControllerPoseInAvatarFrame(controller::Action::HEAD).getTranslation().y < (headDefaultPos.y - 0.05f)) { + _squatCount++; + if ((_squatCount > 300) && !isActive(Vertical) && !isActive(Horizontal)) { + activate(Horizontal); + activate(Vertical); + _squatCount = 0; + } + } else { + _squatCount = 0; + } + if (!isActive(Rotation) && (shouldActivateRotation(myAvatar, desiredBodyMatrix, currentBodyMatrix) || hasDriveInput)) { activate(Rotation); myAvatar.setHeadControllerFacingMovingAverage(myAvatar.getHeadControllerFacing()); diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 6f3858c5bf..78dc6307e8 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -1727,6 +1727,7 @@ private: std::atomic _forceActivateVertical { false }; std::atomic _forceActivateHorizontal { false }; std::atomic _toggleHipsFollowing { true }; + int _squatCount{ 0 }; }; FollowHelper _follow; diff --git a/plugins/oculus/src/OculusHelpers.cpp b/plugins/oculus/src/OculusHelpers.cpp index 511984c657..e543d3ca00 100644 --- a/plugins/oculus/src/OculusHelpers.cpp +++ b/plugins/oculus/src/OculusHelpers.cpp @@ -79,6 +79,12 @@ private: if (!OVR_SUCCESS(ovr_Initialize(&initParams))) { qCWarning(oculusLog) << "Failed to initialze Oculus SDK" << ovr::getError(); return; + } else { + qCWarning(oculusLog) << "successful init of oculus!!!!!!!!"; + ovrTrackingOrigin fred; + fred = ovr_GetTrackingOriginType(session); + qCWarning(oculusLog) << (int)fred; + } ovrGraphicsLuid luid; From fa9abf0fff45ba8205a8647bdb28f18bc068e676 Mon Sep 17 00:00:00 2001 From: amantley Date: Wed, 26 Sep 2018 18:08:00 -0700 Subject: [PATCH 06/27] added the floor at 0.0 in sensor space for oculus. to do: vive --- interface/src/avatar/MyAvatar.cpp | 35 ++++++++++++++++++++-------- interface/src/avatar/MyAvatar.h | 5 ++-- plugins/oculus/src/OculusHelpers.cpp | 13 ++++++----- 3 files changed, 35 insertions(+), 18 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 6394631484..2152c12b64 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -477,10 +477,20 @@ void MyAvatar::update(float deltaTime) { auto sensorHeadPoseDebug = getControllerPoseInSensorFrame(controller::Action::HEAD); glm::vec3 upHead = transformVectorFast(sensorHeadPoseDebug.getMatrix(), glm::vec3(0.0f, 1.0f, 0.0f)); float acosHead = glm::dot(upHead, glm::vec3(0.0f, 1.0f, 0.0f)); + qCDebug(interfaceapp) << "sensor space head pos " << sensorHeadPoseDebug.getTranslation().y; if ((acosHead > 0.98f) && !getIsInSittingState() && (sensorHeadPoseDebug.getTranslation().y < -0.5f)) { - qCDebug(interfaceapp) << "we are going to sitting state because it looks like we should" << sensorHeadPoseDebug.getTranslation().y; + //qCDebug(interfaceapp) << "we are going to sitting state because it looks like we should" << sensorHeadPoseDebug.getTranslation().y; } - + if (!_lastFrameHMDMode && qApp->isHMDMode()) { + // we have entered hmd mode, so make the best guess about sitting or standing + if (sensorHeadPoseDebug.getTranslation().y < 1.3f) { + // then we are sitting. + // setIsInSittingState(true); + } else { + // setIsInSittingState(false); + } + } + // 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()) { @@ -3575,9 +3585,9 @@ glm::vec3 MyAvatar::computeCounterBalance() { } else if (counterBalancedCg.y < sitSquatThreshold) { // do a height reset setResetMode(true); - //_follow.activate(FollowHelper::Vertical); + // _follow.activate(FollowHelper::Vertical); // disable cg behaviour in this case. - //setIsInSittingState(true); + // setIsInSittingState(true); } return counterBalancedCg; } @@ -4090,7 +4100,7 @@ bool MyAvatar::FollowHelper::shouldActivateVertical(MyAvatar& myAvatar, const gl return true; } else if (offset.y > CYLINDER_TOP) { // if we recenter upwards then no longer in sitting state - myAvatar.setIsInSittingState(false); + // myAvatar.setIsInSittingState(false); return true; } else { return false; @@ -4110,15 +4120,20 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat glm::vec3 headDefaultPos = myAvatar.getAbsoluteDefaultJointTranslationInObjectFrame(myAvatar.getJointIndex("Head")); if (myAvatar.getControllerPoseInAvatarFrame(controller::Action::HEAD).getTranslation().y < (headDefaultPos.y - 0.05f)) { _squatCount++; - if ((_squatCount > 300) && !isActive(Vertical) && !isActive(Horizontal)) { - activate(Horizontal); - activate(Vertical); - _squatCount = 0; + if ((_squatCount > 600) && !isActive(Vertical) && !isActive(Horizontal)) { + if (myAvatar.getIsInSittingState()) { + // activate(Horizontal); + activate(Vertical); + _squatCount = 0; + } else { + activate(Horizontal); + _squatCount = 0; + } } } else { _squatCount = 0; } - + if (!isActive(Rotation) && (shouldActivateRotation(myAvatar, desiredBodyMatrix, currentBodyMatrix) || hasDriveInput)) { activate(Rotation); myAvatar.setHeadControllerFacingMovingAverage(myAvatar.getHeadControllerFacing()); diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 1dabe38116..4ccb1a8d75 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -1728,7 +1728,7 @@ private: std::atomic _forceActivateVertical { false }; std::atomic _forceActivateHorizontal { false }; std::atomic _toggleHipsFollowing { true }; - int _squatCount{ 0 }; + int _squatCount { 0 }; }; FollowHelper _follow; @@ -1760,6 +1760,7 @@ private: glm::quat _customListenOrientation; AtRestDetector _hmdAtRestDetector; + bool _lastFrameHMDMode { false } ; bool _lastIsMoving { false }; // all poses are in sensor-frame @@ -1804,7 +1805,7 @@ private: ThreadSafeValueCache _sprintSpeed { AVATAR_SPRINT_SPEED_SCALAR }; float _walkSpeedScalar { AVATAR_WALK_SPEED_SCALAR }; bool _isInWalkingState { false }; - bool _isInSittingState { false }; + bool _isInSittingState { true }; // load avatar scripts once when rig is ready bool _shouldLoadScripts { false }; diff --git a/plugins/oculus/src/OculusHelpers.cpp b/plugins/oculus/src/OculusHelpers.cpp index e543d3ca00..38d93d088d 100644 --- a/plugins/oculus/src/OculusHelpers.cpp +++ b/plugins/oculus/src/OculusHelpers.cpp @@ -79,18 +79,19 @@ private: if (!OVR_SUCCESS(ovr_Initialize(&initParams))) { qCWarning(oculusLog) << "Failed to initialze Oculus SDK" << ovr::getError(); return; - } else { - qCWarning(oculusLog) << "successful init of oculus!!!!!!!!"; - ovrTrackingOrigin fred; - fred = ovr_GetTrackingOriginType(session); - qCWarning(oculusLog) << (int)fred; - } ovrGraphicsLuid luid; if (!OVR_SUCCESS(ovr_Create(&session, &luid))) { qCWarning(oculusLog) << "Failed to acquire Oculus session" << ovr::getError(); return; + } else { + qCWarning(oculusLog) << "successful init of oculus!!!!!!!!"; + ovrTrackingOrigin fred; + //fred = ovr_GetTrackingOriginType(session); + ovrResult retTrackingType = ovr_SetTrackingOriginType(session, ovrTrackingOrigin::ovrTrackingOrigin_FloorLevel); + fred = ovr_GetTrackingOriginType(session); + qCWarning(oculusLog) << OVR_SUCCESS(retTrackingType) << (int)fred; } } From 52355e53f1e69ff4032813c13b229c04c6cfdb0d Mon Sep 17 00:00:00 2001 From: amantley Date: Thu, 27 Sep 2018 16:44:55 -0700 Subject: [PATCH 07/27] adding the menu item to the avatar app for the sit state. --- interface/resources/qml/hifi/AvatarApp.qml | 1 + .../resources/qml/hifi/avatarapp/Settings.qml | 57 +- interface/src/avatar/MyAvatar.cpp | 14 +- interface/src/avatar/MyAvatar.h | 9 + .../src/avatars-renderer/Avatar.cpp | 1 + scripts/developer/objectOrientedStep.js | 688 ++++++++++++++++++ scripts/system/avatarapp.js | 15 +- 7 files changed, 778 insertions(+), 7 deletions(-) create mode 100644 scripts/developer/objectOrientedStep.js diff --git a/interface/resources/qml/hifi/AvatarApp.qml b/interface/resources/qml/hifi/AvatarApp.qml index aea5931627..b06a2ca67c 100644 --- a/interface/resources/qml/hifi/AvatarApp.qml +++ b/interface/resources/qml/hifi/AvatarApp.qml @@ -252,6 +252,7 @@ Rectangle { var avatarSettings = { dominantHand : settings.dominantHandIsLeft ? 'left' : 'right', collisionsEnabled : settings.avatarCollisionsOn, + sittingEnabled : settings.avatarSittingOn, animGraphOverrideUrl : settings.avatarAnimationOverrideJSON, collisionSoundUrl : settings.avatarCollisionSoundUrl }; diff --git a/interface/resources/qml/hifi/avatarapp/Settings.qml b/interface/resources/qml/hifi/avatarapp/Settings.qml index 71bfbb084d..af76ba04d6 100644 --- a/interface/resources/qml/hifi/avatarapp/Settings.qml +++ b/interface/resources/qml/hifi/avatarapp/Settings.qml @@ -20,6 +20,7 @@ Rectangle { property real scaleValue: scaleSlider.value / 10 property alias dominantHandIsLeft: leftHandRadioButton.checked property alias avatarCollisionsOn: collisionsEnabledRadiobutton.checked + property alias avatarSittingOn: sitRadiobutton.checked property alias avatarAnimationOverrideJSON: avatarAnimationUrlInputText.text property alias avatarAnimationJSON: avatarAnimationUrlInputText.placeholderText property alias avatarCollisionSoundUrl: avatarCollisionSoundUrlInputText.text @@ -45,6 +46,12 @@ Rectangle { collisionsDisabledRadioButton.checked = true; } + if (settings.sittingEnabled) { + sitRadiobutton.checked = true; + } else { + standRadioButton.checked = true; + } + avatarAnimationJSON = settings.animGraphUrl; avatarAnimationOverrideJSON = settings.animGraphOverrideUrl; avatarCollisionSoundUrl = settings.collisionSoundUrl; @@ -289,8 +296,56 @@ Rectangle { text: "OFF" boxSize: 20 } - } + + // TextStyle9 + + RalewaySemiBold { + size: 17; + Layout.row: 2 + Layout.column: 0 + + text: "Sitting State" + } + + ButtonGroup { + id: sitStand + } + + HifiControlsUit.RadioButton { + id: sitRadiobutton + + Layout.row: 2 + Layout.column: 1 + Layout.leftMargin: -40 + + ButtonGroup.group: sitStand + checked: true + + colorScheme: hifi.colorSchemes.light + fontSize: 17 + letterSpacing: 1.4 + text: "Sit" + boxSize: 20 + } + + HifiControlsUit.RadioButton { + id: standRadioButton + + Layout.row: 2 + Layout.column: 2 + Layout.rightMargin: 20 + + ButtonGroup.group: sitStand + + colorScheme: hifi.colorSchemes.light + fontSize: 17 + letterSpacing: 1.4 + text: "Stand" + boxSize: 20 + } + } + ColumnLayout { id: avatarAnimationLayout anchors.top: handAndCollisions.bottom diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 2152c12b64..631a4b0670 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -477,7 +477,7 @@ void MyAvatar::update(float deltaTime) { auto sensorHeadPoseDebug = getControllerPoseInSensorFrame(controller::Action::HEAD); glm::vec3 upHead = transformVectorFast(sensorHeadPoseDebug.getMatrix(), glm::vec3(0.0f, 1.0f, 0.0f)); float acosHead = glm::dot(upHead, glm::vec3(0.0f, 1.0f, 0.0f)); - qCDebug(interfaceapp) << "sensor space head pos " << sensorHeadPoseDebug.getTranslation().y; + // qCDebug(interfaceapp) << "sensor space head pos " << sensorHeadPoseDebug.getTranslation().y; if ((acosHead > 0.98f) && !getIsInSittingState() && (sensorHeadPoseDebug.getTranslation().y < -0.5f)) { //qCDebug(interfaceapp) << "we are going to sitting state because it looks like we should" << sensorHeadPoseDebug.getTranslation().y; } @@ -3854,6 +3854,7 @@ void MyAvatar::setIsInWalkingState(bool isWalking) { void MyAvatar::setIsInSittingState(bool isSitting) { _isInSittingState = isSitting; + emit sittingEnabledChanged(isSitting); } void MyAvatar::setWalkSpeed(float value) { @@ -4098,9 +4099,9 @@ bool MyAvatar::FollowHelper::shouldActivateVertical(MyAvatar& myAvatar, const gl if (offset.y < SITTING_BOTTOM) { // we recenter when sitting. return true; - } else if (offset.y > CYLINDER_TOP) { + } else if (offset.y > 2.0*CYLINDER_TOP) { // if we recenter upwards then no longer in sitting state - // myAvatar.setIsInSittingState(false); + myAvatar.setIsInSittingState(false); return true; } else { return false; @@ -4126,7 +4127,12 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat activate(Vertical); _squatCount = 0; } else { - activate(Horizontal); + if (myAvatar.getControllerPoseInAvatarFrame(controller::Action::HEAD).getTranslation().y < (headDefaultPos.y - 0.20f)) { + myAvatar.setIsInSittingState(true); + activate(Vertical); + } else { + activate(Horizontal); + } _squatCount = 0; } } diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 4ccb1a8d75..d9744e93fe 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -239,6 +239,7 @@ class MyAvatar : public Avatar { Q_PROPERTY(float walkSpeed READ getWalkSpeed WRITE setWalkSpeed); Q_PROPERTY(float walkBackwardSpeed READ getWalkBackwardSpeed WRITE setWalkBackwardSpeed); Q_PROPERTY(float sprintSpeed READ getSprintSpeed WRITE setSprintSpeed); + Q_PROPERTY(bool isInSittingState READ getIsInSittingState WRITE setIsInSittingState); const QString DOMINANT_LEFT_HAND = "left"; const QString DOMINANT_RIGHT_HAND = "right"; @@ -1506,6 +1507,14 @@ signals: */ void disableHandTouchForIDChanged(const QUuid& entityID, bool disable); + /**jsdoc + * Triggered when the sit state is enabled or disabled + * @function MyAvatar.sittingEnabledChanged + * @param {boolean} enabled + * @returns {Signal} + */ + void sittingEnabledChanged(bool enabled); + private slots: void leaveDomain(); void updateCollisionCapsuleCache(); diff --git a/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp b/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp index 914a3b7c6e..51c51b6a20 100644 --- a/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp +++ b/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp @@ -1618,6 +1618,7 @@ void Avatar::getCapsule(glm::vec3& start, glm::vec3& end, float& radius) { glm::vec3 Avatar::getWorldFeetPosition() { ShapeInfo shapeInfo; + computeShapeInfo(shapeInfo); glm::vec3 halfExtents = shapeInfo.getHalfExtents(); // x = radius, y = halfHeight glm::vec3 localFeet(0.0f, shapeInfo.getOffset().y - halfExtents.y - halfExtents.x, 0.0f); diff --git a/scripts/developer/objectOrientedStep.js b/scripts/developer/objectOrientedStep.js new file mode 100644 index 0000000000..a5c27e36b9 --- /dev/null +++ b/scripts/developer/objectOrientedStep.js @@ -0,0 +1,688 @@ +/* jslint bitwise: true */ + +/* global Script, Vec3, MyAvatar, Tablet, Messages, Quat, +DebugDraw, Mat4, Entities, Xform, Controller, Camera, console, document*/ + +Script.registerValue("STEPAPP", true); +var CENTIMETERSPERMETER = 100.0; +var LEFT = 0; +var RIGHT = 1; +var INCREASING = 1.0; +var DECREASING = -1.0; +var DEFAULT_AVATAR_HEIGHT = 1.64; +var TABLET_BUTTON_NAME = "STEP"; +var CHANGE_OF_BASIS_ROTATION = { x: 0, y: 1, z: 0, w: 0 }; +// in meters (mostly) +var DEFAULT_ANTERIOR = 0.04; +var DEFAULT_POSTERIOR = 0.06; +var DEFAULT_LATERAL = 0.10; +var DEFAULT_HEIGHT_DIFFERENCE = 0.02; +var DEFAULT_ANGULAR_VELOCITY = 0.3; +var DEFAULT_HAND_VELOCITY = 0.4; +var DEFAULT_ANGULAR_HAND_VELOCITY = 3.3; +var DEFAULT_HEAD_VELOCITY = 0.14; +var DEFAULT_LEVEL_PITCH = 7; +var DEFAULT_LEVEL_ROLL = 7; +var DEFAULT_DIFF = 0.0; +var DEFAULT_DIFF_EULERS = { x: 0.0, y: 0.0, z: 0.0 }; +var DEFAULT_HIPS_POSITION; +var DEFAULT_HEAD_POSITION; +var DEFAULT_TORSO_LENGTH; +var SPINE_STRETCH_LIMIT = 0.02; + +var VELOCITY_EPSILON = 0.02; +var AVERAGING_RATE = 0.03; +var HEIGHT_AVERAGING_RATE = 0.01; +var STEP_TIME_SECS = 0.2; +var MODE_SAMPLE_LENGTH = 100; +var RESET_MODE = false; +var HEAD_TURN_THRESHOLD = 25.0; +var NO_SHARED_DIRECTION = -0.98; +var LOADING_DELAY = 500; +var FAILSAFE_TIMEOUT = 2.5; + +var debugDrawBase = true; +var activated = false; +var documentLoaded = false; +var failsafeFlag = false; +var failsafeSignalTimer = -1.0; +var stepTimer = -1.0; + + +var modeArray = new Array(MODE_SAMPLE_LENGTH); +var modeHeight = -10.0; + +var handPosition; +var handOrientation; +var hands = []; +var hipToHandAverage = []; +var handDotHead = []; +var headAverageOrientation = MyAvatar.orientation; +var headPoseAverageOrientation = { x: 0, y: 0, z: 0, w: 1 }; +var averageHeight = 1.0; +var headEulers = { x: 0.0, y: 0.0, z: 0.0 }; +var headAverageEulers = { x: 0.0, y: 0.0, z: 0.0 }; +var headAveragePosition = { x: 0, y: 0.4, z: 0 }; +var frontLeft = { x: -DEFAULT_LATERAL, y: 0, z: -DEFAULT_ANTERIOR }; +var frontRight = { x: DEFAULT_LATERAL, y: 0, z: -DEFAULT_ANTERIOR }; +var backLeft = { x: -DEFAULT_LATERAL, y: 0, z: DEFAULT_POSTERIOR }; +var backRight = { x: DEFAULT_LATERAL, y: 0, z: DEFAULT_POSTERIOR }; + + +// define state readings constructor +function StateReading(headPose, rhandPose, lhandPose, backLength, diffFromMode, diffFromAverageHeight, diffFromAveragePosition, + diffFromAverageEulers) { + this.headPose = headPose; + this.rhandPose = rhandPose; + this.lhandPose = lhandPose; + this.backLength = backLength; + this.diffFromMode = diffFromMode; + this.diffFromAverageHeight = diffFromAverageHeight; + this.diffFromAveragePosition = diffFromAveragePosition; + this.diffFromAverageEulers = diffFromAverageEulers; +} + +// define current state readings object for holding tracker readings and current differences from averages +var currentStateReadings = new StateReading(Controller.getPoseValue(Controller.Standard.Head), + Controller.getPoseValue(Controller.Standard.RightHand), Controller.getPoseValue(Controller.Standard.LeftHand), + DEFAULT_TORSO_LENGTH, DEFAULT_DIFF, DEFAULT_DIFF, DEFAULT_DIFF, DEFAULT_DIFF_EULERS); + +// declare the checkbox constructor +function AppCheckbox(type,id,eventType,isChecked) { + this.type = type; + this.id = id; + this.eventType = eventType; + this.data = {value: isChecked}; +} + +// define the checkboxes in the html file +var usingAverageHeight = new AppCheckbox("checkboxtick", "runningAverageHeightCheck", "onRunningAverageHeightCheckBox", + false); +var usingModeHeight = new AppCheckbox("checkboxtick","modeCheck","onModeCheckBox",true); +var usingBaseOfSupport = new AppCheckbox("checkboxtick","baseOfSupportCheck","onBaseOfSupportCheckBox",true); +var usingAverageHeadPosition = new AppCheckbox("checkboxtick", "headAveragePositionCheck", "onHeadAveragePositionCheckBox", + false); + +var checkBoxArray = new Array(usingAverageHeight,usingModeHeight,usingBaseOfSupport,usingAverageHeadPosition); + +// declare the html slider constructor +function AppProperty(name, type, eventType, signalType, setFunction, initValue, convertToThreshold, convertToSlider, signalOn) { + this.name = name; + this.type = type; + this.eventType = eventType; + this.signalType = signalType; + this.setValue = setFunction; + this.value = initValue; + this.get = function () { + return this.value; + }; + this.convertToThreshold = convertToThreshold; + this.convertToSlider = convertToSlider; + this.signalOn = signalOn; +} + +// define the sliders +var frontBaseProperty = new AppProperty("#anteriorBase-slider", "slider", "onAnteriorBaseSlider", "frontSignal", + setAnteriorDistance, DEFAULT_ANTERIOR, function (num) { + return convertToMeters(num); + }, function (num) { + return convertToCentimeters(num); + },true); +var backBaseProperty = new AppProperty("#posteriorBase-slider", "slider", "onPosteriorBaseSlider", "backSignal", + setPosteriorDistance, DEFAULT_POSTERIOR, function (num) { + return convertToMeters(num); + }, function (num) { + return convertToCentimeters(num); + }, true); +var lateralBaseProperty = new AppProperty("#lateralBase-slider", "slider", "onLateralBaseSlider", "lateralSignal", + setLateralDistance, DEFAULT_LATERAL, function (num) { + return convertToMeters(num); + }, function (num) { + return convertToCentimeters(num); + }, true); +var headAngularVelocityProperty = new AppProperty("#angularVelocityHead-slider", "slider", "onAngularVelocitySlider", + "angularHeadSignal", setAngularThreshold, DEFAULT_ANGULAR_VELOCITY, function (num) { + var base = 4; + var shift = 2; + return convertExponential(base, num, DECREASING, shift); + }, function (num) { + var base = 4; + var shift = 2; + return convertLog(base, num, DECREASING, shift); + }, true); +var heightDifferenceProperty = new AppProperty("#heightDifference-slider", "slider", "onHeightDifferenceSlider", "heightSignal", + setHeightThreshold, DEFAULT_HEIGHT_DIFFERENCE, function (num) { + return convertToMeters(-num); + }, function (num) { + return convertToCentimeters(-num); + }, true); +var handsVelocityProperty = new AppProperty("#handsVelocity-slider", "slider", "onHandsVelocitySlider", "handVelocitySignal", + setHandVelocityThreshold, DEFAULT_HAND_VELOCITY, function (num) { + return num; + }, function (num) { + return num; + }, true); +var handsAngularVelocityProperty = new AppProperty("#handsAngularVelocity-slider", "slider", "onHandsAngularVelocitySlider", + "handAngularSignal", setHandAngularVelocityThreshold, DEFAULT_ANGULAR_HAND_VELOCITY, function (num) { + var base = 7; + var shift = 2; + return convertExponential(base, num, DECREASING, shift); + }, function (num) { + var base = 7; + var shift = 2; + return convertLog(base, num, DECREASING, shift); + }, true); +var headVelocityProperty = new AppProperty("#headVelocity-slider", "slider", "onHeadVelocitySlider", "headVelocitySignal", + setHeadVelocityThreshold, DEFAULT_HEAD_VELOCITY, function (num) { + var base = 2; + var shift = 0; + return convertExponential(base, num, INCREASING, shift); + }, function (num) { + var base = 2; + var shift = 0; + return convertLog(base, num, INCREASING, shift); + }, true); +var headPitchProperty = new AppProperty("#headPitch-slider", "slider", "onHeadPitchSlider", "headPitchSignal", + setHeadPitchThreshold, DEFAULT_LEVEL_PITCH, function (num) { + var base = 2.5; + var shift = 5; + return convertExponential(base, num, DECREASING, shift); + }, function (num) { + var base = 2.5; + var shift = 5; + return convertLog(base, num, DECREASING, shift); + }, true); +var headRollProperty = new AppProperty("#headRoll-slider", "slider", "onHeadRollSlider", "headRollSignal", setHeadRollThreshold, + DEFAULT_LEVEL_ROLL, function (num) { + var base = 2.5; + var shift = 5; + return convertExponential(base, num, DECREASING, shift); + }, function (num) { + var base = 2.5; + var shift = 5; + return convertLog(base, num, DECREASING, shift); + }, true); + +var propArray = new Array(frontBaseProperty, backBaseProperty, lateralBaseProperty, headAngularVelocityProperty, + heightDifferenceProperty, handsVelocityProperty, handsAngularVelocityProperty, headVelocityProperty, headPitchProperty, + headRollProperty); + +// var HTML_URL = Script.resolvePath("http://hifi-content.s3.amazonaws.com/angus/stepApp/stepApp.html"); +var HTML_URL = Script.resolvePath("http://hifi-content.s3.amazonaws.com/angus/stepApp/stepAppExtra.html"); +var tablet = Tablet.getTablet("com.highfidelity.interface.tablet.system"); + +function manageClick() { + if (activated) { + tablet.gotoHomeScreen(); + } else { + tablet.gotoWebScreen(HTML_URL); + } +} + +var tabletButton = tablet.addButton({ + text: TABLET_BUTTON_NAME, + icon: Script.resolvePath("http://hifi-content.s3.amazonaws.com/angus/stepApp/foot.svg"), + activeIcon: Script.resolvePath("http://hifi-content.s3.amazonaws.com/angus/stepApp/foot.svg") +}); + +function drawBase() { + // transform corners into world space, for rendering. + var worldPointLf = Vec3.sum(MyAvatar.position,Vec3.multiplyQbyV(MyAvatar.orientation, frontLeft)); + var worldPointRf = Vec3.sum(MyAvatar.position,Vec3.multiplyQbyV(MyAvatar.orientation, frontRight)); + var worldPointLb = Vec3.sum(MyAvatar.position,Vec3.multiplyQbyV(MyAvatar.orientation, backLeft)); + var worldPointRb = Vec3.sum(MyAvatar.position,Vec3.multiplyQbyV(MyAvatar.orientation, backRight)); + + var GREEN = { r: 0, g: 1, b: 0, a: 1 }; + // draw border + DebugDraw.drawRay(worldPointLf, worldPointRf, GREEN); + DebugDraw.drawRay(worldPointRf, worldPointRb, GREEN); + DebugDraw.drawRay(worldPointRb, worldPointLb, GREEN); + DebugDraw.drawRay(worldPointLb, worldPointLf, GREEN); +} + +function onKeyPress(event) { + if (event.text === "'") { + // when the sensors are reset, then reset the mode. + RESET_MODE = false; + } +} + +function onWebEventReceived(msg) { + var message = JSON.parse(msg); + print(" we have a message from html dialog " + message.type); + propArray.forEach(function (prop) { + if (prop.eventType === message.type) { + prop.setValue(prop.convertToThreshold(message.data.value)); + print("message from " + prop.name); + // break; + } + }); + checkBoxArray.forEach(function(cbox) { + if (cbox.eventType === message.type) { + cbox.data.value = message.data.value; + // break; + } + }); + if (message.type === "onCreateStepApp") { + print("document loaded"); + documentLoaded = true; + Script.setTimeout(initAppForm, LOADING_DELAY); + } +} + +function initAppForm() { + print("step app is loaded: " + documentLoaded); + if (documentLoaded === true) { + propArray.forEach(function (prop) { + tablet.emitScriptEvent(JSON.stringify({ + "type": "trigger", + "id": prop.signalType, + "data": { "value": "green" } + })); + tablet.emitScriptEvent(JSON.stringify({ + "type": "slider", + "id": prop.name, + "data": { "value": prop.convertToSlider(prop.value) } + })); + }); + checkBoxArray.forEach(function (cbox) { + tablet.emitScriptEvent(JSON.stringify({ + "type": "checkboxtick", + "id": cbox.id, + "data": { "value": cbox.data.value } + })); + }); + } +} + +function updateSignalColors() { + + // force the updates by running the threshold comparisons + withinBaseOfSupport(currentStateReadings.headPose.translation); + withinThresholdOfStandingHeightMode(currentStateReadings.diffFromMode); + headAngularVelocityBelowThreshold(currentStateReadings.headPose.angularVelocity); + handDirectionMatchesHeadDirection(currentStateReadings.lhandPose, currentStateReadings.rhandPose); + handAngularVelocityBelowThreshold(currentStateReadings.lhandPose, currentStateReadings.rhandPose); + headVelocityGreaterThanThreshold(Vec3.length(currentStateReadings.headPose.velocity)); + headMovedAwayFromAveragePosition(currentStateReadings.diffFromAveragePosition); + headLowerThanHeightAverage(currentStateReadings.diffFromAverageHeight); + isHeadLevel(currentStateReadings.diffFromAverageEulers); + + propArray.forEach(function (prop) { + if (prop.signalOn) { + tablet.emitScriptEvent(JSON.stringify({ "type": "trigger", "id": prop.signalType, "data": { "value": "green" } })); + } else { + tablet.emitScriptEvent(JSON.stringify({ "type": "trigger", "id": prop.signalType, "data": { "value": "red" } })); + } + }); +} + +function onScreenChanged(type, url) { + print("Screen changed"); + if (type === "Web" && url === HTML_URL) { + if (!activated) { + // hook up to event bridge + tablet.webEventReceived.connect(onWebEventReceived); + print("after connect web event"); + MyAvatar.hmdLeanRecenterEnabled = false; + + } + activated = true; + } else { + if (activated) { + // disconnect from event bridge + tablet.webEventReceived.disconnect(onWebEventReceived); + documentLoaded = false; + } + activated = false; + } +} + +function getLog(x, y) { + return Math.log(y) / Math.log(x); +} + +function noConversion(num) { + return num; +} + +function convertLog(base, num, direction, shift) { + return direction * getLog(base, (num + 1.0)) + shift; +} + +function convertExponential(base, num, direction, shift) { + return Math.pow(base, (direction*num + shift)) - 1.0; +} + +function convertToCentimeters(num) { + return num * CENTIMETERSPERMETER; +} + +function convertToMeters(num) { + print("convert to meters " + num); + return num / CENTIMETERSPERMETER; +} + +function isInsideLine(a, b, c) { + return (((b.x - a.x)*(c.z - a.z) - (b.z - a.z)*(c.x - a.x)) > 0); +} + +function setAngularThreshold(num) { + headAngularVelocityProperty.value = num; + print("angular threshold " + headAngularVelocityProperty.get()); +} + +function setHeadRollThreshold(num) { + headRollProperty.value = num; + print("head roll threshold " + headRollProperty.get()); +} + +function setHeadPitchThreshold(num) { + headPitchProperty.value = num; + print("head pitch threshold " + headPitchProperty.get()); +} + +function setHeightThreshold(num) { + heightDifferenceProperty.value = num; + print("height threshold " + heightDifferenceProperty.get()); +} + +function setLateralDistance(num) { + lateralBaseProperty.value = num; + frontLeft.x = -lateralBaseProperty.get(); + frontRight.x = lateralBaseProperty.get(); + backLeft.x = -lateralBaseProperty.get(); + backRight.x = lateralBaseProperty.get(); + print("lateral distance " + lateralBaseProperty.get()); +} + +function setAnteriorDistance(num) { + frontBaseProperty.value = num; + frontLeft.z = -frontBaseProperty.get(); + frontRight.z = -frontBaseProperty.get(); + print("anterior distance " + frontBaseProperty.get()); +} + +function setPosteriorDistance(num) { + backBaseProperty.value = num; + backLeft.z = backBaseProperty.get(); + backRight.z = backBaseProperty.get(); + print("posterior distance " + backBaseProperty.get()); +} + +function setHandAngularVelocityThreshold(num) { + handsAngularVelocityProperty.value = num; + print("hand angular velocity threshold " + handsAngularVelocityProperty.get()); +} + +function setHandVelocityThreshold(num) { + handsVelocityProperty.value = num; + print("hand velocity threshold " + handsVelocityProperty.get()); +} + +function setHeadVelocityThreshold(num) { + headVelocityProperty.value = num; + print("headvelocity threshold " + headVelocityProperty.get()); +} + +function withinBaseOfSupport(pos) { + var userScale = 1.0; + frontBaseProperty.signalOn = !(isInsideLine(Vec3.multiply(userScale, frontLeft), Vec3.multiply(userScale, frontRight), pos)); + backBaseProperty.signalOn = !(isInsideLine(Vec3.multiply(userScale, backRight), Vec3.multiply(userScale, backLeft), pos)); + lateralBaseProperty.signalOn = !(isInsideLine(Vec3.multiply(userScale, frontRight), Vec3.multiply(userScale, backRight), pos) + && isInsideLine(Vec3.multiply(userScale, backLeft), Vec3.multiply(userScale, frontLeft), pos)); + return (!frontBaseProperty.signalOn && !backBaseProperty.signalOn && !lateralBaseProperty.signalOn); +} + +function withinThresholdOfStandingHeightMode(heightDiff) { + if (usingModeHeight.data.value) { + heightDifferenceProperty.signalOn = heightDiff < heightDifferenceProperty.get(); + return heightDifferenceProperty.signalOn; + } else { + return true; + } +} + +function headAngularVelocityBelowThreshold(headAngularVelocity) { + var angVel = Vec3.length({ x: headAngularVelocity.x, y: 0, z: headAngularVelocity.z }); + headAngularVelocityProperty.signalOn = angVel < headAngularVelocityProperty.get(); + return headAngularVelocityProperty.signalOn; +} + +function handDirectionMatchesHeadDirection(lhPose, rhPose) { + handsVelocityProperty.signalOn = ((handsVelocityProperty.get() < NO_SHARED_DIRECTION) || + ((!lhPose.valid || ((handDotHead[LEFT] > handsVelocityProperty.get()) && + (Vec3.length(lhPose.velocity) > VELOCITY_EPSILON))) && + (!rhPose.valid || ((handDotHead[RIGHT] > handsVelocityProperty.get()) && + (Vec3.length(rhPose.velocity) > VELOCITY_EPSILON))))); + return handsVelocityProperty.signalOn; +} + +function handAngularVelocityBelowThreshold(lhPose, rhPose) { + var xzRHandAngularVelocity = Vec3.length({ x: rhPose.angularVelocity.x, y: 0.0, z: rhPose.angularVelocity.z }); + var xzLHandAngularVelocity = Vec3.length({ x: lhPose.angularVelocity.x, y: 0.0, z: lhPose.angularVelocity.z }); + handsAngularVelocityProperty.signalOn = ((!rhPose.valid ||(xzRHandAngularVelocity < handsAngularVelocityProperty.get())) + && (!lhPose.valid || (xzLHandAngularVelocity < handsAngularVelocityProperty.get()))); + return handsAngularVelocityProperty.signalOn; +} + +function headVelocityGreaterThanThreshold(headVel) { + headVelocityProperty.signalOn = (headVel > headVelocityProperty.get()) || (headVelocityProperty.get() < VELOCITY_EPSILON); + return headVelocityProperty.signalOn; +} + +function headMovedAwayFromAveragePosition(headDelta) { + return !withinBaseOfSupport(headDelta) || !usingAverageHeadPosition.data.value; +} + +function headLowerThanHeightAverage(heightDiff) { + if (usingAverageHeight.data.value) { + print("head lower than height average"); + heightDifferenceProperty.signalOn = heightDiff < heightDifferenceProperty.get(); + return heightDifferenceProperty.signalOn; + } else { + return true; + } +} + +function isHeadLevel(diffEulers) { + headRollProperty.signalOn = Math.abs(diffEulers.z) < headRollProperty.get(); + headPitchProperty.signalOn = Math.abs(diffEulers.x) < headPitchProperty.get(); + return (headRollProperty.signalOn && headPitchProperty.signalOn); +} + +function findAverage(arr) { + var sum = arr.reduce(function (acc, val) { + return acc + val; + },0); + return sum / arr.length; +} + +function addToModeArray(arr,num) { + for (var i = 0 ;i < (arr.length - 1); i++) { + arr[i] = arr[i+1]; + } + arr[arr.length - 1] = (Math.floor(num*CENTIMETERSPERMETER))/CENTIMETERSPERMETER; +} + +function findMode(ary, currentMode, backLength, defaultBack, currentHeight) { + var numMapping = {}; + var greatestFreq = 0; + var mode; + ary.forEach(function (number) { + numMapping[number] = (numMapping[number] || 0) + 1; + if ((greatestFreq < numMapping[number]) || ((numMapping[number] === MODE_SAMPLE_LENGTH) && (number > currentMode) )) { + greatestFreq = numMapping[number]; + mode = number; + } + }); + if (mode > currentMode) { + return Number(mode); + } else { + if (!RESET_MODE && HMD.active) { + print("resetting the mode............................................. "); + print("resetting the mode............................................. "); + RESET_MODE = true; + var correction = 0.02; + return currentHeight - correction; + } else { + return currentMode; + } + } +} + +function update(dt) { + if (debugDrawBase) { + drawBase(); + } + // Update current state information + currentStateReadings.headPose = Controller.getPoseValue(Controller.Standard.Head); + currentStateReadings.rhandPose = Controller.getPoseValue(Controller.Standard.RightHand); + currentStateReadings.lhandPose = Controller.getPoseValue(Controller.Standard.LeftHand); + + // back length + var headMinusHipLean = Vec3.subtract(currentStateReadings.headPose.translation, DEFAULT_HIPS_POSITION); + currentStateReadings.backLength = Vec3.length(headMinusHipLean); + // print("back length and default " + currentStateReadings.backLength + " " + DEFAULT_TORSO_LENGTH); + + // mode height + addToModeArray(modeArray, currentStateReadings.headPose.translation.y); + modeHeight = findMode(modeArray, modeHeight, currentStateReadings.backLength, DEFAULT_TORSO_LENGTH, + currentStateReadings.headPose.translation.y); + currentStateReadings.diffFromMode = modeHeight - currentStateReadings.headPose.translation.y; + + // hand direction + var leftHandLateralPoseVelocity = currentStateReadings.lhandPose.velocity; + leftHandLateralPoseVelocity.y = 0.0; + var rightHandLateralPoseVelocity = currentStateReadings.rhandPose.velocity; + rightHandLateralPoseVelocity.y = 0.0; + var headLateralPoseVelocity = currentStateReadings.headPose.velocity; + headLateralPoseVelocity.y = 0.0; + handDotHead[LEFT] = Vec3.dot(Vec3.normalize(leftHandLateralPoseVelocity), Vec3.normalize(headLateralPoseVelocity)); + handDotHead[RIGHT] = Vec3.dot(Vec3.normalize(rightHandLateralPoseVelocity), Vec3.normalize(headLateralPoseVelocity)); + + // average head position + headAveragePosition = Vec3.mix(headAveragePosition, currentStateReadings.headPose.translation, AVERAGING_RATE); + currentStateReadings.diffFromAveragePosition = Vec3.subtract(currentStateReadings.headPose.translation, + headAveragePosition); + + // average height + averageHeight = currentStateReadings.headPose.translation.y * HEIGHT_AVERAGING_RATE + + averageHeight * (1.0 - HEIGHT_AVERAGING_RATE); + currentStateReadings.diffFromAverageHeight = Math.abs(currentStateReadings.headPose.translation.y - averageHeight); + + // eulers diff + headEulers = Quat.safeEulerAngles(currentStateReadings.headPose.rotation); + headAverageOrientation = Quat.slerp(headAverageOrientation, currentStateReadings.headPose.rotation, AVERAGING_RATE); + headAverageEulers = Quat.safeEulerAngles(headAverageOrientation); + currentStateReadings.diffFromAverageEulers = Vec3.subtract(headAverageEulers, headEulers); + + // headpose rig space is for determining when to recenter rotation. + var headPoseRigSpace = Quat.multiply(CHANGE_OF_BASIS_ROTATION, currentStateReadings.headPose.rotation); + headPoseAverageOrientation = Quat.slerp(headPoseAverageOrientation, headPoseRigSpace, AVERAGING_RATE); + var headPoseAverageEulers = Quat.safeEulerAngles(headPoseAverageOrientation); + + // make the signal colors reflect the current thresholds that have been crossed + updateSignalColors(); + + SPINE_STRETCH_LIMIT = (0.04) * DEFAULT_TORSO_LENGTH * MyAvatar.scale; + + //print("the spine stretch limit is " + SPINE_STRETCH_LIMIT + " head avatar space is " + currentStateReadings.headPose.translation.y); + //print("the current back length is " + currentStateReadings.backLength + " " + DEFAULT_TORSO_LENGTH); + // Conditions for taking a step. + // 1. off the base of support. front, lateral, back edges. + // 2. head is not lower than the height mode value by more than the maxHeightChange tolerance + // 3. the angular velocity of the head is not greater than the threshold value + // ie this reflects the speed the head is rotating away from having up = (0,1,0) in Avatar frame.. + // 4. the hands velocity vector has the same direction as the head, within the given tolerance + // the tolerance is an acos value, -1 means the hands going in any direction will not block translating + // up to 1 where the hands velocity direction must exactly match that of the head. -1 threshold disables this condition. + // 5. the angular velocity xz magnitude for each hand is below the threshold value + // ie here this reflects the speed that each hand is rotating away from having up = (0,1,0) in Avatar frame. + // 6. head velocity is below step threshold + // 7. head has moved further than the threshold from the running average position of the head. + // 8. head height is not lower than the running average head height with a difference of maxHeightChange. + // 9. head's rotation in avatar space is not pitching or rolling greater than the pitch or roll thresholds + if (!withinBaseOfSupport(currentStateReadings.headPose.translation) && + withinThresholdOfStandingHeightMode(currentStateReadings.diffFromMode) && + headAngularVelocityBelowThreshold(currentStateReadings.headPose.angularVelocity) && + handDirectionMatchesHeadDirection(currentStateReadings.lhandPose, currentStateReadings.rhandPose) && + handAngularVelocityBelowThreshold(currentStateReadings.lhandPose, currentStateReadings.rhandPose) && + headVelocityGreaterThanThreshold(Vec3.length(currentStateReadings.headPose.velocity)) && + headMovedAwayFromAveragePosition(currentStateReadings.diffFromAveragePosition) && + headLowerThanHeightAverage(currentStateReadings.diffFromAverageHeight) && + isHeadLevel(currentStateReadings.diffFromAverageEulers)) { + + if (stepTimer < 0.0) { //!MyAvatar.isRecenteringHorizontally() + print("trigger recenter========================================================"); + MyAvatar.triggerHorizontalRecenter(); + stepTimer = STEP_TIME_SECS; + } + } else if ((currentStateReadings.backLength > (DEFAULT_TORSO_LENGTH + SPINE_STRETCH_LIMIT)) && + (failsafeSignalTimer < 0.0) && HMD.active) { + // do the failsafe recenter. + // failsafeFlag stops repeated setting of failsafe button color. + // RESET_MODE false forces a reset of the height + RESET_MODE = false; + failsafeFlag = true; + failsafeSignalTimer = FAILSAFE_TIMEOUT; + MyAvatar.triggerHorizontalRecenter(); + tablet.emitScriptEvent(JSON.stringify({ "type": "failsafe", "id": "failsafeSignal", "data": { "value": "green" } })); + // in fail safe we debug print the values that were blocking us. + print("failsafe debug---------------------------------------------------------------"); + propArray.forEach(function (prop) { + print(prop.name); + if (!prop.signalOn) { + print(prop.signalType + " contributed to failsafe call"); + } + }); + print("end failsafe debug---------------------------------------------------------------"); + + } + + if ((failsafeSignalTimer < 0.0) && failsafeFlag) { + failsafeFlag = false; + tablet.emitScriptEvent(JSON.stringify({ "type": "failsafe", "id": "failsafeSignal", "data": { "value": "orange" } })); + } + + stepTimer -= dt; + failsafeSignalTimer -= dt; + + if (!HMD.active) { + RESET_MODE = false; + } + + if (Math.abs(headPoseAverageEulers.y) > HEAD_TURN_THRESHOLD) { + // Turn feet + // MyAvatar.triggerRotationRecenter(); + // headPoseAverageOrientation = { x: 0, y: 0, z: 0, w: 1 }; + } +} + +function shutdownTabletApp() { + // GlobalDebugger.stop(); + tablet.removeButton(tabletButton); + if (activated) { + tablet.webEventReceived.disconnect(onWebEventReceived); + tablet.gotoHomeScreen(); + } + tablet.screenChanged.disconnect(onScreenChanged); +} + +tabletButton.clicked.connect(manageClick); +tablet.screenChanged.connect(onScreenChanged); + +Script.setTimeout(function() { + DEFAULT_HIPS_POSITION = MyAvatar.getAbsoluteDefaultJointTranslationInObjectFrame(MyAvatar.getJointIndex("Hips")); + DEFAULT_HEAD_POSITION = MyAvatar.getAbsoluteDefaultJointTranslationInObjectFrame(MyAvatar.getJointIndex("Head")); + DEFAULT_TORSO_LENGTH = Vec3.length(Vec3.subtract(DEFAULT_HEAD_POSITION, DEFAULT_HIPS_POSITION)); + SPINE_STRETCH_LIMIT = (0.04) * DEFAULT_TORSO_LENGTH * MyAvatar.scale; +},(4*LOADING_DELAY)); + +Script.update.connect(update); +Controller.keyPressEvent.connect(onKeyPress); +Script.scriptEnding.connect(function () { + MyAvatar.hmdLeanRecenterEnabled = true; + Script.update.disconnect(update); + shutdownTabletApp(); +}); diff --git a/scripts/system/avatarapp.js b/scripts/system/avatarapp.js index 10ccb66d96..ee4c6736a2 100644 --- a/scripts/system/avatarapp.js +++ b/scripts/system/avatarapp.js @@ -63,7 +63,8 @@ function getMyAvatar() { function getMyAvatarSettings() { return { dominantHand: MyAvatar.getDominantHand(), - collisionsEnabled : MyAvatar.getCollisionsEnabled(), + collisionsEnabled: MyAvatar.getCollisionsEnabled(), + sittingEnabled: MyAvatar.isInSittingState, collisionSoundUrl : MyAvatar.collisionSoundURL, animGraphUrl: MyAvatar.getAnimGraphUrl(), animGraphOverrideUrl : MyAvatar.getAnimGraphOverrideUrl(), @@ -136,6 +137,13 @@ function onCollisionsEnabledChanged(enabled) { } } +function onSittingEnabledChanged(isSitting) { + if (currentAvatarSettings.sittingEnabled !== isSitting) { + currentAvatarSettings.sittingEnabled = isSitting; + sendToQml({ 'method': 'settingChanged', 'name': 'sittingEnabled', 'value': isSitting }) + } +} + function onNewCollisionSoundUrl(url) { if(currentAvatarSettings.collisionSoundUrl !== url) { currentAvatarSettings.collisionSoundUrl = url; @@ -314,9 +322,10 @@ function fromQml(message) { // messages are {method, params}, like json-rpc. See MyAvatar.setDominantHand(message.settings.dominantHand); MyAvatar.setCollisionsEnabled(message.settings.collisionsEnabled); + MyAvatar.isInSittingState = message.settings.sittingEnabled; MyAvatar.collisionSoundURL = message.settings.collisionSoundUrl; MyAvatar.setAnimGraphOverrideUrl(message.settings.animGraphOverrideUrl); - + print("save settings"); settings = getMyAvatarSettings(); break; default: @@ -507,6 +516,7 @@ function off() { MyAvatar.skeletonModelURLChanged.disconnect(onSkeletonModelURLChanged); MyAvatar.dominantHandChanged.disconnect(onDominantHandChanged); MyAvatar.collisionsEnabledChanged.disconnect(onCollisionsEnabledChanged); + MyAvatar.sittingEnabledChanged.disconnect(onSittingEnabledChanged); MyAvatar.newCollisionSoundURL.disconnect(onNewCollisionSoundUrl); MyAvatar.animGraphUrlChanged.disconnect(onAnimGraphUrlChanged); MyAvatar.targetScaleChanged.disconnect(onTargetScaleChanged); @@ -521,6 +531,7 @@ function on() { MyAvatar.skeletonModelURLChanged.connect(onSkeletonModelURLChanged); MyAvatar.dominantHandChanged.connect(onDominantHandChanged); MyAvatar.collisionsEnabledChanged.connect(onCollisionsEnabledChanged); + MyAvatar.sittingEnabledChanged.connect(onSittingEnabledChanged); MyAvatar.newCollisionSoundURL.connect(onNewCollisionSoundUrl); MyAvatar.animGraphUrlChanged.connect(onAnimGraphUrlChanged); MyAvatar.targetScaleChanged.connect(onTargetScaleChanged); From 42cb8a7ef0159f130c65d6a424bf8b54ae14323e Mon Sep 17 00:00:00 2001 From: amantley Date: Thu, 27 Sep 2018 16:59:54 -0700 Subject: [PATCH 08/27] avatar app changes --- interface/src/avatar/MyAvatar.cpp | 2 +- scripts/system/avatarapp.js | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 631a4b0670..d54616f245 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -477,7 +477,7 @@ void MyAvatar::update(float deltaTime) { auto sensorHeadPoseDebug = getControllerPoseInSensorFrame(controller::Action::HEAD); glm::vec3 upHead = transformVectorFast(sensorHeadPoseDebug.getMatrix(), glm::vec3(0.0f, 1.0f, 0.0f)); float acosHead = glm::dot(upHead, glm::vec3(0.0f, 1.0f, 0.0f)); - // qCDebug(interfaceapp) << "sensor space head pos " << sensorHeadPoseDebug.getTranslation().y; + // qCDebug(interfaceapp) << "sensor space head pos " << sensorHeadPoseDebug.getTranslation().y; if ((acosHead > 0.98f) && !getIsInSittingState() && (sensorHeadPoseDebug.getTranslation().y < -0.5f)) { //qCDebug(interfaceapp) << "we are going to sitting state because it looks like we should" << sensorHeadPoseDebug.getTranslation().y; } diff --git a/scripts/system/avatarapp.js b/scripts/system/avatarapp.js index ee4c6736a2..16761f29a6 100644 --- a/scripts/system/avatarapp.js +++ b/scripts/system/avatarapp.js @@ -140,6 +140,7 @@ function onCollisionsEnabledChanged(enabled) { function onSittingEnabledChanged(isSitting) { if (currentAvatarSettings.sittingEnabled !== isSitting) { currentAvatarSettings.sittingEnabled = isSitting; + print("emit sitting changed"); sendToQml({ 'method': 'settingChanged', 'name': 'sittingEnabled', 'value': isSitting }) } } From 884ad66a146541fba08975af43e8cbfe1e46cd16 Mon Sep 17 00:00:00 2001 From: amantley Date: Fri, 28 Sep 2018 17:18:24 -0700 Subject: [PATCH 09/27] added sensor space detection of height of the user in MyAvatar.cpp 1.2 * average sensor space height changes to standing. .833 sensorspace height changes to sitting. there is also a manual override in the avatar app settings now --- interface/src/avatar/MyAvatar.cpp | 38 +++++++++++++++++++++++++------ interface/src/avatar/MyAvatar.h | 4 +++- 2 files changed, 34 insertions(+), 8 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index d54616f245..819778b1e7 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -475,6 +475,12 @@ void MyAvatar::update(float deltaTime) { // if the head is close to the floor in sensor space // and the up of the head is close to sensor up then try switching us to sitting state. auto sensorHeadPoseDebug = getControllerPoseInSensorFrame(controller::Action::HEAD); + if (sensorHeadPoseDebug.isValid()) { + _sumUserHeightSensorSpace += sensorHeadPoseDebug.getTranslation().y; + _averageUserHeightCount++; + } + qCDebug(interfaceapp) << sensorHeadPoseDebug.isValid() << " valid. sensor space average " << (_sumUserHeightSensorSpace / _averageUserHeightCount) << " head position sensor y value " << sensorHeadPoseDebug.getTranslation().y; + glm::vec3 upHead = transformVectorFast(sensorHeadPoseDebug.getMatrix(), glm::vec3(0.0f, 1.0f, 0.0f)); float acosHead = glm::dot(upHead, glm::vec3(0.0f, 1.0f, 0.0f)); // qCDebug(interfaceapp) << "sensor space head pos " << sensorHeadPoseDebug.getTranslation().y; @@ -3854,6 +3860,9 @@ void MyAvatar::setIsInWalkingState(bool isWalking) { void MyAvatar::setIsInSittingState(bool isSitting) { _isInSittingState = isSitting; + controller::Pose sensorHeadPoseDebug = getControllerPoseInSensorFrame(controller::Action::HEAD); + _sumUserHeightSensorSpace = sensorHeadPoseDebug.getTranslation().y; + _averageUserHeightCount = 1; emit sittingEnabledChanged(isSitting); } @@ -4093,21 +4102,36 @@ bool MyAvatar::FollowHelper::shouldActivateVertical(MyAvatar& myAvatar, const gl glm::vec3 headWorldSpace = myAvatar.getHead()->getPosition(); glm::mat4 worldToSensorMatrix = glm::inverse(myAvatar.getSensorToWorldMatrix()); glm::vec3 headSensorSpace = transformVectorFast(myAvatar.getSensorToWorldMatrix(), headWorldSpace); - //qCDebug(interfaceapp) << "sensor space position " << extractTranslation(currentBodyMatrix) << " head position sensor " << sensorHeadPose.getTranslation(); + //get the mode. + //put it in sensor space. + // if we are 20% higher switch to standing. + // 16.6% lower then switch to sitting. + // add this !!!! And the head is upright. + float averageSensorSpaceHeight = myAvatar._sumUserHeightSensorSpace / myAvatar._averageUserHeightCount; if (myAvatar.getIsInSittingState()) { if (offset.y < SITTING_BOTTOM) { // we recenter when sitting. return true; - } else if (offset.y > 2.0*CYLINDER_TOP) { + } else if (sensorHeadPose.getTranslation().y > (1.2f * averageSensorSpaceHeight)) { // if we recenter upwards then no longer in sitting state myAvatar.setIsInSittingState(false); + //myAvatar._sumUserHeightSensorSpace = 1.2f * averageSensorSpaceHeight; + // myAvatar._averageUserHeightCount = 1; return true; } else { return false; } } else { - return (offset.y > CYLINDER_TOP) || (offset.y < CYLINDER_BOTTOM); + // in the standing state + if (sensorHeadPose.getTranslation().y < (0.83f * averageSensorSpaceHeight)) { + myAvatar.setIsInSittingState(true); + // myAvatar._sumUserHeightSensorSpace = 0.83f * averageSensorSpaceHeight; + // myAvatar._averageUserHeightCount = 1; + return true; + } else { + return (offset.y > CYLINDER_TOP) || (offset.y < CYLINDER_BOTTOM); + } } } @@ -4124,14 +4148,14 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat if ((_squatCount > 600) && !isActive(Vertical) && !isActive(Horizontal)) { if (myAvatar.getIsInSittingState()) { // activate(Horizontal); - activate(Vertical); + //activate(Vertical); _squatCount = 0; } else { if (myAvatar.getControllerPoseInAvatarFrame(controller::Action::HEAD).getTranslation().y < (headDefaultPos.y - 0.20f)) { - myAvatar.setIsInSittingState(true); - activate(Vertical); + //myAvatar.setIsInSittingState(true); + //activate(Vertical); } else { - activate(Horizontal); + //activate(Horizontal); } _squatCount = 0; } diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index d9744e93fe..628c358202 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -1805,6 +1805,8 @@ private: // height of user in sensor space, when standing erect. ThreadSafeValueCache _userHeight { DEFAULT_AVATAR_HEIGHT }; + float _sumUserHeightSensorSpace { DEFAULT_AVATAR_HEIGHT }; + int _averageUserHeightCount { 1 }; void updateChildCauterization(SpatiallyNestablePointer object, bool cauterize); @@ -1814,7 +1816,7 @@ private: ThreadSafeValueCache _sprintSpeed { AVATAR_SPRINT_SPEED_SCALAR }; float _walkSpeedScalar { AVATAR_WALK_SPEED_SCALAR }; bool _isInWalkingState { false }; - bool _isInSittingState { true }; + bool _isInSittingState { false }; // load avatar scripts once when rig is ready bool _shouldLoadScripts { false }; From 3a2a8fe4f8d4dfa7eb965fa4dff59d2914b207d3 Mon Sep 17 00:00:00 2001 From: amantley Date: Sat, 29 Sep 2018 15:49:11 -0700 Subject: [PATCH 10/27] adjusted openvr to use 0.0 for the floor instead of -1.6 --- plugins/openvr/src/OpenVrDisplayPlugin.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/plugins/openvr/src/OpenVrDisplayPlugin.cpp b/plugins/openvr/src/OpenVrDisplayPlugin.cpp index fae2144caf..1638a17631 100644 --- a/plugins/openvr/src/OpenVrDisplayPlugin.cpp +++ b/plugins/openvr/src/OpenVrDisplayPlugin.cpp @@ -463,7 +463,7 @@ bool OpenVrDisplayPlugin::internalActivate() { auto chaperone = vr::VRChaperone(); if (chaperone) { float const UI_RADIUS = 1.0f; - float const UI_HEIGHT = 1.6f; + float const UI_HEIGHT = 0.0f; float const UI_Z_OFFSET = 0.5; float xSize, zSize; From 9e400459270ad90fd896d75025ca7cf5c60a109d Mon Sep 17 00:00:00 2001 From: amantley Date: Sun, 30 Sep 2018 17:13:34 -0700 Subject: [PATCH 11/27] added criteria to stop false positive for sit down --- interface/src/avatar/MyAvatar.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 02ea49cda0..f281d8c87d 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -4079,13 +4079,21 @@ bool MyAvatar::FollowHelper::shouldActivateVertical(MyAvatar& myAvatar, const gl // if we are 20% higher switch to standing. // 16.6% lower then switch to sitting. // add this !!!! And the head is upright. + glm::vec3 upHead = transformVectorFast(sensorHeadPose.getMatrix(), glm::vec3(0.0f, 1.0f, 0.0f)); + float acosHead = glm::dot(upHead, glm::vec3(0.0f, 1.0f, 0.0f)); + glm::vec3 avatarHips = myAvatar.getAbsoluteJointTranslationInObjectFrame(myAvatar.getJointIndex("Hips")); + glm::vec3 worldHips = transformVectorFast(myAvatar.getTransform().getMatrix(),avatarHips); + glm::vec3 sensorHips = transformVectorFast(myAvatar.getSensorToWorldMatrix(), worldHips); float averageSensorSpaceHeight = myAvatar._sumUserHeightSensorSpace / myAvatar._averageUserHeightCount; + // we could add a counting here to make sure that a lean forward doesn't accidentally put you in sitting mode. + // but maybe so what. + // the real test is... can I pick something up in standing mode? if (myAvatar.getIsInSittingState()) { if (offset.y < SITTING_BOTTOM) { // we recenter when sitting. return true; - } else if (sensorHeadPose.getTranslation().y > (1.2f * averageSensorSpaceHeight)) { + } else if (sensorHeadPose.getTranslation().y > (1.2f * averageSensorSpaceHeight)) { // if we recenter upwards then no longer in sitting state myAvatar.setIsInSittingState(false); //myAvatar._sumUserHeightSensorSpace = 1.2f * averageSensorSpaceHeight; @@ -4096,7 +4104,7 @@ bool MyAvatar::FollowHelper::shouldActivateVertical(MyAvatar& myAvatar, const gl } } else { // in the standing state - if (sensorHeadPose.getTranslation().y < (0.83f * averageSensorSpaceHeight)) { + if ((sensorHeadPose.getTranslation().y < (0.83f * averageSensorSpaceHeight)) && (acosHead > 0.98f) && !(sensorHips.y > (0.4f * averageSensorSpaceHeight)) { myAvatar.setIsInSittingState(true); // myAvatar._sumUserHeightSensorSpace = 0.83f * averageSensorSpaceHeight; // myAvatar._averageUserHeightCount = 1; From fe7c14e5cf3523beb1be9cb1d5b0feb23188deb5 Mon Sep 17 00:00:00 2001 From: amantley Date: Sun, 30 Sep 2018 17:15:07 -0700 Subject: [PATCH 12/27] removed extraneous script --- scripts/developer/objectOrientedStep.js | 688 ------------------------ 1 file changed, 688 deletions(-) delete mode 100644 scripts/developer/objectOrientedStep.js diff --git a/scripts/developer/objectOrientedStep.js b/scripts/developer/objectOrientedStep.js deleted file mode 100644 index a5c27e36b9..0000000000 --- a/scripts/developer/objectOrientedStep.js +++ /dev/null @@ -1,688 +0,0 @@ -/* jslint bitwise: true */ - -/* global Script, Vec3, MyAvatar, Tablet, Messages, Quat, -DebugDraw, Mat4, Entities, Xform, Controller, Camera, console, document*/ - -Script.registerValue("STEPAPP", true); -var CENTIMETERSPERMETER = 100.0; -var LEFT = 0; -var RIGHT = 1; -var INCREASING = 1.0; -var DECREASING = -1.0; -var DEFAULT_AVATAR_HEIGHT = 1.64; -var TABLET_BUTTON_NAME = "STEP"; -var CHANGE_OF_BASIS_ROTATION = { x: 0, y: 1, z: 0, w: 0 }; -// in meters (mostly) -var DEFAULT_ANTERIOR = 0.04; -var DEFAULT_POSTERIOR = 0.06; -var DEFAULT_LATERAL = 0.10; -var DEFAULT_HEIGHT_DIFFERENCE = 0.02; -var DEFAULT_ANGULAR_VELOCITY = 0.3; -var DEFAULT_HAND_VELOCITY = 0.4; -var DEFAULT_ANGULAR_HAND_VELOCITY = 3.3; -var DEFAULT_HEAD_VELOCITY = 0.14; -var DEFAULT_LEVEL_PITCH = 7; -var DEFAULT_LEVEL_ROLL = 7; -var DEFAULT_DIFF = 0.0; -var DEFAULT_DIFF_EULERS = { x: 0.0, y: 0.0, z: 0.0 }; -var DEFAULT_HIPS_POSITION; -var DEFAULT_HEAD_POSITION; -var DEFAULT_TORSO_LENGTH; -var SPINE_STRETCH_LIMIT = 0.02; - -var VELOCITY_EPSILON = 0.02; -var AVERAGING_RATE = 0.03; -var HEIGHT_AVERAGING_RATE = 0.01; -var STEP_TIME_SECS = 0.2; -var MODE_SAMPLE_LENGTH = 100; -var RESET_MODE = false; -var HEAD_TURN_THRESHOLD = 25.0; -var NO_SHARED_DIRECTION = -0.98; -var LOADING_DELAY = 500; -var FAILSAFE_TIMEOUT = 2.5; - -var debugDrawBase = true; -var activated = false; -var documentLoaded = false; -var failsafeFlag = false; -var failsafeSignalTimer = -1.0; -var stepTimer = -1.0; - - -var modeArray = new Array(MODE_SAMPLE_LENGTH); -var modeHeight = -10.0; - -var handPosition; -var handOrientation; -var hands = []; -var hipToHandAverage = []; -var handDotHead = []; -var headAverageOrientation = MyAvatar.orientation; -var headPoseAverageOrientation = { x: 0, y: 0, z: 0, w: 1 }; -var averageHeight = 1.0; -var headEulers = { x: 0.0, y: 0.0, z: 0.0 }; -var headAverageEulers = { x: 0.0, y: 0.0, z: 0.0 }; -var headAveragePosition = { x: 0, y: 0.4, z: 0 }; -var frontLeft = { x: -DEFAULT_LATERAL, y: 0, z: -DEFAULT_ANTERIOR }; -var frontRight = { x: DEFAULT_LATERAL, y: 0, z: -DEFAULT_ANTERIOR }; -var backLeft = { x: -DEFAULT_LATERAL, y: 0, z: DEFAULT_POSTERIOR }; -var backRight = { x: DEFAULT_LATERAL, y: 0, z: DEFAULT_POSTERIOR }; - - -// define state readings constructor -function StateReading(headPose, rhandPose, lhandPose, backLength, diffFromMode, diffFromAverageHeight, diffFromAveragePosition, - diffFromAverageEulers) { - this.headPose = headPose; - this.rhandPose = rhandPose; - this.lhandPose = lhandPose; - this.backLength = backLength; - this.diffFromMode = diffFromMode; - this.diffFromAverageHeight = diffFromAverageHeight; - this.diffFromAveragePosition = diffFromAveragePosition; - this.diffFromAverageEulers = diffFromAverageEulers; -} - -// define current state readings object for holding tracker readings and current differences from averages -var currentStateReadings = new StateReading(Controller.getPoseValue(Controller.Standard.Head), - Controller.getPoseValue(Controller.Standard.RightHand), Controller.getPoseValue(Controller.Standard.LeftHand), - DEFAULT_TORSO_LENGTH, DEFAULT_DIFF, DEFAULT_DIFF, DEFAULT_DIFF, DEFAULT_DIFF_EULERS); - -// declare the checkbox constructor -function AppCheckbox(type,id,eventType,isChecked) { - this.type = type; - this.id = id; - this.eventType = eventType; - this.data = {value: isChecked}; -} - -// define the checkboxes in the html file -var usingAverageHeight = new AppCheckbox("checkboxtick", "runningAverageHeightCheck", "onRunningAverageHeightCheckBox", - false); -var usingModeHeight = new AppCheckbox("checkboxtick","modeCheck","onModeCheckBox",true); -var usingBaseOfSupport = new AppCheckbox("checkboxtick","baseOfSupportCheck","onBaseOfSupportCheckBox",true); -var usingAverageHeadPosition = new AppCheckbox("checkboxtick", "headAveragePositionCheck", "onHeadAveragePositionCheckBox", - false); - -var checkBoxArray = new Array(usingAverageHeight,usingModeHeight,usingBaseOfSupport,usingAverageHeadPosition); - -// declare the html slider constructor -function AppProperty(name, type, eventType, signalType, setFunction, initValue, convertToThreshold, convertToSlider, signalOn) { - this.name = name; - this.type = type; - this.eventType = eventType; - this.signalType = signalType; - this.setValue = setFunction; - this.value = initValue; - this.get = function () { - return this.value; - }; - this.convertToThreshold = convertToThreshold; - this.convertToSlider = convertToSlider; - this.signalOn = signalOn; -} - -// define the sliders -var frontBaseProperty = new AppProperty("#anteriorBase-slider", "slider", "onAnteriorBaseSlider", "frontSignal", - setAnteriorDistance, DEFAULT_ANTERIOR, function (num) { - return convertToMeters(num); - }, function (num) { - return convertToCentimeters(num); - },true); -var backBaseProperty = new AppProperty("#posteriorBase-slider", "slider", "onPosteriorBaseSlider", "backSignal", - setPosteriorDistance, DEFAULT_POSTERIOR, function (num) { - return convertToMeters(num); - }, function (num) { - return convertToCentimeters(num); - }, true); -var lateralBaseProperty = new AppProperty("#lateralBase-slider", "slider", "onLateralBaseSlider", "lateralSignal", - setLateralDistance, DEFAULT_LATERAL, function (num) { - return convertToMeters(num); - }, function (num) { - return convertToCentimeters(num); - }, true); -var headAngularVelocityProperty = new AppProperty("#angularVelocityHead-slider", "slider", "onAngularVelocitySlider", - "angularHeadSignal", setAngularThreshold, DEFAULT_ANGULAR_VELOCITY, function (num) { - var base = 4; - var shift = 2; - return convertExponential(base, num, DECREASING, shift); - }, function (num) { - var base = 4; - var shift = 2; - return convertLog(base, num, DECREASING, shift); - }, true); -var heightDifferenceProperty = new AppProperty("#heightDifference-slider", "slider", "onHeightDifferenceSlider", "heightSignal", - setHeightThreshold, DEFAULT_HEIGHT_DIFFERENCE, function (num) { - return convertToMeters(-num); - }, function (num) { - return convertToCentimeters(-num); - }, true); -var handsVelocityProperty = new AppProperty("#handsVelocity-slider", "slider", "onHandsVelocitySlider", "handVelocitySignal", - setHandVelocityThreshold, DEFAULT_HAND_VELOCITY, function (num) { - return num; - }, function (num) { - return num; - }, true); -var handsAngularVelocityProperty = new AppProperty("#handsAngularVelocity-slider", "slider", "onHandsAngularVelocitySlider", - "handAngularSignal", setHandAngularVelocityThreshold, DEFAULT_ANGULAR_HAND_VELOCITY, function (num) { - var base = 7; - var shift = 2; - return convertExponential(base, num, DECREASING, shift); - }, function (num) { - var base = 7; - var shift = 2; - return convertLog(base, num, DECREASING, shift); - }, true); -var headVelocityProperty = new AppProperty("#headVelocity-slider", "slider", "onHeadVelocitySlider", "headVelocitySignal", - setHeadVelocityThreshold, DEFAULT_HEAD_VELOCITY, function (num) { - var base = 2; - var shift = 0; - return convertExponential(base, num, INCREASING, shift); - }, function (num) { - var base = 2; - var shift = 0; - return convertLog(base, num, INCREASING, shift); - }, true); -var headPitchProperty = new AppProperty("#headPitch-slider", "slider", "onHeadPitchSlider", "headPitchSignal", - setHeadPitchThreshold, DEFAULT_LEVEL_PITCH, function (num) { - var base = 2.5; - var shift = 5; - return convertExponential(base, num, DECREASING, shift); - }, function (num) { - var base = 2.5; - var shift = 5; - return convertLog(base, num, DECREASING, shift); - }, true); -var headRollProperty = new AppProperty("#headRoll-slider", "slider", "onHeadRollSlider", "headRollSignal", setHeadRollThreshold, - DEFAULT_LEVEL_ROLL, function (num) { - var base = 2.5; - var shift = 5; - return convertExponential(base, num, DECREASING, shift); - }, function (num) { - var base = 2.5; - var shift = 5; - return convertLog(base, num, DECREASING, shift); - }, true); - -var propArray = new Array(frontBaseProperty, backBaseProperty, lateralBaseProperty, headAngularVelocityProperty, - heightDifferenceProperty, handsVelocityProperty, handsAngularVelocityProperty, headVelocityProperty, headPitchProperty, - headRollProperty); - -// var HTML_URL = Script.resolvePath("http://hifi-content.s3.amazonaws.com/angus/stepApp/stepApp.html"); -var HTML_URL = Script.resolvePath("http://hifi-content.s3.amazonaws.com/angus/stepApp/stepAppExtra.html"); -var tablet = Tablet.getTablet("com.highfidelity.interface.tablet.system"); - -function manageClick() { - if (activated) { - tablet.gotoHomeScreen(); - } else { - tablet.gotoWebScreen(HTML_URL); - } -} - -var tabletButton = tablet.addButton({ - text: TABLET_BUTTON_NAME, - icon: Script.resolvePath("http://hifi-content.s3.amazonaws.com/angus/stepApp/foot.svg"), - activeIcon: Script.resolvePath("http://hifi-content.s3.amazonaws.com/angus/stepApp/foot.svg") -}); - -function drawBase() { - // transform corners into world space, for rendering. - var worldPointLf = Vec3.sum(MyAvatar.position,Vec3.multiplyQbyV(MyAvatar.orientation, frontLeft)); - var worldPointRf = Vec3.sum(MyAvatar.position,Vec3.multiplyQbyV(MyAvatar.orientation, frontRight)); - var worldPointLb = Vec3.sum(MyAvatar.position,Vec3.multiplyQbyV(MyAvatar.orientation, backLeft)); - var worldPointRb = Vec3.sum(MyAvatar.position,Vec3.multiplyQbyV(MyAvatar.orientation, backRight)); - - var GREEN = { r: 0, g: 1, b: 0, a: 1 }; - // draw border - DebugDraw.drawRay(worldPointLf, worldPointRf, GREEN); - DebugDraw.drawRay(worldPointRf, worldPointRb, GREEN); - DebugDraw.drawRay(worldPointRb, worldPointLb, GREEN); - DebugDraw.drawRay(worldPointLb, worldPointLf, GREEN); -} - -function onKeyPress(event) { - if (event.text === "'") { - // when the sensors are reset, then reset the mode. - RESET_MODE = false; - } -} - -function onWebEventReceived(msg) { - var message = JSON.parse(msg); - print(" we have a message from html dialog " + message.type); - propArray.forEach(function (prop) { - if (prop.eventType === message.type) { - prop.setValue(prop.convertToThreshold(message.data.value)); - print("message from " + prop.name); - // break; - } - }); - checkBoxArray.forEach(function(cbox) { - if (cbox.eventType === message.type) { - cbox.data.value = message.data.value; - // break; - } - }); - if (message.type === "onCreateStepApp") { - print("document loaded"); - documentLoaded = true; - Script.setTimeout(initAppForm, LOADING_DELAY); - } -} - -function initAppForm() { - print("step app is loaded: " + documentLoaded); - if (documentLoaded === true) { - propArray.forEach(function (prop) { - tablet.emitScriptEvent(JSON.stringify({ - "type": "trigger", - "id": prop.signalType, - "data": { "value": "green" } - })); - tablet.emitScriptEvent(JSON.stringify({ - "type": "slider", - "id": prop.name, - "data": { "value": prop.convertToSlider(prop.value) } - })); - }); - checkBoxArray.forEach(function (cbox) { - tablet.emitScriptEvent(JSON.stringify({ - "type": "checkboxtick", - "id": cbox.id, - "data": { "value": cbox.data.value } - })); - }); - } -} - -function updateSignalColors() { - - // force the updates by running the threshold comparisons - withinBaseOfSupport(currentStateReadings.headPose.translation); - withinThresholdOfStandingHeightMode(currentStateReadings.diffFromMode); - headAngularVelocityBelowThreshold(currentStateReadings.headPose.angularVelocity); - handDirectionMatchesHeadDirection(currentStateReadings.lhandPose, currentStateReadings.rhandPose); - handAngularVelocityBelowThreshold(currentStateReadings.lhandPose, currentStateReadings.rhandPose); - headVelocityGreaterThanThreshold(Vec3.length(currentStateReadings.headPose.velocity)); - headMovedAwayFromAveragePosition(currentStateReadings.diffFromAveragePosition); - headLowerThanHeightAverage(currentStateReadings.diffFromAverageHeight); - isHeadLevel(currentStateReadings.diffFromAverageEulers); - - propArray.forEach(function (prop) { - if (prop.signalOn) { - tablet.emitScriptEvent(JSON.stringify({ "type": "trigger", "id": prop.signalType, "data": { "value": "green" } })); - } else { - tablet.emitScriptEvent(JSON.stringify({ "type": "trigger", "id": prop.signalType, "data": { "value": "red" } })); - } - }); -} - -function onScreenChanged(type, url) { - print("Screen changed"); - if (type === "Web" && url === HTML_URL) { - if (!activated) { - // hook up to event bridge - tablet.webEventReceived.connect(onWebEventReceived); - print("after connect web event"); - MyAvatar.hmdLeanRecenterEnabled = false; - - } - activated = true; - } else { - if (activated) { - // disconnect from event bridge - tablet.webEventReceived.disconnect(onWebEventReceived); - documentLoaded = false; - } - activated = false; - } -} - -function getLog(x, y) { - return Math.log(y) / Math.log(x); -} - -function noConversion(num) { - return num; -} - -function convertLog(base, num, direction, shift) { - return direction * getLog(base, (num + 1.0)) + shift; -} - -function convertExponential(base, num, direction, shift) { - return Math.pow(base, (direction*num + shift)) - 1.0; -} - -function convertToCentimeters(num) { - return num * CENTIMETERSPERMETER; -} - -function convertToMeters(num) { - print("convert to meters " + num); - return num / CENTIMETERSPERMETER; -} - -function isInsideLine(a, b, c) { - return (((b.x - a.x)*(c.z - a.z) - (b.z - a.z)*(c.x - a.x)) > 0); -} - -function setAngularThreshold(num) { - headAngularVelocityProperty.value = num; - print("angular threshold " + headAngularVelocityProperty.get()); -} - -function setHeadRollThreshold(num) { - headRollProperty.value = num; - print("head roll threshold " + headRollProperty.get()); -} - -function setHeadPitchThreshold(num) { - headPitchProperty.value = num; - print("head pitch threshold " + headPitchProperty.get()); -} - -function setHeightThreshold(num) { - heightDifferenceProperty.value = num; - print("height threshold " + heightDifferenceProperty.get()); -} - -function setLateralDistance(num) { - lateralBaseProperty.value = num; - frontLeft.x = -lateralBaseProperty.get(); - frontRight.x = lateralBaseProperty.get(); - backLeft.x = -lateralBaseProperty.get(); - backRight.x = lateralBaseProperty.get(); - print("lateral distance " + lateralBaseProperty.get()); -} - -function setAnteriorDistance(num) { - frontBaseProperty.value = num; - frontLeft.z = -frontBaseProperty.get(); - frontRight.z = -frontBaseProperty.get(); - print("anterior distance " + frontBaseProperty.get()); -} - -function setPosteriorDistance(num) { - backBaseProperty.value = num; - backLeft.z = backBaseProperty.get(); - backRight.z = backBaseProperty.get(); - print("posterior distance " + backBaseProperty.get()); -} - -function setHandAngularVelocityThreshold(num) { - handsAngularVelocityProperty.value = num; - print("hand angular velocity threshold " + handsAngularVelocityProperty.get()); -} - -function setHandVelocityThreshold(num) { - handsVelocityProperty.value = num; - print("hand velocity threshold " + handsVelocityProperty.get()); -} - -function setHeadVelocityThreshold(num) { - headVelocityProperty.value = num; - print("headvelocity threshold " + headVelocityProperty.get()); -} - -function withinBaseOfSupport(pos) { - var userScale = 1.0; - frontBaseProperty.signalOn = !(isInsideLine(Vec3.multiply(userScale, frontLeft), Vec3.multiply(userScale, frontRight), pos)); - backBaseProperty.signalOn = !(isInsideLine(Vec3.multiply(userScale, backRight), Vec3.multiply(userScale, backLeft), pos)); - lateralBaseProperty.signalOn = !(isInsideLine(Vec3.multiply(userScale, frontRight), Vec3.multiply(userScale, backRight), pos) - && isInsideLine(Vec3.multiply(userScale, backLeft), Vec3.multiply(userScale, frontLeft), pos)); - return (!frontBaseProperty.signalOn && !backBaseProperty.signalOn && !lateralBaseProperty.signalOn); -} - -function withinThresholdOfStandingHeightMode(heightDiff) { - if (usingModeHeight.data.value) { - heightDifferenceProperty.signalOn = heightDiff < heightDifferenceProperty.get(); - return heightDifferenceProperty.signalOn; - } else { - return true; - } -} - -function headAngularVelocityBelowThreshold(headAngularVelocity) { - var angVel = Vec3.length({ x: headAngularVelocity.x, y: 0, z: headAngularVelocity.z }); - headAngularVelocityProperty.signalOn = angVel < headAngularVelocityProperty.get(); - return headAngularVelocityProperty.signalOn; -} - -function handDirectionMatchesHeadDirection(lhPose, rhPose) { - handsVelocityProperty.signalOn = ((handsVelocityProperty.get() < NO_SHARED_DIRECTION) || - ((!lhPose.valid || ((handDotHead[LEFT] > handsVelocityProperty.get()) && - (Vec3.length(lhPose.velocity) > VELOCITY_EPSILON))) && - (!rhPose.valid || ((handDotHead[RIGHT] > handsVelocityProperty.get()) && - (Vec3.length(rhPose.velocity) > VELOCITY_EPSILON))))); - return handsVelocityProperty.signalOn; -} - -function handAngularVelocityBelowThreshold(lhPose, rhPose) { - var xzRHandAngularVelocity = Vec3.length({ x: rhPose.angularVelocity.x, y: 0.0, z: rhPose.angularVelocity.z }); - var xzLHandAngularVelocity = Vec3.length({ x: lhPose.angularVelocity.x, y: 0.0, z: lhPose.angularVelocity.z }); - handsAngularVelocityProperty.signalOn = ((!rhPose.valid ||(xzRHandAngularVelocity < handsAngularVelocityProperty.get())) - && (!lhPose.valid || (xzLHandAngularVelocity < handsAngularVelocityProperty.get()))); - return handsAngularVelocityProperty.signalOn; -} - -function headVelocityGreaterThanThreshold(headVel) { - headVelocityProperty.signalOn = (headVel > headVelocityProperty.get()) || (headVelocityProperty.get() < VELOCITY_EPSILON); - return headVelocityProperty.signalOn; -} - -function headMovedAwayFromAveragePosition(headDelta) { - return !withinBaseOfSupport(headDelta) || !usingAverageHeadPosition.data.value; -} - -function headLowerThanHeightAverage(heightDiff) { - if (usingAverageHeight.data.value) { - print("head lower than height average"); - heightDifferenceProperty.signalOn = heightDiff < heightDifferenceProperty.get(); - return heightDifferenceProperty.signalOn; - } else { - return true; - } -} - -function isHeadLevel(diffEulers) { - headRollProperty.signalOn = Math.abs(diffEulers.z) < headRollProperty.get(); - headPitchProperty.signalOn = Math.abs(diffEulers.x) < headPitchProperty.get(); - return (headRollProperty.signalOn && headPitchProperty.signalOn); -} - -function findAverage(arr) { - var sum = arr.reduce(function (acc, val) { - return acc + val; - },0); - return sum / arr.length; -} - -function addToModeArray(arr,num) { - for (var i = 0 ;i < (arr.length - 1); i++) { - arr[i] = arr[i+1]; - } - arr[arr.length - 1] = (Math.floor(num*CENTIMETERSPERMETER))/CENTIMETERSPERMETER; -} - -function findMode(ary, currentMode, backLength, defaultBack, currentHeight) { - var numMapping = {}; - var greatestFreq = 0; - var mode; - ary.forEach(function (number) { - numMapping[number] = (numMapping[number] || 0) + 1; - if ((greatestFreq < numMapping[number]) || ((numMapping[number] === MODE_SAMPLE_LENGTH) && (number > currentMode) )) { - greatestFreq = numMapping[number]; - mode = number; - } - }); - if (mode > currentMode) { - return Number(mode); - } else { - if (!RESET_MODE && HMD.active) { - print("resetting the mode............................................. "); - print("resetting the mode............................................. "); - RESET_MODE = true; - var correction = 0.02; - return currentHeight - correction; - } else { - return currentMode; - } - } -} - -function update(dt) { - if (debugDrawBase) { - drawBase(); - } - // Update current state information - currentStateReadings.headPose = Controller.getPoseValue(Controller.Standard.Head); - currentStateReadings.rhandPose = Controller.getPoseValue(Controller.Standard.RightHand); - currentStateReadings.lhandPose = Controller.getPoseValue(Controller.Standard.LeftHand); - - // back length - var headMinusHipLean = Vec3.subtract(currentStateReadings.headPose.translation, DEFAULT_HIPS_POSITION); - currentStateReadings.backLength = Vec3.length(headMinusHipLean); - // print("back length and default " + currentStateReadings.backLength + " " + DEFAULT_TORSO_LENGTH); - - // mode height - addToModeArray(modeArray, currentStateReadings.headPose.translation.y); - modeHeight = findMode(modeArray, modeHeight, currentStateReadings.backLength, DEFAULT_TORSO_LENGTH, - currentStateReadings.headPose.translation.y); - currentStateReadings.diffFromMode = modeHeight - currentStateReadings.headPose.translation.y; - - // hand direction - var leftHandLateralPoseVelocity = currentStateReadings.lhandPose.velocity; - leftHandLateralPoseVelocity.y = 0.0; - var rightHandLateralPoseVelocity = currentStateReadings.rhandPose.velocity; - rightHandLateralPoseVelocity.y = 0.0; - var headLateralPoseVelocity = currentStateReadings.headPose.velocity; - headLateralPoseVelocity.y = 0.0; - handDotHead[LEFT] = Vec3.dot(Vec3.normalize(leftHandLateralPoseVelocity), Vec3.normalize(headLateralPoseVelocity)); - handDotHead[RIGHT] = Vec3.dot(Vec3.normalize(rightHandLateralPoseVelocity), Vec3.normalize(headLateralPoseVelocity)); - - // average head position - headAveragePosition = Vec3.mix(headAveragePosition, currentStateReadings.headPose.translation, AVERAGING_RATE); - currentStateReadings.diffFromAveragePosition = Vec3.subtract(currentStateReadings.headPose.translation, - headAveragePosition); - - // average height - averageHeight = currentStateReadings.headPose.translation.y * HEIGHT_AVERAGING_RATE + - averageHeight * (1.0 - HEIGHT_AVERAGING_RATE); - currentStateReadings.diffFromAverageHeight = Math.abs(currentStateReadings.headPose.translation.y - averageHeight); - - // eulers diff - headEulers = Quat.safeEulerAngles(currentStateReadings.headPose.rotation); - headAverageOrientation = Quat.slerp(headAverageOrientation, currentStateReadings.headPose.rotation, AVERAGING_RATE); - headAverageEulers = Quat.safeEulerAngles(headAverageOrientation); - currentStateReadings.diffFromAverageEulers = Vec3.subtract(headAverageEulers, headEulers); - - // headpose rig space is for determining when to recenter rotation. - var headPoseRigSpace = Quat.multiply(CHANGE_OF_BASIS_ROTATION, currentStateReadings.headPose.rotation); - headPoseAverageOrientation = Quat.slerp(headPoseAverageOrientation, headPoseRigSpace, AVERAGING_RATE); - var headPoseAverageEulers = Quat.safeEulerAngles(headPoseAverageOrientation); - - // make the signal colors reflect the current thresholds that have been crossed - updateSignalColors(); - - SPINE_STRETCH_LIMIT = (0.04) * DEFAULT_TORSO_LENGTH * MyAvatar.scale; - - //print("the spine stretch limit is " + SPINE_STRETCH_LIMIT + " head avatar space is " + currentStateReadings.headPose.translation.y); - //print("the current back length is " + currentStateReadings.backLength + " " + DEFAULT_TORSO_LENGTH); - // Conditions for taking a step. - // 1. off the base of support. front, lateral, back edges. - // 2. head is not lower than the height mode value by more than the maxHeightChange tolerance - // 3. the angular velocity of the head is not greater than the threshold value - // ie this reflects the speed the head is rotating away from having up = (0,1,0) in Avatar frame.. - // 4. the hands velocity vector has the same direction as the head, within the given tolerance - // the tolerance is an acos value, -1 means the hands going in any direction will not block translating - // up to 1 where the hands velocity direction must exactly match that of the head. -1 threshold disables this condition. - // 5. the angular velocity xz magnitude for each hand is below the threshold value - // ie here this reflects the speed that each hand is rotating away from having up = (0,1,0) in Avatar frame. - // 6. head velocity is below step threshold - // 7. head has moved further than the threshold from the running average position of the head. - // 8. head height is not lower than the running average head height with a difference of maxHeightChange. - // 9. head's rotation in avatar space is not pitching or rolling greater than the pitch or roll thresholds - if (!withinBaseOfSupport(currentStateReadings.headPose.translation) && - withinThresholdOfStandingHeightMode(currentStateReadings.diffFromMode) && - headAngularVelocityBelowThreshold(currentStateReadings.headPose.angularVelocity) && - handDirectionMatchesHeadDirection(currentStateReadings.lhandPose, currentStateReadings.rhandPose) && - handAngularVelocityBelowThreshold(currentStateReadings.lhandPose, currentStateReadings.rhandPose) && - headVelocityGreaterThanThreshold(Vec3.length(currentStateReadings.headPose.velocity)) && - headMovedAwayFromAveragePosition(currentStateReadings.diffFromAveragePosition) && - headLowerThanHeightAverage(currentStateReadings.diffFromAverageHeight) && - isHeadLevel(currentStateReadings.diffFromAverageEulers)) { - - if (stepTimer < 0.0) { //!MyAvatar.isRecenteringHorizontally() - print("trigger recenter========================================================"); - MyAvatar.triggerHorizontalRecenter(); - stepTimer = STEP_TIME_SECS; - } - } else if ((currentStateReadings.backLength > (DEFAULT_TORSO_LENGTH + SPINE_STRETCH_LIMIT)) && - (failsafeSignalTimer < 0.0) && HMD.active) { - // do the failsafe recenter. - // failsafeFlag stops repeated setting of failsafe button color. - // RESET_MODE false forces a reset of the height - RESET_MODE = false; - failsafeFlag = true; - failsafeSignalTimer = FAILSAFE_TIMEOUT; - MyAvatar.triggerHorizontalRecenter(); - tablet.emitScriptEvent(JSON.stringify({ "type": "failsafe", "id": "failsafeSignal", "data": { "value": "green" } })); - // in fail safe we debug print the values that were blocking us. - print("failsafe debug---------------------------------------------------------------"); - propArray.forEach(function (prop) { - print(prop.name); - if (!prop.signalOn) { - print(prop.signalType + " contributed to failsafe call"); - } - }); - print("end failsafe debug---------------------------------------------------------------"); - - } - - if ((failsafeSignalTimer < 0.0) && failsafeFlag) { - failsafeFlag = false; - tablet.emitScriptEvent(JSON.stringify({ "type": "failsafe", "id": "failsafeSignal", "data": { "value": "orange" } })); - } - - stepTimer -= dt; - failsafeSignalTimer -= dt; - - if (!HMD.active) { - RESET_MODE = false; - } - - if (Math.abs(headPoseAverageEulers.y) > HEAD_TURN_THRESHOLD) { - // Turn feet - // MyAvatar.triggerRotationRecenter(); - // headPoseAverageOrientation = { x: 0, y: 0, z: 0, w: 1 }; - } -} - -function shutdownTabletApp() { - // GlobalDebugger.stop(); - tablet.removeButton(tabletButton); - if (activated) { - tablet.webEventReceived.disconnect(onWebEventReceived); - tablet.gotoHomeScreen(); - } - tablet.screenChanged.disconnect(onScreenChanged); -} - -tabletButton.clicked.connect(manageClick); -tablet.screenChanged.connect(onScreenChanged); - -Script.setTimeout(function() { - DEFAULT_HIPS_POSITION = MyAvatar.getAbsoluteDefaultJointTranslationInObjectFrame(MyAvatar.getJointIndex("Hips")); - DEFAULT_HEAD_POSITION = MyAvatar.getAbsoluteDefaultJointTranslationInObjectFrame(MyAvatar.getJointIndex("Head")); - DEFAULT_TORSO_LENGTH = Vec3.length(Vec3.subtract(DEFAULT_HEAD_POSITION, DEFAULT_HIPS_POSITION)); - SPINE_STRETCH_LIMIT = (0.04) * DEFAULT_TORSO_LENGTH * MyAvatar.scale; -},(4*LOADING_DELAY)); - -Script.update.connect(update); -Controller.keyPressEvent.connect(onKeyPress); -Script.scriptEnding.connect(function () { - MyAvatar.hmdLeanRecenterEnabled = true; - Script.update.disconnect(update); - shutdownTabletApp(); -}); From 1a0e2c6ea181bbae814554967ed8b60729eb56b8 Mon Sep 17 00:00:00 2001 From: amantley Date: Mon, 1 Oct 2018 08:30:51 -0700 Subject: [PATCH 13/27] clean up for pr --- interface/resources/qml/hifi/avatarapp/Settings.qml | 3 +-- interface/src/avatar/MyAvatar.cpp | 12 ------------ 2 files changed, 1 insertion(+), 14 deletions(-) diff --git a/interface/resources/qml/hifi/avatarapp/Settings.qml b/interface/resources/qml/hifi/avatarapp/Settings.qml index af76ba04d6..c4289ca650 100644 --- a/interface/resources/qml/hifi/avatarapp/Settings.qml +++ b/interface/resources/qml/hifi/avatarapp/Settings.qml @@ -296,7 +296,6 @@ Rectangle { text: "OFF" boxSize: 20 } - // TextStyle9 @@ -345,7 +344,7 @@ Rectangle { boxSize: 20 } } - + ColumnLayout { id: avatarAnimationLayout anchors.top: handAndCollisions.bottom diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index f281d8c87d..1bd8d4b2f0 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -485,18 +485,6 @@ void MyAvatar::update(float deltaTime) { glm::vec3 upHead = transformVectorFast(sensorHeadPoseDebug.getMatrix(), glm::vec3(0.0f, 1.0f, 0.0f)); float acosHead = glm::dot(upHead, glm::vec3(0.0f, 1.0f, 0.0f)); // qCDebug(interfaceapp) << "sensor space head pos " << sensorHeadPoseDebug.getTranslation().y; - if ((acosHead > 0.98f) && !getIsInSittingState() && (sensorHeadPoseDebug.getTranslation().y < -0.5f)) { - //qCDebug(interfaceapp) << "we are going to sitting state because it looks like we should" << sensorHeadPoseDebug.getTranslation().y; - } - if (!_lastFrameHMDMode && qApp->isHMDMode()) { - // we have entered hmd mode, so make the best guess about sitting or standing - if (sensorHeadPoseDebug.getTranslation().y < 1.3f) { - // then we are sitting. - // setIsInSittingState(true); - } else { - // setIsInSittingState(false); - } - } // put the average hand azimuth into sensor space. // then mix it with head facing direction to determine rotation recenter From 90feeffa9db5076f3ca7e2bf8012d60ad11b2c1e Mon Sep 17 00:00:00 2001 From: amantley Date: Mon, 1 Oct 2018 14:21:00 -0700 Subject: [PATCH 14/27] cleaning up. putting squat fix in vertical recenter --- interface/src/avatar/MyAvatar.cpp | 70 +++++++++++++--------------- interface/src/avatar/MyAvatar.h | 6 +-- plugins/oculus/src/OculusHelpers.cpp | 10 ++-- scripts/system/avatarapp.js | 6 +-- 4 files changed, 43 insertions(+), 49 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 1bd8d4b2f0..278d2703ab 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -484,6 +484,19 @@ void MyAvatar::update(float deltaTime) { glm::vec3 upHead = transformVectorFast(sensorHeadPoseDebug.getMatrix(), glm::vec3(0.0f, 1.0f, 0.0f)); float acosHead = glm::dot(upHead, glm::vec3(0.0f, 1.0f, 0.0f)); + + glm::vec3 headDefaultPositionAvatarSpace = getAbsoluteDefaultJointTranslationInObjectFrame(getJointIndex("Head")); + glm::quat spine2OrientationAvatarSpace = getAbsoluteJointRotationInObjectFrame(getJointIndex("Spine2")); + glm::vec3 headCurrentPositionAvatarSpace = getControllerPoseInAvatarFrame(controller::Action::HEAD).getTranslation(); + glm::vec3 upSpine2 = spine2OrientationAvatarSpace * glm::vec3(0.0f, 1.0f, 0.0f); + float angleSpine2 = glm::dot(upSpine2, glm::vec3(0.0f, 1.0f, 0.0f)); + if (headCurrentPositionAvatarSpace.y < (headDefaultPositionAvatarSpace.y - 0.05) && (angleSpine2 > 0.98f)) { + _squatCount++; + } else { + _squatCount = 0; + } + + // qCDebug(interfaceapp) << "sensor space head pos " << sensorHeadPoseDebug.getTranslation().y; // put the average hand azimuth into sensor space. @@ -3795,7 +3808,7 @@ bool MyAvatar::getIsInWalkingState() const { } bool MyAvatar::getIsInSittingState() const { - return _isInSittingState; + return _isInSittingState.get(); } float MyAvatar::getWalkSpeed() const { @@ -3819,7 +3832,7 @@ void MyAvatar::setIsInWalkingState(bool isWalking) { } void MyAvatar::setIsInSittingState(bool isSitting) { - _isInSittingState = isSitting; + _isInSittingState.set(isSitting); controller::Pose sensorHeadPoseDebug = getControllerPoseInSensorFrame(controller::Action::HEAD); _sumUserHeightSensorSpace = sensorHeadPoseDebug.getTranslation().y; _averageUserHeightCount = 1; @@ -4064,8 +4077,8 @@ bool MyAvatar::FollowHelper::shouldActivateVertical(MyAvatar& myAvatar, const gl glm::vec3 headSensorSpace = transformVectorFast(myAvatar.getSensorToWorldMatrix(), headWorldSpace); //get the mode. //put it in sensor space. - // if we are 20% higher switch to standing. - // 16.6% lower then switch to sitting. + // if we are 20% higher switch to standing. + // 16.6% lower then switch to sitting. // add this !!!! And the head is upright. glm::vec3 upHead = transformVectorFast(sensorHeadPose.getMatrix(), glm::vec3(0.0f, 1.0f, 0.0f)); float acosHead = glm::dot(upHead, glm::vec3(0.0f, 1.0f, 0.0f)); @@ -4074,8 +4087,9 @@ bool MyAvatar::FollowHelper::shouldActivateVertical(MyAvatar& myAvatar, const gl glm::vec3 sensorHips = transformVectorFast(myAvatar.getSensorToWorldMatrix(), worldHips); float averageSensorSpaceHeight = myAvatar._sumUserHeightSensorSpace / myAvatar._averageUserHeightCount; // we could add a counting here to make sure that a lean forward doesn't accidentally put you in sitting mode. - // but maybe so what. + // but maybe so what. // the real test is... can I pick something up in standing mode? + if (myAvatar.getIsInSittingState()) { if (offset.y < SITTING_BOTTOM) { @@ -4084,22 +4098,27 @@ bool MyAvatar::FollowHelper::shouldActivateVertical(MyAvatar& myAvatar, const gl } else if (sensorHeadPose.getTranslation().y > (1.2f * averageSensorSpaceHeight)) { // if we recenter upwards then no longer in sitting state myAvatar.setIsInSittingState(false); - //myAvatar._sumUserHeightSensorSpace = 1.2f * averageSensorSpaceHeight; - // myAvatar._averageUserHeightCount = 1; return true; } else { return false; } } else { - // in the standing state - if ((sensorHeadPose.getTranslation().y < (0.83f * averageSensorSpaceHeight)) && (acosHead > 0.98f) && !(sensorHips.y > (0.4f * averageSensorSpaceHeight)) { - myAvatar.setIsInSittingState(true); - // myAvatar._sumUserHeightSensorSpace = 0.83f * averageSensorSpaceHeight; - // myAvatar._averageUserHeightCount = 1; - return true; + // in the standing state + if ((sensorHeadPose.getTranslation().y < (0.83f * averageSensorSpaceHeight)) && (acosHead > 0.98f) && !(sensorHips.y > (0.4f * averageSensorSpaceHeight))) { + myAvatar._sitStandStateCount++; + if (myAvatar._sitStandStateCount > 300) { + myAvatar.setIsInSittingState(true); + myAvatar._sitStandStateCount = 0; + myAvatar._squatCount = 0; + return true; + } } else { - return (offset.y > CYLINDER_TOP) || (offset.y < CYLINDER_BOTTOM); + if (myAvatar._squatCount > 600) { + return true; + myAvatar._squatCount = 0; + } } + return (offset.y > CYLINDER_TOP) || (offset.y < CYLINDER_BOTTOM); } } @@ -4109,29 +4128,6 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat if (myAvatar.getHMDLeanRecenterEnabled() && qApp->getCamera().getMode() != CAMERA_MODE_MIRROR) { - // debug head hips angle - glm::vec3 headDefaultPos = myAvatar.getAbsoluteDefaultJointTranslationInObjectFrame(myAvatar.getJointIndex("Head")); - if (myAvatar.getControllerPoseInAvatarFrame(controller::Action::HEAD).getTranslation().y < (headDefaultPos.y - 0.05f)) { - _squatCount++; - if ((_squatCount > 600) && !isActive(Vertical) && !isActive(Horizontal)) { - if (myAvatar.getIsInSittingState()) { - // activate(Horizontal); - //activate(Vertical); - _squatCount = 0; - } else { - if (myAvatar.getControllerPoseInAvatarFrame(controller::Action::HEAD).getTranslation().y < (headDefaultPos.y - 0.20f)) { - //myAvatar.setIsInSittingState(true); - //activate(Vertical); - } else { - //activate(Horizontal); - } - _squatCount = 0; - } - } - } else { - _squatCount = 0; - } - if (!isActive(Rotation) && (shouldActivateRotation(myAvatar, desiredBodyMatrix, currentBodyMatrix) || hasDriveInput)) { activate(Rotation); myAvatar.setHeadControllerFacingMovingAverage(myAvatar.getHeadControllerFacing()); diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 62ba8d68b1..c7f6fa89ae 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -1737,7 +1737,6 @@ private: std::atomic _forceActivateVertical { false }; std::atomic _forceActivateHorizontal { false }; std::atomic _toggleHipsFollowing { true }; - int _squatCount { 0 }; }; FollowHelper _follow; @@ -1770,7 +1769,6 @@ private: glm::quat _customListenOrientation; AtRestDetector _hmdAtRestDetector; - bool _lastFrameHMDMode { false } ; bool _lastIsMoving { false }; // all poses are in sensor-frame @@ -1817,7 +1815,9 @@ private: ThreadSafeValueCache _sprintSpeed { AVATAR_SPRINT_SPEED_SCALAR }; float _walkSpeedScalar { AVATAR_WALK_SPEED_SCALAR }; bool _isInWalkingState { false }; - bool _isInSittingState { false }; + ThreadSafeValueCache _isInSittingState { false }; + int _sitStandStateCount { 0 }; + int _squatCount { 0 }; // load avatar scripts once when rig is ready bool _shouldLoadScripts { false }; diff --git a/plugins/oculus/src/OculusHelpers.cpp b/plugins/oculus/src/OculusHelpers.cpp index 38d93d088d..402b05f39c 100644 --- a/plugins/oculus/src/OculusHelpers.cpp +++ b/plugins/oculus/src/OculusHelpers.cpp @@ -86,12 +86,10 @@ private: qCWarning(oculusLog) << "Failed to acquire Oculus session" << ovr::getError(); return; } else { - qCWarning(oculusLog) << "successful init of oculus!!!!!!!!"; - ovrTrackingOrigin fred; - //fred = ovr_GetTrackingOriginType(session); - ovrResult retTrackingType = ovr_SetTrackingOriginType(session, ovrTrackingOrigin::ovrTrackingOrigin_FloorLevel); - fred = ovr_GetTrackingOriginType(session); - qCWarning(oculusLog) << OVR_SUCCESS(retTrackingType) << (int)fred; + ovrResult setFloorLevelOrigin = ovr_SetTrackingOriginType(session, ovrTrackingOrigin::ovrTrackingOrigin_FloorLevel); + if (!OVR_SUCCESS(setFloorLevelOrigin)) { + qCWarning(oculusLog) << "Failed to set the Oculus tracking origin to floor level" << ovr::getError(); + } } } diff --git a/scripts/system/avatarapp.js b/scripts/system/avatarapp.js index 16761f29a6..faf624392a 100644 --- a/scripts/system/avatarapp.js +++ b/scripts/system/avatarapp.js @@ -63,8 +63,8 @@ function getMyAvatar() { function getMyAvatarSettings() { return { dominantHand: MyAvatar.getDominantHand(), - collisionsEnabled: MyAvatar.getCollisionsEnabled(), - sittingEnabled: MyAvatar.isInSittingState, + collisionsEnabled : MyAvatar.getCollisionsEnabled(), + sittingEnabled : MyAvatar.isInSittingState, collisionSoundUrl : MyAvatar.collisionSoundURL, animGraphUrl: MyAvatar.getAnimGraphUrl(), animGraphOverrideUrl : MyAvatar.getAnimGraphOverrideUrl(), @@ -326,7 +326,7 @@ function fromQml(message) { // messages are {method, params}, like json-rpc. See MyAvatar.isInSittingState = message.settings.sittingEnabled; MyAvatar.collisionSoundURL = message.settings.collisionSoundUrl; MyAvatar.setAnimGraphOverrideUrl(message.settings.animGraphOverrideUrl); - print("save settings"); + settings = getMyAvatarSettings(); break; default: From f0676d796c74a8464954bf1a8b7aae3e40df00a0 Mon Sep 17 00:00:00 2001 From: amantley Date: Mon, 1 Oct 2018 17:57:00 -0700 Subject: [PATCH 15/27] added constants and tipping point for running average. this should be replaced by the mode --- interface/src/avatar/MyAvatar.cpp | 86 +++++++++++++------------------ interface/src/avatar/MyAvatar.h | 2 + 2 files changed, 38 insertions(+), 50 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 278d2703ab..4f28ae8e90 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -471,34 +471,6 @@ void MyAvatar::update(float deltaTime) { float tau = deltaTime / HMD_FACING_TIMESCALE; setHipToHandController(computeHandAzimuth()); - // qCDebug(interfaceapp) << "sitting state is " << getIsInSittingState(); - // debug setting for sitting state if you start in the chair. - // if the head is close to the floor in sensor space - // and the up of the head is close to sensor up then try switching us to sitting state. - auto sensorHeadPoseDebug = getControllerPoseInSensorFrame(controller::Action::HEAD); - if (sensorHeadPoseDebug.isValid()) { - _sumUserHeightSensorSpace += sensorHeadPoseDebug.getTranslation().y; - _averageUserHeightCount++; - } - qCDebug(interfaceapp) << sensorHeadPoseDebug.isValid() << " valid. sensor space average " << (_sumUserHeightSensorSpace / _averageUserHeightCount) << " head position sensor y value " << sensorHeadPoseDebug.getTranslation().y; - - glm::vec3 upHead = transformVectorFast(sensorHeadPoseDebug.getMatrix(), glm::vec3(0.0f, 1.0f, 0.0f)); - float acosHead = glm::dot(upHead, glm::vec3(0.0f, 1.0f, 0.0f)); - - glm::vec3 headDefaultPositionAvatarSpace = getAbsoluteDefaultJointTranslationInObjectFrame(getJointIndex("Head")); - glm::quat spine2OrientationAvatarSpace = getAbsoluteJointRotationInObjectFrame(getJointIndex("Spine2")); - glm::vec3 headCurrentPositionAvatarSpace = getControllerPoseInAvatarFrame(controller::Action::HEAD).getTranslation(); - glm::vec3 upSpine2 = spine2OrientationAvatarSpace * glm::vec3(0.0f, 1.0f, 0.0f); - float angleSpine2 = glm::dot(upSpine2, glm::vec3(0.0f, 1.0f, 0.0f)); - if (headCurrentPositionAvatarSpace.y < (headDefaultPositionAvatarSpace.y - 0.05) && (angleSpine2 > 0.98f)) { - _squatCount++; - } else { - _squatCount = 0; - } - - - // qCDebug(interfaceapp) << "sensor space head pos " << sensorHeadPoseDebug.getTranslation().y; - // 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()) { @@ -527,6 +499,29 @@ void MyAvatar::update(float deltaTime) { setCurrentStandingHeight(computeStandingHeightMode(getControllerPoseInAvatarFrame(controller::Action::HEAD))); setAverageHeadRotation(computeAverageHeadRotation(getControllerPoseInAvatarFrame(controller::Action::HEAD))); + // if the head tracker reading is valid then add it to the running average. + auto sensorHeadPoseDebug = getControllerPoseInSensorFrame(controller::Action::HEAD); + if (sensorHeadPoseDebug.isValid()) { + _sumUserHeightSensorSpace += sensorHeadPoseDebug.getTranslation().y; + _averageUserHeightCount++; + } + + // if the spine is straight and the head is below the default position by 5 cm then increment squatty count. + const float SQUAT_THRESHOLD = 0.05f; + const float COSINE_TEN_DEGREES = 0.98f; + glm::vec3 headDefaultPositionAvatarSpace = getAbsoluteDefaultJointTranslationInObjectFrame(getJointIndex("Head")); + glm::quat spine2OrientationAvatarSpace = getAbsoluteJointRotationInObjectFrame(getJointIndex("Spine2")); + glm::vec3 upSpine2 = spine2OrientationAvatarSpace * glm::vec3(0.0f, 1.0f, 0.0f); + if (glm::length(upSpine2) > 0.0f) { + upSpine2 = glm::normalize(upSpine2); + } + float angleSpine2 = glm::dot(upSpine2, glm::vec3(0.0f, 1.0f, 0.0f)); + if (newHeightReading < (headDefaultPositionAvatarSpace.y - SQUAT_THRESHOLD) && (angleSpine2 > COSINE_TEN_DEGREES)) { + _squatCount++; + } else { + _squatCount = 0; + } + if (_drawAverageFacingEnabled) { auto sensorHeadPose = getControllerPoseInSensorFrame(controller::Action::HEAD); glm::vec3 worldHeadPos = transformPoint(getSensorToWorldMatrix(), sensorHeadPose.getTranslation()); @@ -3561,12 +3556,6 @@ glm::vec3 MyAvatar::computeCounterBalance() { if (counterBalancedCg.y > (tposeHips.y + 0.05f)) { // if the height is higher than default hips, clamp to default hips counterBalancedCg.y = tposeHips.y + 0.05f; - } else if (counterBalancedCg.y < sitSquatThreshold) { - // do a height reset - setResetMode(true); - // _follow.activate(FollowHelper::Vertical); - // disable cg behaviour in this case. - // setIsInSittingState(true); } return counterBalancedCg; } @@ -4068,34 +4057,29 @@ bool MyAvatar::FollowHelper::shouldActivateVertical(MyAvatar& myAvatar, const gl const float CYLINDER_TOP = 0.1f; const float CYLINDER_BOTTOM = -1.5f; const float SITTING_BOTTOM = -0.02f; + const float STANDING_HEIGHT_MULTIPLE = 1.2f; + const float SITTING_HEIGHT_MULTIPLE = 0.833f; + const float COSINE_TEN_DEGREES = 0.98f; + const int SITTING_COUNT_THRESHOLD = 300; + const int SQUATTY_COUNT_THRESHOLD = 600; glm::vec3 offset = extractTranslation(desiredBodyMatrix) - extractTranslation(currentBodyMatrix); auto sensorHeadPose = myAvatar.getControllerPoseInSensorFrame(controller::Action::HEAD); - glm::vec3 headWorldSpace = myAvatar.getHead()->getPosition(); - glm::mat4 worldToSensorMatrix = glm::inverse(myAvatar.getSensorToWorldMatrix()); - glm::vec3 headSensorSpace = transformVectorFast(myAvatar.getSensorToWorldMatrix(), headWorldSpace); - //get the mode. - //put it in sensor space. - // if we are 20% higher switch to standing. - // 16.6% lower then switch to sitting. - // add this !!!! And the head is upright. glm::vec3 upHead = transformVectorFast(sensorHeadPose.getMatrix(), glm::vec3(0.0f, 1.0f, 0.0f)); float acosHead = glm::dot(upHead, glm::vec3(0.0f, 1.0f, 0.0f)); + glm::vec3 avatarHips = myAvatar.getAbsoluteJointTranslationInObjectFrame(myAvatar.getJointIndex("Hips")); glm::vec3 worldHips = transformVectorFast(myAvatar.getTransform().getMatrix(),avatarHips); glm::vec3 sensorHips = transformVectorFast(myAvatar.getSensorToWorldMatrix(), worldHips); + float averageSensorSpaceHeight = myAvatar._sumUserHeightSensorSpace / myAvatar._averageUserHeightCount; - // we could add a counting here to make sure that a lean forward doesn't accidentally put you in sitting mode. - // but maybe so what. - // the real test is... can I pick something up in standing mode? - if (myAvatar.getIsInSittingState()) { if (offset.y < SITTING_BOTTOM) { // we recenter when sitting. return true; - } else if (sensorHeadPose.getTranslation().y > (1.2f * averageSensorSpaceHeight)) { + } else if (sensorHeadPose.getTranslation().y > (STANDING_HEIGHT_MULTIPLE * averageSensorSpaceHeight)) { // if we recenter upwards then no longer in sitting state myAvatar.setIsInSittingState(false); return true; @@ -4104,16 +4088,18 @@ bool MyAvatar::FollowHelper::shouldActivateVertical(MyAvatar& myAvatar, const gl } } else { // in the standing state - if ((sensorHeadPose.getTranslation().y < (0.83f * averageSensorSpaceHeight)) && (acosHead > 0.98f) && !(sensorHips.y > (0.4f * averageSensorSpaceHeight))) { + if ((sensorHeadPose.getTranslation().y < (SITTING_HEIGHT_MULTIPLE * myAvatar._tippingPoint)) && (acosHead > COSINE_TEN_DEGREES)) { //&& !(sensorHips.y > (0.4f * averageSensorSpaceHeight) myAvatar._sitStandStateCount++; - if (myAvatar._sitStandStateCount > 300) { + if (myAvatar._sitStandStateCount > SITTING_COUNT_THRESHOLD) { myAvatar.setIsInSittingState(true); + myAvatar._tippingPoint = averageSensorSpaceHeight; myAvatar._sitStandStateCount = 0; myAvatar._squatCount = 0; return true; } } else { - if (myAvatar._squatCount > 600) { + myAvatar._tippingPoint = averageSensorSpaceHeight; + if (myAvatar._squatCount > SQUATTY_COUNT_THRESHOLD) { return true; myAvatar._squatCount = 0; } diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index c7f6fa89ae..6fe5aeda47 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -140,6 +140,7 @@ class MyAvatar : public Avatar { * @property {number} walkSpeed * @property {number} walkBackwardSpeed * @property {number} sprintSpeed + * @property {number} isInSittingState * * @property {Vec3} skeletonOffset - Can be used to apply a translation offset between the avatar's position and the * registration point of the 3D model. @@ -1818,6 +1819,7 @@ private: ThreadSafeValueCache _isInSittingState { false }; int _sitStandStateCount { 0 }; int _squatCount { 0 }; + float _tippingPoint { DEFAULT_AVATAR_HEIGHT }; // load avatar scripts once when rig is ready bool _shouldLoadScripts { false }; From 8a0fbc3fe9049e3322fd88e42024d698ae344d18 Mon Sep 17 00:00:00 2001 From: amantley Date: Tue, 2 Oct 2018 17:47:56 -0700 Subject: [PATCH 16/27] changed the height to use the mode . to do fix sensor space bug. --- interface/src/avatar/MyAvatar.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 4f28ae8e90..4892f98da1 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -4074,12 +4074,18 @@ bool MyAvatar::FollowHelper::shouldActivateVertical(MyAvatar& myAvatar, const gl glm::vec3 sensorHips = transformVectorFast(myAvatar.getSensorToWorldMatrix(), worldHips); float averageSensorSpaceHeight = myAvatar._sumUserHeightSensorSpace / myAvatar._averageUserHeightCount; + glm::vec3 modeWorldSpace = myAvatar.getTransform().getRotation() * glm::vec3(0.0f,myAvatar.getCurrentStandingHeight(),0.0f); + glm::vec3 modeSensorSpace = extractRotation(myAvatar.getSensorToWorldMatrix()) * modeWorldSpace; + glm::vec3 bodyInSensorSpace = transformPoint(glm::inverse(myAvatar.getSensorToWorldMatrix()), myAvatar.getTransform().getTranslation()); + modeSensorSpace.y = modeSensorSpace.y + bodyInSensorSpace.y; + qCDebug(interfaceapp) << "mode world sensor " << myAvatar.getCurrentStandingHeight() << " " << modeWorldSpace << " " << modeSensorSpace << " " << bodyInSensorSpace; if (myAvatar.getIsInSittingState()) { if (offset.y < SITTING_BOTTOM) { // we recenter when sitting. return true; - } else if (sensorHeadPose.getTranslation().y > (STANDING_HEIGHT_MULTIPLE * averageSensorSpaceHeight)) { + // } else if (sensorHeadPose.getTranslation().y > (STANDING_HEIGHT_MULTIPLE * averageSensorSpaceHeight)) { + } else if (sensorHeadPose.getTranslation().y > (STANDING_HEIGHT_MULTIPLE * modeSensorSpace.y)) { // if we recenter upwards then no longer in sitting state myAvatar.setIsInSittingState(false); return true; @@ -4088,10 +4094,12 @@ bool MyAvatar::FollowHelper::shouldActivateVertical(MyAvatar& myAvatar, const gl } } else { // in the standing state - if ((sensorHeadPose.getTranslation().y < (SITTING_HEIGHT_MULTIPLE * myAvatar._tippingPoint)) && (acosHead > COSINE_TEN_DEGREES)) { //&& !(sensorHips.y > (0.4f * averageSensorSpaceHeight) + // if ((sensorHeadPose.getTranslation().y < (SITTING_HEIGHT_MULTIPLE * myAvatar._tippingPoint)) && (acosHead > COSINE_TEN_DEGREES)) { //&& !(sensorHips.y > (0.4f * averageSensorSpaceHeight) + if ((sensorHeadPose.getTranslation().y < (SITTING_HEIGHT_MULTIPLE * modeSensorSpace.y)) && (acosHead > COSINE_TEN_DEGREES)) { myAvatar._sitStandStateCount++; if (myAvatar._sitStandStateCount > SITTING_COUNT_THRESHOLD) { myAvatar.setIsInSittingState(true); + myAvatar.setResetMode(true); myAvatar._tippingPoint = averageSensorSpaceHeight; myAvatar._sitStandStateCount = 0; myAvatar._squatCount = 0; From 809ca0e51266ee8b25edb2ceeb5ce4e8b5491cd0 Mon Sep 17 00:00:00 2001 From: amantley Date: Wed, 3 Oct 2018 18:05:30 -0700 Subject: [PATCH 17/27] vive hmd input is implemented. not using mode --- interface/src/avatar/MyAvatar.cpp | 45 ++++++++++++++++++------------- 1 file changed, 26 insertions(+), 19 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 4892f98da1..9db07347c3 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -3821,10 +3821,14 @@ void MyAvatar::setIsInWalkingState(bool isWalking) { } void MyAvatar::setIsInSittingState(bool isSitting) { - _isInSittingState.set(isSitting); + _sitStandStateCount = 0; + _squatCount = 0; controller::Pose sensorHeadPoseDebug = getControllerPoseInSensorFrame(controller::Action::HEAD); _sumUserHeightSensorSpace = sensorHeadPoseDebug.getTranslation().y; + _tippingPoint = sensorHeadPoseDebug.getTranslation().y; _averageUserHeightCount = 1; + setResetMode(true); + _isInSittingState.set(isSitting); emit sittingEnabledChanged(isSitting); } @@ -4070,45 +4074,48 @@ bool MyAvatar::FollowHelper::shouldActivateVertical(MyAvatar& myAvatar, const gl float acosHead = glm::dot(upHead, glm::vec3(0.0f, 1.0f, 0.0f)); glm::vec3 avatarHips = myAvatar.getAbsoluteJointTranslationInObjectFrame(myAvatar.getJointIndex("Hips")); - glm::vec3 worldHips = transformVectorFast(myAvatar.getTransform().getMatrix(),avatarHips); - glm::vec3 sensorHips = transformVectorFast(myAvatar.getSensorToWorldMatrix(), worldHips); + glm::vec3 worldHips = transformPoint(myAvatar.getTransform().getMatrix(),avatarHips); + glm::vec3 sensorHips = transformPoint(glm::inverse(myAvatar.getSensorToWorldMatrix()), worldHips); float averageSensorSpaceHeight = myAvatar._sumUserHeightSensorSpace / myAvatar._averageUserHeightCount; - glm::vec3 modeWorldSpace = myAvatar.getTransform().getRotation() * glm::vec3(0.0f,myAvatar.getCurrentStandingHeight(),0.0f); - glm::vec3 modeSensorSpace = extractRotation(myAvatar.getSensorToWorldMatrix()) * modeWorldSpace; - glm::vec3 bodyInSensorSpace = transformPoint(glm::inverse(myAvatar.getSensorToWorldMatrix()), myAvatar.getTransform().getTranslation()); - modeSensorSpace.y = modeSensorSpace.y + bodyInSensorSpace.y; - qCDebug(interfaceapp) << "mode world sensor " << myAvatar.getCurrentStandingHeight() << " " << modeWorldSpace << " " << modeSensorSpace << " " << bodyInSensorSpace; + glm::vec3 modeWorldSpace = transformPoint(myAvatar.getTransform().getMatrix(), glm::vec3(0.0f,myAvatar.getCurrentStandingHeight(),0.0f)); + glm::vec3 modeSensorSpace = transformPoint(glm::inverse(myAvatar.getSensorToWorldMatrix()), modeWorldSpace); + //qCDebug(interfaceapp) << "mode reading avatar " << myAvatar.getCurrentStandingHeight() << "mode w " << modeWorldSpace << "mode s " << modeSensorSpace << "sensor pose " < (STANDING_HEIGHT_MULTIPLE * averageSensorSpaceHeight)) { - } else if (sensorHeadPose.getTranslation().y > (STANDING_HEIGHT_MULTIPLE * modeSensorSpace.y)) { + } else if (sensorHeadPose.getTranslation().y > (STANDING_HEIGHT_MULTIPLE * averageSensorSpaceHeight)) { + // } else if (sensorHeadPose.getTranslation().y > (STANDING_HEIGHT_MULTIPLE * modeSensorSpace.y)) { // if we recenter upwards then no longer in sitting state - myAvatar.setIsInSittingState(false); - return true; + //myAvatar._sitStandStateCount++; + //if (myAvatar._sitStandStateCount > SITTING_COUNT_THRESHOLD) { + myAvatar.setIsInSittingState(false); + return true; + //} } else { + myAvatar._sitStandStateCount = 0; + myAvatar._tippingPoint = averageSensorSpaceHeight; return false; } } else { // in the standing state // if ((sensorHeadPose.getTranslation().y < (SITTING_HEIGHT_MULTIPLE * myAvatar._tippingPoint)) && (acosHead > COSINE_TEN_DEGREES)) { //&& !(sensorHips.y > (0.4f * averageSensorSpaceHeight) - if ((sensorHeadPose.getTranslation().y < (SITTING_HEIGHT_MULTIPLE * modeSensorSpace.y)) && (acosHead > COSINE_TEN_DEGREES)) { + if ((sensorHeadPose.getTranslation().y < (SITTING_HEIGHT_MULTIPLE * myAvatar._tippingPoint))) { myAvatar._sitStandStateCount++; if (myAvatar._sitStandStateCount > SITTING_COUNT_THRESHOLD) { myAvatar.setIsInSittingState(true); - myAvatar.setResetMode(true); - myAvatar._tippingPoint = averageSensorSpaceHeight; - myAvatar._sitStandStateCount = 0; - myAvatar._squatCount = 0; return true; } } else { myAvatar._tippingPoint = averageSensorSpaceHeight; + myAvatar._sitStandStateCount = 0; if (myAvatar._squatCount > SQUATTY_COUNT_THRESHOLD) { - return true; + // return true; myAvatar._squatCount = 0; } } @@ -4145,7 +4152,7 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat } } if (!isActive(Vertical) && (shouldActivateVertical(myAvatar, desiredBodyMatrix, currentBodyMatrix) || hasDriveInput)) { - activate(Vertical); + //activate(Vertical); } } else { if (!isActive(Rotation) && getForceActivateRotation()) { From acce675efc4280740508d8e3562e6d39ea85bf40 Mon Sep 17 00:00:00 2001 From: amantley Date: Thu, 4 Oct 2018 14:53:51 -0700 Subject: [PATCH 18/27] added snap for recentering with sit stand state change. using average height with tipping point for thresholds --- interface/src/avatar/MyAvatar.cpp | 41 ++++++++++++++++++++----------- interface/src/avatar/MyAvatar.h | 1 + 2 files changed, 28 insertions(+), 14 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 9db07347c3..55536748c7 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -4081,26 +4081,26 @@ bool MyAvatar::FollowHelper::shouldActivateVertical(MyAvatar& myAvatar, const gl glm::vec3 modeWorldSpace = transformPoint(myAvatar.getTransform().getMatrix(), glm::vec3(0.0f,myAvatar.getCurrentStandingHeight(),0.0f)); glm::vec3 modeSensorSpace = transformPoint(glm::inverse(myAvatar.getSensorToWorldMatrix()), modeWorldSpace); - //qCDebug(interfaceapp) << "mode reading avatar " << myAvatar.getCurrentStandingHeight() << "mode w " << modeWorldSpace << "mode s " << modeSensorSpace << "sensor pose " < (STANDING_HEIGHT_MULTIPLE * averageSensorSpaceHeight)) { + //returnValue = true; + } else if (sensorHeadPose.getTranslation().y > (STANDING_HEIGHT_MULTIPLE * myAvatar._tippingPoint)) { // } else if (sensorHeadPose.getTranslation().y > (STANDING_HEIGHT_MULTIPLE * modeSensorSpace.y)) { // if we recenter upwards then no longer in sitting state - //myAvatar._sitStandStateCount++; - //if (myAvatar._sitStandStateCount > SITTING_COUNT_THRESHOLD) { + myAvatar._sitStandStateCount++; + if (myAvatar._sitStandStateCount > SITTING_COUNT_THRESHOLD) { myAvatar.setIsInSittingState(false); - return true; - //} + myAvatar._sitStandStateChange = true; + returnValue = true; + } } else { myAvatar._sitStandStateCount = 0; myAvatar._tippingPoint = averageSensorSpaceHeight; - return false; } } else { // in the standing state @@ -4109,7 +4109,8 @@ bool MyAvatar::FollowHelper::shouldActivateVertical(MyAvatar& myAvatar, const gl myAvatar._sitStandStateCount++; if (myAvatar._sitStandStateCount > SITTING_COUNT_THRESHOLD) { myAvatar.setIsInSittingState(true); - return true; + myAvatar._sitStandStateChange = true; + returnValue = true; } } else { myAvatar._tippingPoint = averageSensorSpaceHeight; @@ -4119,8 +4120,9 @@ bool MyAvatar::FollowHelper::shouldActivateVertical(MyAvatar& myAvatar, const gl myAvatar._squatCount = 0; } } - return (offset.y > CYLINDER_TOP) || (offset.y < CYLINDER_BOTTOM); + // returnValue = (offset.y > CYLINDER_TOP) || (offset.y < CYLINDER_BOTTOM); } + return returnValue; } void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, @@ -4152,7 +4154,9 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat } } if (!isActive(Vertical) && (shouldActivateVertical(myAvatar, desiredBodyMatrix, currentBodyMatrix) || hasDriveInput)) { - //activate(Vertical); + activate(Vertical); + _timeRemaining[(int)Vertical] = 0.1f; + qCDebug(interfaceapp) << "recenter vertically!!!!!! " << hasDriveInput; } } else { if (!isActive(Rotation) && getForceActivateRotation()) { @@ -4209,6 +4213,9 @@ glm::mat4 MyAvatar::FollowHelper::postPhysicsUpdate(const MyAvatar& myAvatar, co // apply follow displacement to the body matrix. glm::vec3 worldLinearDisplacement = myAvatar.getCharacterController()->getFollowLinearDisplacement(); + if (worldLinearDisplacement.y > 0.0001f) { + qCDebug(interfaceapp) << "linear displacement " << worldLinearDisplacement << " time remaining " << getMaxTimeRemaining(); + } glm::quat worldAngularDisplacement = myAvatar.getCharacterController()->getFollowAngularDisplacement(); glm::mat4 sensorToWorldMatrix = myAvatar.getSensorToWorldMatrix(); @@ -4219,6 +4226,12 @@ glm::mat4 MyAvatar::FollowHelper::postPhysicsUpdate(const MyAvatar& myAvatar, co glm::mat4 newBodyMat = createMatFromQuatAndPos(sensorAngularDisplacement * glmExtractRotation(currentBodyMatrix), sensorLinearDisplacement + extractTranslation(currentBodyMatrix)); + if (isActive(Vertical)) { + deactivate(Vertical); + return myAvatar.deriveBodyFromHMDSensor(); + } else { + return newBodyMat; + } return newBodyMat; } else { return currentBodyMatrix; diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 6fe5aeda47..7854f5cb41 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -1807,6 +1807,7 @@ private: ThreadSafeValueCache _userHeight { DEFAULT_AVATAR_HEIGHT }; float _sumUserHeightSensorSpace { DEFAULT_AVATAR_HEIGHT }; int _averageUserHeightCount { 1 }; + bool _sitStandStateChange { false }; void updateChildCauterization(SpatiallyNestablePointer object, bool cauterize); From 82f8c6743678527e6c14ae8b13b66038f2bc6143 Mon Sep 17 00:00:00 2001 From: amantley Date: Thu, 4 Oct 2018 15:37:01 -0700 Subject: [PATCH 19/27] put the mode in sensor space for cg --- interface/src/avatar/MyAvatar.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 55536748c7..db37b5003b 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -493,7 +493,7 @@ void MyAvatar::update(float deltaTime) { _smoothOrientationTimer += deltaTime; } - float newHeightReading = getControllerPoseInAvatarFrame(controller::Action::HEAD).getTranslation().y; + float newHeightReading = getControllerPoseInSensorFrame(controller::Action::HEAD).getTranslation().y; int newHeightReadingInCentimeters = glm::floor(newHeightReading * CENTIMETERS_PER_METER); _recentModeReadings.insert(newHeightReadingInCentimeters); setCurrentStandingHeight(computeStandingHeightMode(getControllerPoseInAvatarFrame(controller::Action::HEAD))); @@ -4015,6 +4015,7 @@ bool MyAvatar::FollowHelper::shouldActivateHorizontalCG(MyAvatar& myAvatar) cons controller::Pose currentHeadPose = myAvatar.getControllerPoseInAvatarFrame(controller::Action::HEAD); controller::Pose currentLeftHandPose = myAvatar.getControllerPoseInAvatarFrame(controller::Action::LEFT_HAND); controller::Pose currentRightHandPose = myAvatar.getControllerPoseInAvatarFrame(controller::Action::RIGHT_HAND); + controller::Pose currentHeadSensorPose = myAvatar.getControllerPoseInSensorFrame(controller::Action::HEAD); bool stepDetected = false; float myScale = myAvatar.getAvatarScale(); @@ -4028,7 +4029,7 @@ bool MyAvatar::FollowHelper::shouldActivateHorizontalCG(MyAvatar& myAvatar) cons } else { if (!withinBaseOfSupport(currentHeadPose) && headAngularVelocityBelowThreshold(currentHeadPose) && - isWithinThresholdHeightMode(currentHeadPose, myAvatar.getCurrentStandingHeight(), myScale) && + isWithinThresholdHeightMode(currentHeadSensorPose, myAvatar.getCurrentStandingHeight(), myScale) && handDirectionMatchesHeadDirection(currentLeftHandPose, currentRightHandPose, currentHeadPose) && handAngularVelocityBelowThreshold(currentLeftHandPose, currentRightHandPose) && headVelocityGreaterThanThreshold(currentHeadPose) && @@ -4083,7 +4084,7 @@ bool MyAvatar::FollowHelper::shouldActivateVertical(MyAvatar& myAvatar, const gl // qCDebug(interfaceapp) << "mode reading avatar " << myAvatar.getCurrentStandingHeight() << "mode w " << modeWorldSpace << "mode s " << modeSensorSpace << "sensor pose " < Date: Thu, 4 Oct 2018 18:05:05 -0700 Subject: [PATCH 20/27] changed pre physics to use cg for desired body when in cg mode --- interface/src/avatar/MyAvatar.cpp | 58 +++++++++++++------------------ interface/src/avatar/MyAvatar.h | 4 +-- 2 files changed, 27 insertions(+), 35 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index db37b5003b..09896e66c3 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -493,17 +493,14 @@ void MyAvatar::update(float deltaTime) { _smoothOrientationTimer += deltaTime; } - float newHeightReading = getControllerPoseInSensorFrame(controller::Action::HEAD).getTranslation().y; - int newHeightReadingInCentimeters = glm::floor(newHeightReading * CENTIMETERS_PER_METER); - _recentModeReadings.insert(newHeightReadingInCentimeters); - setCurrentStandingHeight(computeStandingHeightMode(getControllerPoseInAvatarFrame(controller::Action::HEAD))); - setAverageHeadRotation(computeAverageHeadRotation(getControllerPoseInAvatarFrame(controller::Action::HEAD))); - - // if the head tracker reading is valid then add it to the running average. - auto sensorHeadPoseDebug = getControllerPoseInSensorFrame(controller::Action::HEAD); - if (sensorHeadPoseDebug.isValid()) { - _sumUserHeightSensorSpace += sensorHeadPoseDebug.getTranslation().y; + controller::Pose newHeightReading = getControllerPoseInSensorFrame(controller::Action::HEAD); + if (newHeightReading.isValid()) { + int newHeightReadingInCentimeters = glm::floor(newHeightReading.getTranslation().y * CENTIMETERS_PER_METER); + _sumUserHeightSensorSpace += newHeightReading.getTranslation().y; _averageUserHeightCount++; + _recentModeReadings.insert(newHeightReadingInCentimeters); + setCurrentStandingHeight(computeStandingHeightMode(newHeightReading)); + setAverageHeadRotation(computeAverageHeadRotation(getControllerPoseInAvatarFrame(controller::Action::HEAD))); } // if the spine is straight and the head is below the default position by 5 cm then increment squatty count. @@ -516,7 +513,7 @@ void MyAvatar::update(float deltaTime) { upSpine2 = glm::normalize(upSpine2); } float angleSpine2 = glm::dot(upSpine2, glm::vec3(0.0f, 1.0f, 0.0f)); - if (newHeightReading < (headDefaultPositionAvatarSpace.y - SQUAT_THRESHOLD) && (angleSpine2 > COSINE_TEN_DEGREES)) { + if (getControllerPoseInAvatarFrame(controller::Action::HEAD).getTranslation().y < (headDefaultPositionAvatarSpace.y - SQUAT_THRESHOLD) && (angleSpine2 > COSINE_TEN_DEGREES)) { _squatCount++; } else { _squatCount = 0; @@ -2031,6 +2028,11 @@ void MyAvatar::prepareForPhysicsSimulation() { _characterController.setPositionAndOrientation(getWorldPosition(), getWorldOrientation()); auto headPose = getControllerPoseInAvatarFrame(controller::Action::HEAD); if (headPose.isValid()) { + if (getCenterOfGravityModelEnabled() && !getIsInSittingState()) { + _follow.prePhysicsUpdate(*this, deriveBodyUsingCgModel(), _bodySensorMatrix, hasDriveInput()); + } else { + _follow.prePhysicsUpdate(*this, deriveBodyFromHMDSensor(), _bodySensorMatrix, hasDriveInput()); + } _follow.prePhysicsUpdate(*this, deriveBodyFromHMDSensor(), _bodySensorMatrix, hasDriveInput()); } else { _follow.deactivate(); @@ -3823,9 +3825,11 @@ void MyAvatar::setIsInWalkingState(bool isWalking) { void MyAvatar::setIsInSittingState(bool isSitting) { _sitStandStateCount = 0; _squatCount = 0; - controller::Pose sensorHeadPoseDebug = getControllerPoseInSensorFrame(controller::Action::HEAD); - _sumUserHeightSensorSpace = sensorHeadPoseDebug.getTranslation().y; - _tippingPoint = sensorHeadPoseDebug.getTranslation().y; + controller::Pose sensorHeadPose = getControllerPoseInSensorFrame(controller::Action::HEAD); + if (sensorHeadPose.isValid()) { + _sumUserHeightSensorSpace = sensorHeadPose.getTranslation().y; + _tippingPoint = sensorHeadPose.getTranslation().y; + } _averageUserHeightCount = 1; setResetMode(true); _isInSittingState.set(isSitting); @@ -4077,21 +4081,14 @@ bool MyAvatar::FollowHelper::shouldActivateVertical(MyAvatar& myAvatar, const gl glm::vec3 avatarHips = myAvatar.getAbsoluteJointTranslationInObjectFrame(myAvatar.getJointIndex("Hips")); glm::vec3 worldHips = transformPoint(myAvatar.getTransform().getMatrix(),avatarHips); glm::vec3 sensorHips = transformPoint(glm::inverse(myAvatar.getSensorToWorldMatrix()), worldHips); - + //qCDebug(interfaceapp) << " current mode " << myAvatar.getCurrentStandingHeight() << " " << sensorHeadPose.getTranslation().y << " state " << myAvatar.getIsInSittingState(); float averageSensorSpaceHeight = myAvatar._sumUserHeightSensorSpace / myAvatar._averageUserHeightCount; - glm::vec3 modeWorldSpace = transformPoint(myAvatar.getTransform().getMatrix(), glm::vec3(0.0f,myAvatar.getCurrentStandingHeight(),0.0f)); - glm::vec3 modeSensorSpace = transformPoint(glm::inverse(myAvatar.getSensorToWorldMatrix()), modeWorldSpace); - - // qCDebug(interfaceapp) << "mode reading avatar " << myAvatar.getCurrentStandingHeight() << "mode w " << modeWorldSpace << "mode s " << modeSensorSpace << "sensor pose " < (STANDING_HEIGHT_MULTIPLE * myAvatar._tippingPoint)) { - // } else if (sensorHeadPose.getTranslation().y > (STANDING_HEIGHT_MULTIPLE * modeSensorSpace.y)) { // if we recenter upwards then no longer in sitting state myAvatar._sitStandStateCount++; if (myAvatar._sitStandStateCount > SITTING_COUNT_THRESHOLD) { @@ -4105,7 +4102,7 @@ bool MyAvatar::FollowHelper::shouldActivateVertical(MyAvatar& myAvatar, const gl } } else { // in the standing state - // if ((sensorHeadPose.getTranslation().y < (SITTING_HEIGHT_MULTIPLE * myAvatar._tippingPoint)) && (acosHead > COSINE_TEN_DEGREES)) { //&& !(sensorHips.y > (0.4f * averageSensorSpaceHeight) + // && (acosHead > COSINE_TEN_DEGREES)) { //&& !(sensorHips.y > (0.4f * averageSensorSpaceHeight) if ((sensorHeadPose.getTranslation().y < (SITTING_HEIGHT_MULTIPLE * myAvatar._tippingPoint))) { myAvatar._sitStandStateCount++; if (myAvatar._sitStandStateCount > SITTING_COUNT_THRESHOLD) { @@ -4114,14 +4111,14 @@ bool MyAvatar::FollowHelper::shouldActivateVertical(MyAvatar& myAvatar, const gl returnValue = true; } } else { - myAvatar._tippingPoint = averageSensorSpaceHeight; + myAvatar._tippingPoint = myAvatar.getCurrentStandingHeight(); myAvatar._sitStandStateCount = 0; if (myAvatar._squatCount > SQUATTY_COUNT_THRESHOLD) { // return true; myAvatar._squatCount = 0; } } - // returnValue = (offset.y > CYLINDER_TOP) || (offset.y < CYLINDER_BOTTOM); + returnValue = (offset.y > CYLINDER_TOP) || (offset.y < CYLINDER_BOTTOM); } return returnValue; } @@ -4156,7 +4153,6 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat } if (!isActive(Vertical) && (shouldActivateVertical(myAvatar, desiredBodyMatrix, currentBodyMatrix) || hasDriveInput)) { activate(Vertical); - _timeRemaining[(int)Vertical] = 0.1f; qCDebug(interfaceapp) << "recenter vertically!!!!!! " << hasDriveInput; } } else { @@ -4214,9 +4210,6 @@ glm::mat4 MyAvatar::FollowHelper::postPhysicsUpdate(const MyAvatar& myAvatar, co // apply follow displacement to the body matrix. glm::vec3 worldLinearDisplacement = myAvatar.getCharacterController()->getFollowLinearDisplacement(); - if (worldLinearDisplacement.y > 0.0001f) { - qCDebug(interfaceapp) << "linear displacement " << worldLinearDisplacement << " time remaining " << getMaxTimeRemaining(); - } glm::quat worldAngularDisplacement = myAvatar.getCharacterController()->getFollowAngularDisplacement(); glm::mat4 sensorToWorldMatrix = myAvatar.getSensorToWorldMatrix(); @@ -4228,10 +4221,9 @@ glm::mat4 MyAvatar::FollowHelper::postPhysicsUpdate(const MyAvatar& myAvatar, co glm::mat4 newBodyMat = createMatFromQuatAndPos(sensorAngularDisplacement * glmExtractRotation(currentBodyMatrix), sensorLinearDisplacement + extractTranslation(currentBodyMatrix)); if (isActive(Vertical)) { + // myAvatar._sitStandStateChange = false; deactivate(Vertical); - return myAvatar.deriveBodyFromHMDSensor(); - } else { - return newBodyMat; + newBodyMat = myAvatar.deriveBodyFromHMDSensor(); } return newBodyMat; } else { diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 7854f5cb41..0c6b1b01a5 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -1805,7 +1805,7 @@ private: // height of user in sensor space, when standing erect. ThreadSafeValueCache _userHeight { DEFAULT_AVATAR_HEIGHT }; - float _sumUserHeightSensorSpace { DEFAULT_AVATAR_HEIGHT }; + float _sumUserHeightSensorSpace { 0.0f }; int _averageUserHeightCount { 1 }; bool _sitStandStateChange { false }; @@ -1820,7 +1820,7 @@ private: ThreadSafeValueCache _isInSittingState { false }; int _sitStandStateCount { 0 }; int _squatCount { 0 }; - float _tippingPoint { DEFAULT_AVATAR_HEIGHT }; + float _tippingPoint { 0.0f }; // load avatar scripts once when rig is ready bool _shouldLoadScripts { false }; From cbe638bfdb18d1420dacc470d46671ee4bb8c79e Mon Sep 17 00:00:00 2001 From: amantley Date: Thu, 4 Oct 2018 18:16:39 -0700 Subject: [PATCH 21/27] added flag for vertical recentering that is state change, snap, versus the usual .5 second recentering --- interface/src/avatar/MyAvatar.cpp | 6 +++--- interface/src/avatar/MyAvatar.h | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 09896e66c3..8756f1bbd8 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -4203,7 +4203,7 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat myAvatar.getCharacterController()->setFollowParameters(followWorldPose, getMaxTimeRemaining()); } -glm::mat4 MyAvatar::FollowHelper::postPhysicsUpdate(const MyAvatar& myAvatar, const glm::mat4& currentBodyMatrix) { +glm::mat4 MyAvatar::FollowHelper::postPhysicsUpdate(MyAvatar& myAvatar, const glm::mat4& currentBodyMatrix) { if (isActive()) { float dt = myAvatar.getCharacterController()->getFollowTime(); decrementTimeRemaining(dt); @@ -4220,8 +4220,8 @@ glm::mat4 MyAvatar::FollowHelper::postPhysicsUpdate(const MyAvatar& myAvatar, co glm::mat4 newBodyMat = createMatFromQuatAndPos(sensorAngularDisplacement * glmExtractRotation(currentBodyMatrix), sensorLinearDisplacement + extractTranslation(currentBodyMatrix)); - if (isActive(Vertical)) { - // myAvatar._sitStandStateChange = false; + if (myAvatar._sitStandStateChange) { + myAvatar._sitStandStateChange = false; deactivate(Vertical); newBodyMat = myAvatar.deriveBodyFromHMDSensor(); } diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 0c6b1b01a5..072ea04ee7 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -1725,7 +1725,7 @@ private: bool shouldActivateHorizontal(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) const; bool shouldActivateHorizontalCG(MyAvatar& myAvatar) const; void prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat4& bodySensorMatrix, const glm::mat4& currentBodyMatrix, bool hasDriveInput); - glm::mat4 postPhysicsUpdate(const MyAvatar& myAvatar, const glm::mat4& currentBodyMatrix); + glm::mat4 postPhysicsUpdate(MyAvatar& myAvatar, const glm::mat4& currentBodyMatrix); bool getForceActivateRotation() const; void setForceActivateRotation(bool val); bool getForceActivateVertical() const; From ad46b7196634e301f9fdd5d5f8d0e11342959446 Mon Sep 17 00:00:00 2001 From: amantley Date: Fri, 5 Oct 2018 13:04:14 -0700 Subject: [PATCH 22/27] before moving the step state counting to myavatar::update() --- interface/src/avatar/MyAvatar.cpp | 28 ++++++++++++++++++++++------ interface/src/avatar/MyAvatar.h | 1 + 2 files changed, 23 insertions(+), 6 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 8756f1bbd8..c5defe3d72 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -4103,6 +4103,7 @@ bool MyAvatar::FollowHelper::shouldActivateVertical(MyAvatar& myAvatar, const gl } else { // in the standing state // && (acosHead > COSINE_TEN_DEGREES)) { //&& !(sensorHips.y > (0.4f * averageSensorSpaceHeight) + if ((sensorHeadPose.getTranslation().y < (SITTING_HEIGHT_MULTIPLE * myAvatar._tippingPoint))) { myAvatar._sitStandStateCount++; if (myAvatar._sitStandStateCount > SITTING_COUNT_THRESHOLD) { @@ -4111,14 +4112,15 @@ bool MyAvatar::FollowHelper::shouldActivateVertical(MyAvatar& myAvatar, const gl returnValue = true; } } else { + returnValue = (offset.y > CYLINDER_TOP) || (offset.y < CYLINDER_BOTTOM); myAvatar._tippingPoint = myAvatar.getCurrentStandingHeight(); myAvatar._sitStandStateCount = 0; if (myAvatar._squatCount > SQUATTY_COUNT_THRESHOLD) { - // return true; + // returnValue = true; myAvatar._squatCount = 0; } } - returnValue = (offset.y > CYLINDER_TOP) || (offset.y < CYLINDER_BOTTOM); + } return returnValue; } @@ -4151,10 +4153,20 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat } } } - if (!isActive(Vertical) && (shouldActivateVertical(myAvatar, desiredBodyMatrix, currentBodyMatrix) || hasDriveInput)) { - activate(Vertical); - qCDebug(interfaceapp) << "recenter vertically!!!!!! " << hasDriveInput; + + qCDebug(interfaceapp) << "velocity of headset " << glm::length(myAvatar.getControllerPoseInSensorFrame(controller::Action::HEAD).getVelocity()); + + if (_velocityCount > 60) { + if (!isActive(Vertical) && (shouldActivateVertical(myAvatar, desiredBodyMatrix, currentBodyMatrix) || hasDriveInput)) { + activate(Vertical); + qCDebug(interfaceapp) << "recenter vertically!!!!!! " << hasDriveInput; + } + } else { + if ((glm::length(myAvatar.getControllerPoseInSensorFrame(controller::Action::HEAD).getVelocity()) > 0.1f)) { + _velocityCount++; + } } + } else { if (!isActive(Rotation) && getForceActivateRotation()) { activate(Rotation); @@ -4223,7 +4235,11 @@ glm::mat4 MyAvatar::FollowHelper::postPhysicsUpdate(MyAvatar& myAvatar, const gl if (myAvatar._sitStandStateChange) { myAvatar._sitStandStateChange = false; deactivate(Vertical); - newBodyMat = myAvatar.deriveBodyFromHMDSensor(); + + qCDebug(interfaceapp) << "before snap " << extractTranslation(newBodyMat); + //newBodyMat = myAvatar.deriveBodyFromHMDSensor(); + setTranslation(newBodyMat, extractTranslation(myAvatar.deriveBodyFromHMDSensor())); + qCDebug(interfaceapp) << "after snap " << extractTranslation(newBodyMat); } return newBodyMat; } else { diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 072ea04ee7..24a58745a3 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -1738,6 +1738,7 @@ private: std::atomic _forceActivateVertical { false }; std::atomic _forceActivateHorizontal { false }; std::atomic _toggleHipsFollowing { true }; + int _velocityCount { 0 }; }; FollowHelper _follow; From 96872f841202f4a60fc8b3b2a77f9632d0197ce8 Mon Sep 17 00:00:00 2001 From: amantley Date: Fri, 5 Oct 2018 14:55:32 -0700 Subject: [PATCH 23/27] moved all the state update for sit and stand to myavatar::update() also made it so that sit horizontal reset is handled in non-cg recentering. to do: revisit what should be sent for desired body in prephysics update call --- interface/src/avatar/MyAvatar.cpp | 143 ++++++++++++++++++------------ 1 file changed, 87 insertions(+), 56 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index c5defe3d72..58a0ef4046 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -467,6 +467,11 @@ 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 + const float STANDING_HEIGHT_MULTIPLE = 1.2f; + const float SITTING_HEIGHT_MULTIPLE = 0.833f; + const float COSINE_TEN_DEGREES = 0.98f; + const int SITTING_COUNT_THRESHOLD = 300; + const int SQUATTY_COUNT_THRESHOLD = 600; float tau = deltaTime / HMD_FACING_TIMESCALE; setHipToHandController(computeHandAzimuth()); @@ -505,7 +510,6 @@ void MyAvatar::update(float deltaTime) { // if the spine is straight and the head is below the default position by 5 cm then increment squatty count. const float SQUAT_THRESHOLD = 0.05f; - const float COSINE_TEN_DEGREES = 0.98f; glm::vec3 headDefaultPositionAvatarSpace = getAbsoluteDefaultJointTranslationInObjectFrame(getJointIndex("Head")); glm::quat spine2OrientationAvatarSpace = getAbsoluteJointRotationInObjectFrame(getJointIndex("Spine2")); glm::vec3 upSpine2 = spine2OrientationAvatarSpace * glm::vec3(0.0f, 1.0f, 0.0f); @@ -519,6 +523,65 @@ void MyAvatar::update(float deltaTime) { _squatCount = 0; } + float averageSensorSpaceHeight = _sumUserHeightSensorSpace / _averageUserHeightCount; + //auto sensorHeadPose = myAvatar.getControllerPoseInSensorFrame(controller::Action::HEAD); + //glm::vec3 upHead = transformVectorFast(sensorHeadPose.getMatrix(), glm::vec3(0.0f, 1.0f, 0.0f)); + //float acosHead = glm::dot(upHead, glm::vec3(0.0f, 1.0f, 0.0f)); + + glm::vec3 avatarHips = getAbsoluteJointTranslationInObjectFrame(getJointIndex("Hips")); + glm::vec3 worldHips = transformPoint(getTransform().getMatrix(), avatarHips); + glm::vec3 sensorHips = transformPoint(glm::inverse(getSensorToWorldMatrix()), worldHips); + + // put update sit stand state counts here + if (getIsInSittingState()) { + if (newHeightReading.getTranslation().y > (STANDING_HEIGHT_MULTIPLE * _tippingPoint)) { + // if we recenter upwards then no longer in sitting state + _sitStandStateCount++; + if (_sitStandStateCount > SITTING_COUNT_THRESHOLD) { + _sitStandStateCount = 0; + _squatCount = 0; + if (newHeightReading.isValid()) { + _sumUserHeightSensorSpace = newHeightReading.getTranslation().y; + _tippingPoint = newHeightReading.getTranslation().y; + } + _averageUserHeightCount = 1; + setResetMode(true); + setIsInSittingState(false); + setCenterOfGravityModelEnabled(true); + _sitStandStateChange = true; + } + } else { + _sitStandStateCount = 0; + // tipping point is average height when sitting. + _tippingPoint = averageSensorSpaceHeight; + } + } else { + // in the standing state + if ((newHeightReading.getTranslation().y < (SITTING_HEIGHT_MULTIPLE * _tippingPoint))) {// && (angleSpine2 > COSINE_TEN_DEGREES) && !(sensorHips.y > (0.4f * averageSensorSpaceHeight))) { + _sitStandStateCount++; + if (_sitStandStateCount > SITTING_COUNT_THRESHOLD) { + _sitStandStateCount = 0; + _squatCount = 0; + if (newHeightReading.isValid()) { + _sumUserHeightSensorSpace = newHeightReading.getTranslation().y; + _tippingPoint = newHeightReading.getTranslation().y; + } + _averageUserHeightCount = 1; + setResetMode(true); + setIsInSittingState(true); + setCenterOfGravityModelEnabled(false); + _sitStandStateChange = true; + } + } else { + // returnValue = (offset.y > CYLINDER_TOP) || (offset.y < CYLINDER_BOTTOM); + // use the mode height for the tipping point when we are standing. + _tippingPoint = getCurrentStandingHeight(); + _sitStandStateCount = 0; + } + + } + + if (_drawAverageFacingEnabled) { auto sensorHeadPose = getControllerPoseInSensorFrame(controller::Action::HEAD); glm::vec3 worldHeadPos = transformPoint(getSensorToWorldMatrix(), sensorHeadPose.getTranslation()); @@ -2028,10 +2091,10 @@ void MyAvatar::prepareForPhysicsSimulation() { _characterController.setPositionAndOrientation(getWorldPosition(), getWorldOrientation()); auto headPose = getControllerPoseInAvatarFrame(controller::Action::HEAD); if (headPose.isValid()) { - if (getCenterOfGravityModelEnabled() && !getIsInSittingState()) { - _follow.prePhysicsUpdate(*this, deriveBodyUsingCgModel(), _bodySensorMatrix, hasDriveInput()); + if (getCenterOfGravityModelEnabled()) { + //_follow.prePhysicsUpdate(*this, deriveBodyUsingCgModel(), _bodySensorMatrix, hasDriveInput()); } else { - _follow.prePhysicsUpdate(*this, deriveBodyFromHMDSensor(), _bodySensorMatrix, hasDriveInput()); + //_follow.prePhysicsUpdate(*this, deriveBodyFromHMDSensor(), _bodySensorMatrix, hasDriveInput()); } _follow.prePhysicsUpdate(*this, deriveBodyFromHMDSensor(), _bodySensorMatrix, hasDriveInput()); } else { @@ -3995,6 +4058,7 @@ bool MyAvatar::FollowHelper::shouldActivateHorizontal(const MyAvatar& myAvatar, // x axis of currentBodyMatrix in world space. glm::vec3 right = glm::normalize(glm::vec3(currentBodyMatrix[0][0], currentBodyMatrix[1][0], currentBodyMatrix[2][0])); glm::vec3 offset = extractTranslation(desiredBodyMatrix) - extractTranslation(currentBodyMatrix); + controller::Pose currentHeadPose = myAvatar.getControllerPoseInAvatarFrame(controller::Action::HEAD); float forwardLeanAmount = glm::dot(forward, offset); float lateralLeanAmount = glm::dot(right, offset); @@ -4003,14 +4067,19 @@ bool MyAvatar::FollowHelper::shouldActivateHorizontal(const MyAvatar& myAvatar, const float MAX_FORWARD_LEAN = 0.15f; const float MAX_BACKWARD_LEAN = 0.1f; - - if (forwardLeanAmount > 0 && forwardLeanAmount > MAX_FORWARD_LEAN) { - return true; + bool stepDetected = false; + if (myAvatar.getIsInSittingState()) { + if (!withinBaseOfSupport(currentHeadPose)) { + stepDetected = true; + } + } else if (forwardLeanAmount > 0 && forwardLeanAmount > MAX_FORWARD_LEAN) { + stepDetected = true; } else if (forwardLeanAmount < 0 && forwardLeanAmount < -MAX_BACKWARD_LEAN) { - return true; + stepDetected = true; + } else { + stepDetected = fabs(lateralLeanAmount) > MAX_LATERAL_LEAN; } - - return fabs(lateralLeanAmount) > MAX_LATERAL_LEAN; + return stepDetected; } bool MyAvatar::FollowHelper::shouldActivateHorizontalCG(MyAvatar& myAvatar) const { @@ -4026,10 +4095,6 @@ bool MyAvatar::FollowHelper::shouldActivateHorizontalCG(MyAvatar& myAvatar) cons if (myAvatar.getIsInWalkingState()) { stepDetected = true; - } else if (myAvatar.getIsInSittingState()) { - if (!withinBaseOfSupport(currentHeadPose)) { - stepDetected = true; - } } else { if (!withinBaseOfSupport(currentHeadPose) && headAngularVelocityBelowThreshold(currentHeadPose) && @@ -4066,61 +4131,27 @@ bool MyAvatar::FollowHelper::shouldActivateVertical(MyAvatar& myAvatar, const gl const float CYLINDER_TOP = 0.1f; const float CYLINDER_BOTTOM = -1.5f; const float SITTING_BOTTOM = -0.02f; - const float STANDING_HEIGHT_MULTIPLE = 1.2f; - const float SITTING_HEIGHT_MULTIPLE = 0.833f; - const float COSINE_TEN_DEGREES = 0.98f; - const int SITTING_COUNT_THRESHOLD = 300; const int SQUATTY_COUNT_THRESHOLD = 600; glm::vec3 offset = extractTranslation(desiredBodyMatrix) - extractTranslation(currentBodyMatrix); - - auto sensorHeadPose = myAvatar.getControllerPoseInSensorFrame(controller::Action::HEAD); - glm::vec3 upHead = transformVectorFast(sensorHeadPose.getMatrix(), glm::vec3(0.0f, 1.0f, 0.0f)); - float acosHead = glm::dot(upHead, glm::vec3(0.0f, 1.0f, 0.0f)); - - glm::vec3 avatarHips = myAvatar.getAbsoluteJointTranslationInObjectFrame(myAvatar.getJointIndex("Hips")); - glm::vec3 worldHips = transformPoint(myAvatar.getTransform().getMatrix(),avatarHips); - glm::vec3 sensorHips = transformPoint(glm::inverse(myAvatar.getSensorToWorldMatrix()), worldHips); //qCDebug(interfaceapp) << " current mode " << myAvatar.getCurrentStandingHeight() << " " << sensorHeadPose.getTranslation().y << " state " << myAvatar.getIsInSittingState(); - float averageSensorSpaceHeight = myAvatar._sumUserHeightSensorSpace / myAvatar._averageUserHeightCount; + bool returnValue = false; + if (myAvatar._sitStandStateChange) { + returnValue = true; + } if (myAvatar.getIsInSittingState()) { if (offset.y < SITTING_BOTTOM) { // we recenter when sitting. returnValue = true; - } else if (sensorHeadPose.getTranslation().y > (STANDING_HEIGHT_MULTIPLE * myAvatar._tippingPoint)) { - // if we recenter upwards then no longer in sitting state - myAvatar._sitStandStateCount++; - if (myAvatar._sitStandStateCount > SITTING_COUNT_THRESHOLD) { - myAvatar.setIsInSittingState(false); - myAvatar._sitStandStateChange = true; - returnValue = true; - } - } else { - myAvatar._sitStandStateCount = 0; - myAvatar._tippingPoint = averageSensorSpaceHeight; } } else { // in the standing state - // && (acosHead > COSINE_TEN_DEGREES)) { //&& !(sensorHips.y > (0.4f * averageSensorSpaceHeight) - - if ((sensorHeadPose.getTranslation().y < (SITTING_HEIGHT_MULTIPLE * myAvatar._tippingPoint))) { - myAvatar._sitStandStateCount++; - if (myAvatar._sitStandStateCount > SITTING_COUNT_THRESHOLD) { - myAvatar.setIsInSittingState(true); - myAvatar._sitStandStateChange = true; - returnValue = true; - } - } else { - returnValue = (offset.y > CYLINDER_TOP) || (offset.y < CYLINDER_BOTTOM); - myAvatar._tippingPoint = myAvatar.getCurrentStandingHeight(); - myAvatar._sitStandStateCount = 0; - if (myAvatar._squatCount > SQUATTY_COUNT_THRESHOLD) { - // returnValue = true; - myAvatar._squatCount = 0; - } + returnValue = (offset.y > CYLINDER_TOP) || (offset.y < CYLINDER_BOTTOM); + if (myAvatar._squatCount > SQUATTY_COUNT_THRESHOLD) { + myAvatar._squatCount = 0; + returnValue = true; } - } return returnValue; } From 3517905435d5b0fa2c276f9c313de8de5460712e Mon Sep 17 00:00:00 2001 From: amantley Date: Fri, 5 Oct 2018 16:22:30 -0700 Subject: [PATCH 24/27] added getter and setter for sitstandstatechange bool --- interface/src/avatar/MyAvatar.cpp | 50 ++++++++++--------------------- interface/src/avatar/MyAvatar.h | 18 ++++++----- 2 files changed, 27 insertions(+), 41 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 58a0ef4046..995126f3ca 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -524,14 +524,11 @@ void MyAvatar::update(float deltaTime) { } float averageSensorSpaceHeight = _sumUserHeightSensorSpace / _averageUserHeightCount; - //auto sensorHeadPose = myAvatar.getControllerPoseInSensorFrame(controller::Action::HEAD); - //glm::vec3 upHead = transformVectorFast(sensorHeadPose.getMatrix(), glm::vec3(0.0f, 1.0f, 0.0f)); - //float acosHead = glm::dot(upHead, glm::vec3(0.0f, 1.0f, 0.0f)); glm::vec3 avatarHips = getAbsoluteJointTranslationInObjectFrame(getJointIndex("Hips")); glm::vec3 worldHips = transformPoint(getTransform().getMatrix(), avatarHips); glm::vec3 sensorHips = transformPoint(glm::inverse(getSensorToWorldMatrix()), worldHips); - + // put update sit stand state counts here if (getIsInSittingState()) { if (newHeightReading.getTranslation().y > (STANDING_HEIGHT_MULTIPLE * _tippingPoint)) { @@ -548,11 +545,11 @@ void MyAvatar::update(float deltaTime) { setResetMode(true); setIsInSittingState(false); setCenterOfGravityModelEnabled(true); - _sitStandStateChange = true; + setSitStandStateChange(true); } } else { _sitStandStateCount = 0; - // tipping point is average height when sitting. + // tipping point is average height when sitting. _tippingPoint = averageSensorSpaceHeight; } } else { @@ -570,7 +567,7 @@ void MyAvatar::update(float deltaTime) { setResetMode(true); setIsInSittingState(true); setCenterOfGravityModelEnabled(false); - _sitStandStateChange = true; + setSitStandStateChange(true); } } else { // returnValue = (offset.y > CYLINDER_TOP) || (offset.y < CYLINDER_BOTTOM); @@ -2091,11 +2088,6 @@ void MyAvatar::prepareForPhysicsSimulation() { _characterController.setPositionAndOrientation(getWorldPosition(), getWorldOrientation()); auto headPose = getControllerPoseInAvatarFrame(controller::Action::HEAD); if (headPose.isValid()) { - if (getCenterOfGravityModelEnabled()) { - //_follow.prePhysicsUpdate(*this, deriveBodyUsingCgModel(), _bodySensorMatrix, hasDriveInput()); - } else { - //_follow.prePhysicsUpdate(*this, deriveBodyFromHMDSensor(), _bodySensorMatrix, hasDriveInput()); - } _follow.prePhysicsUpdate(*this, deriveBodyFromHMDSensor(), _bodySensorMatrix, hasDriveInput()); } else { _follow.deactivate(); @@ -3886,15 +3878,6 @@ void MyAvatar::setIsInWalkingState(bool isWalking) { } void MyAvatar::setIsInSittingState(bool isSitting) { - _sitStandStateCount = 0; - _squatCount = 0; - controller::Pose sensorHeadPose = getControllerPoseInSensorFrame(controller::Action::HEAD); - if (sensorHeadPose.isValid()) { - _sumUserHeightSensorSpace = sensorHeadPose.getTranslation().y; - _tippingPoint = sensorHeadPose.getTranslation().y; - } - _averageUserHeightCount = 1; - setResetMode(true); _isInSittingState.set(isSitting); emit sittingEnabledChanged(isSitting); } @@ -3915,6 +3898,14 @@ float MyAvatar::getSprintSpeed() const { return _sprintSpeed.get(); } +void MyAvatar::setSitStandStateChange(bool stateChanged) { + _sitStandStateChange = stateChanged; +} + +float MyAvatar::getSitStandStateChange() const { + return _sitStandStateChange; +} + QVector MyAvatar::getScriptUrls() { QVector scripts = _skeletonModel->isLoaded() ? _skeletonModel->getFBXGeometry().scripts : QVector(); return scripts; @@ -4134,10 +4125,8 @@ bool MyAvatar::FollowHelper::shouldActivateVertical(MyAvatar& myAvatar, const gl const int SQUATTY_COUNT_THRESHOLD = 600; glm::vec3 offset = extractTranslation(desiredBodyMatrix) - extractTranslation(currentBodyMatrix); - //qCDebug(interfaceapp) << " current mode " << myAvatar.getCurrentStandingHeight() << " " << sensorHeadPose.getTranslation().y << " state " << myAvatar.getIsInSittingState(); - bool returnValue = false; - if (myAvatar._sitStandStateChange) { + if (myAvatar.getSitStandStateChange()) { returnValue = true; } if (myAvatar.getIsInSittingState()) { @@ -4184,13 +4173,10 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat } } } - - qCDebug(interfaceapp) << "velocity of headset " << glm::length(myAvatar.getControllerPoseInSensorFrame(controller::Action::HEAD).getVelocity()); - + if (_velocityCount > 60) { if (!isActive(Vertical) && (shouldActivateVertical(myAvatar, desiredBodyMatrix, currentBodyMatrix) || hasDriveInput)) { activate(Vertical); - qCDebug(interfaceapp) << "recenter vertically!!!!!! " << hasDriveInput; } } else { if ((glm::length(myAvatar.getControllerPoseInSensorFrame(controller::Action::HEAD).getVelocity()) > 0.1f)) { @@ -4263,14 +4249,10 @@ glm::mat4 MyAvatar::FollowHelper::postPhysicsUpdate(MyAvatar& myAvatar, const gl glm::mat4 newBodyMat = createMatFromQuatAndPos(sensorAngularDisplacement * glmExtractRotation(currentBodyMatrix), sensorLinearDisplacement + extractTranslation(currentBodyMatrix)); - if (myAvatar._sitStandStateChange) { - myAvatar._sitStandStateChange = false; + if (myAvatar.getSitStandStateChange()) { + myAvatar.setSitStandStateChange(false); deactivate(Vertical); - - qCDebug(interfaceapp) << "before snap " << extractTranslation(newBodyMat); - //newBodyMat = myAvatar.deriveBodyFromHMDSensor(); setTranslation(newBodyMat, extractTranslation(myAvatar.deriveBodyFromHMDSensor())); - qCDebug(interfaceapp) << "after snap " << extractTranslation(newBodyMat); } return newBodyMat; } else { diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 24a58745a3..54aa015aff 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -1105,6 +1105,8 @@ public: float getWalkBackwardSpeed() const; void setSprintSpeed(float value); float getSprintSpeed() const; + void setSitStandStateChange(bool stateChanged); + float getSitStandStateChange() const; QVector getScriptUrls(); @@ -1804,14 +1806,16 @@ private: std::mutex _pinnedJointsMutex; std::vector _pinnedJoints; - // height of user in sensor space, when standing erect. - ThreadSafeValueCache _userHeight { DEFAULT_AVATAR_HEIGHT }; - float _sumUserHeightSensorSpace { 0.0f }; - int _averageUserHeightCount { 1 }; - bool _sitStandStateChange { false }; - void updateChildCauterization(SpatiallyNestablePointer object, bool cauterize); + const float DEFAULT_FLOOR_HEIGHT = 0.0f; + + // height of user in sensor space, when standing erect. + ThreadSafeValueCache _userHeight{ DEFAULT_AVATAR_HEIGHT }; + float _sumUserHeightSensorSpace{ DEFAULT_AVATAR_HEIGHT }; + int _averageUserHeightCount{ 1 }; + bool _sitStandStateChange{ false }; + // max unscaled forward movement speed ThreadSafeValueCache _walkSpeed { DEFAULT_AVATAR_MAX_WALKING_SPEED }; ThreadSafeValueCache _walkBackwardSpeed { DEFAULT_AVATAR_MAX_WALKING_BACKWARD_SPEED }; @@ -1821,7 +1825,7 @@ private: ThreadSafeValueCache _isInSittingState { false }; int _sitStandStateCount { 0 }; int _squatCount { 0 }; - float _tippingPoint { 0.0f }; + float _tippingPoint { DEFAULT_FLOOR_HEIGHT }; // load avatar scripts once when rig is ready bool _shouldLoadScripts { false }; From 5faf26abd104b0db6d4539b5e074aee4f201bf82 Mon Sep 17 00:00:00 2001 From: amantley Date: Thu, 11 Oct 2018 10:38:59 -0700 Subject: [PATCH 25/27] turned off shoulder rotation when we don't have hmd lean recentering on. this temporarily fixes the behavior of the shoulders in this mode. todo: use the spine2 position to determine azimuth of hands, then they will work in all conditions --- interface/src/avatar/MyAvatar.cpp | 7 +++++++ interface/src/avatar/MySkeletonModel.cpp | 3 ++- 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 1f38971ab4..77c66d062f 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -591,8 +591,15 @@ void MyAvatar::update(float deltaTime) { // 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)); + + } + // temp: draw spine 2 position for hand azimuth purposes. + int spine2Index = getJointIndex("Spine2"); + glm::vec3 spine2WorldPosition = transformPoint(getTransform().getMatrix(), getAbsoluteJointTranslationInObjectFrame(spine2Index)); + DebugDraw::getInstance().addMarker("spine2 location", Quaternions::IDENTITY, spine2WorldPosition, glm::vec4(1)); + if (_goToPending) { setWorldPosition(_goToPosition); setWorldOrientation(_goToOrientation); diff --git a/interface/src/avatar/MySkeletonModel.cpp b/interface/src/avatar/MySkeletonModel.cpp index 42ec582c47..1f97ce03f8 100644 --- a/interface/src/avatar/MySkeletonModel.cpp +++ b/interface/src/avatar/MySkeletonModel.cpp @@ -239,7 +239,7 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { 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() && + if (myAvatar->getHMDLeanRecenterEnabled() && myAvatar->getControllerPoseInAvatarFrame(controller::Action::RIGHT_HAND).isValid() && myAvatar->getControllerPoseInAvatarFrame(controller::Action::LEFT_HAND).isValid() && !(params.primaryControllerFlags[Rig::PrimaryControllerType_Spine2] & (uint8_t)Rig::ControllerFlags::Enabled)) { @@ -250,6 +250,7 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { bool headExists = _rig.getAbsoluteJointPoseInRigFrame(_rig.indexOfJoint("Head"), currentHeadPose); bool hipsExists = _rig.getAbsoluteJointPoseInRigFrame(_rig.indexOfJoint("Hips"), currentHipsPose); if (spine2Exists && headExists && hipsExists) { + // qCDebug(interfaceapp) << "hips forward direction "<< (currentHipsPose.rot() * glm::vec3(0.0f, 0.0f, 1.0f)); AnimPose rigSpaceYaw(myAvatar->getSpine2RotationRigSpace()); glm::vec3 u, v, w; glm::vec3 fwd = rigSpaceYaw.rot() * glm::vec3(0.0f, 0.0f, 1.0f); From f73d974ad77b62a41b4d540e60b1cc1c584db98f Mon Sep 17 00:00:00 2001 From: amantley Date: Thu, 11 Oct 2018 15:14:04 -0700 Subject: [PATCH 26/27] added a lock state so you can lock sit or stand state, also made the hand azimuth relative to the spine2 location in avatar space. This stops the arms from behaving badly when the hands are in front of the chest but behind the root position of the avatar --- interface/src/avatar/MyAvatar.cpp | 119 ++++++++++++++--------- interface/src/avatar/MyAvatar.h | 1 + interface/src/avatar/MySkeletonModel.cpp | 3 +- 3 files changed, 75 insertions(+), 48 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 77c66d062f..6b4459e97c 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -531,52 +531,57 @@ void MyAvatar::update(float deltaTime) { glm::vec3 sensorHips = transformPoint(glm::inverse(getSensorToWorldMatrix()), worldHips); // put update sit stand state counts here - if (getIsInSittingState()) { - if (newHeightReading.getTranslation().y > (STANDING_HEIGHT_MULTIPLE * _tippingPoint)) { - // if we recenter upwards then no longer in sitting state - _sitStandStateCount++; - if (_sitStandStateCount > SITTING_COUNT_THRESHOLD) { - _sitStandStateCount = 0; - _squatCount = 0; - if (newHeightReading.isValid()) { - _sumUserHeightSensorSpace = newHeightReading.getTranslation().y; - _tippingPoint = newHeightReading.getTranslation().y; + if (!_lockSitStandState) { + if (getIsInSittingState()) { + if (newHeightReading.getTranslation().y > (STANDING_HEIGHT_MULTIPLE * _tippingPoint)) { + // if we recenter upwards then no longer in sitting state + _sitStandStateCount++; + if (_sitStandStateCount > SITTING_COUNT_THRESHOLD) { + _sitStandStateCount = 0; + _squatCount = 0; + if (newHeightReading.isValid()) { + _sumUserHeightSensorSpace = newHeightReading.getTranslation().y; + _tippingPoint = newHeightReading.getTranslation().y; + } + _averageUserHeightCount = 1; + setResetMode(true); + setIsInSittingState(false); + setCenterOfGravityModelEnabled(true); + setSitStandStateChange(true); } - _averageUserHeightCount = 1; - setResetMode(true); - setIsInSittingState(false); - setCenterOfGravityModelEnabled(true); - setSitStandStateChange(true); + qCDebug(interfaceapp) << "going to stand state"; + } else { + _sitStandStateCount = 0; + // tipping point is average height when sitting. + _tippingPoint = averageSensorSpaceHeight; } } else { - _sitStandStateCount = 0; - // tipping point is average height when sitting. - _tippingPoint = averageSensorSpaceHeight; - } - } else { - // in the standing state - if ((newHeightReading.getTranslation().y < (SITTING_HEIGHT_MULTIPLE * _tippingPoint))) {// && (angleSpine2 > COSINE_TEN_DEGREES) && !(sensorHips.y > (0.4f * averageSensorSpaceHeight))) { - _sitStandStateCount++; - if (_sitStandStateCount > SITTING_COUNT_THRESHOLD) { - _sitStandStateCount = 0; - _squatCount = 0; - if (newHeightReading.isValid()) { - _sumUserHeightSensorSpace = newHeightReading.getTranslation().y; - _tippingPoint = newHeightReading.getTranslation().y; - } - _averageUserHeightCount = 1; - setResetMode(true); - setIsInSittingState(true); - setCenterOfGravityModelEnabled(false); - setSitStandStateChange(true); - } - } else { - // returnValue = (offset.y > CYLINDER_TOP) || (offset.y < CYLINDER_BOTTOM); - // use the mode height for the tipping point when we are standing. - _tippingPoint = getCurrentStandingHeight(); - _sitStandStateCount = 0; - } + // in the standing state + if ((newHeightReading.getTranslation().y < (SITTING_HEIGHT_MULTIPLE * _tippingPoint))) {// && (angleSpine2 > COSINE_TEN_DEGREES) && !(sensorHips.y > (0.4f * averageSensorSpaceHeight))) { + _sitStandStateCount++; + if (_sitStandStateCount > SITTING_COUNT_THRESHOLD) { + _sitStandStateCount = 0; + _squatCount = 0; + if (newHeightReading.isValid()) { + _sumUserHeightSensorSpace = newHeightReading.getTranslation().y; + _tippingPoint = newHeightReading.getTranslation().y; + } + _averageUserHeightCount = 1; + setResetMode(true); + setIsInSittingState(true); + setCenterOfGravityModelEnabled(false); + setSitStandStateChange(true); + } + qCDebug(interfaceapp) << "going to sit state"; + } else { + // returnValue = (offset.y > CYLINDER_TOP) || (offset.y < CYLINDER_BOTTOM); + // use the mode height for the tipping point when we are standing. + _tippingPoint = getCurrentStandingHeight(); + _sitStandStateCount = 0; + } + + } } @@ -939,6 +944,15 @@ 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 { + int spine2Index = _skeletonModel->getRig().indexOfJoint("Spine2"); + glm::vec3 azimuthOrigin(0.0f,0.0f,0.0f); + if (!(spine2Index < 0)) { + // use the spine for the azimuth origin. + azimuthOrigin = getAbsoluteJointTranslationInObjectFrame(spine2Index); + } else { + // use the avatar root as the azimuth origin. + } + controller::Pose leftHandPoseAvatarSpace = getLeftHandPose(); controller::Pose rightHandPoseAvatarSpace = getRightHandPose(); controller::Pose headPoseAvatarSpace = getControllerPoseInAvatarFrame(controller::Action::HEAD); @@ -946,11 +960,13 @@ glm::vec2 MyAvatar::computeHandAzimuth() const { glm::vec2 latestHipToHandController = _hipToHandController; if (leftHandPoseAvatarSpace.isValid() && rightHandPoseAvatarSpace.isValid() && headPoseAvatarSpace.isValid()) { + glm::vec3 rightHandOffset = rightHandPoseAvatarSpace.translation - azimuthOrigin; + glm::vec3 leftHandOffset = leftHandPoseAvatarSpace.translation - azimuthOrigin; // 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); + if ((glm::length(glm::vec2(rightHandOffset.x, rightHandOffset.z)) > 0.0f) && (glm::length(glm::vec2(leftHandOffset.x, leftHandOffset.z)) > 0.0f)) { + latestHipToHandController = lerp(glm::normalize(glm::vec2(rightHandOffset.x, rightHandOffset.z)), glm::normalize(glm::vec2(leftHandOffset.x, leftHandOffset.z)), HALFWAY); } else { latestHipToHandController = glm::vec2(0.0f, -1.0f); } @@ -4134,21 +4150,29 @@ bool MyAvatar::FollowHelper::shouldActivateVertical(MyAvatar& myAvatar, const gl const int SQUATTY_COUNT_THRESHOLD = 600; glm::vec3 offset = extractTranslation(desiredBodyMatrix) - extractTranslation(currentBodyMatrix); + bool returnValue = false; + returnValue = (offset.y > CYLINDER_TOP);// || (offset.y < CYLINDER_BOTTOM); + if (myAvatar.getSitStandStateChange()) { + qCDebug(interfaceapp) << "sit state change"; returnValue = true; } if (myAvatar.getIsInSittingState()) { if (offset.y < SITTING_BOTTOM) { // we recenter when sitting. + qCDebug(interfaceapp) << "lean back sitting "; returnValue = true; } } else { // in the standing state - returnValue = (offset.y > CYLINDER_TOP) || (offset.y < CYLINDER_BOTTOM); if (myAvatar._squatCount > SQUATTY_COUNT_THRESHOLD) { myAvatar._squatCount = 0; - returnValue = true; + qCDebug(interfaceapp) << "squatting "; + // returnValue = true; + } + if (returnValue == true) { + qCDebug(interfaceapp) << "above or below capsule in standing"; } } return returnValue; @@ -4182,7 +4206,7 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat } } } - + if (_velocityCount > 60) { if (!isActive(Vertical) && (shouldActivateVertical(myAvatar, desiredBodyMatrix, currentBodyMatrix) || hasDriveInput)) { activate(Vertical); @@ -4192,6 +4216,7 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat _velocityCount++; } } + } else { if (!isActive(Rotation) && getForceActivateRotation()) { diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index f268a05a15..f026f39493 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -1824,6 +1824,7 @@ private: float _sumUserHeightSensorSpace{ DEFAULT_AVATAR_HEIGHT }; int _averageUserHeightCount{ 1 }; bool _sitStandStateChange{ false }; + bool _lockSitStandState { true }; // max unscaled forward movement speed ThreadSafeValueCache _walkSpeed { DEFAULT_AVATAR_MAX_WALKING_SPEED }; diff --git a/interface/src/avatar/MySkeletonModel.cpp b/interface/src/avatar/MySkeletonModel.cpp index 1f97ce03f8..ce8fefa0c5 100644 --- a/interface/src/avatar/MySkeletonModel.cpp +++ b/interface/src/avatar/MySkeletonModel.cpp @@ -239,7 +239,8 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { params.primaryControllerFlags[Rig::PrimaryControllerType_Hips] = (uint8_t)Rig::ControllerFlags::Enabled | (uint8_t)Rig::ControllerFlags::Estimated; // set spine2 if we have hand controllers - if (myAvatar->getHMDLeanRecenterEnabled() && myAvatar->getControllerPoseInAvatarFrame(controller::Action::RIGHT_HAND).isValid() && + // myAvatar->getHMDLeanRecenterEnabled() && + 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)) { From 67afc862224dcb753acdf62b4a8d77f276806b8c Mon Sep 17 00:00:00 2001 From: amantley Date: Thu, 11 Oct 2018 18:39:44 -0700 Subject: [PATCH 27/27] cleanup --- interface/resources/qml/hifi/AvatarApp.qml | 1 + .../resources/qml/hifi/avatarapp/Settings.qml | 19 ++++++++++++ interface/src/avatar/MyAvatar.cpp | 29 +++++++++---------- interface/src/avatar/MyAvatar.h | 13 ++++++++- interface/src/avatar/MySkeletonModel.cpp | 3 +- scripts/system/avatarapp.js | 14 ++++++++- 6 files changed, 60 insertions(+), 19 deletions(-) diff --git a/interface/resources/qml/hifi/AvatarApp.qml b/interface/resources/qml/hifi/AvatarApp.qml index b06a2ca67c..bf647b65bb 100644 --- a/interface/resources/qml/hifi/AvatarApp.qml +++ b/interface/resources/qml/hifi/AvatarApp.qml @@ -253,6 +253,7 @@ Rectangle { dominantHand : settings.dominantHandIsLeft ? 'left' : 'right', collisionsEnabled : settings.avatarCollisionsOn, sittingEnabled : settings.avatarSittingOn, + lockStateEnabled : settings.avatarLockSitStandStateOn, animGraphOverrideUrl : settings.avatarAnimationOverrideJSON, collisionSoundUrl : settings.avatarCollisionSoundUrl }; diff --git a/interface/resources/qml/hifi/avatarapp/Settings.qml b/interface/resources/qml/hifi/avatarapp/Settings.qml index c4289ca650..8749079940 100644 --- a/interface/resources/qml/hifi/avatarapp/Settings.qml +++ b/interface/resources/qml/hifi/avatarapp/Settings.qml @@ -21,6 +21,7 @@ Rectangle { property alias dominantHandIsLeft: leftHandRadioButton.checked property alias avatarCollisionsOn: collisionsEnabledRadiobutton.checked property alias avatarSittingOn: sitRadiobutton.checked + property alias avatarLockSitStandStateOn: lockSitStandStateCheckbox.checked property alias avatarAnimationOverrideJSON: avatarAnimationUrlInputText.text property alias avatarAnimationJSON: avatarAnimationUrlInputText.placeholderText property alias avatarCollisionSoundUrl: avatarCollisionSoundUrlInputText.text @@ -52,6 +53,12 @@ Rectangle { standRadioButton.checked = true; } + if (settings.lockStateEnabled) { + lockSitStandStateCheckbox.checked = true; + } else { + lockSitStandStateCheckbox.checked = false; + } + avatarAnimationJSON = settings.animGraphUrl; avatarAnimationOverrideJSON = settings.animGraphOverrideUrl; avatarCollisionSoundUrl = settings.collisionSoundUrl; @@ -343,6 +350,18 @@ Rectangle { text: "Stand" boxSize: 20 } + + // "Lock State" Checkbox + + HifiControlsUit.CheckBox { + id: lockSitStandStateCheckbox; + visible: activeTab == "nearbyTab"; + anchors.right: reloadNearbyContainer.left; + anchors.rightMargin: 20; + checked: settings.lockStateEnabled; + text: "lock"; + boxSize: 24; + } } ColumnLayout { diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 6b4459e97c..745705f7b8 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -531,7 +531,7 @@ void MyAvatar::update(float deltaTime) { glm::vec3 sensorHips = transformPoint(glm::inverse(getSensorToWorldMatrix()), worldHips); // put update sit stand state counts here - if (!_lockSitStandState) { + if (getIsSitStandStateLocked()) { if (getIsInSittingState()) { if (newHeightReading.getTranslation().y > (STANDING_HEIGHT_MULTIPLE * _tippingPoint)) { // if we recenter upwards then no longer in sitting state @@ -584,7 +584,6 @@ void MyAvatar::update(float deltaTime) { } } - if (_drawAverageFacingEnabled) { auto sensorHeadPose = getControllerPoseInSensorFrame(controller::Action::HEAD); glm::vec3 worldHeadPos = transformPoint(getSensorToWorldMatrix(), sensorHeadPose.getTranslation()); @@ -595,16 +594,9 @@ void MyAvatar::update(float deltaTime) { // 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)); - - + DebugDraw::getInstance().drawRay(getWorldPosition(), handAzimuthMidpoint, glm::vec4(0.0f, 1.0f, 1.0f, 1.0f)); } - // temp: draw spine 2 position for hand azimuth purposes. - int spine2Index = getJointIndex("Spine2"); - glm::vec3 spine2WorldPosition = transformPoint(getTransform().getMatrix(), getAbsoluteJointTranslationInObjectFrame(spine2Index)); - DebugDraw::getInstance().addMarker("spine2 location", Quaternions::IDENTITY, spine2WorldPosition, glm::vec4(1)); - if (_goToPending) { setWorldPosition(_goToPosition); setWorldOrientation(_goToOrientation); @@ -949,8 +941,6 @@ glm::vec2 MyAvatar::computeHandAzimuth() const { if (!(spine2Index < 0)) { // use the spine for the azimuth origin. azimuthOrigin = getAbsoluteJointTranslationInObjectFrame(spine2Index); - } else { - // use the avatar root as the azimuth origin. } controller::Pose leftHandPoseAvatarSpace = getLeftHandPose(); @@ -3882,6 +3872,10 @@ bool MyAvatar::getIsInSittingState() const { return _isInSittingState.get(); } +bool MyAvatar::getIsSitStandStateLocked() const { + return _lockSitStandState.get(); +} + float MyAvatar::getWalkSpeed() const { return _walkSpeed.get() * _walkSpeedScalar; } @@ -3907,6 +3901,11 @@ void MyAvatar::setIsInSittingState(bool isSitting) { emit sittingEnabledChanged(isSitting); } +void MyAvatar::setIsSitStandStateLocked(bool isLocked) { + _lockSitStandState.set(isLocked); + emit sitStandStateLockEnabledChanged(isLocked); +} + void MyAvatar::setWalkSpeed(float value) { _walkSpeed.set(value); } @@ -4152,7 +4151,7 @@ bool MyAvatar::FollowHelper::shouldActivateVertical(MyAvatar& myAvatar, const gl glm::vec3 offset = extractTranslation(desiredBodyMatrix) - extractTranslation(currentBodyMatrix); bool returnValue = false; - returnValue = (offset.y > CYLINDER_TOP);// || (offset.y < CYLINDER_BOTTOM); + returnValue = (offset.y > CYLINDER_TOP) || (offset.y < CYLINDER_BOTTOM); if (myAvatar.getSitStandStateChange()) { qCDebug(interfaceapp) << "sit state change"; @@ -4206,7 +4205,7 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat } } } - + if (_velocityCount > 60) { if (!isActive(Vertical) && (shouldActivateVertical(myAvatar, desiredBodyMatrix, currentBodyMatrix) || hasDriveInput)) { activate(Vertical); @@ -4216,7 +4215,7 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat _velocityCount++; } } - + } else { if (!isActive(Rotation) && getForceActivateRotation()) { diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index f026f39493..674d4b8b70 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -244,6 +244,7 @@ class MyAvatar : public Avatar { Q_PROPERTY(float walkBackwardSpeed READ getWalkBackwardSpeed WRITE setWalkBackwardSpeed); Q_PROPERTY(float sprintSpeed READ getSprintSpeed WRITE setSprintSpeed); Q_PROPERTY(bool isInSittingState READ getIsInSittingState WRITE setIsInSittingState); + Q_PROPERTY(bool isSitStandStateLocked READ getIsSitStandStateLocked WRITE setIsSitStandStateLocked); const QString DOMINANT_LEFT_HAND = "left"; const QString DOMINANT_RIGHT_HAND = "right"; @@ -1105,6 +1106,8 @@ public: bool getIsInWalkingState() const; void setIsInSittingState(bool isSitting); bool getIsInSittingState() const; + void setIsSitStandStateLocked(bool isLocked); + bool getIsSitStandStateLocked() const; void setWalkSpeed(float value); float getWalkSpeed() const; void setWalkBackwardSpeed(float value); @@ -1526,6 +1529,14 @@ signals: */ void sittingEnabledChanged(bool enabled); + /**jsdoc + * Triggered when the sit state is enabled or disabled + * @function MyAvatar.sitStandStateLockEnabledChanged + * @param {boolean} enabled + * @returns {Signal} + */ + void sitStandStateLockEnabledChanged(bool enabled); + private slots: void leaveDomain(); void updateCollisionCapsuleCache(); @@ -1824,7 +1835,7 @@ private: float _sumUserHeightSensorSpace{ DEFAULT_AVATAR_HEIGHT }; int _averageUserHeightCount{ 1 }; bool _sitStandStateChange{ false }; - bool _lockSitStandState { true }; + ThreadSafeValueCache _lockSitStandState { true }; // max unscaled forward movement speed ThreadSafeValueCache _walkSpeed { DEFAULT_AVATAR_MAX_WALKING_SPEED }; diff --git a/interface/src/avatar/MySkeletonModel.cpp b/interface/src/avatar/MySkeletonModel.cpp index ce8fefa0c5..78c5c03cc9 100644 --- a/interface/src/avatar/MySkeletonModel.cpp +++ b/interface/src/avatar/MySkeletonModel.cpp @@ -239,7 +239,6 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { params.primaryControllerFlags[Rig::PrimaryControllerType_Hips] = (uint8_t)Rig::ControllerFlags::Enabled | (uint8_t)Rig::ControllerFlags::Estimated; // set spine2 if we have hand controllers - // myAvatar->getHMDLeanRecenterEnabled() && 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)) { @@ -251,7 +250,7 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { bool headExists = _rig.getAbsoluteJointPoseInRigFrame(_rig.indexOfJoint("Head"), currentHeadPose); bool hipsExists = _rig.getAbsoluteJointPoseInRigFrame(_rig.indexOfJoint("Hips"), currentHipsPose); if (spine2Exists && headExists && hipsExists) { - // qCDebug(interfaceapp) << "hips forward direction "<< (currentHipsPose.rot() * glm::vec3(0.0f, 0.0f, 1.0f)); + AnimPose rigSpaceYaw(myAvatar->getSpine2RotationRigSpace()); glm::vec3 u, v, w; glm::vec3 fwd = rigSpaceYaw.rot() * glm::vec3(0.0f, 0.0f, 1.0f); diff --git a/scripts/system/avatarapp.js b/scripts/system/avatarapp.js index faf624392a..4a25ab9551 100644 --- a/scripts/system/avatarapp.js +++ b/scripts/system/avatarapp.js @@ -64,7 +64,8 @@ function getMyAvatarSettings() { return { dominantHand: MyAvatar.getDominantHand(), collisionsEnabled : MyAvatar.getCollisionsEnabled(), - sittingEnabled : MyAvatar.isInSittingState, + sittingEnabled: MyAvatar.isInSittingState, + lockStateEnabled: MyAvatar.isSitStandStateLocked, collisionSoundUrl : MyAvatar.collisionSoundURL, animGraphUrl: MyAvatar.getAnimGraphUrl(), animGraphOverrideUrl : MyAvatar.getAnimGraphOverrideUrl(), @@ -145,6 +146,14 @@ function onSittingEnabledChanged(isSitting) { } } +function onSitStandStateLockedEnabledChanged(isLocked) { + if (currentAvatarSettings.lockStateEnabled !== isLocked) { + currentAvatarSettings.lockStateEnabled = isLocked; + print("emit lock sit stand state changed"); + sendToQml({ 'method': 'settingChanged', 'name': 'lockStateEnabled', 'value': isLocked }) + } +} + function onNewCollisionSoundUrl(url) { if(currentAvatarSettings.collisionSoundUrl !== url) { currentAvatarSettings.collisionSoundUrl = url; @@ -324,6 +333,7 @@ function fromQml(message) { // messages are {method, params}, like json-rpc. See MyAvatar.setDominantHand(message.settings.dominantHand); MyAvatar.setCollisionsEnabled(message.settings.collisionsEnabled); MyAvatar.isInSittingState = message.settings.sittingEnabled; + MyAvatar.isSitStandStateLocked = message.settings.lockStateEnabled; MyAvatar.collisionSoundURL = message.settings.collisionSoundUrl; MyAvatar.setAnimGraphOverrideUrl(message.settings.animGraphOverrideUrl); @@ -518,6 +528,7 @@ function off() { MyAvatar.dominantHandChanged.disconnect(onDominantHandChanged); MyAvatar.collisionsEnabledChanged.disconnect(onCollisionsEnabledChanged); MyAvatar.sittingEnabledChanged.disconnect(onSittingEnabledChanged); + MyAvatar.sitStandStateLockEnabledChanged.disconnect(onSitStandStateLockedEnabledChanged); MyAvatar.newCollisionSoundURL.disconnect(onNewCollisionSoundUrl); MyAvatar.animGraphUrlChanged.disconnect(onAnimGraphUrlChanged); MyAvatar.targetScaleChanged.disconnect(onTargetScaleChanged); @@ -533,6 +544,7 @@ function on() { MyAvatar.dominantHandChanged.connect(onDominantHandChanged); MyAvatar.collisionsEnabledChanged.connect(onCollisionsEnabledChanged); MyAvatar.sittingEnabledChanged.connect(onSittingEnabledChanged); + MyAvatar.sitStandStateLockEnabledChanged.connect(onSitStandStateLockedEnabledChanged); MyAvatar.newCollisionSoundURL.connect(onNewCollisionSoundUrl); MyAvatar.animGraphUrlChanged.connect(onAnimGraphUrlChanged); MyAvatar.targetScaleChanged.connect(onTargetScaleChanged);