From 33f9acde67e8ac1a7707e9880c5662de271a3eb1 Mon Sep 17 00:00:00 2001 From: amantley Date: Tue, 17 Jul 2018 17:42:11 -0700 Subject: [PATCH 01/24] latest --- interface/src/avatar/MyAvatar.cpp | 20 +++++++++++++++++++- interface/src/avatar/MyAvatar.h | 2 ++ 2 files changed, 21 insertions(+), 1 deletion(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index d084ca69f9..9eaf42d25d 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -89,9 +89,10 @@ const float MyAvatar::ZOOM_MAX = 25.0f; const float MyAvatar::ZOOM_DEFAULT = 1.5f; const float MIN_SCALE_CHANGED_DELTA = 0.001f; const int MODE_READINGS_RING_BUFFER_SIZE = 500; +const int HEAD_FACING_RING_BUFFER_SIZE = 250; const float CENTIMETERS_PER_METER = 100.0f; -//#define DEBUG_DRAW_HMD_MOVING_AVERAGE +#define DEBUG_DRAW_HMD_MOVING_AVERAGE MyAvatar::MyAvatar(QThread* thread) : Avatar(thread), @@ -112,6 +113,7 @@ MyAvatar::MyAvatar(QThread* thread) : _hmdSensorMatrix(), _hmdSensorOrientation(), _hmdSensorPosition(), + _headFacingBuffer(HEAD_FACING_RING_BUFFER_SIZE), _recentModeReadings(MODE_READINGS_RING_BUFFER_SIZE), _bodySensorMatrix(), _goToPending(false), @@ -434,11 +436,25 @@ void MyAvatar::update(float deltaTime) { _recentModeReadings.insert(newHeightReadingInCentimeters); setCurrentStandingHeight(computeStandingHeightMode(getControllerPoseInAvatarFrame(controller::Action::HEAD))); setAverageHeadRotation(computeAverageHeadRotation(getControllerPoseInAvatarFrame(controller::Action::HEAD))); + + glm::vec2 bodyFacing = getFacingDir2D(_bodySensorMatrix); + _headFacingBuffer.insert(_headControllerFacing); + //qCDebug(interfaceapp) << "the body facing is " << -bodyFacing.x << " " << -bodyFacing.y << " the head controller facing is " << _headControllerFacing.x << " " << _headControllerFacing.y; + + float sumHeadFacingX = 0.0f; + float sumHeadFacingY = 0.0f; + for (auto headFacingIterator = _headFacingBuffer.begin(); headFacingIterator != _headFacingBuffer.end(); ++headFacingIterator) { + sumHeadFacingX += (*headFacingIterator).x; + sumHeadFacingY += (*headFacingIterator).y; + } + _averageFacing.x = sumHeadFacingX / (float)_headFacingBuffer.getNumEntries(); + _averageFacing.y= sumHeadFacingY / (float)_headFacingBuffer.getNumEntries(); #ifdef DEBUG_DRAW_HMD_MOVING_AVERAGE auto sensorHeadPose = getControllerPoseInSensorFrame(controller::Action::HEAD); glm::vec3 worldHeadPos = transformPoint(getSensorToWorldMatrix(), sensorHeadPose.getTranslation()); glm::vec3 worldFacingAverage = transformVectorFast(getSensorToWorldMatrix(), glm::vec3(_headControllerFacingMovingAverage.x, 0.0f, _headControllerFacingMovingAverage.y)); + //glm::vec3 worldFacingAverage = transformVectorFast(getSensorToWorldMatrix(), glm::vec3(_averageFacing.x, 0.0f, _averageFacing.y)); glm::vec3 worldFacing = transformVectorFast(getSensorToWorldMatrix(), glm::vec3(_headControllerFacing.x, 0.0f, _headControllerFacing.y)); DebugDraw::getInstance().drawRay(worldHeadPos, worldHeadPos + worldFacing, glm::vec4(0.0f, 1.0f, 0.0f, 1.0f)); DebugDraw::getInstance().drawRay(worldHeadPos, worldHeadPos + worldFacingAverage, glm::vec4(0.0f, 0.0f, 1.0f, 1.0f)); @@ -3478,6 +3494,8 @@ bool MyAvatar::FollowHelper::shouldActivateRotation(const MyAvatar& myAvatar, co const float FOLLOW_ROTATION_THRESHOLD = cosf(myAvatar.getRotationThreshold()); glm::vec2 bodyFacing = getFacingDir2D(currentBodyMatrix); return glm::dot(-myAvatar.getHeadControllerFacingMovingAverage(), bodyFacing) < FOLLOW_ROTATION_THRESHOLD; + + //return glm::dot(myAvatar._averageFacing, -bodyFacing) < FOLLOW_ROTATION_THRESHOLD; } bool MyAvatar::FollowHelper::shouldActivateHorizontal(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) const { diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 819d5b0066..828ca48e32 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -1546,6 +1546,8 @@ private: // cache head controller pose in sensor space glm::vec2 _headControllerFacing; // facing vector in xz plane (sensor space) glm::vec2 _headControllerFacingMovingAverage { 0.0f, 0.0f }; // facing vector in xz plane (sensor space) + RingBufferHistory _headFacingBuffer; + vec2 _averageFacing { 0.0f, 0.0f }; glm::quat _averageHeadRotation { 0.0f, 0.0f, 0.0f, 1.0f }; float _currentStandingHeight { 0.0f }; From 7cc724a27bf59709be09e9dc4f48ed0ffcd11eb2 Mon Sep 17 00:00:00 2001 From: amantley Date: Wed, 18 Jul 2018 17:08:04 -0700 Subject: [PATCH 02/24] added properties to allow the toggling of the debug draw the look at vector, and the reset moving average look at on step detection --- interface/src/avatar/MyAvatar.cpp | 22 +++++++++++++++------- interface/src/avatar/MyAvatar.h | 8 ++++++++ 2 files changed, 23 insertions(+), 7 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 1b9eed9187..63a26fb2ad 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -471,13 +471,15 @@ void MyAvatar::update(float deltaTime) { _averageFacing.y= sumHeadFacingY / (float)_headFacingBuffer.getNumEntries(); #ifdef DEBUG_DRAW_HMD_MOVING_AVERAGE - auto sensorHeadPose = getControllerPoseInSensorFrame(controller::Action::HEAD); - glm::vec3 worldHeadPos = transformPoint(getSensorToWorldMatrix(), sensorHeadPose.getTranslation()); - glm::vec3 worldFacingAverage = transformVectorFast(getSensorToWorldMatrix(), glm::vec3(_headControllerFacingMovingAverage.x, 0.0f, _headControllerFacingMovingAverage.y)); - //glm::vec3 worldFacingAverage = transformVectorFast(getSensorToWorldMatrix(), glm::vec3(_averageFacing.x, 0.0f, _averageFacing.y)); - glm::vec3 worldFacing = transformVectorFast(getSensorToWorldMatrix(), glm::vec3(_headControllerFacing.x, 0.0f, _headControllerFacing.y)); - DebugDraw::getInstance().drawRay(worldHeadPos, worldHeadPos + worldFacing, glm::vec4(0.0f, 1.0f, 0.0f, 1.0f)); - DebugDraw::getInstance().drawRay(worldHeadPos, worldHeadPos + worldFacingAverage, glm::vec4(0.0f, 0.0f, 1.0f, 1.0f)); + if (_drawAverageFacingEnabled) { + auto sensorHeadPose = getControllerPoseInSensorFrame(controller::Action::HEAD); + glm::vec3 worldHeadPos = transformPoint(getSensorToWorldMatrix(), sensorHeadPose.getTranslation()); + glm::vec3 worldFacingAverage = transformVectorFast(getSensorToWorldMatrix(), glm::vec3(_headControllerFacingMovingAverage.x, 0.0f, _headControllerFacingMovingAverage.y)); + //glm::vec3 worldFacingAverage = transformVectorFast(getSensorToWorldMatrix(), glm::vec3(_averageFacing.x, 0.0f, _averageFacing.y)); + glm::vec3 worldFacing = transformVectorFast(getSensorToWorldMatrix(), glm::vec3(_headControllerFacing.x, 0.0f, _headControllerFacing.y)); + DebugDraw::getInstance().drawRay(worldHeadPos, worldHeadPos + worldFacing, glm::vec4(0.0f, 1.0f, 0.0f, 1.0f)); + DebugDraw::getInstance().drawRay(worldHeadPos, worldHeadPos + worldFacingAverage, glm::vec4(0.0f, 0.0f, 1.0f, 1.0f)); + } #endif if (_goToPending) { @@ -3594,10 +3596,16 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat if (myAvatar.getCenterOfGravityModelEnabled()) { if (!isActive(Horizontal) && (shouldActivateHorizontalCG(myAvatar) || hasDriveInput)) { activate(Horizontal); + if (myAvatar.getEnableStepResetRotation()) { + myAvatar.setHeadControllerFacingMovingAverage(myAvatar._headControllerFacing); + } } } else { if (!isActive(Horizontal) && (shouldActivateHorizontal(myAvatar, desiredBodyMatrix, currentBodyMatrix) || hasDriveInput)) { activate(Horizontal); + if (myAvatar.getEnableStepResetRotation()) { + myAvatar.setHeadControllerFacingMovingAverage(myAvatar._headControllerFacing); + } } } diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 0a40ac471c..c0f0166e49 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -198,6 +198,8 @@ class MyAvatar : public Avatar { Q_PROPERTY(bool hasAudioEnabledFaceMovement READ getHasAudioEnabledFaceMovement WRITE setHasAudioEnabledFaceMovement) Q_PROPERTY(float rotationRecenterFilterLength READ getRotationRecenterFilterLength WRITE setRotationRecenterFilterLength) Q_PROPERTY(float rotationThreshold READ getRotationThreshold WRITE setRotationThreshold) + Q_PROPERTY(bool enableStepResetRotation READ getEnableStepResetRotation WRITE setEnableStepResetRotation) + Q_PROPERTY(bool enableDrawAverageFacing READ getEnableDrawAverageFacing WRITE setEnableDrawAverageFacing) //TODO: make gravity feature work Q_PROPERTY(glm::vec3 gravity READ getGravity WRITE setGravity) Q_PROPERTY(glm::vec3 leftHandPosition READ getLeftHandPosition) @@ -1464,6 +1466,10 @@ private: float getRotationRecenterFilterLength() const { return _rotationRecenterFilterLength; } void setRotationThreshold(float angleRadians); float getRotationThreshold() const { return _rotationThreshold; } + void setEnableStepResetRotation(bool stepReset) { _stepResetRotationEnabled = stepReset; } + bool getEnableStepResetRotation() const { return _stepResetRotationEnabled; } + void setEnableDrawAverageFacing(bool drawAverage) { _drawAverageFacingEnabled = drawAverage; } + bool getEnableDrawAverageFacing() const { return _drawAverageFacingEnabled; } bool isMyAvatar() const override { return true; } virtual int parseDataFromBuffer(const QByteArray& buffer) override; virtual glm::vec3 getSkeletonPosition() const override; @@ -1574,6 +1580,8 @@ private: std::atomic _hasScriptedBlendShapes { false }; std::atomic _rotationRecenterFilterLength { 4.0f }; std::atomic _rotationThreshold { 0.5235f }; // 30 degrees in radians + std::atomic _stepResetRotationEnabled { false }; + std::atomic _drawAverageFacingEnabled { false }; // working copy -- see AvatarData for thread-safe _sensorToWorldMatrixCache, used for outward facing access glm::mat4 _sensorToWorldMatrix { glm::mat4() }; From a8c4f4e6507913a8fde6fce37f14418f4a11027c Mon Sep 17 00:00:00 2001 From: amantley Date: Wed, 18 Jul 2018 17:10:09 -0700 Subject: [PATCH 03/24] removed extra whitespace --- interface/src/avatar/MyAvatar.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 63a26fb2ad..34d5017c50 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -456,11 +456,11 @@ void MyAvatar::update(float deltaTime) { _recentModeReadings.insert(newHeightReadingInCentimeters); setCurrentStandingHeight(computeStandingHeightMode(getControllerPoseInAvatarFrame(controller::Action::HEAD))); setAverageHeadRotation(computeAverageHeadRotation(getControllerPoseInAvatarFrame(controller::Action::HEAD))); - + glm::vec2 bodyFacing = getFacingDir2D(_bodySensorMatrix); _headFacingBuffer.insert(_headControllerFacing); //qCDebug(interfaceapp) << "the body facing is " << -bodyFacing.x << " " << -bodyFacing.y << " the head controller facing is " << _headControllerFacing.x << " " << _headControllerFacing.y; - + float sumHeadFacingX = 0.0f; float sumHeadFacingY = 0.0f; for (auto headFacingIterator = _headFacingBuffer.begin(); headFacingIterator != _headFacingBuffer.end(); ++headFacingIterator) { From 5ec7ab3a7d10f50eaefd4638852d68af8b365d6b Mon Sep 17 00:00:00 2001 From: amantley Date: Wed, 18 Jul 2018 17:20:47 -0700 Subject: [PATCH 04/24] removed the other way of computing the average lookat which used a ring buffer --- interface/src/avatar/MyAvatar.cpp | 17 +---------------- interface/src/avatar/MyAvatar.h | 2 -- 2 files changed, 1 insertion(+), 18 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 34d5017c50..10e38afbe3 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -89,7 +89,6 @@ const float MyAvatar::ZOOM_MAX = 25.0f; const float MyAvatar::ZOOM_DEFAULT = 1.5f; const float MIN_SCALE_CHANGED_DELTA = 0.001f; const int MODE_READINGS_RING_BUFFER_SIZE = 500; -const int HEAD_FACING_RING_BUFFER_SIZE = 250; const float CENTIMETERS_PER_METER = 100.0f; #define DEBUG_DRAW_HMD_MOVING_AVERAGE @@ -113,7 +112,6 @@ MyAvatar::MyAvatar(QThread* thread) : _hmdSensorMatrix(), _hmdSensorOrientation(), _hmdSensorPosition(), - _headFacingBuffer(HEAD_FACING_RING_BUFFER_SIZE), _recentModeReadings(MODE_READINGS_RING_BUFFER_SIZE), _bodySensorMatrix(), _goToPending(false), @@ -457,25 +455,14 @@ void MyAvatar::update(float deltaTime) { setCurrentStandingHeight(computeStandingHeightMode(getControllerPoseInAvatarFrame(controller::Action::HEAD))); setAverageHeadRotation(computeAverageHeadRotation(getControllerPoseInAvatarFrame(controller::Action::HEAD))); - glm::vec2 bodyFacing = getFacingDir2D(_bodySensorMatrix); - _headFacingBuffer.insert(_headControllerFacing); - //qCDebug(interfaceapp) << "the body facing is " << -bodyFacing.x << " " << -bodyFacing.y << " the head controller facing is " << _headControllerFacing.x << " " << _headControllerFacing.y; - float sumHeadFacingX = 0.0f; - float sumHeadFacingY = 0.0f; - for (auto headFacingIterator = _headFacingBuffer.begin(); headFacingIterator != _headFacingBuffer.end(); ++headFacingIterator) { - sumHeadFacingX += (*headFacingIterator).x; - sumHeadFacingY += (*headFacingIterator).y; - } - _averageFacing.x = sumHeadFacingX / (float)_headFacingBuffer.getNumEntries(); - _averageFacing.y= sumHeadFacingY / (float)_headFacingBuffer.getNumEntries(); + #ifdef DEBUG_DRAW_HMD_MOVING_AVERAGE if (_drawAverageFacingEnabled) { auto sensorHeadPose = getControllerPoseInSensorFrame(controller::Action::HEAD); glm::vec3 worldHeadPos = transformPoint(getSensorToWorldMatrix(), sensorHeadPose.getTranslation()); glm::vec3 worldFacingAverage = transformVectorFast(getSensorToWorldMatrix(), glm::vec3(_headControllerFacingMovingAverage.x, 0.0f, _headControllerFacingMovingAverage.y)); - //glm::vec3 worldFacingAverage = transformVectorFast(getSensorToWorldMatrix(), glm::vec3(_averageFacing.x, 0.0f, _averageFacing.y)); glm::vec3 worldFacing = transformVectorFast(getSensorToWorldMatrix(), glm::vec3(_headControllerFacing.x, 0.0f, _headControllerFacing.y)); DebugDraw::getInstance().drawRay(worldHeadPos, worldHeadPos + worldFacing, glm::vec4(0.0f, 1.0f, 0.0f, 1.0f)); DebugDraw::getInstance().drawRay(worldHeadPos, worldHeadPos + worldFacingAverage, glm::vec4(0.0f, 0.0f, 1.0f, 1.0f)); @@ -3516,8 +3503,6 @@ bool MyAvatar::FollowHelper::shouldActivateRotation(const MyAvatar& myAvatar, co const float FOLLOW_ROTATION_THRESHOLD = cosf(myAvatar.getRotationThreshold()); glm::vec2 bodyFacing = getFacingDir2D(currentBodyMatrix); return glm::dot(-myAvatar.getHeadControllerFacingMovingAverage(), bodyFacing) < FOLLOW_ROTATION_THRESHOLD; - - //return glm::dot(myAvatar._averageFacing, -bodyFacing) < FOLLOW_ROTATION_THRESHOLD; } bool MyAvatar::FollowHelper::shouldActivateHorizontal(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) const { diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index c0f0166e49..81315900a7 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -1593,8 +1593,6 @@ private: // cache head controller pose in sensor space glm::vec2 _headControllerFacing; // facing vector in xz plane (sensor space) glm::vec2 _headControllerFacingMovingAverage { 0.0f, 0.0f }; // facing vector in xz plane (sensor space) - RingBufferHistory _headFacingBuffer; - vec2 _averageFacing { 0.0f, 0.0f }; glm::quat _averageHeadRotation { 0.0f, 0.0f, 0.0f, 1.0f }; float _currentStandingHeight { 0.0f }; From 94026fb4dda683a0af532fdc61e693201f2997f1 Mon Sep 17 00:00:00 2001 From: amantley Date: Mon, 20 Aug 2018 16:47:14 -0700 Subject: [PATCH 05/24] added activate(rotation) to prephysics update --- interface/src/avatar/MyAvatar.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 10e38afbe3..2a6cb0017a 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -3582,6 +3582,7 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat if (!isActive(Horizontal) && (shouldActivateHorizontalCG(myAvatar) || hasDriveInput)) { activate(Horizontal); if (myAvatar.getEnableStepResetRotation()) { + activate(Rotation); myAvatar.setHeadControllerFacingMovingAverage(myAvatar._headControllerFacing); } } @@ -3589,6 +3590,7 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat if (!isActive(Horizontal) && (shouldActivateHorizontal(myAvatar, desiredBodyMatrix, currentBodyMatrix) || hasDriveInput)) { activate(Horizontal); if (myAvatar.getEnableStepResetRotation()) { + activate(Rotation); myAvatar.setHeadControllerFacingMovingAverage(myAvatar._headControllerFacing); } } From 11c1845dfa2ca7e1e01abc663582fff876562dbb Mon Sep 17 00:00:00 2001 From: amantley Date: Tue, 21 Aug 2018 16:11:45 -0700 Subject: [PATCH 06/24] added rotate script changes that allow using the hand hip azimuth --- scripts/developer/rotateApp.js | 340 +++++++++++++++++++++++++++++++++ 1 file changed, 340 insertions(+) create mode 100644 scripts/developer/rotateApp.js diff --git a/scripts/developer/rotateApp.js b/scripts/developer/rotateApp.js new file mode 100644 index 0000000000..d65eedfbbe --- /dev/null +++ b/scripts/developer/rotateApp.js @@ -0,0 +1,340 @@ +/* global Script, Vec3, MyAvatar, Tablet, Messages, Quat, +DebugDraw, Mat4, Entities, Xform, Controller, Camera, console, document*/ + +Script.registerValue("ROTATEAPP", true); + +var TABLET_BUTTON_NAME = "ROTATE"; +var CHANGE_OF_BASIS_ROTATION = { x: 0, y: 1, z: 0, w: 0 }; +var DEFAULT_HEAD_TURN_THRESHOLD = 0.5333; +var DEFAULT_HEAD_TURN_FILTER_LENGTH = 4.0; +var LOADING_DELAY = 2000; +var AVERAGING_RATE = 0.03; +var INCREASING = 1.0; +var DECREASING = -1.0; +var DEGREES_PER_PI_RADIANS = 180.0; +var FILTER_FUDGE_RANGE = 0.9; + +var activated = false; +var documentLoaded = false; +var sciptLoaded = false; +var headPoseAverageOrientation = { x: 0, y: 0, z: 0, w: 1 }; +var hipToLeftHandAverage = 0.0; // { x: 0, y: 0, z: 0, w: 1 }; +var hipToRightHandAverage = 0.0; // { x: 0, y: 0, z: 0, w: 1 }; +var averageAzimuth = 0.0; +var hipsPositionRigSpace = { x: 0, y: 0, z: 0 }; +var spine2PositionRigSpace = { x: 0, y: 0, z: 0 }; +var hipsRotationRigSpace = { x: 0, y: 0, z: 0, w: 1 }; +var spine2RotationRigSpace = { x: 0, y: 0, z: 0, w: 1 }; +var spine2Rotation = { x: 0, y: 0, z: 0, w: 1 }; +var hipToLHandAverage = { x: 0, y: 0, z: 0, w: 1 }; +var hipToRHandAverage = { x: 0, y: 0, z: 0, w: 1 }; + +var ikTypes = { + RotationAndPosition: 0, + RotationOnly: 1, + HmdHead: 2, + HipsRelativeRotationAndPosition: 3, + Spline: 4, + Unknown: 5 +}; + + +var ANIM_VARS = [ + //"headType", + "spine2Type", + //"hipsType", + "spine2Position", + "spine2Rotation", + //"hipsPosition", + //"hipsRotation" +]; + +var handlerId = MyAvatar.addAnimationStateHandler(function (props) { + //print("in callback"); + //print("props spine2 pos: " + props.spine2Position.x + " " + props.spine2Position.y + " " + props.spine2Position.z); + //print("props hip pos: " + props.hipsPosition.x + " " + props.hipsPosition.y + " " + props.hipsPosition.z); + var result = {}; + //{x:0,y:0,z:0} + //result.headType = ikTypes.HmdHead; + //result.hipsType = ikTypes.RotationAndPosition; + //result.hipsPosition = hipsPositionRigSpace; // { x: 0, y: 0, z: 0 }; + //result.hipsRotation = hipsRotationRigSpace;//{ x: 0, y: 0, z: 0, w: 1 }; // + result.spine2Type = ikTypes.Spline; + result.spine2Position = { x: 0, y: 1.3, z: 0 }; + result.spine2Rotation = spine2Rotation; + + return result; +}, ANIM_VARS); + +// define state readings constructor +function StateReading(headPose, rhandPose, lhandPose, diffFromAverageEulers) { + this.headPose = headPose; + this.rhandPose = rhandPose; + this.lhandPose = lhandPose; + 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), + { x: 0, y: 0, z: 0 }); + +// declare the checkbox constructor +function AppCheckbox(type,id,eventType,isChecked) { + this.type = type; + this.id = id; + this.eventType = eventType; + this.data = {value: isChecked}; +} + +var usingStepResetRotationDirection = new AppCheckbox("checkboxtick", "stepReset", "onStepResetCheckBox", false); +var usingDrawAverageFacing = new AppCheckbox("checkboxtick", "drawAverage", "onDrawAverageFacingCheckBox", false); +var checkBoxArray = new Array(usingStepResetRotationDirection, usingDrawAverageFacing); + +// 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; +} + +// var HTML_URL = Script.resolvePath("file:///c:/dev/hifi_fork/hifi/scripts/developer/rotateRecenterApp.html"); +var HTML_URL = Script.resolvePath("http://hifi-content.s3.amazonaws.com/angus/stepApp/rotateRecenterApp.html"); +var tablet = Tablet.getTablet("com.highfidelity.interface.tablet.system"); + +// define the sliders +var filterLengthProperty = new AppProperty("#filterLength-slider", "slider", "onFilterLengthSlider", "filterSignal", + setFilterLength, DEFAULT_HEAD_TURN_FILTER_LENGTH, function (num) { + var base = 5; + var shift = 0; + return convertExponential(base, num, INCREASING, shift); + }, function (num) { + var base = 5; + var shift = 0; + return convertLog(base, num, INCREASING, shift); + }, true); +var angleThresholdProperty = new AppProperty("#angleThreshold-slider", "slider", "onAngleThresholdSlider", "angleSignal", + setAngleThreshold, DEFAULT_HEAD_TURN_THRESHOLD, function (num) { + return convertToRadians(num); + }, function (num) { + return convertToDegrees(num); + }, true); + +var propArray = new Array(filterLengthProperty, angleThresholdProperty); + +function setFilterLength(num) { + filterLengthProperty.value = num; + MyAvatar.rotationRecenterFilterLength = filterLengthProperty.value; + +} + +function setAngleThreshold(num) { + angleThresholdProperty.value = num; + MyAvatar.rotationThreshold = angleThresholdProperty.value; +} + +function convertToRadians(num) { + return (num / DEGREES_PER_PI_RADIANS) * Math.PI; +} + +function convertToDegrees(num) { + return (num / Math.PI) * DEGREES_PER_PI_RADIANS; +} + +function getLog(x, y) { + return Math.log(y) / Math.log(x); +} + +function convertLog(base, num, direction, shift) { + return direction * getLog(base, (num + FILTER_FUDGE_RANGE)) + shift; +} + +function convertExponential(base, num, direction, shift) { + return Math.pow(base, (direction * num + shift)) - FILTER_FUDGE_RANGE; +} + +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 onKeyPress(event) { + if (event.text === "'") { + // when the sensors are reset, then reset the mode. + } +} + +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; + if (cbox.id === "stepReset") { + MyAvatar.enableStepResetRotation = cbox.data.value; + } + if (cbox.id === "drawAverage") { + MyAvatar.enableDrawAverageFacing = cbox.data.value; + } + // break; + } + + }); + if (message.type === "onCreateRotateApp") { + 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) { + print(prop.name); + 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 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 = true; + } + activated = true; + } else { + if (activated) { + // disconnect from event bridge + tablet.webEventReceived.disconnect(onWebEventReceived); + documentLoaded = false; + } + activated = false; + } +} + +function limitAngle(angle) { + return (angle + 180) % 360 - 180; +} + +function computeHandAzimuths() { + var leftHand = currentStateReadings.lhandPose.translation; + var rightHand = currentStateReadings.rhandPose.translation; + var head = currentStateReadings.headPose.translation; + var lHandMinusHead = Vec3.subtract(leftHand, head); + lHandMinusHead.y = 0.0; + var rHandMinusHead = Vec3.subtract(rightHand, head); + rHandMinusHead.y = 0.0; + print(JSON.stringify(leftHand)); + print(JSON.stringify(head)); + var avatarZAxis = { x: 0.0, y: 0.0, z: 1.0 }; + var hipToLHand = Quat.lookAtSimple({ x: 0, y: 0, z: 0 }, lHandMinusHead); + var hipToRHand = Quat.lookAtSimple({ x: 0, y: 0, z: 0 }, rHandMinusHead); + hipToLHandAverage = Quat.slerp(hipToLHandAverage, hipToLHand, 0.99); + hipToRHandAverage = Quat.slerp(hipToRHandAverage, hipToRHand, 0.99); + + // var angleToLeft = limitAngle(Quat.safeEulerAngles(hipToLHandAverage).y); + // var angleToRight = limitAngle(Quat.safeEulerAngles(hipToRHandAverage).y); + var leftRightMidpoint = (Quat.safeEulerAngles(hipToLHandAverage).y + Quat.safeEulerAngles(hipToRHandAverage).y) / 2.0; + print(leftRightMidpoint); + + return Quat.fromVec3Degrees({ x: 0, y: leftRightMidpoint, z: 0 }); + + +} + +function update(dt) { + //update state readings + currentStateReadings.head = Controller.getPoseValue(Controller.Standard.Head); + currentStateReadings.rhandPose = Controller.getPoseValue(Controller.Standard.RightHand); + currentStateReadings.lhandPose = Controller.getPoseValue(Controller.Standard.LeftHand); + + print(JSON.stringify(currentStateReadings.head)); + + var latestSpineRotation = computeHandAzimuths(); + var zAxisSpineRotation = Vec3.multiplyQbyV(latestSpineRotation, { x: 0, y: 0, z: 1 }); + var zAxisWorldSpace = Vec3.multiplyQbyV(MyAvatar.rotation, zAxisSpineRotation); + DebugDraw.drawRay(MyAvatar.position, Vec3.sum(MyAvatar.position, zAxisSpineRotation), { x: 1, y: 0, z: 0 }); + spine2Rotation = latestSpineRotation; + /* + if (HMD.active && !scriptLoaded) { + //Script.load("rotateApp.js"); + scriptLoaded = true; + } + + if (!HMD.active) { + scriptLoaded = false; + } + */ + + // handle the azimuth of the arms + +} + +function shutdownTabletApp() { + tablet.removeButton(tabletButton); + if (activated) { + tablet.webEventReceived.disconnect(onWebEventReceived); + tablet.gotoHomeScreen(); + } + tablet.screenChanged.disconnect(onScreenChanged); +} + +Script.setTimeout(function () { + tabletButton.clicked.connect(manageClick); + tablet.screenChanged.connect(onScreenChanged); + Script.update.connect(update); + Controller.keyPressEvent.connect(onKeyPress); +}, (LOADING_DELAY)); + +Script.scriptEnding.connect(function () { + // if (handlerId) { + // print("removing animation state handler"); + // handlerId = MyAvatar.removeAnimationStateHandler(handlerId); + // } + MyAvatar.hmdLeanRecenterEnabled = true; + Script.update.disconnect(update); + shutdownTabletApp(); +}); \ No newline at end of file From d74fef2ea4c68611ca25070fab22458d00219f6f Mon Sep 17 00:00:00 2001 From: amantley Date: Tue, 21 Aug 2018 16:27:47 -0700 Subject: [PATCH 07/24] fixed spine 2 position updating --- scripts/developer/rotateApp.js | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/scripts/developer/rotateApp.js b/scripts/developer/rotateApp.js index d65eedfbbe..ea616b1697 100644 --- a/scripts/developer/rotateApp.js +++ b/scripts/developer/rotateApp.js @@ -60,7 +60,7 @@ var handlerId = MyAvatar.addAnimationStateHandler(function (props) { //result.hipsPosition = hipsPositionRigSpace; // { x: 0, y: 0, z: 0 }; //result.hipsRotation = hipsRotationRigSpace;//{ x: 0, y: 0, z: 0, w: 1 }; // result.spine2Type = ikTypes.Spline; - result.spine2Position = { x: 0, y: 1.3, z: 0 }; + result.spine2Position = spine2PositionRigSpace;// { x: 0, y: 1.3, z: 0 }; result.spine2Rotation = spine2Rotation; return result; @@ -298,6 +298,8 @@ function update(dt) { var zAxisWorldSpace = Vec3.multiplyQbyV(MyAvatar.rotation, zAxisSpineRotation); DebugDraw.drawRay(MyAvatar.position, Vec3.sum(MyAvatar.position, zAxisSpineRotation), { x: 1, y: 0, z: 0 }); spine2Rotation = latestSpineRotation; + var spine2Pos = MyAvatar.getAbsoluteJointTranslationInObjectFrame(MyAvatar.getJointIndex("Spine2")); + spine2PositionRigSpace = Vec3.multiplyQbyV(CHANGE_OF_BASIS_ROTATION, spine2Pos); /* if (HMD.active && !scriptLoaded) { //Script.load("rotateApp.js"); From 456b8bf2a927c40499cc420b44ae6a18e18070a4 Mon Sep 17 00:00:00 2001 From: amantley Date: Wed, 22 Aug 2018 09:36:52 -0700 Subject: [PATCH 08/24] tweaked the animvars callback to update the spine2 position --- interface/src/avatar/MyAvatar.cpp | 5 ++++ scripts/developer/rotateApp.js | 38 +++++++++++++++++++++---------- 2 files changed, 31 insertions(+), 12 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index c3d8b8df4f..04a6d58aae 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -3937,6 +3937,11 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat myAvatar.setHeadControllerFacingMovingAverage(myAvatar._headControllerFacing); } } + if (!isActive(Rotation) && getForceActivateRotation()) { + activate(Rotation); + myAvatar.setHeadControllerFacingMovingAverage(myAvatar._headControllerFacing); + setForceActivateRotation(false); + } } else { if (!isActive(Horizontal) && (shouldActivateHorizontal(myAvatar, desiredBodyMatrix, currentBodyMatrix) || hasDriveInput)) { activate(Horizontal); diff --git a/scripts/developer/rotateApp.js b/scripts/developer/rotateApp.js index ea616b1697..7794232196 100644 --- a/scripts/developer/rotateApp.js +++ b/scripts/developer/rotateApp.js @@ -44,7 +44,7 @@ var ANIM_VARS = [ "spine2Type", //"hipsType", "spine2Position", - "spine2Rotation", + "spine2Rotation" //"hipsPosition", //"hipsRotation" ]; @@ -60,7 +60,7 @@ var handlerId = MyAvatar.addAnimationStateHandler(function (props) { //result.hipsPosition = hipsPositionRigSpace; // { x: 0, y: 0, z: 0 }; //result.hipsRotation = hipsRotationRigSpace;//{ x: 0, y: 0, z: 0, w: 1 }; // result.spine2Type = ikTypes.Spline; - result.spine2Position = spine2PositionRigSpace;// { x: 0, y: 1.3, z: 0 }; + result.spine2Position = spine2PositionRigSpace; // { x: 0, y: 1.3, z: 0 }; result.spine2Rotation = spine2Rotation; return result; @@ -267,18 +267,34 @@ function computeHandAzimuths() { lHandMinusHead.y = 0.0; var rHandMinusHead = Vec3.subtract(rightHand, head); rHandMinusHead.y = 0.0; - print(JSON.stringify(leftHand)); - print(JSON.stringify(head)); + //print(JSON.stringify(leftHand)); + //print(JSON.stringify(head)); var avatarZAxis = { x: 0.0, y: 0.0, z: 1.0 }; var hipToLHand = Quat.lookAtSimple({ x: 0, y: 0, z: 0 }, lHandMinusHead); var hipToRHand = Quat.lookAtSimple({ x: 0, y: 0, z: 0 }, rHandMinusHead); - hipToLHandAverage = Quat.slerp(hipToLHandAverage, hipToLHand, 0.99); - hipToRHandAverage = Quat.slerp(hipToRHandAverage, hipToRHand, 0.99); + hipToLHandAverage = Quat.slerp(hipToLHandAverage, hipToLHand, AVERAGING_RATE); + hipToRHandAverage = Quat.slerp(hipToRHandAverage, hipToRHand, AVERAGING_RATE); // var angleToLeft = limitAngle(Quat.safeEulerAngles(hipToLHandAverage).y); // var angleToRight = limitAngle(Quat.safeEulerAngles(hipToRHandAverage).y); - var leftRightMidpoint = (Quat.safeEulerAngles(hipToLHandAverage).y + Quat.safeEulerAngles(hipToRHandAverage).y) / 2.0; - print(leftRightMidpoint); + var leftRightMidpoint = (Quat.safeEulerAngles(hipToLHand).y + Quat.safeEulerAngles(hipToRHand).y) / 2.0; + var leftRightMidpointAverage = (Quat.safeEulerAngles(hipToLHandAverage).y + Quat.safeEulerAngles(hipToRHandAverage).y) / 2.0; + + // limit the angle because we are flipped by 180, fix this tomorrow. + // print(leftRightMidpointAverage/180.0); + // print("threshold value " + angleThresholdProperty.value); + // get it into radians too!! + if ((Math.abs(leftRightMidpointAverage/180.0) * Math.PI) > angleThresholdProperty.value) { + print("recenter the feet under the head"); + MyAvatar.triggerRotationRecenter(); + } + + var raySpineRotation = Quat.fromVec3Degrees({ x: 0, y: leftRightMidpointAverage, z: 0 }); + var zAxisSpineRotation = Vec3.multiplyQbyV(raySpineRotation, { x: 0, y: 0, z: -1 }); + var zAxisWorldSpace = Vec3.multiplyQbyV(MyAvatar.orientation, zAxisSpineRotation); + DebugDraw.drawRay(MyAvatar.position, Vec3.sum(MyAvatar.position, zAxisWorldSpace), { x: 1, y: 0, z: 0, w: 1 }); + + //print(leftRightMidpoint); return Quat.fromVec3Degrees({ x: 0, y: leftRightMidpoint, z: 0 }); @@ -291,12 +307,10 @@ function update(dt) { currentStateReadings.rhandPose = Controller.getPoseValue(Controller.Standard.RightHand); currentStateReadings.lhandPose = Controller.getPoseValue(Controller.Standard.LeftHand); - print(JSON.stringify(currentStateReadings.head)); + //print(JSON.stringify(currentStateReadings.head)); var latestSpineRotation = computeHandAzimuths(); - var zAxisSpineRotation = Vec3.multiplyQbyV(latestSpineRotation, { x: 0, y: 0, z: 1 }); - var zAxisWorldSpace = Vec3.multiplyQbyV(MyAvatar.rotation, zAxisSpineRotation); - DebugDraw.drawRay(MyAvatar.position, Vec3.sum(MyAvatar.position, zAxisSpineRotation), { x: 1, y: 0, z: 0 }); + spine2Rotation = latestSpineRotation; var spine2Pos = MyAvatar.getAbsoluteJointTranslationInObjectFrame(MyAvatar.getJointIndex("Spine2")); spine2PositionRigSpace = Vec3.multiplyQbyV(CHANGE_OF_BASIS_ROTATION, spine2Pos); From 1e3ca6f902fc9df3cab552c5ebb9a260ddb685df Mon Sep 17 00:00:00 2001 From: amantley Date: Wed, 22 Aug 2018 12:57:06 -0700 Subject: [PATCH 09/24] added quicker follow code and added head to rotate calculation --- interface/src/avatar/MyAvatar.cpp | 21 +++++++++------- libraries/shared/src/AvatarConstants.h | 2 +- scripts/developer/rotateApp.js | 33 +++++++++++++++++++------- 3 files changed, 38 insertions(+), 18 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 04a6d58aae..c79f589b15 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -3797,7 +3797,7 @@ void MyAvatar::lateUpdatePalms() { } -static const float FOLLOW_TIME = 0.5f; +static const float FOLLOW_TIME = 0.1f; MyAvatar::FollowHelper::FollowHelper() { deactivate(); @@ -3902,8 +3902,13 @@ bool MyAvatar::FollowHelper::shouldActivateHorizontalCG(MyAvatar& myAvatar) cons glm::vec3 defaultHeadPosition = myAvatar.getAbsoluteDefaultJointTranslationInObjectFrame(myAvatar.getJointIndex("Head")); glm::vec3 currentHeadPosition = currentHeadPose.getTranslation(); float anatomicalHeadToHipsDistance = glm::length(defaultHeadPosition - defaultHipsPosition); + //if (!isActive(Horizontal) && + // (glm::length(currentHeadPosition - defaultHipsPosition) > (anatomicalHeadToHipsDistance + DEFAULT_AVATAR_SPINE_STRETCH_LIMIT))) { + // myAvatar.setResetMode(true); + // stepDetected = true; + //} if (!isActive(Horizontal) && - (glm::length(currentHeadPosition - defaultHipsPosition) > (anatomicalHeadToHipsDistance + DEFAULT_AVATAR_SPINE_STRETCH_LIMIT))) { + (glm::length(currentHeadPosition - defaultHipsPosition) > (anatomicalHeadToHipsDistance + (DEFAULT_AVATAR_SPINE_STRETCH_LIMIT * anatomicalHeadToHipsDistance * myAvatar.getAvatarScale())))) { myAvatar.setResetMode(true); stepDetected = true; } @@ -3925,10 +3930,15 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat if (myAvatar.getHMDLeanRecenterEnabled() && qApp->getCamera().getMode() != CAMERA_MODE_MIRROR) { - if (!isActive(Rotation) && (shouldActivateRotation(myAvatar, desiredBodyMatrix, currentBodyMatrix) || hasDriveInput)) { + if (!isActive(Rotation) && getForceActivateRotation()) { activate(Rotation); myAvatar.setHeadControllerFacingMovingAverage(myAvatar._headControllerFacing); + setForceActivateRotation(false); } + //if (!isActive(Rotation) && (shouldActivateRotation(myAvatar, desiredBodyMatrix, currentBodyMatrix) || hasDriveInput)) { + // activate(Rotation); + // myAvatar.setHeadControllerFacingMovingAverage(myAvatar._headControllerFacing); + //} if (myAvatar.getCenterOfGravityModelEnabled()) { if (!isActive(Horizontal) && (shouldActivateHorizontalCG(myAvatar) || hasDriveInput)) { activate(Horizontal); @@ -3937,11 +3947,6 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat myAvatar.setHeadControllerFacingMovingAverage(myAvatar._headControllerFacing); } } - if (!isActive(Rotation) && getForceActivateRotation()) { - activate(Rotation); - myAvatar.setHeadControllerFacingMovingAverage(myAvatar._headControllerFacing); - setForceActivateRotation(false); - } } else { if (!isActive(Horizontal) && (shouldActivateHorizontal(myAvatar, desiredBodyMatrix, currentBodyMatrix) || hasDriveInput)) { activate(Horizontal); diff --git a/libraries/shared/src/AvatarConstants.h b/libraries/shared/src/AvatarConstants.h index 39117de7ef..b97f586d32 100644 --- a/libraries/shared/src/AvatarConstants.h +++ b/libraries/shared/src/AvatarConstants.h @@ -34,7 +34,7 @@ const float DEFAULT_HANDS_ANGULAR_VELOCITY_STEPPING_THRESHOLD = 3.3f; const float DEFAULT_HEAD_VELOCITY_STEPPING_THRESHOLD = 0.18f; const float DEFAULT_HEAD_PITCH_STEPPING_TOLERANCE = 7.0f; const float DEFAULT_HEAD_ROLL_STEPPING_TOLERANCE = 7.0f; -const float DEFAULT_AVATAR_SPINE_STRETCH_LIMIT = 0.07f; +const float DEFAULT_AVATAR_SPINE_STRETCH_LIMIT = 0.05f; const float DEFAULT_AVATAR_FORWARD_DAMPENING_FACTOR = 0.5f; const float DEFAULT_AVATAR_LATERAL_DAMPENING_FACTOR = 2.0f; const float DEFAULT_AVATAR_HIPS_MASS = 40.0f; diff --git a/scripts/developer/rotateApp.js b/scripts/developer/rotateApp.js index 7794232196..4d18910335 100644 --- a/scripts/developer/rotateApp.js +++ b/scripts/developer/rotateApp.js @@ -259,7 +259,7 @@ function limitAngle(angle) { return (angle + 180) % 360 - 180; } -function computeHandAzimuths() { +function computeHandAzimuths(timeElapsed) { var leftHand = currentStateReadings.lhandPose.translation; var rightHand = currentStateReadings.rhandPose.translation; var head = currentStateReadings.headPose.translation; @@ -272,8 +272,9 @@ function computeHandAzimuths() { var avatarZAxis = { x: 0.0, y: 0.0, z: 1.0 }; var hipToLHand = Quat.lookAtSimple({ x: 0, y: 0, z: 0 }, lHandMinusHead); var hipToRHand = Quat.lookAtSimple({ x: 0, y: 0, z: 0 }, rHandMinusHead); - hipToLHandAverage = Quat.slerp(hipToLHandAverage, hipToLHand, AVERAGING_RATE); - hipToRHandAverage = Quat.slerp(hipToRHandAverage, hipToRHand, AVERAGING_RATE); + var tau = timeElapsed / filterLengthProperty.value; + hipToLHandAverage = Quat.slerp(hipToLHandAverage, hipToLHand, tau); + hipToRHandAverage = Quat.slerp(hipToRHandAverage, hipToRHand, tau); // var angleToLeft = limitAngle(Quat.safeEulerAngles(hipToLHandAverage).y); // var angleToRight = limitAngle(Quat.safeEulerAngles(hipToRHandAverage).y); @@ -283,11 +284,7 @@ function computeHandAzimuths() { // limit the angle because we are flipped by 180, fix this tomorrow. // print(leftRightMidpointAverage/180.0); // print("threshold value " + angleThresholdProperty.value); - // get it into radians too!! - if ((Math.abs(leftRightMidpointAverage/180.0) * Math.PI) > angleThresholdProperty.value) { - print("recenter the feet under the head"); - MyAvatar.triggerRotationRecenter(); - } + var raySpineRotation = Quat.fromVec3Degrees({ x: 0, y: leftRightMidpointAverage, z: 0 }); var zAxisSpineRotation = Vec3.multiplyQbyV(raySpineRotation, { x: 0, y: 0, z: -1 }); @@ -295,6 +292,22 @@ function computeHandAzimuths() { DebugDraw.drawRay(MyAvatar.position, Vec3.sum(MyAvatar.position, zAxisWorldSpace), { x: 1, y: 0, z: 0, w: 1 }); //print(leftRightMidpoint); + + var headPoseRigSpace = Quat.multiply(CHANGE_OF_BASIS_ROTATION, currentStateReadings.head.rotation); + headPoseAverageOrientation = Quat.slerp(headPoseAverageOrientation, headPoseRigSpace, tau); + var headPoseAverageEulers = Quat.safeEulerAngles(headPoseAverageOrientation); + + // get it into radians too!! + // average head and hands 50/50 + print("hands azimuth " + leftRightMidpointAverage + " head azimuth " + headPoseAverageEulers.y) + var headPlusHands = (leftRightMidpointAverage + headPoseAverageEulers.y) / 2.0; + if ((Math.abs(headPlusHands / 180.0) * Math.PI) > angleThresholdProperty.value) { + print("recenter the feet under the head"); + MyAvatar.triggerRotationRecenter(); + hipToLHandAverage = { x: 0, y: 0, z: 0, w: 1 }; + hipToRHandAverage = { x: 0, y: 0, z: 0, w: 1 }; + headPoseAverageOrientation = { x: 0, y: 0, z: 0, w: 1 }; + } return Quat.fromVec3Degrees({ x: 0, y: leftRightMidpoint, z: 0 }); @@ -309,7 +322,9 @@ function update(dt) { //print(JSON.stringify(currentStateReadings.head)); - var latestSpineRotation = computeHandAzimuths(); + + + var latestSpineRotation = computeHandAzimuths(dt); spine2Rotation = latestSpineRotation; var spine2Pos = MyAvatar.getAbsoluteJointTranslationInObjectFrame(MyAvatar.getJointIndex("Spine2")); From 9dfaf32b7240f22e88ea611966e3a4373b2d552a Mon Sep 17 00:00:00 2001 From: amantley Date: Wed, 22 Aug 2018 14:12:19 -0700 Subject: [PATCH 10/24] fixed the step animation but it still needs work --- interface/src/avatar/MyAvatar.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 46a5cbe414..44ee5e30c6 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -3933,6 +3933,9 @@ bool MyAvatar::FollowHelper::shouldActivateVertical(const MyAvatar& myAvatar, co void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix, bool hasDriveInput) { + const float VELOCITY_THRESHHOLD = 1.0f; + float currentVelocity = glm::length(myAvatar.getLocalVelocity() / myAvatar.getSensorToWorldScale()); + if (myAvatar.getHMDLeanRecenterEnabled() && qApp->getCamera().getMode() != CAMERA_MODE_MIRROR) { @@ -3946,7 +3949,7 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat // myAvatar.setHeadControllerFacingMovingAverage(myAvatar._headControllerFacing); //} if (myAvatar.getCenterOfGravityModelEnabled()) { - if (!isActive(Horizontal) && (shouldActivateHorizontalCG(myAvatar) || hasDriveInput)) { + if ((!isActive(Horizontal) && (shouldActivateHorizontalCG(myAvatar) || hasDriveInput)) || (isActive(Horizontal) && (currentVelocity > VELOCITY_THRESHHOLD))) { activate(Horizontal); if (myAvatar.getEnableStepResetRotation()) { activate(Rotation); From 874d9251e76996f1371c4d755db598bb66972906 Mon Sep 17 00:00:00 2001 From: amantley Date: Wed, 22 Aug 2018 17:32:46 -0700 Subject: [PATCH 11/24] demo version, next to implement in c++ --- scripts/developer/rotateApp.js | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) diff --git a/scripts/developer/rotateApp.js b/scripts/developer/rotateApp.js index 4d18910335..60d78d66d9 100644 --- a/scripts/developer/rotateApp.js +++ b/scripts/developer/rotateApp.js @@ -260,6 +260,15 @@ function limitAngle(angle) { } function computeHandAzimuths(timeElapsed) { + + // var leftHandPositionRigSpace = Vec3.multiplyQbyV(Quat.inverse(CHANGE_OF_BASIS_ROTATION), currentStateReadings.lhandPose.translation); + // var rightHandPositionRigSpace = Vec3.multiplyQbyV(Quat.inverse(CHANGE_OF_BASIS_ROTATION), currentStateReadings.rhandPose.translation); + + + // var hipToLeftHand = Quat.lookAtSimple({ x: 0, y: 0, z: 0 }, { x: leftHandPositionRigSpace.x, y: 0, z: leftHandPositionRigSpace.z }); + // var hipToRightHand = Quat.lookAtSimple({ x: 0, y: 0, z: 0 }, { x: rightHandPositionRigSpace.x, y: 0, z: rightHandPositionRigSpace.z }); + // var hipToHandHalfway = Quat.slerp(hipToLeftHand, hipToRightHand, 0.5); + var leftHand = currentStateReadings.lhandPose.translation; var rightHand = currentStateReadings.rhandPose.translation; var head = currentStateReadings.headPose.translation; @@ -299,17 +308,21 @@ function computeHandAzimuths(timeElapsed) { // get it into radians too!! // average head and hands 50/50 - print("hands azimuth " + leftRightMidpointAverage + " head azimuth " + headPoseAverageEulers.y) + // print("hands azimuth " + leftRightMidpointAverage + " head azimuth " + headPoseAverageEulers.y); var headPlusHands = (leftRightMidpointAverage + headPoseAverageEulers.y) / 2.0; if ((Math.abs(headPlusHands / 180.0) * Math.PI) > angleThresholdProperty.value) { - print("recenter the feet under the head"); + //print("recenter the feet under the head"); MyAvatar.triggerRotationRecenter(); hipToLHandAverage = { x: 0, y: 0, z: 0, w: 1 }; hipToRHandAverage = { x: 0, y: 0, z: 0, w: 1 }; headPoseAverageOrientation = { x: 0, y: 0, z: 0, w: 1 }; } - return Quat.fromVec3Degrees({ x: 0, y: leftRightMidpoint, z: 0 }); + // put a hard max on this easing function. + var rotateAngle = ((Math.cos((leftRightMidpoint / 180.0) * Math.PI) + 2.0)/3.0) * leftRightMidpoint; + print("rotate angle " + rotateAngle); + + return Quat.fromVec3Degrees({ x: 0, y: rotateAngle, z: 0 }); } From 30ef544c6debac94cf1df5a0f5ee714c029143a9 Mon Sep 17 00:00:00 2001 From: Angus Antley Date: Thu, 23 Aug 2018 15:57:14 +0100 Subject: [PATCH 12/24] starting the c++ version of shoulder rotation --- interface/src/avatar/MyAvatar.cpp | 11 +++++++++++ interface/src/avatar/MyAvatar.h | 5 +++++ 2 files changed, 16 insertions(+) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 44ee5e30c6..6211e6f0d8 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -801,6 +801,17 @@ void MyAvatar::updateFromHMDSensorMatrix(const glm::mat4& hmdSensorMatrix) { } } +void MyAvatar::computeHandAzimuth() { + controller::Pose leftHandPoseAvatarSpace = getLeftHandPose(); + controller::Pose rightHandPoseAvatarSpace = getRightHandPose(); + glm::vec3 localLookat(0.0f, 0.0f, 1.0f); + glm::vec3 rightHandRigSpace = rightHandPoseAvatarSpace.rotation * localLookat; + glm::vec3 lefttHandRigSpace = leftHandPoseAvatarSpace.rotation * localLookat; + + _hipToHandController = glm::vec2(rightHandRigSpace.x, rightHandRigSpace.z); + +} + void MyAvatar::updateJointFromController(controller::Action poseKey, ThreadSafeValueCache& matrixCache) { assert(QThread::currentThread() == thread()); auto userInputMapper = DependencyManager::get(); diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 10b5884d5a..6b7c4e83a1 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -320,6 +320,9 @@ public: // as it moves through the world. void updateFromHMDSensorMatrix(const glm::mat4& hmdSensorMatrix); + // compute the hip to hand average azimuth. + void computeHandAzimuth(); + // read the location of a hand controller and save the transform void updateJointFromController(controller::Action poseKey, ThreadSafeValueCache& matrixCache); @@ -1658,6 +1661,8 @@ private: glm::vec2 _headControllerFacingMovingAverage { 0.0f, 0.0f }; // facing vector in xz plane (sensor space) glm::quat _averageHeadRotation { 0.0f, 0.0f, 0.0f, 1.0f }; + glm::vec2 _hipToHandController; // facing vector in xz plane (sensor space) + float _currentStandingHeight { 0.0f }; bool _resetMode { true }; RingBufferHistory _recentModeReadings; From 0e0561bb24bfbf93f62b71ef18276e15d8c415fa Mon Sep 17 00:00:00 2001 From: amantley Date: Thu, 23 Aug 2018 17:59:18 -0700 Subject: [PATCH 13/24] latest rotate code --- interface/src/avatar/MyAvatar.cpp | 20 +++++++++++++++----- scripts/developer/rotateApp.js | 2 +- 2 files changed, 16 insertions(+), 6 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 6211e6f0d8..8d9c503e5e 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -462,7 +462,7 @@ void MyAvatar::update(float deltaTime) { setCurrentStandingHeight(computeStandingHeightMode(getControllerPoseInAvatarFrame(controller::Action::HEAD))); setAverageHeadRotation(computeAverageHeadRotation(getControllerPoseInAvatarFrame(controller::Action::HEAD))); - + computeHandAzimuth(); #ifdef DEBUG_DRAW_HMD_MOVING_AVERAGE @@ -473,6 +473,14 @@ void MyAvatar::update(float deltaTime) { glm::vec3 worldFacing = transformVectorFast(getSensorToWorldMatrix(), glm::vec3(_headControllerFacing.x, 0.0f, _headControllerFacing.y)); DebugDraw::getInstance().drawRay(worldHeadPos, worldHeadPos + worldFacing, glm::vec4(0.0f, 1.0f, 0.0f, 1.0f)); DebugDraw::getInstance().drawRay(worldHeadPos, worldHeadPos + worldFacingAverage, glm::vec4(0.0f, 0.0f, 1.0f, 1.0f)); + + // draw hand azimuth vector + + glm::vec3 worldRHandAzimuth = transformPoint(getTransform().getMatrix(), getControllerPoseInAvatarFrame(controller::Action::RIGHT_HAND).getTranslation()); + glm::vec3 worldLHandAzimuth = transformPoint(getTransform().getMatrix(), getControllerPoseInAvatarFrame(controller::Action::LEFT_HAND).getTranslation()); + qCDebug(interfaceapp) << "the right hand in avatar space" << worldRHandAzimuth << " " << getWorldPosition(); + DebugDraw::getInstance().drawRay(getWorldPosition(), worldRHandAzimuth, glm::vec4(0.0f, 1.0f, 0.0f, 1.0f)); + DebugDraw::getInstance().drawRay(getWorldPosition(), worldLHandAzimuth, glm::vec4(0.0f, 1.0f, 0.0f, 1.0f)); } #endif @@ -802,11 +810,13 @@ void MyAvatar::updateFromHMDSensorMatrix(const glm::mat4& hmdSensorMatrix) { } void MyAvatar::computeHandAzimuth() { - controller::Pose leftHandPoseAvatarSpace = getLeftHandPose(); - controller::Pose rightHandPoseAvatarSpace = getRightHandPose(); + auto leftHandPoseAvatarSpace = getLeftHandPose(); + auto rightHandPoseAvatarSpace = getRightHandPose(); glm::vec3 localLookat(0.0f, 0.0f, 1.0f); - glm::vec3 rightHandRigSpace = rightHandPoseAvatarSpace.rotation * localLookat; - glm::vec3 lefttHandRigSpace = leftHandPoseAvatarSpace.rotation * localLookat; + glm::vec3 rightHandRigSpace = rightHandPoseAvatarSpace.translation;// transformVectorFast(getTransform().getMatrix(), rightHandPoseAvatarSpace.translation); + glm::vec3 leftHandRigSpace = leftHandPoseAvatarSpace.translation;// transformVectorFast(getTransform().getMatrix(), leftHandPoseAvatarSpace.translation); + + //qCDebug(interfaceapp) << "the right hand in avatar space" << rightHandRigSpace; _hipToHandController = glm::vec2(rightHandRigSpace.x, rightHandRigSpace.z); diff --git a/scripts/developer/rotateApp.js b/scripts/developer/rotateApp.js index 60d78d66d9..e4080c9bb0 100644 --- a/scripts/developer/rotateApp.js +++ b/scripts/developer/rotateApp.js @@ -320,7 +320,7 @@ function computeHandAzimuths(timeElapsed) { // put a hard max on this easing function. var rotateAngle = ((Math.cos((leftRightMidpoint / 180.0) * Math.PI) + 2.0)/3.0) * leftRightMidpoint; - print("rotate angle " + rotateAngle); + //print("rotate angle " + rotateAngle); return Quat.fromVec3Degrees({ x: 0, y: rotateAngle, z: 0 }); From 44a6c7df5f1b648796d8f45fc419acea8c774873 Mon Sep 17 00:00:00 2001 From: Angus Antley Date: Fri, 24 Aug 2018 07:17:17 +0100 Subject: [PATCH 14/24] added hook to myskeleton model to update spine2, need to get the rotation from myavatar then we have spine rot --- interface/src/avatar/MyAvatar.cpp | 4 +++- interface/src/avatar/MySkeletonModel.cpp | 8 ++++++++ 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 8d9c503e5e..2771f567b3 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -476,11 +476,13 @@ void MyAvatar::update(float deltaTime) { // draw hand azimuth vector + glm::vec3 handAzimuthMidpoint = transformPoint(getTransform().getMatrix(), glm::vec3(_hipToHandController.x, 0.0f, _hipToHandController.y)); glm::vec3 worldRHandAzimuth = transformPoint(getTransform().getMatrix(), getControllerPoseInAvatarFrame(controller::Action::RIGHT_HAND).getTranslation()); glm::vec3 worldLHandAzimuth = transformPoint(getTransform().getMatrix(), getControllerPoseInAvatarFrame(controller::Action::LEFT_HAND).getTranslation()); qCDebug(interfaceapp) << "the right hand in avatar space" << worldRHandAzimuth << " " << getWorldPosition(); DebugDraw::getInstance().drawRay(getWorldPosition(), worldRHandAzimuth, glm::vec4(0.0f, 1.0f, 0.0f, 1.0f)); DebugDraw::getInstance().drawRay(getWorldPosition(), worldLHandAzimuth, glm::vec4(0.0f, 1.0f, 0.0f, 1.0f)); + DebugDraw::getInstance().drawRay(getWorldPosition(), handAzimuthMidpoint, glm::vec4(0.0f, 1.0f, 1.0f, 1.0f)); } #endif @@ -818,7 +820,7 @@ void MyAvatar::computeHandAzimuth() { //qCDebug(interfaceapp) << "the right hand in avatar space" << rightHandRigSpace; - _hipToHandController = glm::vec2(rightHandRigSpace.x, rightHandRigSpace.z); + _hipToHandController = lerp(glm::vec2(rightHandRigSpace.x, rightHandRigSpace.z), glm::vec2(leftHandRigSpace.x, leftHandRigSpace.z),0.5f); } diff --git a/interface/src/avatar/MySkeletonModel.cpp b/interface/src/avatar/MySkeletonModel.cpp index 0fc5e7521e..491fab466d 100644 --- a/interface/src/avatar/MySkeletonModel.cpp +++ b/interface/src/avatar/MySkeletonModel.cpp @@ -238,6 +238,14 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { params.primaryControllerPoses[Rig::PrimaryControllerType_Hips] = sensorToRigPose * hips; params.primaryControllerFlags[Rig::PrimaryControllerType_Hips] = (uint8_t)Rig::ControllerFlags::Enabled | (uint8_t)Rig::ControllerFlags::Estimated; + + // spine 2 hack to be improved. + AnimPose newSpinePose; + bool fred = _rig.getAbsoluteJointPoseInRigFrame(_rig.indexOfJoint("Spine2"), newSpinePose); + newSpinePose.rot() = glm::quat(0.7071f, 0.0f, 0.0f, 0.7071f)*newSpinePose.rot(); + params.primaryControllerPoses[Rig::PrimaryControllerType_Spine2] = newSpinePose; + params.primaryControllerFlags[Rig::PrimaryControllerType_Spine2] = (uint8_t)Rig::ControllerFlags::Enabled | (uint8_t)Rig::ControllerFlags::Estimated; + } else { _prevHipsValid = false; } From 25610c75876ba7db1de50f26855582d05a4a9ac2 Mon Sep 17 00:00:00 2001 From: Angus Antley Date: Fri, 24 Aug 2018 11:04:36 +0100 Subject: [PATCH 15/24] got the hands turning the shoulders, needs error checking --- interface/src/avatar/MyAvatar.cpp | 15 +++++++++++++++ interface/src/avatar/MyAvatar.h | 2 ++ interface/src/avatar/MySkeletonModel.cpp | 5 ++--- 3 files changed, 19 insertions(+), 3 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 2771f567b3..97179f5d22 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -3349,6 +3349,21 @@ glm::mat4 MyAvatar::deriveBodyFromHMDSensor() const { return createMatFromQuatAndPos(headOrientationYawOnly, bodyPos); } +glm::mat4 MyAvatar::getSpine2RotationRigSpace() const { + + static const glm::quat RIG_CHANGE_OF_BASIS = Quaternions::Y_180; + glm::vec3 hipToHandRigSpace = RIG_CHANGE_OF_BASIS * glm::vec3(_hipToHandController.x, 0.0f, _hipToHandController.y); + + //to do: check for zero before normalizing. + glm::vec3 u, v, w; + generateBasisVectors(glm::vec3(0.0f,1.0f,0.0f), hipToHandRigSpace, u, v, w); + glm::mat4 spine2RigSpace(glm::vec4(w, 0.0f), glm::vec4(u, 0.0f), glm::vec4(v, 0.0f), glm::vec4(glm::vec3(0.0f, 0.0f, 0.0f), 1.0f)); + + //glm::quat spine2RigSpace = glm::quat_cast(glm::lookAt(glm::vec3(0.0f, 0.0f, 0.0f), glm::normalize(hipToHandRigSpace), glm::vec3(0.0f, 1.0f, 0.0f))); + + return spine2RigSpace; +} + // ease in function for dampening cg movement static float slope(float num) { const float CURVE_CONSTANT = 1.0f; diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 6b7c4e83a1..3c951264e8 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -1050,6 +1050,8 @@ public: // results are in sensor frame (-z forward) glm::mat4 deriveBodyFromHMDSensor() const; + glm::mat4 getSpine2RotationRigSpace() const; + glm::vec3 computeCounterBalance(); // derive avatar body position and orientation from using the current HMD Sensor location in relation to the previous diff --git a/interface/src/avatar/MySkeletonModel.cpp b/interface/src/avatar/MySkeletonModel.cpp index 491fab466d..5272b1499d 100644 --- a/interface/src/avatar/MySkeletonModel.cpp +++ b/interface/src/avatar/MySkeletonModel.cpp @@ -240,9 +240,8 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { // spine 2 hack to be improved. - AnimPose newSpinePose; - bool fred = _rig.getAbsoluteJointPoseInRigFrame(_rig.indexOfJoint("Spine2"), newSpinePose); - newSpinePose.rot() = glm::quat(0.7071f, 0.0f, 0.0f, 0.7071f)*newSpinePose.rot(); + AnimPose newSpinePose(myAvatar->getSpine2RotationRigSpace()); + //newSpinePose.rot() = myAvatar->getSpine2RotationRigSpace();// *newSpinePose.rot(); params.primaryControllerPoses[Rig::PrimaryControllerType_Spine2] = newSpinePose; params.primaryControllerFlags[Rig::PrimaryControllerType_Spine2] = (uint8_t)Rig::ControllerFlags::Enabled | (uint8_t)Rig::ControllerFlags::Estimated; From a60ab863a39a78653a680b06d3886f2ca5d5c646 Mon Sep 17 00:00:00 2001 From: amantley Date: Fri, 24 Aug 2018 11:06:36 -0700 Subject: [PATCH 16/24] demo version of the head and hands determining the rotate function --- interface/src/avatar/MyAvatar.cpp | 45 +++++++++++++++--------- interface/src/avatar/MyAvatar.h | 2 +- interface/src/avatar/MySkeletonModel.cpp | 5 ++- scripts/developer/rotateApp.js | 7 ++-- 4 files changed, 38 insertions(+), 21 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 97179f5d22..a1e30144ee 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -449,7 +449,18 @@ void MyAvatar::update(float deltaTime) { const float HMD_FACING_TIMESCALE = getRotationRecenterFilterLength(); float tau = deltaTime / HMD_FACING_TIMESCALE; - _headControllerFacingMovingAverage = lerp(_headControllerFacingMovingAverage, _headControllerFacing, tau); + + // put the spine facing in sensor space. + // then mix it with head facing to determine rotation recenter + glm::vec3 handHipAzimuthWorldSpace = transformVectorFast(getTransform().getMatrix(), glm::vec3(_hipToHandController.x, 0.0f, _hipToHandController.y)); + glm::mat4 sensorToWorldMat = getSensorToWorldMatrix(); + glm::mat4 worldToSensorMat = glm::inverse(sensorToWorldMat); + glm::vec3 handHipAzimuthSensorSpace = transformVectorFast(worldToSensorMat, glm::vec3(handHipAzimuthWorldSpace.x, 0.0f, handHipAzimuthWorldSpace.z)); + glm::vec2 normedHandHip = glm::normalize(glm::vec2(handHipAzimuthSensorSpace.x, handHipAzimuthSensorSpace.z)); + glm::vec2 headFacingPlusHandHipAzimuthMix = lerp(normedHandHip, _headControllerFacing, 0.5f); + //qCDebug(interfaceapp) << "the hand hip azimuth " << normedHandHip << "the head look at" << _headControllerFacing; + _headControllerFacingMovingAverage = lerp(_headControllerFacingMovingAverage, headFacingPlusHandHipAzimuthMix, tau); + qCDebug(interfaceapp) << "the hand hip azimuth average " << _headControllerFacingMovingAverage; if (_smoothOrientationTimer < SMOOTH_TIME_ORIENTATION) { _rotationChanged = usecTimestampNow(); @@ -479,7 +490,7 @@ void MyAvatar::update(float deltaTime) { glm::vec3 handAzimuthMidpoint = transformPoint(getTransform().getMatrix(), glm::vec3(_hipToHandController.x, 0.0f, _hipToHandController.y)); glm::vec3 worldRHandAzimuth = transformPoint(getTransform().getMatrix(), getControllerPoseInAvatarFrame(controller::Action::RIGHT_HAND).getTranslation()); glm::vec3 worldLHandAzimuth = transformPoint(getTransform().getMatrix(), getControllerPoseInAvatarFrame(controller::Action::LEFT_HAND).getTranslation()); - qCDebug(interfaceapp) << "the right hand in avatar space" << worldRHandAzimuth << " " << getWorldPosition(); + //qCDebug(interfaceapp) << "the right hand in avatar space" << worldRHandAzimuth << " " << getWorldPosition(); DebugDraw::getInstance().drawRay(getWorldPosition(), worldRHandAzimuth, glm::vec4(0.0f, 1.0f, 0.0f, 1.0f)); DebugDraw::getInstance().drawRay(getWorldPosition(), worldLHandAzimuth, glm::vec4(0.0f, 1.0f, 0.0f, 1.0f)); DebugDraw::getInstance().drawRay(getWorldPosition(), handAzimuthMidpoint, glm::vec4(0.0f, 1.0f, 1.0f, 1.0f)); @@ -815,13 +826,17 @@ void MyAvatar::computeHandAzimuth() { auto leftHandPoseAvatarSpace = getLeftHandPose(); auto rightHandPoseAvatarSpace = getRightHandPose(); glm::vec3 localLookat(0.0f, 0.0f, 1.0f); - glm::vec3 rightHandRigSpace = rightHandPoseAvatarSpace.translation;// transformVectorFast(getTransform().getMatrix(), rightHandPoseAvatarSpace.translation); - glm::vec3 leftHandRigSpace = leftHandPoseAvatarSpace.translation;// transformVectorFast(getTransform().getMatrix(), leftHandPoseAvatarSpace.translation); - - //qCDebug(interfaceapp) << "the right hand in avatar space" << rightHandRigSpace; - - _hipToHandController = lerp(glm::vec2(rightHandRigSpace.x, rightHandRigSpace.z), glm::vec2(leftHandRigSpace.x, leftHandRigSpace.z),0.5f); + // glm::vec3 rightHandRigSpace = rightHandPoseAvatarSpace.translation;// transformVectorFast(getTransform().getMatrix(), rightHandPoseAvatarSpace.translation); + // glm::vec3 leftHandRigSpace = leftHandPoseAvatarSpace.translation;// transformVectorFast(getTransform().getMatrix(), leftHandPoseAvatarSpace.translation); + // qCDebug(interfaceapp) << "the right hand in avatar space" << rightHandRigSpace; + if (leftHandPoseAvatarSpace.isValid() && rightHandPoseAvatarSpace.isValid()) { + _hipToHandController = lerp(glm::normalize(glm::vec2(rightHandPoseAvatarSpace.translation.x, rightHandPoseAvatarSpace.translation.z)), glm::normalize(glm::vec2(leftHandPoseAvatarSpace.translation.x, leftHandPoseAvatarSpace.translation.z)), 0.5f); + //need to use easing function here. + //var rotateAngle = ((Math.cos((leftRightMidpoint / 180.0) * Math.PI) + 2.0) / 3.0) * leftRightMidpoint; + } else { + _hipToHandController = glm::vec2(0.0f, 1.0f); + } } void MyAvatar::updateJointFromController(controller::Action poseKey, ThreadSafeValueCache& matrixCache) { @@ -3359,8 +3374,6 @@ glm::mat4 MyAvatar::getSpine2RotationRigSpace() const { generateBasisVectors(glm::vec3(0.0f,1.0f,0.0f), hipToHandRigSpace, u, v, w); glm::mat4 spine2RigSpace(glm::vec4(w, 0.0f), glm::vec4(u, 0.0f), glm::vec4(v, 0.0f), glm::vec4(glm::vec3(0.0f, 0.0f, 0.0f), 1.0f)); - //glm::quat spine2RigSpace = glm::quat_cast(glm::lookAt(glm::vec3(0.0f, 0.0f, 0.0f), glm::normalize(hipToHandRigSpace), glm::vec3(0.0f, 1.0f, 0.0f))); - return spine2RigSpace; } @@ -3977,15 +3990,15 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat if (myAvatar.getHMDLeanRecenterEnabled() && qApp->getCamera().getMode() != CAMERA_MODE_MIRROR) { - if (!isActive(Rotation) && getForceActivateRotation()) { - activate(Rotation); - myAvatar.setHeadControllerFacingMovingAverage(myAvatar._headControllerFacing); - setForceActivateRotation(false); - } - //if (!isActive(Rotation) && (shouldActivateRotation(myAvatar, desiredBodyMatrix, currentBodyMatrix) || hasDriveInput)) { + //if (!isActive(Rotation) && getForceActivateRotation()) { // activate(Rotation); // myAvatar.setHeadControllerFacingMovingAverage(myAvatar._headControllerFacing); + // setForceActivateRotation(false); //} + if (!isActive(Rotation) && (shouldActivateRotation(myAvatar, desiredBodyMatrix, currentBodyMatrix) || hasDriveInput)) { + activate(Rotation); + myAvatar.setHeadControllerFacingMovingAverage(myAvatar._headControllerFacing); + } if (myAvatar.getCenterOfGravityModelEnabled()) { if ((!isActive(Horizontal) && (shouldActivateHorizontalCG(myAvatar) || hasDriveInput)) || (isActive(Horizontal) && (currentVelocity > VELOCITY_THRESHHOLD))) { activate(Horizontal); diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 3c951264e8..177b453267 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -1663,7 +1663,7 @@ private: glm::vec2 _headControllerFacingMovingAverage { 0.0f, 0.0f }; // facing vector in xz plane (sensor space) glm::quat _averageHeadRotation { 0.0f, 0.0f, 0.0f, 1.0f }; - glm::vec2 _hipToHandController; // facing vector in xz plane (sensor space) + glm::vec2 _hipToHandController; // spine2 facing vector in xz plane (avatar space) float _currentStandingHeight { 0.0f }; bool _resetMode { true }; diff --git a/interface/src/avatar/MySkeletonModel.cpp b/interface/src/avatar/MySkeletonModel.cpp index 5272b1499d..158f9bb0b6 100644 --- a/interface/src/avatar/MySkeletonModel.cpp +++ b/interface/src/avatar/MySkeletonModel.cpp @@ -240,9 +240,12 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { // spine 2 hack to be improved. + AnimPose currentPose; + bool ret = _rig.getAbsoluteJointPoseInRigFrame(_rig.indexOfJoint("Spine2"),currentPose); AnimPose newSpinePose(myAvatar->getSpine2RotationRigSpace()); + currentPose.rot() = newSpinePose.rot(); //newSpinePose.rot() = myAvatar->getSpine2RotationRigSpace();// *newSpinePose.rot(); - params.primaryControllerPoses[Rig::PrimaryControllerType_Spine2] = newSpinePose; + params.primaryControllerPoses[Rig::PrimaryControllerType_Spine2] = currentPose; params.primaryControllerFlags[Rig::PrimaryControllerType_Spine2] = (uint8_t)Rig::ControllerFlags::Enabled | (uint8_t)Rig::ControllerFlags::Estimated; } else { diff --git a/scripts/developer/rotateApp.js b/scripts/developer/rotateApp.js index e4080c9bb0..030385b6fb 100644 --- a/scripts/developer/rotateApp.js +++ b/scripts/developer/rotateApp.js @@ -29,6 +29,7 @@ var spine2Rotation = { x: 0, y: 0, z: 0, w: 1 }; var hipToLHandAverage = { x: 0, y: 0, z: 0, w: 1 }; var hipToRHandAverage = { x: 0, y: 0, z: 0, w: 1 }; +/* var ikTypes = { RotationAndPosition: 0, RotationOnly: 1, @@ -65,7 +66,7 @@ var handlerId = MyAvatar.addAnimationStateHandler(function (props) { return result; }, ANIM_VARS); - +*/ // define state readings constructor function StateReading(headPose, rhandPose, lhandPose, diffFromAverageEulers) { this.headPose = headPose; @@ -298,7 +299,7 @@ function computeHandAzimuths(timeElapsed) { var raySpineRotation = Quat.fromVec3Degrees({ x: 0, y: leftRightMidpointAverage, z: 0 }); var zAxisSpineRotation = Vec3.multiplyQbyV(raySpineRotation, { x: 0, y: 0, z: -1 }); var zAxisWorldSpace = Vec3.multiplyQbyV(MyAvatar.orientation, zAxisSpineRotation); - DebugDraw.drawRay(MyAvatar.position, Vec3.sum(MyAvatar.position, zAxisWorldSpace), { x: 1, y: 0, z: 0, w: 1 }); + // DebugDraw.drawRay(MyAvatar.position, Vec3.sum(MyAvatar.position, zAxisWorldSpace), { x: 1, y: 0, z: 0, w: 1 }); //print(leftRightMidpoint); @@ -312,7 +313,7 @@ function computeHandAzimuths(timeElapsed) { var headPlusHands = (leftRightMidpointAverage + headPoseAverageEulers.y) / 2.0; if ((Math.abs(headPlusHands / 180.0) * Math.PI) > angleThresholdProperty.value) { //print("recenter the feet under the head"); - MyAvatar.triggerRotationRecenter(); + // MyAvatar.triggerRotationRecenter(); hipToLHandAverage = { x: 0, y: 0, z: 0, w: 1 }; hipToRHandAverage = { x: 0, y: 0, z: 0, w: 1 }; headPoseAverageOrientation = { x: 0, y: 0, z: 0, w: 1 }; From 2bec8678639e2aff14f83063be6e2154c34ee983 Mon Sep 17 00:00:00 2001 From: amantley Date: Fri, 24 Aug 2018 16:29:08 -0700 Subject: [PATCH 17/24] added the sanity check to see if the shoulders have flipped --- interface/src/avatar/MyAvatar.cpp | 19 ++++++++++++++++--- interface/src/avatar/MyAvatar.h | 2 +- 2 files changed, 17 insertions(+), 4 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index a1e30144ee..ae02eacf75 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -458,9 +458,11 @@ void MyAvatar::update(float deltaTime) { glm::vec3 handHipAzimuthSensorSpace = transformVectorFast(worldToSensorMat, glm::vec3(handHipAzimuthWorldSpace.x, 0.0f, handHipAzimuthWorldSpace.z)); glm::vec2 normedHandHip = glm::normalize(glm::vec2(handHipAzimuthSensorSpace.x, handHipAzimuthSensorSpace.z)); glm::vec2 headFacingPlusHandHipAzimuthMix = lerp(normedHandHip, _headControllerFacing, 0.5f); - //qCDebug(interfaceapp) << "the hand hip azimuth " << normedHandHip << "the head look at" << _headControllerFacing; + // if head facing dot mid point hands facing is close to -1 and the hands midpoint is close to -1 you then flip + + // qCDebug(interfaceapp) << "the hand hip azimuth " << normedHandHip << "the head look at" << _headControllerFacing; _headControllerFacingMovingAverage = lerp(_headControllerFacingMovingAverage, headFacingPlusHandHipAzimuthMix, tau); - qCDebug(interfaceapp) << "the hand hip azimuth average " << _headControllerFacingMovingAverage; + // qCDebug(interfaceapp) << "the hand hip azimuth average " << _headControllerFacingMovingAverage; if (_smoothOrientationTimer < SMOOTH_TIME_ORIENTATION) { _rotationChanged = usecTimestampNow(); @@ -831,11 +833,22 @@ void MyAvatar::computeHandAzimuth() { // qCDebug(interfaceapp) << "the right hand in avatar space" << rightHandRigSpace; if (leftHandPoseAvatarSpace.isValid() && rightHandPoseAvatarSpace.isValid()) { + vec2 oldAzimuthReading = _hipToHandController; _hipToHandController = lerp(glm::normalize(glm::vec2(rightHandPoseAvatarSpace.translation.x, rightHandPoseAvatarSpace.translation.z)), glm::normalize(glm::vec2(leftHandPoseAvatarSpace.translation.x, leftHandPoseAvatarSpace.translation.z)), 0.5f); + + //if they are 180 apart + float cosForwardAngle = glm::dot(_hipToHandController, oldAzimuthReading); + float cosBackwardAngle = glm::dot(_hipToHandController, -oldAzimuthReading); + if (cosBackwardAngle > cosForwardAngle) { + // this means we have lost continuity with the current chest position + _hipToHandController = -_hipToHandController; + } + + //need to use easing function here. //var rotateAngle = ((Math.cos((leftRightMidpoint / 180.0) * Math.PI) + 2.0) / 3.0) * leftRightMidpoint; } else { - _hipToHandController = glm::vec2(0.0f, 1.0f); + // _hipToHandController = glm::vec2(0.0f, -1.0f); } } diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 177b453267..ea5059bf39 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -1663,7 +1663,7 @@ private: glm::vec2 _headControllerFacingMovingAverage { 0.0f, 0.0f }; // facing vector in xz plane (sensor space) glm::quat _averageHeadRotation { 0.0f, 0.0f, 0.0f, 1.0f }; - glm::vec2 _hipToHandController; // spine2 facing vector in xz plane (avatar space) + glm::vec2 _hipToHandController { 0.0f, -1.0f }; // spine2 facing vector in xz plane (avatar space) float _currentStandingHeight { 0.0f }; bool _resetMode { true }; From 055f03878e1879f0b461e909e54d1cb1877379df Mon Sep 17 00:00:00 2001 From: amantley Date: Mon, 27 Aug 2018 14:44:27 -0700 Subject: [PATCH 18/24] made changes to add roll to the spine2 rotation. also only the shoulders control the rotation recenter now --- interface/src/avatar/MyAvatar.cpp | 70 +++++++++--------------- interface/src/avatar/MyAvatar.h | 4 +- interface/src/avatar/MySkeletonModel.cpp | 17 ++++-- libraries/shared/src/AvatarConstants.h | 2 +- 4 files changed, 41 insertions(+), 52 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 6012bc4419..c4d855ee77 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -447,6 +447,7 @@ void MyAvatar::reset(bool andRecenter, bool andReload, bool andHead) { void MyAvatar::update(float deltaTime) { // update moving average of HMD facing in xz plane. const float HMD_FACING_TIMESCALE = getRotationRecenterFilterLength(); + const float PERCENTAGE_WEIGHT_SHOULDERS_VS_HEAD_AZIMUTH = 0.0f; // 100 percent shoulders float tau = deltaTime / HMD_FACING_TIMESCALE; @@ -456,13 +457,13 @@ void MyAvatar::update(float deltaTime) { glm::mat4 sensorToWorldMat = getSensorToWorldMatrix(); glm::mat4 worldToSensorMat = glm::inverse(sensorToWorldMat); glm::vec3 handHipAzimuthSensorSpace = transformVectorFast(worldToSensorMat, glm::vec3(handHipAzimuthWorldSpace.x, 0.0f, handHipAzimuthWorldSpace.z)); - glm::vec2 normedHandHip = glm::normalize(glm::vec2(handHipAzimuthSensorSpace.x, handHipAzimuthSensorSpace.z)); - glm::vec2 headFacingPlusHandHipAzimuthMix = lerp(normedHandHip, _headControllerFacing, 0.5f); - // if head facing dot mid point hands facing is close to -1 and the hands midpoint is close to -1 you then flip - - // qCDebug(interfaceapp) << "the hand hip azimuth " << normedHandHip << "the head look at" << _headControllerFacing; - _headControllerFacingMovingAverage = lerp(_headControllerFacingMovingAverage, headFacingPlusHandHipAzimuthMix, tau); - // qCDebug(interfaceapp) << "the hand hip azimuth average " << _headControllerFacingMovingAverage; + glm::vec2 normedHandHipAzimuthSensorSpace = glm::normalize(glm::vec2(handHipAzimuthSensorSpace.x, handHipAzimuthSensorSpace.z)); + glm::vec2 headFacingPlusHandHipAzimuthMix = lerp(normedHandHipAzimuthSensorSpace, _headControllerFacing, PERCENTAGE_WEIGHT_SHOULDERS_VS_HEAD_AZIMUTH); + if (getControllerPoseInAvatarFrame(controller::Action::LEFT_HAND).isValid() && getControllerPoseInAvatarFrame(controller::Action::RIGHT_HAND).isValid()) { + _headControllerFacingMovingAverage = lerp(_headControllerFacingMovingAverage, headFacingPlusHandHipAzimuthMix, tau); + } else { + _headControllerFacingMovingAverage = _headControllerFacing; + } if (_smoothOrientationTimer < SMOOTH_TIME_ORIENTATION) { _rotationChanged = usecTimestampNow(); @@ -477,7 +478,6 @@ void MyAvatar::update(float deltaTime) { computeHandAzimuth(); - #ifdef DEBUG_DRAW_HMD_MOVING_AVERAGE if (_drawAverageFacingEnabled) { auto sensorHeadPose = getControllerPoseInSensorFrame(controller::Action::HEAD); @@ -488,13 +488,7 @@ void MyAvatar::update(float deltaTime) { DebugDraw::getInstance().drawRay(worldHeadPos, worldHeadPos + worldFacingAverage, glm::vec4(0.0f, 0.0f, 1.0f, 1.0f)); // draw hand azimuth vector - glm::vec3 handAzimuthMidpoint = transformPoint(getTransform().getMatrix(), glm::vec3(_hipToHandController.x, 0.0f, _hipToHandController.y)); - glm::vec3 worldRHandAzimuth = transformPoint(getTransform().getMatrix(), getControllerPoseInAvatarFrame(controller::Action::RIGHT_HAND).getTranslation()); - glm::vec3 worldLHandAzimuth = transformPoint(getTransform().getMatrix(), getControllerPoseInAvatarFrame(controller::Action::LEFT_HAND).getTranslation()); - //qCDebug(interfaceapp) << "the right hand in avatar space" << worldRHandAzimuth << " " << getWorldPosition(); - DebugDraw::getInstance().drawRay(getWorldPosition(), worldRHandAzimuth, glm::vec4(0.0f, 1.0f, 0.0f, 1.0f)); - DebugDraw::getInstance().drawRay(getWorldPosition(), worldLHandAzimuth, glm::vec4(0.0f, 1.0f, 0.0f, 1.0f)); DebugDraw::getInstance().drawRay(getWorldPosition(), handAzimuthMidpoint, glm::vec4(0.0f, 1.0f, 1.0f, 1.0f)); } #endif @@ -502,7 +496,11 @@ void MyAvatar::update(float deltaTime) { if (_goToPending) { setWorldPosition(_goToPosition); setWorldOrientation(_goToOrientation); - _headControllerFacingMovingAverage = _headControllerFacing; // reset moving average + if (getControllerPoseInAvatarFrame(controller::Action::LEFT_HAND).isValid() && getControllerPoseInAvatarFrame(controller::Action::RIGHT_HAND).isValid()) { + _headControllerFacingMovingAverage = headFacingPlusHandHipAzimuthMix; + } else { + _headControllerFacingMovingAverage = _headControllerFacing; // reset moving average + } _goToPending = false; // updateFromHMDSensorMatrix (called from paintGL) expects that the sensorToWorldMatrix is updated for any position changes // that happen between render and Application::update (which calls updateSensorToWorldMatrix to do so). @@ -824,31 +822,29 @@ 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 void MyAvatar::computeHandAzimuth() { auto leftHandPoseAvatarSpace = getLeftHandPose(); auto rightHandPoseAvatarSpace = getRightHandPose(); glm::vec3 localLookat(0.0f, 0.0f, 1.0f); - // glm::vec3 rightHandRigSpace = rightHandPoseAvatarSpace.translation;// transformVectorFast(getTransform().getMatrix(), rightHandPoseAvatarSpace.translation); - // glm::vec3 leftHandRigSpace = leftHandPoseAvatarSpace.translation;// transformVectorFast(getTransform().getMatrix(), leftHandPoseAvatarSpace.translation); + const float HALFWAY = 0.50f; - // qCDebug(interfaceapp) << "the right hand in avatar space" << rightHandRigSpace; if (leftHandPoseAvatarSpace.isValid() && rightHandPoseAvatarSpace.isValid()) { + // we need the old azimuth reading to prevent flipping the facing direction 180 + // in the case where the hands go from being slightly less than 180 apart to slightly more than 180 apart. vec2 oldAzimuthReading = _hipToHandController; - _hipToHandController = lerp(glm::normalize(glm::vec2(rightHandPoseAvatarSpace.translation.x, rightHandPoseAvatarSpace.translation.z)), glm::normalize(glm::vec2(leftHandPoseAvatarSpace.translation.x, leftHandPoseAvatarSpace.translation.z)), 0.5f); - - //if they are 180 apart + _hipToHandController = lerp(glm::normalize(glm::vec2(rightHandPoseAvatarSpace.translation.x, rightHandPoseAvatarSpace.translation.z)), glm::normalize(glm::vec2(leftHandPoseAvatarSpace.translation.x, leftHandPoseAvatarSpace.translation.z)), HALFWAY); + + // check the angular distance from forward and back float cosForwardAngle = glm::dot(_hipToHandController, oldAzimuthReading); float cosBackwardAngle = glm::dot(_hipToHandController, -oldAzimuthReading); + // if we are now closer to the 180 flip of the previous chest forward + // then we negate our computed _hipToHandController to keep the chest in the same direction. if (cosBackwardAngle > cosForwardAngle) { // this means we have lost continuity with the current chest position _hipToHandController = -_hipToHandController; } - - - //need to use easing function here. - //var rotateAngle = ((Math.cos((leftRightMidpoint / 180.0) * Math.PI) + 2.0) / 3.0) * leftRightMidpoint; - } else { - // _hipToHandController = glm::vec2(0.0f, -1.0f); } } @@ -3382,11 +3378,9 @@ glm::mat4 MyAvatar::getSpine2RotationRigSpace() const { static const glm::quat RIG_CHANGE_OF_BASIS = Quaternions::Y_180; glm::vec3 hipToHandRigSpace = RIG_CHANGE_OF_BASIS * glm::vec3(_hipToHandController.x, 0.0f, _hipToHandController.y); - //to do: check for zero before normalizing. glm::vec3 u, v, w; generateBasisVectors(glm::vec3(0.0f,1.0f,0.0f), hipToHandRigSpace, u, v, w); glm::mat4 spine2RigSpace(glm::vec4(w, 0.0f), glm::vec4(u, 0.0f), glm::vec4(v, 0.0f), glm::vec4(glm::vec3(0.0f, 0.0f, 0.0f), 1.0f)); - return spine2RigSpace; } @@ -3867,7 +3861,7 @@ void MyAvatar::lateUpdatePalms() { } -static const float FOLLOW_TIME = 0.1f; +static const float FOLLOW_TIME = 0.5f; MyAvatar::FollowHelper::FollowHelper() { deactivate(); @@ -3972,11 +3966,6 @@ bool MyAvatar::FollowHelper::shouldActivateHorizontalCG(MyAvatar& myAvatar) cons glm::vec3 defaultHeadPosition = myAvatar.getAbsoluteDefaultJointTranslationInObjectFrame(myAvatar.getJointIndex("Head")); glm::vec3 currentHeadPosition = currentHeadPose.getTranslation(); float anatomicalHeadToHipsDistance = glm::length(defaultHeadPosition - defaultHipsPosition); - //if (!isActive(Horizontal) && - // (glm::length(currentHeadPosition - defaultHipsPosition) > (anatomicalHeadToHipsDistance + DEFAULT_AVATAR_SPINE_STRETCH_LIMIT))) { - // myAvatar.setResetMode(true); - // stepDetected = true; - //} if (!isActive(Horizontal) && (glm::length(currentHeadPosition - defaultHipsPosition) > (anatomicalHeadToHipsDistance + (DEFAULT_AVATAR_SPINE_STRETCH_LIMIT * anatomicalHeadToHipsDistance * myAvatar.getAvatarScale())))) { myAvatar.setResetMode(true); @@ -3997,23 +3986,15 @@ bool MyAvatar::FollowHelper::shouldActivateVertical(const MyAvatar& myAvatar, co void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix, bool hasDriveInput) { - const float VELOCITY_THRESHHOLD = 1.0f; - float currentVelocity = glm::length(myAvatar.getLocalVelocity() / myAvatar.getSensorToWorldScale()); - if (myAvatar.getHMDLeanRecenterEnabled() && qApp->getCamera().getMode() != CAMERA_MODE_MIRROR) { - //if (!isActive(Rotation) && getForceActivateRotation()) { - // activate(Rotation); - // myAvatar.setHeadControllerFacingMovingAverage(myAvatar._headControllerFacing); - // setForceActivateRotation(false); - //} if (!isActive(Rotation) && (shouldActivateRotation(myAvatar, desiredBodyMatrix, currentBodyMatrix) || hasDriveInput)) { activate(Rotation); myAvatar.setHeadControllerFacingMovingAverage(myAvatar._headControllerFacing); } if (myAvatar.getCenterOfGravityModelEnabled()) { - if ((!isActive(Horizontal) && (shouldActivateHorizontalCG(myAvatar) || hasDriveInput)) || (isActive(Horizontal) && (currentVelocity > VELOCITY_THRESHHOLD))) { + if (!isActive(Horizontal) && (shouldActivateHorizontalCG(myAvatar) || hasDriveInput)) { activate(Horizontal); if (myAvatar.getEnableStepResetRotation()) { activate(Rotation); @@ -4029,7 +4010,6 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat } } } - if (!isActive(Vertical) && (shouldActivateVertical(myAvatar, desiredBodyMatrix, currentBodyMatrix) || hasDriveInput)) { activate(Vertical); } diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index ea5059bf39..f819c417de 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -285,7 +285,7 @@ public: Q_INVOKABLE void centerBody(); // thread-safe - /**jsdoc + /**jsdoc_hmdSensorMatrix * The internal inverse-kinematics system maintains a record of which joints are "locked". Sometimes it is useful to forget this history, to prevent * contorted joints. * @function MyAvatar.clearIKJointLimitHistory @@ -914,6 +914,8 @@ public: virtual void rebuildCollisionShape() override; + const glm::vec2& getHeadControllerFacing() const { return _headControllerFacing; } + void setHeadControllerFacing(glm::vec2 currentHeadControllerFacing) { _headControllerFacing = currentHeadControllerFacing; } const glm::vec2& getHeadControllerFacingMovingAverage() const { return _headControllerFacingMovingAverage; } void setHeadControllerFacingMovingAverage(glm::vec2 currentHeadControllerFacing) { _headControllerFacingMovingAverage = currentHeadControllerFacing; } float getCurrentStandingHeight() const { return _currentStandingHeight; } diff --git a/interface/src/avatar/MySkeletonModel.cpp b/interface/src/avatar/MySkeletonModel.cpp index 158f9bb0b6..f150379f0c 100644 --- a/interface/src/avatar/MySkeletonModel.cpp +++ b/interface/src/avatar/MySkeletonModel.cpp @@ -238,13 +238,20 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { params.primaryControllerPoses[Rig::PrimaryControllerType_Hips] = sensorToRigPose * hips; params.primaryControllerFlags[Rig::PrimaryControllerType_Hips] = (uint8_t)Rig::ControllerFlags::Enabled | (uint8_t)Rig::ControllerFlags::Estimated; - - // spine 2 hack to be improved. + // set spine2 if we have hand controllers AnimPose currentPose; bool ret = _rig.getAbsoluteJointPoseInRigFrame(_rig.indexOfJoint("Spine2"),currentPose); - AnimPose newSpinePose(myAvatar->getSpine2RotationRigSpace()); - currentPose.rot() = newSpinePose.rot(); - //newSpinePose.rot() = myAvatar->getSpine2RotationRigSpace();// *newSpinePose.rot(); + AnimPose rigSpaceYaw(myAvatar->getSpine2RotationRigSpace()); + glm::vec3 u, v, w; + glm::vec3 fwd = rigSpaceYaw.rot() * glm::vec3(0.0f, 0.0f, 1.0f); + glm::vec3 up = currentPose.rot() * glm::vec3(0.0f, 1.0f, 0.0f); + generateBasisVectors(up, fwd, u, v, w); + AnimPose newSpinePose(glm::mat4(glm::vec4(w, 0.0f), glm::vec4(u, 0.0f), glm::vec4(v, 0.0f), glm::vec4(glm::vec3(0.0f, 0.0f, 0.0f), 1.0f))); + + if (myAvatar->getControllerPoseInAvatarFrame(controller::Action::RIGHT_HAND).isValid() && myAvatar->getControllerPoseInAvatarFrame(controller::Action::LEFT_HAND).isValid()) { + AnimPose newSpinePose(myAvatar->getSpine2RotationRigSpace()); + currentPose.rot() = newSpinePose.rot(); + } params.primaryControllerPoses[Rig::PrimaryControllerType_Spine2] = currentPose; params.primaryControllerFlags[Rig::PrimaryControllerType_Spine2] = (uint8_t)Rig::ControllerFlags::Enabled | (uint8_t)Rig::ControllerFlags::Estimated; diff --git a/libraries/shared/src/AvatarConstants.h b/libraries/shared/src/AvatarConstants.h index b97f586d32..c3e8a3f173 100644 --- a/libraries/shared/src/AvatarConstants.h +++ b/libraries/shared/src/AvatarConstants.h @@ -34,7 +34,7 @@ const float DEFAULT_HANDS_ANGULAR_VELOCITY_STEPPING_THRESHOLD = 3.3f; const float DEFAULT_HEAD_VELOCITY_STEPPING_THRESHOLD = 0.18f; const float DEFAULT_HEAD_PITCH_STEPPING_TOLERANCE = 7.0f; const float DEFAULT_HEAD_ROLL_STEPPING_TOLERANCE = 7.0f; -const float DEFAULT_AVATAR_SPINE_STRETCH_LIMIT = 0.05f; +const float DEFAULT_AVATAR_SPINE_STRETCH_LIMIT = 0.04f; const float DEFAULT_AVATAR_FORWARD_DAMPENING_FACTOR = 0.5f; const float DEFAULT_AVATAR_LATERAL_DAMPENING_FACTOR = 2.0f; const float DEFAULT_AVATAR_HIPS_MASS = 40.0f; From 89b508251e94472518fa8c9a4d00d02540d66e86 Mon Sep 17 00:00:00 2001 From: amantley Date: Mon, 27 Aug 2018 17:57:50 -0700 Subject: [PATCH 19/24] added some comment to explain rig to avatar transform --- interface/src/avatar/MyAvatar.cpp | 38 +++++++++++++++++-------------- interface/src/avatar/MyAvatar.h | 4 ++-- 2 files changed, 23 insertions(+), 19 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index c4d855ee77..23feff88dd 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -453,13 +453,16 @@ void MyAvatar::update(float deltaTime) { // put the spine facing in sensor space. // then mix it with head facing to determine rotation recenter - glm::vec3 handHipAzimuthWorldSpace = transformVectorFast(getTransform().getMatrix(), glm::vec3(_hipToHandController.x, 0.0f, _hipToHandController.y)); - glm::mat4 sensorToWorldMat = getSensorToWorldMatrix(); - glm::mat4 worldToSensorMat = glm::inverse(sensorToWorldMat); - glm::vec3 handHipAzimuthSensorSpace = transformVectorFast(worldToSensorMat, glm::vec3(handHipAzimuthWorldSpace.x, 0.0f, handHipAzimuthWorldSpace.z)); - glm::vec2 normedHandHipAzimuthSensorSpace = glm::normalize(glm::vec2(handHipAzimuthSensorSpace.x, handHipAzimuthSensorSpace.z)); - glm::vec2 headFacingPlusHandHipAzimuthMix = lerp(normedHandHipAzimuthSensorSpace, _headControllerFacing, PERCENTAGE_WEIGHT_SHOULDERS_VS_HEAD_AZIMUTH); if (getControllerPoseInAvatarFrame(controller::Action::LEFT_HAND).isValid() && getControllerPoseInAvatarFrame(controller::Action::RIGHT_HAND).isValid()) { + glm::vec3 handHipAzimuthWorldSpace = transformVectorFast(getTransform().getMatrix(), glm::vec3(_hipToHandController.x, 0.0f, _hipToHandController.y)); + glm::mat4 sensorToWorldMat = getSensorToWorldMatrix(); + glm::mat4 worldToSensorMat = glm::inverse(sensorToWorldMat); + glm::vec3 handHipAzimuthSensorSpace = transformVectorFast(worldToSensorMat, handHipAzimuthWorldSpace); + glm::vec2 normedHandHipAzimuthSensorSpace(0.0f, 1.0f); + if (glm::length(glm::vec2(handHipAzimuthSensorSpace.x, handHipAzimuthSensorSpace.z)) > 0.0f) { + normedHandHipAzimuthSensorSpace = glm::normalize(glm::vec2(handHipAzimuthSensorSpace.x, handHipAzimuthSensorSpace.z)); + } + glm::vec2 headFacingPlusHandHipAzimuthMix = lerp(normedHandHipAzimuthSensorSpace, _headControllerFacing, PERCENTAGE_WEIGHT_SHOULDERS_VS_HEAD_AZIMUTH); _headControllerFacingMovingAverage = lerp(_headControllerFacingMovingAverage, headFacingPlusHandHipAzimuthMix, tau); } else { _headControllerFacingMovingAverage = _headControllerFacing; @@ -475,7 +478,6 @@ void MyAvatar::update(float deltaTime) { _recentModeReadings.insert(newHeightReadingInCentimeters); setCurrentStandingHeight(computeStandingHeightMode(getControllerPoseInAvatarFrame(controller::Action::HEAD))); setAverageHeadRotation(computeAverageHeadRotation(getControllerPoseInAvatarFrame(controller::Action::HEAD))); - computeHandAzimuth(); #ifdef DEBUG_DRAW_HMD_MOVING_AVERAGE @@ -495,12 +497,8 @@ void MyAvatar::update(float deltaTime) { if (_goToPending) { setWorldPosition(_goToPosition); - setWorldOrientation(_goToOrientation); - if (getControllerPoseInAvatarFrame(controller::Action::LEFT_HAND).isValid() && getControllerPoseInAvatarFrame(controller::Action::RIGHT_HAND).isValid()) { - _headControllerFacingMovingAverage = headFacingPlusHandHipAzimuthMix; - } else { - _headControllerFacingMovingAverage = _headControllerFacing; // reset moving average - } + setWorldOrientation(_goToOrientation); + _headControllerFacingMovingAverage = _headControllerFacing; _goToPending = false; // updateFromHMDSensorMatrix (called from paintGL) expects that the sensorToWorldMatrix is updated for any position changes // that happen between render and Application::update (which calls updateSensorToWorldMatrix to do so). @@ -827,15 +825,17 @@ void MyAvatar::updateFromHMDSensorMatrix(const glm::mat4& hmdSensorMatrix) { void MyAvatar::computeHandAzimuth() { auto leftHandPoseAvatarSpace = getLeftHandPose(); auto rightHandPoseAvatarSpace = getRightHandPose(); - glm::vec3 localLookat(0.0f, 0.0f, 1.0f); const float HALFWAY = 0.50f; if (leftHandPoseAvatarSpace.isValid() && rightHandPoseAvatarSpace.isValid()) { // we need the old azimuth reading to prevent flipping the facing direction 180 // in the case where the hands go from being slightly less than 180 apart to slightly more than 180 apart. vec2 oldAzimuthReading = _hipToHandController; - _hipToHandController = 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(rightHandPoseAvatarSpace.translation.x, rightHandPoseAvatarSpace.translation.z)) > 0.0f) && (glm::length(glm::vec2(leftHandPoseAvatarSpace.translation.x, leftHandPoseAvatarSpace.translation.z)) > 0.0f)) { + _hipToHandController = lerp(glm::normalize(glm::vec2(rightHandPoseAvatarSpace.translation.x, rightHandPoseAvatarSpace.translation.z)), glm::normalize(glm::vec2(leftHandPoseAvatarSpace.translation.x, leftHandPoseAvatarSpace.translation.z)), HALFWAY); + } else { + _hipToHandController = glm::vec2(0.0f, 1.0f); + } // check the angular distance from forward and back float cosForwardAngle = glm::dot(_hipToHandController, oldAzimuthReading); float cosBackwardAngle = glm::dot(_hipToHandController, -oldAzimuthReading); @@ -3376,7 +3376,11 @@ glm::mat4 MyAvatar::deriveBodyFromHMDSensor() const { glm::mat4 MyAvatar::getSpine2RotationRigSpace() const { static const glm::quat RIG_CHANGE_OF_BASIS = Quaternions::Y_180; - glm::vec3 hipToHandRigSpace = RIG_CHANGE_OF_BASIS * glm::vec3(_hipToHandController.x, 0.0f, _hipToHandController.y); + // RIG_CHANGE_OF_BASIS * INITIAL_RIG_ROTATION * Quaternions::IDENTITY(explicit rig rotation) * inverse(RIG_CHANGE_OF_BASIS) = Quaternions::Y_180(avatar Space); + // INITIAL_RIG_ROTATION = Quaternions::Y_180; + static const glm::quat INITIAL_RIG_ROTATION = Quaternions::Y_180; + glm::quat avatarToRigSpace = INITIAL_RIG_ROTATION * Quaternions::IDENTITY; + glm::vec3 hipToHandRigSpace = avatarToRigSpace * glm::vec3(_hipToHandController.x, 0.0f, _hipToHandController.y); glm::vec3 u, v, w; generateBasisVectors(glm::vec3(0.0f,1.0f,0.0f), hipToHandRigSpace, u, v, w); diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index f819c417de..8cccb03ea5 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -285,7 +285,7 @@ public: Q_INVOKABLE void centerBody(); // thread-safe - /**jsdoc_hmdSensorMatrix + /**jsdoc * The internal inverse-kinematics system maintains a record of which joints are "locked". Sometimes it is useful to forget this history, to prevent * contorted joints. * @function MyAvatar.clearIKJointLimitHistory @@ -1650,7 +1650,7 @@ private: std::atomic _hasScriptedBlendShapes { false }; std::atomic _rotationRecenterFilterLength { 4.0f }; std::atomic _rotationThreshold { 0.5235f }; // 30 degrees in radians - std::atomic _stepResetRotationEnabled { false }; + std::atomic _stepResetRotationEnabled { true }; std::atomic _drawAverageFacingEnabled { false }; // working copy -- see AvatarData for thread-safe _sensorToWorldMatrixCache, used for outward facing access From 3c792a04c1dfe8096ab789ed0b146d50d4f31c89 Mon Sep 17 00:00:00 2001 From: amantley Date: Tue, 28 Aug 2018 11:25:42 -0700 Subject: [PATCH 20/24] fixed roll rotation added to azimuth in MySkeletonModel.cpp --- interface/src/avatar/MyAvatar.cpp | 35 +-- interface/src/avatar/MyAvatar.h | 2 + interface/src/avatar/MySkeletonModel.cpp | 33 +- scripts/developer/rotateApp.js | 385 ----------------------- 4 files changed, 38 insertions(+), 417 deletions(-) delete mode 100644 scripts/developer/rotateApp.js diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 23feff88dd..32822fc1b9 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -447,12 +447,12 @@ void MyAvatar::reset(bool andRecenter, bool andReload, bool andHead) { void MyAvatar::update(float deltaTime) { // update moving average of HMD facing in xz plane. const float HMD_FACING_TIMESCALE = getRotationRecenterFilterLength(); - const float PERCENTAGE_WEIGHT_SHOULDERS_VS_HEAD_AZIMUTH = 0.0f; // 100 percent shoulders + const float PERCENTAGE_WEIGHT_HEAD_VS_SHOULDERS_AZIMUTH = 0.0f; // 100 percent shoulders float tau = deltaTime / HMD_FACING_TIMESCALE; - // put the spine facing in sensor space. - // then mix it with head facing to determine rotation recenter + // put the average hand azimuth into sensor space. + // then mix it with head facing direction to determine rotation recenter if (getControllerPoseInAvatarFrame(controller::Action::LEFT_HAND).isValid() && getControllerPoseInAvatarFrame(controller::Action::RIGHT_HAND).isValid()) { glm::vec3 handHipAzimuthWorldSpace = transformVectorFast(getTransform().getMatrix(), glm::vec3(_hipToHandController.x, 0.0f, _hipToHandController.y)); glm::mat4 sensorToWorldMat = getSensorToWorldMatrix(); @@ -462,7 +462,7 @@ void MyAvatar::update(float deltaTime) { if (glm::length(glm::vec2(handHipAzimuthSensorSpace.x, handHipAzimuthSensorSpace.z)) > 0.0f) { normedHandHipAzimuthSensorSpace = glm::normalize(glm::vec2(handHipAzimuthSensorSpace.x, handHipAzimuthSensorSpace.z)); } - glm::vec2 headFacingPlusHandHipAzimuthMix = lerp(normedHandHipAzimuthSensorSpace, _headControllerFacing, PERCENTAGE_WEIGHT_SHOULDERS_VS_HEAD_AZIMUTH); + glm::vec2 headFacingPlusHandHipAzimuthMix = lerp(normedHandHipAzimuthSensorSpace, _headControllerFacing, PERCENTAGE_WEIGHT_HEAD_VS_SHOULDERS_AZIMUTH); _headControllerFacingMovingAverage = lerp(_headControllerFacingMovingAverage, headFacingPlusHandHipAzimuthMix, tau); } else { _headControllerFacingMovingAverage = _headControllerFacing; @@ -497,7 +497,7 @@ void MyAvatar::update(float deltaTime) { if (_goToPending) { setWorldPosition(_goToPosition); - setWorldOrientation(_goToOrientation); + setWorldOrientation(_goToOrientation); _headControllerFacingMovingAverage = _headControllerFacing; _goToPending = false; // updateFromHMDSensorMatrix (called from paintGL) expects that the sensorToWorldMatrix is updated for any position changes @@ -834,15 +834,14 @@ void MyAvatar::computeHandAzimuth() { 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)) { _hipToHandController = lerp(glm::normalize(glm::vec2(rightHandPoseAvatarSpace.translation.x, rightHandPoseAvatarSpace.translation.z)), glm::normalize(glm::vec2(leftHandPoseAvatarSpace.translation.x, leftHandPoseAvatarSpace.translation.z)), HALFWAY); } else { - _hipToHandController = glm::vec2(0.0f, 1.0f); + _hipToHandController = glm::vec2(0.0f, -1.0f); } // check the angular distance from forward and back float cosForwardAngle = glm::dot(_hipToHandController, oldAzimuthReading); float cosBackwardAngle = glm::dot(_hipToHandController, -oldAzimuthReading); // if we are now closer to the 180 flip of the previous chest forward - // then we negate our computed _hipToHandController to keep the chest in the same direction. + // then we negate our computed _hipToHandController to keep the chest from flipping. if (cosBackwardAngle > cosForwardAngle) { - // this means we have lost continuity with the current chest position _hipToHandController = -_hipToHandController; } } @@ -3375,12 +3374,10 @@ glm::mat4 MyAvatar::deriveBodyFromHMDSensor() const { glm::mat4 MyAvatar::getSpine2RotationRigSpace() const { - static const glm::quat RIG_CHANGE_OF_BASIS = Quaternions::Y_180; - // RIG_CHANGE_OF_BASIS * INITIAL_RIG_ROTATION * Quaternions::IDENTITY(explicit rig rotation) * inverse(RIG_CHANGE_OF_BASIS) = Quaternions::Y_180(avatar Space); - // INITIAL_RIG_ROTATION = Quaternions::Y_180; - static const glm::quat INITIAL_RIG_ROTATION = Quaternions::Y_180; - glm::quat avatarToRigSpace = INITIAL_RIG_ROTATION * Quaternions::IDENTITY; - glm::vec3 hipToHandRigSpace = avatarToRigSpace * glm::vec3(_hipToHandController.x, 0.0f, _hipToHandController.y); + // static const glm::quat RIG_CHANGE_OF_BASIS = Quaternions::Y_180; + // RIG_CHANGE_OF_BASIS * AVATAR_TO_RIG_ROTATION * inverse(RIG_CHANGE_OF_BASIS) = Quaternions::Y_180; //avatar Space; + const glm::quat AVATAR_TO_RIG_ROTATION = Quaternions::Y_180; + glm::vec3 hipToHandRigSpace = AVATAR_TO_RIG_ROTATION * glm::vec3(_hipToHandController.x, 0.0f, _hipToHandController.y); glm::vec3 u, v, w; generateBasisVectors(glm::vec3(0.0f,1.0f,0.0f), hipToHandRigSpace, u, v, w); @@ -3971,7 +3968,7 @@ bool MyAvatar::FollowHelper::shouldActivateHorizontalCG(MyAvatar& myAvatar) cons glm::vec3 currentHeadPosition = currentHeadPose.getTranslation(); float anatomicalHeadToHipsDistance = glm::length(defaultHeadPosition - defaultHipsPosition); if (!isActive(Horizontal) && - (glm::length(currentHeadPosition - defaultHipsPosition) > (anatomicalHeadToHipsDistance + (DEFAULT_AVATAR_SPINE_STRETCH_LIMIT * anatomicalHeadToHipsDistance * myAvatar.getAvatarScale())))) { + (glm::length(currentHeadPosition - defaultHipsPosition) > (anatomicalHeadToHipsDistance + (DEFAULT_AVATAR_SPINE_STRETCH_LIMIT * anatomicalHeadToHipsDistance)))) { myAvatar.setResetMode(true); stepDetected = true; } @@ -3995,14 +3992,14 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat qApp->getCamera().getMode() != CAMERA_MODE_MIRROR) { if (!isActive(Rotation) && (shouldActivateRotation(myAvatar, desiredBodyMatrix, currentBodyMatrix) || hasDriveInput)) { activate(Rotation); - myAvatar.setHeadControllerFacingMovingAverage(myAvatar._headControllerFacing); + myAvatar.setHeadControllerFacingMovingAverage(myAvatar.getHeadControllerFacing()); } if (myAvatar.getCenterOfGravityModelEnabled()) { if (!isActive(Horizontal) && (shouldActivateHorizontalCG(myAvatar) || hasDriveInput)) { activate(Horizontal); if (myAvatar.getEnableStepResetRotation()) { activate(Rotation); - myAvatar.setHeadControllerFacingMovingAverage(myAvatar._headControllerFacing); + myAvatar.setHeadControllerFacingMovingAverage(myAvatar.getHeadControllerFacing()); } } } else { @@ -4010,7 +4007,7 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat activate(Horizontal); if (myAvatar.getEnableStepResetRotation()) { activate(Rotation); - myAvatar.setHeadControllerFacingMovingAverage(myAvatar._headControllerFacing); + myAvatar.setHeadControllerFacingMovingAverage(myAvatar.getHeadControllerFacing()); } } } @@ -4020,7 +4017,7 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat } else { if (!isActive(Rotation) && getForceActivateRotation()) { activate(Rotation); - myAvatar.setHeadControllerFacingMovingAverage(myAvatar._headControllerFacing); + myAvatar.setHeadControllerFacingMovingAverage(myAvatar.getHeadControllerFacing()); setForceActivateRotation(false); } if (!isActive(Horizontal) && getForceActivateHorizontal()) { diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 8cccb03ea5..8b5d9d8c55 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -914,6 +914,8 @@ public: virtual void rebuildCollisionShape() override; + const glm::vec2& getHipToHandController() const { return _hipToHandController; } + void setHipToHandController(glm::vec2 currentHipToHandController) { _hipToHandController = currentHipToHandController; } const glm::vec2& getHeadControllerFacing() const { return _headControllerFacing; } void setHeadControllerFacing(glm::vec2 currentHeadControllerFacing) { _headControllerFacing = currentHeadControllerFacing; } const glm::vec2& getHeadControllerFacingMovingAverage() const { return _headControllerFacingMovingAverage; } diff --git a/interface/src/avatar/MySkeletonModel.cpp b/interface/src/avatar/MySkeletonModel.cpp index f150379f0c..e06a06cf0e 100644 --- a/interface/src/avatar/MySkeletonModel.cpp +++ b/interface/src/avatar/MySkeletonModel.cpp @@ -239,21 +239,28 @@ 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 - AnimPose currentPose; - bool ret = _rig.getAbsoluteJointPoseInRigFrame(_rig.indexOfJoint("Spine2"),currentPose); - AnimPose rigSpaceYaw(myAvatar->getSpine2RotationRigSpace()); - glm::vec3 u, v, w; - glm::vec3 fwd = rigSpaceYaw.rot() * glm::vec3(0.0f, 0.0f, 1.0f); - glm::vec3 up = currentPose.rot() * glm::vec3(0.0f, 1.0f, 0.0f); - generateBasisVectors(up, fwd, u, v, w); - AnimPose newSpinePose(glm::mat4(glm::vec4(w, 0.0f), glm::vec4(u, 0.0f), glm::vec4(v, 0.0f), glm::vec4(glm::vec3(0.0f, 0.0f, 0.0f), 1.0f))); - if (myAvatar->getControllerPoseInAvatarFrame(controller::Action::RIGHT_HAND).isValid() && myAvatar->getControllerPoseInAvatarFrame(controller::Action::LEFT_HAND).isValid()) { - AnimPose newSpinePose(myAvatar->getSpine2RotationRigSpace()); - currentPose.rot() = newSpinePose.rot(); + AnimPose currentSpine2Pose; + AnimPose currentHeadPose; + AnimPose currentHipsPose; + bool ret = _rig.getAbsoluteJointPoseInRigFrame(_rig.indexOfJoint("Spine2"), currentSpine2Pose); + bool ret1 = _rig.getAbsoluteJointPoseInRigFrame(_rig.indexOfJoint("Head"), currentHeadPose); + bool ret2 = _rig.getAbsoluteJointPoseInRigFrame(_rig.indexOfJoint("Hips"), currentHipsPose); + AnimPose rigSpaceYaw(myAvatar->getSpine2RotationRigSpace()); + glm::vec3 u, v, w; + glm::vec3 fwd = rigSpaceYaw.rot() * glm::vec3(0.0f, 0.0f, 1.0f); + glm::vec3 up = currentHeadPose.trans() - currentHipsPose.trans(); + if (glm::length(up) > 0.0f) { + up = glm::normalize(up); + } else { + up = glm::vec3(0.0f, 1.0f, 0.0f); + } + generateBasisVectors(up, fwd, u, v, w); + AnimPose newSpinePose(glm::mat4(glm::vec4(w, 0.0f), glm::vec4(u, 0.0f), glm::vec4(v, 0.0f), glm::vec4(glm::vec3(0.0f, 0.0f, 0.0f), 1.0f))); + currentSpine2Pose.rot() = newSpinePose.rot(); + params.primaryControllerPoses[Rig::PrimaryControllerType_Spine2] = currentSpine2Pose; + params.primaryControllerFlags[Rig::PrimaryControllerType_Spine2] = (uint8_t)Rig::ControllerFlags::Enabled | (uint8_t)Rig::ControllerFlags::Estimated; } - params.primaryControllerPoses[Rig::PrimaryControllerType_Spine2] = currentPose; - params.primaryControllerFlags[Rig::PrimaryControllerType_Spine2] = (uint8_t)Rig::ControllerFlags::Enabled | (uint8_t)Rig::ControllerFlags::Estimated; } else { _prevHipsValid = false; diff --git a/scripts/developer/rotateApp.js b/scripts/developer/rotateApp.js deleted file mode 100644 index 030385b6fb..0000000000 --- a/scripts/developer/rotateApp.js +++ /dev/null @@ -1,385 +0,0 @@ -/* global Script, Vec3, MyAvatar, Tablet, Messages, Quat, -DebugDraw, Mat4, Entities, Xform, Controller, Camera, console, document*/ - -Script.registerValue("ROTATEAPP", true); - -var TABLET_BUTTON_NAME = "ROTATE"; -var CHANGE_OF_BASIS_ROTATION = { x: 0, y: 1, z: 0, w: 0 }; -var DEFAULT_HEAD_TURN_THRESHOLD = 0.5333; -var DEFAULT_HEAD_TURN_FILTER_LENGTH = 4.0; -var LOADING_DELAY = 2000; -var AVERAGING_RATE = 0.03; -var INCREASING = 1.0; -var DECREASING = -1.0; -var DEGREES_PER_PI_RADIANS = 180.0; -var FILTER_FUDGE_RANGE = 0.9; - -var activated = false; -var documentLoaded = false; -var sciptLoaded = false; -var headPoseAverageOrientation = { x: 0, y: 0, z: 0, w: 1 }; -var hipToLeftHandAverage = 0.0; // { x: 0, y: 0, z: 0, w: 1 }; -var hipToRightHandAverage = 0.0; // { x: 0, y: 0, z: 0, w: 1 }; -var averageAzimuth = 0.0; -var hipsPositionRigSpace = { x: 0, y: 0, z: 0 }; -var spine2PositionRigSpace = { x: 0, y: 0, z: 0 }; -var hipsRotationRigSpace = { x: 0, y: 0, z: 0, w: 1 }; -var spine2RotationRigSpace = { x: 0, y: 0, z: 0, w: 1 }; -var spine2Rotation = { x: 0, y: 0, z: 0, w: 1 }; -var hipToLHandAverage = { x: 0, y: 0, z: 0, w: 1 }; -var hipToRHandAverage = { x: 0, y: 0, z: 0, w: 1 }; - -/* -var ikTypes = { - RotationAndPosition: 0, - RotationOnly: 1, - HmdHead: 2, - HipsRelativeRotationAndPosition: 3, - Spline: 4, - Unknown: 5 -}; - - -var ANIM_VARS = [ - //"headType", - "spine2Type", - //"hipsType", - "spine2Position", - "spine2Rotation" - //"hipsPosition", - //"hipsRotation" -]; - -var handlerId = MyAvatar.addAnimationStateHandler(function (props) { - //print("in callback"); - //print("props spine2 pos: " + props.spine2Position.x + " " + props.spine2Position.y + " " + props.spine2Position.z); - //print("props hip pos: " + props.hipsPosition.x + " " + props.hipsPosition.y + " " + props.hipsPosition.z); - var result = {}; - //{x:0,y:0,z:0} - //result.headType = ikTypes.HmdHead; - //result.hipsType = ikTypes.RotationAndPosition; - //result.hipsPosition = hipsPositionRigSpace; // { x: 0, y: 0, z: 0 }; - //result.hipsRotation = hipsRotationRigSpace;//{ x: 0, y: 0, z: 0, w: 1 }; // - result.spine2Type = ikTypes.Spline; - result.spine2Position = spine2PositionRigSpace; // { x: 0, y: 1.3, z: 0 }; - result.spine2Rotation = spine2Rotation; - - return result; -}, ANIM_VARS); -*/ -// define state readings constructor -function StateReading(headPose, rhandPose, lhandPose, diffFromAverageEulers) { - this.headPose = headPose; - this.rhandPose = rhandPose; - this.lhandPose = lhandPose; - 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), - { x: 0, y: 0, z: 0 }); - -// declare the checkbox constructor -function AppCheckbox(type,id,eventType,isChecked) { - this.type = type; - this.id = id; - this.eventType = eventType; - this.data = {value: isChecked}; -} - -var usingStepResetRotationDirection = new AppCheckbox("checkboxtick", "stepReset", "onStepResetCheckBox", false); -var usingDrawAverageFacing = new AppCheckbox("checkboxtick", "drawAverage", "onDrawAverageFacingCheckBox", false); -var checkBoxArray = new Array(usingStepResetRotationDirection, usingDrawAverageFacing); - -// 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; -} - -// var HTML_URL = Script.resolvePath("file:///c:/dev/hifi_fork/hifi/scripts/developer/rotateRecenterApp.html"); -var HTML_URL = Script.resolvePath("http://hifi-content.s3.amazonaws.com/angus/stepApp/rotateRecenterApp.html"); -var tablet = Tablet.getTablet("com.highfidelity.interface.tablet.system"); - -// define the sliders -var filterLengthProperty = new AppProperty("#filterLength-slider", "slider", "onFilterLengthSlider", "filterSignal", - setFilterLength, DEFAULT_HEAD_TURN_FILTER_LENGTH, function (num) { - var base = 5; - var shift = 0; - return convertExponential(base, num, INCREASING, shift); - }, function (num) { - var base = 5; - var shift = 0; - return convertLog(base, num, INCREASING, shift); - }, true); -var angleThresholdProperty = new AppProperty("#angleThreshold-slider", "slider", "onAngleThresholdSlider", "angleSignal", - setAngleThreshold, DEFAULT_HEAD_TURN_THRESHOLD, function (num) { - return convertToRadians(num); - }, function (num) { - return convertToDegrees(num); - }, true); - -var propArray = new Array(filterLengthProperty, angleThresholdProperty); - -function setFilterLength(num) { - filterLengthProperty.value = num; - MyAvatar.rotationRecenterFilterLength = filterLengthProperty.value; - -} - -function setAngleThreshold(num) { - angleThresholdProperty.value = num; - MyAvatar.rotationThreshold = angleThresholdProperty.value; -} - -function convertToRadians(num) { - return (num / DEGREES_PER_PI_RADIANS) * Math.PI; -} - -function convertToDegrees(num) { - return (num / Math.PI) * DEGREES_PER_PI_RADIANS; -} - -function getLog(x, y) { - return Math.log(y) / Math.log(x); -} - -function convertLog(base, num, direction, shift) { - return direction * getLog(base, (num + FILTER_FUDGE_RANGE)) + shift; -} - -function convertExponential(base, num, direction, shift) { - return Math.pow(base, (direction * num + shift)) - FILTER_FUDGE_RANGE; -} - -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 onKeyPress(event) { - if (event.text === "'") { - // when the sensors are reset, then reset the mode. - } -} - -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; - if (cbox.id === "stepReset") { - MyAvatar.enableStepResetRotation = cbox.data.value; - } - if (cbox.id === "drawAverage") { - MyAvatar.enableDrawAverageFacing = cbox.data.value; - } - // break; - } - - }); - if (message.type === "onCreateRotateApp") { - 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) { - print(prop.name); - 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 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 = true; - } - activated = true; - } else { - if (activated) { - // disconnect from event bridge - tablet.webEventReceived.disconnect(onWebEventReceived); - documentLoaded = false; - } - activated = false; - } -} - -function limitAngle(angle) { - return (angle + 180) % 360 - 180; -} - -function computeHandAzimuths(timeElapsed) { - - // var leftHandPositionRigSpace = Vec3.multiplyQbyV(Quat.inverse(CHANGE_OF_BASIS_ROTATION), currentStateReadings.lhandPose.translation); - // var rightHandPositionRigSpace = Vec3.multiplyQbyV(Quat.inverse(CHANGE_OF_BASIS_ROTATION), currentStateReadings.rhandPose.translation); - - - // var hipToLeftHand = Quat.lookAtSimple({ x: 0, y: 0, z: 0 }, { x: leftHandPositionRigSpace.x, y: 0, z: leftHandPositionRigSpace.z }); - // var hipToRightHand = Quat.lookAtSimple({ x: 0, y: 0, z: 0 }, { x: rightHandPositionRigSpace.x, y: 0, z: rightHandPositionRigSpace.z }); - // var hipToHandHalfway = Quat.slerp(hipToLeftHand, hipToRightHand, 0.5); - - var leftHand = currentStateReadings.lhandPose.translation; - var rightHand = currentStateReadings.rhandPose.translation; - var head = currentStateReadings.headPose.translation; - var lHandMinusHead = Vec3.subtract(leftHand, head); - lHandMinusHead.y = 0.0; - var rHandMinusHead = Vec3.subtract(rightHand, head); - rHandMinusHead.y = 0.0; - //print(JSON.stringify(leftHand)); - //print(JSON.stringify(head)); - var avatarZAxis = { x: 0.0, y: 0.0, z: 1.0 }; - var hipToLHand = Quat.lookAtSimple({ x: 0, y: 0, z: 0 }, lHandMinusHead); - var hipToRHand = Quat.lookAtSimple({ x: 0, y: 0, z: 0 }, rHandMinusHead); - var tau = timeElapsed / filterLengthProperty.value; - hipToLHandAverage = Quat.slerp(hipToLHandAverage, hipToLHand, tau); - hipToRHandAverage = Quat.slerp(hipToRHandAverage, hipToRHand, tau); - - // var angleToLeft = limitAngle(Quat.safeEulerAngles(hipToLHandAverage).y); - // var angleToRight = limitAngle(Quat.safeEulerAngles(hipToRHandAverage).y); - var leftRightMidpoint = (Quat.safeEulerAngles(hipToLHand).y + Quat.safeEulerAngles(hipToRHand).y) / 2.0; - var leftRightMidpointAverage = (Quat.safeEulerAngles(hipToLHandAverage).y + Quat.safeEulerAngles(hipToRHandAverage).y) / 2.0; - - // limit the angle because we are flipped by 180, fix this tomorrow. - // print(leftRightMidpointAverage/180.0); - // print("threshold value " + angleThresholdProperty.value); - - - var raySpineRotation = Quat.fromVec3Degrees({ x: 0, y: leftRightMidpointAverage, z: 0 }); - var zAxisSpineRotation = Vec3.multiplyQbyV(raySpineRotation, { x: 0, y: 0, z: -1 }); - var zAxisWorldSpace = Vec3.multiplyQbyV(MyAvatar.orientation, zAxisSpineRotation); - // DebugDraw.drawRay(MyAvatar.position, Vec3.sum(MyAvatar.position, zAxisWorldSpace), { x: 1, y: 0, z: 0, w: 1 }); - - //print(leftRightMidpoint); - - var headPoseRigSpace = Quat.multiply(CHANGE_OF_BASIS_ROTATION, currentStateReadings.head.rotation); - headPoseAverageOrientation = Quat.slerp(headPoseAverageOrientation, headPoseRigSpace, tau); - var headPoseAverageEulers = Quat.safeEulerAngles(headPoseAverageOrientation); - - // get it into radians too!! - // average head and hands 50/50 - // print("hands azimuth " + leftRightMidpointAverage + " head azimuth " + headPoseAverageEulers.y); - var headPlusHands = (leftRightMidpointAverage + headPoseAverageEulers.y) / 2.0; - if ((Math.abs(headPlusHands / 180.0) * Math.PI) > angleThresholdProperty.value) { - //print("recenter the feet under the head"); - // MyAvatar.triggerRotationRecenter(); - hipToLHandAverage = { x: 0, y: 0, z: 0, w: 1 }; - hipToRHandAverage = { x: 0, y: 0, z: 0, w: 1 }; - headPoseAverageOrientation = { x: 0, y: 0, z: 0, w: 1 }; - } - - // put a hard max on this easing function. - var rotateAngle = ((Math.cos((leftRightMidpoint / 180.0) * Math.PI) + 2.0)/3.0) * leftRightMidpoint; - //print("rotate angle " + rotateAngle); - - return Quat.fromVec3Degrees({ x: 0, y: rotateAngle, z: 0 }); - - -} - -function update(dt) { - //update state readings - currentStateReadings.head = Controller.getPoseValue(Controller.Standard.Head); - currentStateReadings.rhandPose = Controller.getPoseValue(Controller.Standard.RightHand); - currentStateReadings.lhandPose = Controller.getPoseValue(Controller.Standard.LeftHand); - - //print(JSON.stringify(currentStateReadings.head)); - - - - var latestSpineRotation = computeHandAzimuths(dt); - - spine2Rotation = latestSpineRotation; - var spine2Pos = MyAvatar.getAbsoluteJointTranslationInObjectFrame(MyAvatar.getJointIndex("Spine2")); - spine2PositionRigSpace = Vec3.multiplyQbyV(CHANGE_OF_BASIS_ROTATION, spine2Pos); - /* - if (HMD.active && !scriptLoaded) { - //Script.load("rotateApp.js"); - scriptLoaded = true; - } - - if (!HMD.active) { - scriptLoaded = false; - } - */ - - // handle the azimuth of the arms - -} - -function shutdownTabletApp() { - tablet.removeButton(tabletButton); - if (activated) { - tablet.webEventReceived.disconnect(onWebEventReceived); - tablet.gotoHomeScreen(); - } - tablet.screenChanged.disconnect(onScreenChanged); -} - -Script.setTimeout(function () { - tabletButton.clicked.connect(manageClick); - tablet.screenChanged.connect(onScreenChanged); - Script.update.connect(update); - Controller.keyPressEvent.connect(onKeyPress); -}, (LOADING_DELAY)); - -Script.scriptEnding.connect(function () { - // if (handlerId) { - // print("removing animation state handler"); - // handlerId = MyAvatar.removeAnimationStateHandler(handlerId); - // } - MyAvatar.hmdLeanRecenterEnabled = true; - Script.update.disconnect(update); - shutdownTabletApp(); -}); \ No newline at end of file From ee33f5dc97610c96a8e29b89049bc9adc608311e Mon Sep 17 00:00:00 2001 From: Angus Antley Date: Wed, 29 Aug 2018 01:57:49 +0100 Subject: [PATCH 21/24] normalize the hip hand vector in getSpine2RotationRigSpace, made computeHandAzimuth a const function that returns a vec2 --- interface/src/avatar/MyAvatar.cpp | 36 +++++++++++++----------- interface/src/avatar/MyAvatar.h | 2 +- interface/src/avatar/MySkeletonModel.cpp | 34 +++++++++++----------- 3 files changed, 38 insertions(+), 34 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 32822fc1b9..75becd2c6d 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -91,8 +91,6 @@ const float MIN_SCALE_CHANGED_DELTA = 0.001f; const int MODE_READINGS_RING_BUFFER_SIZE = 500; const float CENTIMETERS_PER_METER = 100.0f; -#define DEBUG_DRAW_HMD_MOVING_AVERAGE - MyAvatar::MyAvatar(QThread* thread) : Avatar(thread), _yawSpeed(YAW_SPEED_DEFAULT), @@ -450,6 +448,7 @@ void MyAvatar::update(float deltaTime) { const float PERCENTAGE_WEIGHT_HEAD_VS_SHOULDERS_AZIMUTH = 0.0f; // 100 percent shoulders float tau = deltaTime / HMD_FACING_TIMESCALE; + setHipToHandController(computeHandAzimuth()); // put the average hand azimuth into sensor space. // then mix it with head facing direction to determine rotation recenter @@ -465,7 +464,7 @@ void MyAvatar::update(float deltaTime) { glm::vec2 headFacingPlusHandHipAzimuthMix = lerp(normedHandHipAzimuthSensorSpace, _headControllerFacing, PERCENTAGE_WEIGHT_HEAD_VS_SHOULDERS_AZIMUTH); _headControllerFacingMovingAverage = lerp(_headControllerFacingMovingAverage, headFacingPlusHandHipAzimuthMix, tau); } else { - _headControllerFacingMovingAverage = _headControllerFacing; + _headControllerFacingMovingAverage = lerp(_headControllerFacingMovingAverage, _headControllerFacing, tau); } if (_smoothOrientationTimer < SMOOTH_TIME_ORIENTATION) { @@ -478,10 +477,8 @@ void MyAvatar::update(float deltaTime) { _recentModeReadings.insert(newHeightReadingInCentimeters); setCurrentStandingHeight(computeStandingHeightMode(getControllerPoseInAvatarFrame(controller::Action::HEAD))); setAverageHeadRotation(computeAverageHeadRotation(getControllerPoseInAvatarFrame(controller::Action::HEAD))); - computeHandAzimuth(); -#ifdef DEBUG_DRAW_HMD_MOVING_AVERAGE - if (_drawAverageFacingEnabled) { + if (_drawAverageFacingEnabled) { auto sensorHeadPose = getControllerPoseInSensorFrame(controller::Action::HEAD); glm::vec3 worldHeadPos = transformPoint(getSensorToWorldMatrix(), sensorHeadPose.getTranslation()); glm::vec3 worldFacingAverage = transformVectorFast(getSensorToWorldMatrix(), glm::vec3(_headControllerFacingMovingAverage.x, 0.0f, _headControllerFacingMovingAverage.y)); @@ -493,12 +490,11 @@ void MyAvatar::update(float deltaTime) { 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)); } -#endif if (_goToPending) { setWorldPosition(_goToPosition); setWorldOrientation(_goToOrientation); - _headControllerFacingMovingAverage = _headControllerFacing; + _headControllerFacingMovingAverage = _headControllerFacing; // reset moving average _goToPending = false; // updateFromHMDSensorMatrix (called from paintGL) expects that the sensorToWorldMatrix is updated for any position changes // that happen between render and Application::update (which calls updateSensorToWorldMatrix to do so). @@ -822,29 +818,30 @@ 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 -void MyAvatar::computeHandAzimuth() { +glm::vec2 MyAvatar::computeHandAzimuth() const { auto leftHandPoseAvatarSpace = getLeftHandPose(); auto rightHandPoseAvatarSpace = getRightHandPose(); const float HALFWAY = 0.50f; + glm::vec2 latestHipToHandController = _hipToHandController; if (leftHandPoseAvatarSpace.isValid() && rightHandPoseAvatarSpace.isValid()) { // we need the old azimuth reading to prevent flipping the facing direction 180 // in the case where the hands go from being slightly less than 180 apart to slightly more than 180 apart. - vec2 oldAzimuthReading = _hipToHandController; + 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)) { - _hipToHandController = lerp(glm::normalize(glm::vec2(rightHandPoseAvatarSpace.translation.x, rightHandPoseAvatarSpace.translation.z)), glm::normalize(glm::vec2(leftHandPoseAvatarSpace.translation.x, leftHandPoseAvatarSpace.translation.z)), HALFWAY); + latestHipToHandController = lerp(glm::normalize(glm::vec2(rightHandPoseAvatarSpace.translation.x, rightHandPoseAvatarSpace.translation.z)), glm::normalize(glm::vec2(leftHandPoseAvatarSpace.translation.x, leftHandPoseAvatarSpace.translation.z)), HALFWAY); } else { - _hipToHandController = glm::vec2(0.0f, -1.0f); + latestHipToHandController = glm::vec2(0.0f, -1.0f); } // check the angular distance from forward and back - float cosForwardAngle = glm::dot(_hipToHandController, oldAzimuthReading); - float cosBackwardAngle = glm::dot(_hipToHandController, -oldAzimuthReading); + float cosForwardAngle = glm::dot(latestHipToHandController, oldAzimuthReading); // if we are now closer to the 180 flip of the previous chest forward - // then we negate our computed _hipToHandController to keep the chest from flipping. - if (cosBackwardAngle > cosForwardAngle) { - _hipToHandController = -_hipToHandController; + // then we negate our computed latestHipToHandController to keep the chest from flipping. + if (cosForwardAngle < 0.0f) { + latestHipToHandController = -latestHipToHandController; } } + return latestHipToHandController; } void MyAvatar::updateJointFromController(controller::Action poseKey, ThreadSafeValueCache& matrixCache) { @@ -3380,6 +3377,11 @@ glm::mat4 MyAvatar::getSpine2RotationRigSpace() const { glm::vec3 hipToHandRigSpace = AVATAR_TO_RIG_ROTATION * glm::vec3(_hipToHandController.x, 0.0f, _hipToHandController.y); glm::vec3 u, v, w; + if (glm::length(hipToHandRigSpace) > 0.0f) { + hipToHandRigSpace = glm::normalize(hipToHandRigSpace); + } else { + hipToHandRigSpace = glm::vec3(0.0f, 0.0f, 1.0f); + } generateBasisVectors(glm::vec3(0.0f,1.0f,0.0f), hipToHandRigSpace, u, v, w); glm::mat4 spine2RigSpace(glm::vec4(w, 0.0f), glm::vec4(u, 0.0f), glm::vec4(v, 0.0f), glm::vec4(glm::vec3(0.0f, 0.0f, 0.0f), 1.0f)); return spine2RigSpace; diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 8b5d9d8c55..f82c5b4018 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -321,7 +321,7 @@ public: void updateFromHMDSensorMatrix(const glm::mat4& hmdSensorMatrix); // compute the hip to hand average azimuth. - void computeHandAzimuth(); + glm::vec2 computeHandAzimuth() const; // read the location of a hand controller and save the transform void updateJointFromController(controller::Action poseKey, ThreadSafeValueCache& matrixCache); diff --git a/interface/src/avatar/MySkeletonModel.cpp b/interface/src/avatar/MySkeletonModel.cpp index e06a06cf0e..8fd7d31956 100644 --- a/interface/src/avatar/MySkeletonModel.cpp +++ b/interface/src/avatar/MySkeletonModel.cpp @@ -243,23 +243,25 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { AnimPose currentSpine2Pose; AnimPose currentHeadPose; AnimPose currentHipsPose; - bool ret = _rig.getAbsoluteJointPoseInRigFrame(_rig.indexOfJoint("Spine2"), currentSpine2Pose); - bool ret1 = _rig.getAbsoluteJointPoseInRigFrame(_rig.indexOfJoint("Head"), currentHeadPose); - bool ret2 = _rig.getAbsoluteJointPoseInRigFrame(_rig.indexOfJoint("Hips"), currentHipsPose); - AnimPose rigSpaceYaw(myAvatar->getSpine2RotationRigSpace()); - glm::vec3 u, v, w; - glm::vec3 fwd = rigSpaceYaw.rot() * glm::vec3(0.0f, 0.0f, 1.0f); - glm::vec3 up = currentHeadPose.trans() - currentHipsPose.trans(); - if (glm::length(up) > 0.0f) { - up = glm::normalize(up); - } else { - up = glm::vec3(0.0f, 1.0f, 0.0f); + bool spine2Exists = _rig.getAbsoluteJointPoseInRigFrame(_rig.indexOfJoint("Spine2"), currentSpine2Pose); + bool headExists = _rig.getAbsoluteJointPoseInRigFrame(_rig.indexOfJoint("Head"), currentHeadPose); + bool hipsExists = _rig.getAbsoluteJointPoseInRigFrame(_rig.indexOfJoint("Hips"), currentHipsPose); + if (spine2Exists && headExists && hipsExists) { + AnimPose rigSpaceYaw(myAvatar->getSpine2RotationRigSpace()); + glm::vec3 u, v, w; + glm::vec3 fwd = rigSpaceYaw.rot() * glm::vec3(0.0f, 0.0f, 1.0f); + glm::vec3 up = currentHeadPose.trans() - currentHipsPose.trans(); + if (glm::length(up) > 0.0f) { + up = glm::normalize(up); + } else { + up = glm::vec3(0.0f, 1.0f, 0.0f); + } + generateBasisVectors(up, fwd, u, v, w); + AnimPose newSpinePose(glm::mat4(glm::vec4(w, 0.0f), glm::vec4(u, 0.0f), glm::vec4(v, 0.0f), glm::vec4(glm::vec3(0.0f, 0.0f, 0.0f), 1.0f))); + currentSpine2Pose.rot() = newSpinePose.rot(); + params.primaryControllerPoses[Rig::PrimaryControllerType_Spine2] = currentSpine2Pose; + params.primaryControllerFlags[Rig::PrimaryControllerType_Spine2] = (uint8_t)Rig::ControllerFlags::Enabled | (uint8_t)Rig::ControllerFlags::Estimated; } - generateBasisVectors(up, fwd, u, v, w); - AnimPose newSpinePose(glm::mat4(glm::vec4(w, 0.0f), glm::vec4(u, 0.0f), glm::vec4(v, 0.0f), glm::vec4(glm::vec3(0.0f, 0.0f, 0.0f), 1.0f))); - currentSpine2Pose.rot() = newSpinePose.rot(); - params.primaryControllerPoses[Rig::PrimaryControllerType_Spine2] = currentSpine2Pose; - params.primaryControllerFlags[Rig::PrimaryControllerType_Spine2] = (uint8_t)Rig::ControllerFlags::Enabled | (uint8_t)Rig::ControllerFlags::Estimated; } } else { From f7d0cdda025964ae4d8680d2fdbdc0870c97e4bc Mon Sep 17 00:00:00 2001 From: Angus Antley Date: Wed, 29 Aug 2018 20:44:21 +0100 Subject: [PATCH 22/24] added a check for whether the head to shoulder azimuth is greater than 100 degrees. If so unflip the chest azimuth --- interface/src/avatar/MyAvatar.cpp | 21 +++++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 75becd2c6d..78affe2052 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -819,12 +819,13 @@ 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 { - auto leftHandPoseAvatarSpace = getLeftHandPose(); - auto rightHandPoseAvatarSpace = getRightHandPose(); + controller::Pose leftHandPoseAvatarSpace = getLeftHandPose(); + controller::Pose rightHandPoseAvatarSpace = getRightHandPose(); + controller::Pose headPoseAvatarSpace = getControllerPoseInAvatarFrame(controller::Action::HEAD); const float HALFWAY = 0.50f; glm::vec2 latestHipToHandController = _hipToHandController; - if (leftHandPoseAvatarSpace.isValid() && rightHandPoseAvatarSpace.isValid()) { + if (leftHandPoseAvatarSpace.isValid() && rightHandPoseAvatarSpace.isValid() && headPoseAvatarSpace.isValid()) { // we need the old azimuth reading to prevent flipping the facing direction 180 // in the case where the hands go from being slightly less than 180 apart to slightly more than 180 apart. glm::vec2 oldAzimuthReading = _hipToHandController; @@ -833,11 +834,23 @@ glm::vec2 MyAvatar::computeHandAzimuth() const { } else { latestHipToHandController = glm::vec2(0.0f, -1.0f); } + + glm::vec3 headLookAtAvatarSpace = transformVectorFast(headPoseAvatarSpace.getMatrix(), glm::vec3(0.0f, 0.0f, 1.0f)); + glm::vec2 headAzimuthAvatarSpace = glm::vec2(headLookAtAvatarSpace.x, headLookAtAvatarSpace.z); + if (glm::length(headAzimuthAvatarSpace) > 0.0f) { + headAzimuthAvatarSpace = glm::normalize(headAzimuthAvatarSpace); + } else { + headAzimuthAvatarSpace = -latestHipToHandController; + } + // check the angular distance from forward and back float cosForwardAngle = glm::dot(latestHipToHandController, oldAzimuthReading); + float cosHeadShoulder = glm::dot(-latestHipToHandController, headAzimuthAvatarSpace); // if we are now closer to the 180 flip of the previous chest forward // then we negate our computed latestHipToHandController to keep the chest from flipping. - if (cosForwardAngle < 0.0f) { + // also check the head to shoulder azimuth difference if we negate. + // don't negate the chest azimuth if this is greater than 100 degrees. + if ((cosForwardAngle < 0.0f) && !(cosHeadShoulder < -0.2f)) { latestHipToHandController = -latestHipToHandController; } } From 9fb0f8b7c00fef665b94b7e56005063b394832f0 Mon Sep 17 00:00:00 2001 From: Angus Antley Date: Thu, 30 Aug 2018 00:25:53 +0100 Subject: [PATCH 23/24] added condition to prevent overwriting tracker input with estimated orientation for spine2 joint --- interface/src/avatar/MySkeletonModel.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/interface/src/avatar/MySkeletonModel.cpp b/interface/src/avatar/MySkeletonModel.cpp index 8fd7d31956..77d1a87195 100644 --- a/interface/src/avatar/MySkeletonModel.cpp +++ b/interface/src/avatar/MySkeletonModel.cpp @@ -239,7 +239,10 @@ 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() && myAvatar->getControllerPoseInAvatarFrame(controller::Action::LEFT_HAND).isValid()) { + if (myAvatar->getControllerPoseInAvatarFrame(controller::Action::RIGHT_HAND).isValid() && + myAvatar->getControllerPoseInAvatarFrame(controller::Action::LEFT_HAND).isValid() && + !(params.primaryControllerFlags[Rig::PrimaryControllerType_Spine2] & (uint8_t)Rig::ControllerFlags::Enabled)) { + AnimPose currentSpine2Pose; AnimPose currentHeadPose; AnimPose currentHipsPose; From 8b1fcfcce909c735b448df52b2ada545e0f042c1 Mon Sep 17 00:00:00 2001 From: David Back Date: Thu, 30 Aug 2018 10:16:27 -0700 Subject: [PATCH 24/24] fix double windows platform --- interface/src/Application.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index c365e61624..eae42ea32e 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -913,7 +913,7 @@ bool setupEssentials(int& argc, char** argv, bool runningMarkerExisted) { controller::StateController::setStateVariables({ { STATE_IN_HMD, STATE_CAMERA_FULL_SCREEN_MIRROR, STATE_CAMERA_FIRST_PERSON, STATE_CAMERA_THIRD_PERSON, STATE_CAMERA_ENTITY, STATE_CAMERA_INDEPENDENT, STATE_SNAP_TURN, STATE_ADVANCED_MOVEMENT_CONTROLS, STATE_GROUNDED, STATE_NAV_FOCUSED, - STATE_PLATFORM_WINDOWS, STATE_PLATFORM_MAC, STATE_PLATFORM_WINDOWS } }); + STATE_PLATFORM_WINDOWS, STATE_PLATFORM_MAC, STATE_PLATFORM_ANDROID } }); DependencyManager::set(); DependencyManager::set(); DependencyManager::set();