From 3a10cacec003d840cf7bd283d70cacec2ac8e2cb Mon Sep 17 00:00:00 2001 From: amantley Date: Thu, 19 Apr 2018 14:04:54 -0700 Subject: [PATCH 01/16] 1.added the swing twist code to update the follow rotation in MyAvatar.cpp 2.added step.js and cg_lean.js to the developer folder 3.added menu item for toggling the hips following between head and hips 4.added new function to return true if the root of MyAvatar is translating to be under the head. Used to smooth the path of the hips 5.added computeCounterBalance to MyAvatar.h and cpp 6.added the menu item under developer/avatar to enable debug draw of the base of support in menu.h menu.cpp MyAvatar.h MyAvatar.cpp 7.added head yaw into the calculation of the hip rotation for the center of gravity. This is already was happening in the deriveBodyFromHMD code that is default 8.Changed Constants in Avatar constants for the base of support 9.fixed the over rotation of the shoulders 10.fixed scaling problem in cg computation 11.added room for going up on the toes without stretching --- interface/src/Menu.cpp | 4 + interface/src/Menu.h | 2 + interface/src/avatar/MyAvatar.cpp | 276 ++++++++++- interface/src/avatar/MyAvatar.h | 52 ++- interface/src/avatar/MySkeletonModel.cpp | 16 +- libraries/shared/src/AvatarConstants.h | 4 + scripts/defaultScripts.js | 4 +- scripts/developer/cg_lean.js | 553 +++++++++++++++++++++++ tests/animation/src/data/avatar.json | 2 +- 9 files changed, 902 insertions(+), 11 deletions(-) create mode 100644 scripts/developer/cg_lean.js diff --git a/interface/src/Menu.cpp b/interface/src/Menu.cpp index bf0fc05350..41831bf3c5 100644 --- a/interface/src/Menu.cpp +++ b/interface/src/Menu.cpp @@ -600,6 +600,10 @@ Menu::Menu() { }); addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::FixGaze, 0, false); + addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::ToggleHipsFollowing, 0, false, + avatar.get(), SLOT(setToggleHips(bool))); + addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::AnimDebugDrawBaseOfSupport, 0, false, + avatar.get(), SLOT(setEnableDebugDrawBaseOfSupport(bool))); addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::AnimDebugDrawDefaultPose, 0, false, avatar.get(), SLOT(setEnableDebugDrawDefaultPose(bool))); addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::AnimDebugDrawAnimPose, 0, false, diff --git a/interface/src/Menu.h b/interface/src/Menu.h index 20375a71b2..936062b960 100644 --- a/interface/src/Menu.h +++ b/interface/src/Menu.h @@ -30,6 +30,7 @@ namespace MenuOption { const QString AddressBar = "Show Address Bar"; const QString Animations = "Animations..."; const QString AnimDebugDrawAnimPose = "Debug Draw Animation"; + const QString AnimDebugDrawBaseOfSupport = "Debug Draw Base of Support"; const QString AnimDebugDrawDefaultPose = "Debug Draw Default Pose"; const QString AnimDebugDrawPosition= "Debug Draw Position"; const QString AskToResetSettings = "Ask To Reset Settings"; @@ -203,6 +204,7 @@ namespace MenuOption { const QString ThirdPerson = "Third Person"; const QString ThreePointCalibration = "3 Point Calibration"; const QString ThrottleFPSIfNotFocus = "Throttle FPS If Not Focus"; // FIXME - this value duplicated in Basic2DWindowOpenGLDisplayPlugin.cpp + const QString ToggleHipsFollowing = "Toggle Hips Following"; const QString ToolWindow = "Tool Window"; const QString TransmitterDrive = "Transmitter Drive"; const QString TurnWithHead = "Turn using Head"; diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 15b220c63b..c3df9f6143 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -1068,6 +1068,22 @@ float loadSetting(Settings& settings, const QString& name, float defaultValue) { return value; } +void MyAvatar::setToggleHips(bool followHead) { + _follow.setToggleHipsFollowing(followHead); +} + +void MyAvatar::FollowHelper::setToggleHipsFollowing(bool followHead) { + _toggleHipsFollowing = followHead; +} + +bool MyAvatar::FollowHelper::getToggleHipsFollowing() const { + return _toggleHipsFollowing; +} + +void MyAvatar::setEnableDebugDrawBaseOfSupport(bool isEnabled) { + _enableDebugDrawBaseOfSupport = isEnabled; +} + void MyAvatar::setEnableDebugDrawDefaultPose(bool isEnabled) { _enableDebugDrawDefaultPose = isEnabled; @@ -1195,6 +1211,8 @@ void MyAvatar::loadData() { settings.endGroup(); setEnableMeshVisible(Menu::getInstance()->isOptionChecked(MenuOption::MeshVisible)); + _follow.setToggleHipsFollowing (Menu::getInstance()->isOptionChecked(MenuOption::ToggleHipsFollowing)); + setEnableDebugDrawBaseOfSupport(Menu::getInstance()->isOptionChecked(MenuOption::AnimDebugDrawBaseOfSupport)); setEnableDebugDrawDefaultPose(Menu::getInstance()->isOptionChecked(MenuOption::AnimDebugDrawDefaultPose)); setEnableDebugDrawAnimPose(Menu::getInstance()->isOptionChecked(MenuOption::AnimDebugDrawAnimPose)); setEnableDebugDrawPosition(Menu::getInstance()->isOptionChecked(MenuOption::AnimDebugDrawPosition)); @@ -2829,6 +2847,245 @@ glm::mat4 MyAvatar::deriveBodyFromHMDSensor() const { return createMatFromQuatAndPos(headOrientationYawOnly, bodyPos); } +float slope(float num) { + float constantK = 1.0; + float ret = 1.0f; + if (num > 0.0f) { + ret = 1.0f - (1.0f / (1.0f + constantK * num)); + } + return ret; +} + +glm::vec3 dampenCgMovement(glm::vec3 rawCg, float baseOfSupportScale) { + float distanceFromCenterZ = rawCg.z; + float distanceFromCenterX = rawCg.x; + + // The dampening scale factors makes the slope function soft clamp the + // cg at the edge of the base of support of the feet, in the lateral and posterior directions. + // In the forward direction we need a different scale because forward is in + // the direction of the hip extensor joint, which means bending usually happens + // well before reaching the edge of the base of support. + // The scale of the base of support reflects the size of the user in real life. + float forwardDampeningFactor = 0.5f; + float lateralAndBackDampeningScaleFactor = 2.0f; + float clampFront = DEFAULT_AVATAR_SUPPORT_BASE_FRONT * forwardDampeningFactor * baseOfSupportScale; + float clampBack = DEFAULT_AVATAR_SUPPORT_BASE_BACK * lateralAndBackDampeningScaleFactor * baseOfSupportScale; + float clampLeft = DEFAULT_AVATAR_SUPPORT_BASE_LEFT * lateralAndBackDampeningScaleFactor * baseOfSupportScale; + float clampRight = DEFAULT_AVATAR_SUPPORT_BASE_RIGHT * lateralAndBackDampeningScaleFactor * baseOfSupportScale; + glm::vec3 dampedCg = {0.0f,0.0f,0.0f}; + + // find the damped z coord of the cg + if (rawCg.z < 0.0f) { + // forward displacement + float inputFront; + inputFront = fabs(distanceFromCenterZ / clampFront); + float scaleFrontNew = slope(inputFront); + dampedCg.z = scaleFrontNew * clampFront; + } else { + // backwards displacement + float inputBack; + inputBack = fabs(distanceFromCenterZ / clampBack); + float scaleBackNew = slope(inputBack); + dampedCg.z = scaleBackNew * clampBack; + } + + // find the damped x coord of the cg + if (rawCg.x > 0.0f) { + // right of center + float inputRight; + inputRight = fabs(distanceFromCenterX / clampRight); + float scaleRightNew = slope(inputRight); + dampedCg.x = scaleRightNew * clampRight; + } else { + // left of center + float inputLeft; + inputLeft = fabs(distanceFromCenterX / clampLeft); + float scaleLeftNew = slope(inputLeft); + dampedCg.x = scaleLeftNew * clampLeft; + } + return dampedCg; +} + +glm::vec3 MyAvatar::computeCounterBalance() const { + struct jointMass { + QString name; + float weight; + glm::vec3 position; + } cgMasses[3]; + // init the body part weights + cgMasses[0].name = "Head"; + cgMasses[0].weight = 20.0f; + cgMasses[0].position = { 0.0f, 0.0f, 0.0f }; + cgMasses[1].name = "LeftHand"; + cgMasses[1].weight = 2.0f; + cgMasses[1].position = { 0.0f, 0.0f, 0.0f }; + cgMasses[2].name = "RightHand"; + cgMasses[2].weight = 2.0f; + cgMasses[2].position = { 0.0f, 0.0f, 0.0f }; + // find the current center of gravity position based on head and hand moments + float hipsMass = 40.0f; + float totalMass = 0.0f; + glm::vec3 sumOfMoments = { 0.0f, 0.0f, 0.0f }; + for (int i = 0; i < 3; i++) { + const QString jointName = cgMasses[i].name; + cgMasses[i].position = getAbsoluteJointTranslationInObjectFrame(_skeletonModel->getRig().indexOfJoint(jointName)); + sumOfMoments += cgMasses[i].weight * cgMasses[i].position; + totalMass += cgMasses[i].weight; + } + glm::vec3 currentCg = (1 / totalMass) * sumOfMoments; + currentCg.y = 0.0f; + // dampening the center of gravity, in effect, limits the value to the perimeter of the base of support + float baseScale = 1.0f; + if (getUserEyeHeight() > 0.0f) { + baseScale = getUserEyeHeight() / DEFAULT_AVATAR_EYE_HEIGHT; + } + glm::vec3 desiredCg = dampenCgMovement(currentCg, baseScale); + + // compute hips position to maintain desiredCg + glm::vec3 counterBalancedForHead = ((totalMass + hipsMass) * desiredCg) - (cgMasses[0].position * cgMasses[0].weight); + glm::vec3 counterBalancedForLeftHand = counterBalancedForHead - (cgMasses[1].weight * cgMasses[1].position); + glm::vec3 counterBalancedForRightHand = counterBalancedForLeftHand - (cgMasses[2].weight * cgMasses[2].position); + glm::vec3 counterBalancedCg = (1.0f / hipsMass) * counterBalancedForRightHand; + + // find the height of the hips + glm::vec3 currentHead = getAbsoluteJointTranslationInObjectFrame(_skeletonModel->getRig().indexOfJoint("Head")); + glm::vec3 tposeHead = getAbsoluteDefaultJointTranslationInObjectFrame(_skeletonModel->getRig().indexOfJoint("Head")); + glm::vec3 tposeHips = getAbsoluteDefaultJointTranslationInObjectFrame(_skeletonModel->getRig().indexOfJoint("Hips")); + glm::vec3 xzDiff = {(currentHead.x - counterBalancedCg.x), 0.0f, (currentHead.z - counterBalancedCg.z)}; + float headMinusHipXz = glm::length(xzDiff); + float headHipDefault = glm::length(tposeHead - tposeHips); + float hipHeight = 0.0f; + if (headHipDefault > headMinusHipXz) { + hipHeight = sqrtf((headHipDefault * headHipDefault) - (headMinusHipXz * headMinusHipXz)); + } + counterBalancedCg.y = (currentHead.y - hipHeight); + + // this is to be sure that the feet don't lift off the floor. + // add 5 centimeters to allow for going up on the toes. + if (counterBalancedCg.y > (tposeHips.y + 0.05f)) { + // if the height is higher than default hips, clamp to default hips + counterBalancedCg.y = tposeHips.y + 0.05f; + } + return counterBalancedCg; +} + +glm::quat computeNewHipsRotation(glm::quat hipYawRot, glm::vec3 curHead, glm::vec3 hipPos) { + glm::vec3 spineVec = curHead - hipPos; + glm::quat finalRot = Quaternions::IDENTITY; + + if (spineVec.y > 0.0f) { + + glm::vec3 newYaxisHips = glm::normalize(spineVec); + glm::vec3 forward = { 0.0f, 0.0f, 1.0f }; + glm::vec3 oldZaxisHips = glm::normalize(forward); + glm::vec3 newXaxisHips = glm::normalize(glm::cross(newYaxisHips, oldZaxisHips)); + glm::vec3 newZaxisHips = glm::normalize(glm::cross(newXaxisHips, newYaxisHips)); + + // create mat4 with the new axes + glm::vec4 left = { newXaxisHips.x, newXaxisHips.y, newXaxisHips.z, 0.0f }; + glm::vec4 up = { newYaxisHips.x, newYaxisHips.y, newYaxisHips.z, 0.0f }; + glm::vec4 view = { newZaxisHips.x, newZaxisHips.y, newZaxisHips.z, 0.0f }; + glm::vec4 translation = { 0.0f, 0.0f, 0.0f, 1.0f }; + glm::mat4 newRotHips(left, up, view, translation); + finalRot = glm::toQuat(newRotHips); + } else if (spineVec.y < 0.0f) { + + glm::vec3 newYaxisHips = glm::normalize(-spineVec); + glm::vec3 forward = { 0.0f, 0.0f, 1.0f }; + glm::vec3 oldZaxisHips = glm::normalize(forward); + glm::vec3 newXaxisHips = glm::normalize(glm::cross(newYaxisHips, oldZaxisHips)); + glm::vec3 newZaxisHips = glm::normalize(glm::cross(newXaxisHips, newYaxisHips)); + + // create mat4 with the new axes + glm::vec4 left = { newXaxisHips.x, newXaxisHips.y, newXaxisHips.z, 0.0f }; + glm::vec4 up = { newYaxisHips.x, newYaxisHips.y, newYaxisHips.z, 0.0f }; + glm::vec4 view = { newZaxisHips.x, newZaxisHips.y, newZaxisHips.z, 0.0f }; + glm::vec4 translation = { 0.0f, 0.0f, 0.0f, 1.0f }; + glm::mat4 newRotHips(left, up, view, translation); + finalRot = glm::toQuat(newRotHips); + } else { + + //y equals zero. + if (glm::length(spineVec) > 0.0f) { + glm::vec3 newYaxisHips = glm::normalize(spineVec); + glm::vec3 forward = { 0.0f, 1.0f, 0.0f }; + glm::vec3 oldZaxisHips = forward; + glm::vec3 newXaxisHips = glm::normalize(glm::cross(newYaxisHips, oldZaxisHips)); + glm::vec3 newZaxisHips = oldZaxisHips; + + // create mat4 with the new axes + glm::vec4 left = { newXaxisHips.x, newXaxisHips.y, newXaxisHips.z, 0.0f }; + glm::vec4 up = { newYaxisHips.x, newYaxisHips.y, newYaxisHips.z, 0.0f }; + glm::vec4 view = { newZaxisHips.x, newZaxisHips.y, newZaxisHips.z, 0.0f }; + glm::vec4 translation = { 0.0f, 0.0f, 0.0f, 1.0f }; + glm::mat4 newRotHips(left, up, view, translation); + finalRot = glm::toQuat(newRotHips); + } + // otherwise, head and hips are equal so leave finalRot identity + } + glm::quat hipsRotation = hipYawRot*finalRot; + return hipsRotation; +} + +void drawBaseOfSupport(float baseOfSupportScale, float footLocal, glm::mat4 avatarToWorld) { + // scale the base of support based on user height + float clampFront = DEFAULT_AVATAR_SUPPORT_BASE_FRONT * baseOfSupportScale; + float clampBack = DEFAULT_AVATAR_SUPPORT_BASE_BACK * baseOfSupportScale; + float clampLeft = DEFAULT_AVATAR_SUPPORT_BASE_LEFT * baseOfSupportScale; + float clampRight = DEFAULT_AVATAR_SUPPORT_BASE_RIGHT * baseOfSupportScale; + float floor = footLocal + 0.05f; + + // transform the base of support corners to world space + glm::vec3 frontRight = transformPoint(avatarToWorld, { clampRight, floor, clampFront }); + glm::vec3 frontLeft = transformPoint(avatarToWorld, { clampLeft, floor, clampFront }); + glm::vec3 backRight = transformPoint(avatarToWorld, { clampRight, floor, clampBack }); + glm::vec3 backLeft = transformPoint(avatarToWorld, { clampLeft, floor, clampBack }); + + // draw the borders + const glm::vec4 rayColor = { 1.0f, 0.0f, 0.0f, 1.0f }; + DebugDraw::getInstance().drawRay(backLeft, frontLeft, rayColor); + DebugDraw::getInstance().drawRay(backLeft, backRight, rayColor); + DebugDraw::getInstance().drawRay(backRight, frontRight, rayColor); + DebugDraw::getInstance().drawRay(frontLeft, frontRight, rayColor); +} + +glm::mat4 MyAvatar::deriveBodyUsingCgModel() const { + glm::mat4 worldToSensorMat = glm::inverse(getSensorToWorldMatrix()); + glm::mat4 avatarToWorldMat = getTransform().getMatrix(); + glm::mat4 avatarToSensorMat = worldToSensorMat * avatarToWorldMat; + + glm::vec3 headPosition; + glm::quat headOrientation; + auto headPose = getControllerPoseInSensorFrame(controller::Action::HEAD); + if (headPose.isValid()) { + headPosition = headPose.translation; + // rotate by 180 Y to put the head in same frame as the avatar + headOrientation = headPose.rotation * Quaternions::Y_180; + } + const glm::quat headOrientationYawOnly = cancelOutRollAndPitch(headOrientation); + const float MIX_RATIO = 0.15f; + // here we mix in some of the head yaw into the hip yaw + glm::quat hipYawRot = glm::normalize(glm::lerp(glmExtractRotation(avatarToSensorMat), headOrientationYawOnly, MIX_RATIO)); + glm::quat deltaRot = glm::inverse(glmExtractRotation(avatarToSensorMat))*hipYawRot; + glm::vec3 headPositionLocal = getAbsoluteJointTranslationInObjectFrame(_skeletonModel->getRig().indexOfJoint("Head")); + glm::vec3 headLocalAfterDelta = glm::inverse(deltaRot)*headPositionLocal; + + if (_enableDebugDrawBaseOfSupport) { + // default height is ~ 1.64 meters + float scaleBaseOfSupport = getUserEyeHeight() / DEFAULT_AVATAR_EYE_HEIGHT; + glm::vec3 rightFootPositionLocal = getAbsoluteJointTranslationInObjectFrame(_skeletonModel->getRig().indexOfJoint("RightFoot")); + drawBaseOfSupport(scaleBaseOfSupport, rightFootPositionLocal.y, avatarToWorldMat); + } + + // get the new center of gravity + const glm::vec3 cgHipsPosition = computeCounterBalance(); + glm::vec3 hipsPositionFinal = transformPoint(avatarToSensorMat, cgHipsPosition); + + //find the new hips rotation using the new head-hips axis as the up axis + glm::quat newHipsRotation = computeNewHipsRotation( hipYawRot, headLocalAfterDelta, cgHipsPosition); + return createMatFromQuatAndPos(newHipsRotation, hipsPositionFinal); +} + float MyAvatar::getUserHeight() const { return _userHeight.get(); } @@ -3066,11 +3323,24 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat AnimPose followWorldPose(currentWorldMatrix); + glm::quat currentHipsLocal = myAvatar.getAbsoluteJointRotationInObjectFrame(myAvatar.getJointIndex("Hips")); + const glm::quat hipsinWorldSpace = followWorldPose.rot() * (Quaternions::Y_180 * (currentHipsLocal)); + const glm::vec3 avatarUpWorld = glm::normalize(followWorldPose.rot()*(Vectors::UP)); + glm::quat resultingSwingInWorld; + glm::quat resultingTwistInWorld; + swingTwistDecomposition(hipsinWorldSpace, avatarUpWorld, resultingSwingInWorld, resultingTwistInWorld); + // remove scale present from sensorToWorldMatrix followWorldPose.scale() = glm::vec3(1.0f); if (isActive(Rotation)) { - followWorldPose.rot() = glmExtractRotation(desiredWorldMatrix); + if (getToggleHipsFollowing()) { + //use the hmd reading for the hips follow + followWorldPose.rot() = glmExtractRotation(desiredWorldMatrix); + } else { + //use the hips as changed by the arms azimuth for the hips to follow. + followWorldPose.rot() = resultingTwistInWorld; + } } if (isActive(Horizontal)) { glm::vec3 desiredTranslation = extractTranslation(desiredWorldMatrix); @@ -3466,6 +3736,10 @@ void MyAvatar::updateHoldActions(const AnimPose& prePhysicsPose, const AnimPose& } } +bool MyAvatar::isRecenteringHorizontally() const { + return _follow.isActive(FollowHelper::Horizontal); +} + const MyHead* MyAvatar::getMyHead() const { return static_cast(getHead()); } diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index ac3d3cd2f4..3293109004 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -105,6 +105,9 @@ class MyAvatar : public Avatar { * by 30cm. Read-only. * @property {Pose} rightHandTipPose - The pose of the right hand as determined by the hand controllers, with the position * by 30cm. Read-only. + * @property {boolean} centerOfGravityModelEnabled=true - If true then the avatar hips are placed according to the center of + * gravity model that balance the center of gravity over the base of support of the feet. Setting the value false + * will result in the default behaviour where the hips are placed under the head. * @property {boolean} hmdLeanRecenterEnabled=true - If true then the avatar is re-centered to be under the * head's position. In room-scale VR, this behavior is what causes your avatar to follow your HMD as you walk around * the room. Setting the value false is useful if you want to pin the avatar to a fixed position. @@ -199,6 +202,7 @@ class MyAvatar : public Avatar { Q_PROPERTY(float energy READ getEnergy WRITE setEnergy) Q_PROPERTY(bool isAway READ getIsAway WRITE setAway) + Q_PROPERTY(bool centerOfGravityModelEnabled READ getCenterOfGravityModelEnabled WRITE setCenterOfGravityModelEnabled) Q_PROPERTY(bool hmdLeanRecenterEnabled READ getHMDLeanRecenterEnabled WRITE setHMDLeanRecenterEnabled) Q_PROPERTY(bool collisionsEnabled READ getCollisionsEnabled WRITE setCollisionsEnabled) Q_PROPERTY(bool characterControllerEnabled READ getCharacterControllerEnabled WRITE setCharacterControllerEnabled) @@ -480,7 +484,16 @@ public: */ Q_INVOKABLE QString getDominantHand() const { return _dominantHand; } - + /**jsdoc + * @function MyAvatar.setCenterOfGravityModelEnabled + * @param {boolean} enabled + */ + Q_INVOKABLE void setCenterOfGravityModelEnabled(bool value) { _centerOfGravityModelEnabled = value; } + /**jsdoc + * @function MyAvatar.getCenterOfGravityModelEnabled + * @returns {boolean} + */ + Q_INVOKABLE bool getCenterOfGravityModelEnabled() const { return _centerOfGravityModelEnabled; } /**jsdoc * @function MyAvatar.setHMDLeanRecenterEnabled * @param {boolean} enabled @@ -564,6 +577,13 @@ public: */ Q_INVOKABLE void triggerRotationRecenter(); + /**jsdoc + *The isRecenteringHorizontally function returns true if MyAvatar + *is translating the root of the Avatar to keep the center of gravity under the head. + *isActive(Horizontal) is returned. + *@function MyAvatar.isRecenteringHorizontally + */ + Q_INVOKABLE bool isRecenteringHorizontally() const; eyeContactTarget getEyeContactTarget(); @@ -956,10 +976,18 @@ public: void removeHoldAction(AvatarActionHold* holdAction); // thread-safe void updateHoldActions(const AnimPose& prePhysicsPose, const AnimPose& postUpdatePose); + // derive avatar body position and orientation from the current HMD Sensor location. // results are in HMD frame glm::mat4 deriveBodyFromHMDSensor() const; + glm::vec3 computeCounterBalance() const; + + // derive avatar body position and orientation from using the current HMD Sensor location in relation to the previous + // location of the base of support of the avatar. + // results are in HMD frame + glm::mat4 deriveBodyUsingCgModel() const; + /**jsdoc * @function MyAvatar.isUp * @param {Vec3} direction @@ -1107,7 +1135,16 @@ public slots: */ Q_INVOKABLE void updateMotionBehaviorFromMenu(); - + /**jsdoc + * @function MyAvatar.setToggleHips + * @param {boolean} enabled + */ + void setToggleHips(bool followHead); + /**jsdoc + * @function MyAvatar.setEnableDebugDrawBaseOfSupport + * @param {boolean} enabled + */ + void setEnableDebugDrawBaseOfSupport(bool isEnabled); /**jsdoc * @function MyAvatar.setEnableDebugDrawDefaultPose * @param {boolean} enabled @@ -1495,9 +1532,12 @@ private: void setForceActivateVertical(bool val); bool getForceActivateHorizontal() const; void setForceActivateHorizontal(bool val); - std::atomic _forceActivateRotation{ false }; - std::atomic _forceActivateVertical{ false }; - std::atomic _forceActivateHorizontal{ false }; + bool getToggleHipsFollowing() const; + void setToggleHipsFollowing(bool followHead); + std::atomic _forceActivateRotation { false }; + std::atomic _forceActivateVertical { false }; + std::atomic _forceActivateHorizontal { false }; + std::atomic _toggleHipsFollowing { true }; }; FollowHelper _follow; @@ -1510,6 +1550,7 @@ private: bool _prevShouldDrawHead; bool _rigEnabled { true }; + bool _enableDebugDrawBaseOfSupport { false }; bool _enableDebugDrawDefaultPose { false }; bool _enableDebugDrawAnimPose { false }; bool _enableDebugDrawHandControllers { false }; @@ -1532,6 +1573,7 @@ private: std::map _controllerPoseMap; mutable std::mutex _controllerPoseMapMutex; + bool _centerOfGravityModelEnabled { true }; bool _hmdLeanRecenterEnabled { true }; bool _sprint { false }; diff --git a/interface/src/avatar/MySkeletonModel.cpp b/interface/src/avatar/MySkeletonModel.cpp index fd57657d33..f7f55db369 100644 --- a/interface/src/avatar/MySkeletonModel.cpp +++ b/interface/src/avatar/MySkeletonModel.cpp @@ -45,7 +45,14 @@ static AnimPose computeHipsInSensorFrame(MyAvatar* myAvatar, bool isFlying) { return result; } - glm::mat4 hipsMat = myAvatar->deriveBodyFromHMDSensor(); + glm::mat4 hipsMat; + if (myAvatar->getCenterOfGravityModelEnabled()) { + // then we use center of gravity model + hipsMat = myAvatar->deriveBodyUsingCgModel(); + } else { + // otherwise use the default of putting the hips under the head + hipsMat = myAvatar->deriveBodyFromHMDSensor(); + } glm::vec3 hipsPos = extractTranslation(hipsMat); glm::quat hipsRot = glmExtractRotation(hipsMat); @@ -53,8 +60,11 @@ static AnimPose computeHipsInSensorFrame(MyAvatar* myAvatar, bool isFlying) { glm::mat4 avatarToSensorMat = worldToSensorMat * avatarToWorldMat; // dampen hips rotation, by mixing it with the avatar orientation in sensor space - const float MIX_RATIO = 0.5f; - hipsRot = safeLerp(glmExtractRotation(avatarToSensorMat), hipsRot, MIX_RATIO); + // turning this off for center of gravity model because it is already mixed in there + if (!(myAvatar->getCenterOfGravityModelEnabled())) { + const float MIX_RATIO = 0.5f; + hipsRot = safeLerp(glmExtractRotation(avatarToSensorMat), hipsRot, MIX_RATIO); + } if (isFlying) { // rotate the hips back to match the flying animation. diff --git a/libraries/shared/src/AvatarConstants.h b/libraries/shared/src/AvatarConstants.h index 930da6a494..9f0c789b9d 100644 --- a/libraries/shared/src/AvatarConstants.h +++ b/libraries/shared/src/AvatarConstants.h @@ -20,6 +20,10 @@ const float DEFAULT_AVATAR_EYE_TO_TOP_OF_HEAD = 0.11f; // meters const float DEFAULT_AVATAR_NECK_TO_TOP_OF_HEAD = 0.185f; // meters const float DEFAULT_AVATAR_NECK_HEIGHT = DEFAULT_AVATAR_HEIGHT - DEFAULT_AVATAR_NECK_TO_TOP_OF_HEAD; const float DEFAULT_AVATAR_EYE_HEIGHT = DEFAULT_AVATAR_HEIGHT - DEFAULT_AVATAR_EYE_TO_TOP_OF_HEAD; +const float DEFAULT_AVATAR_SUPPORT_BASE_LEFT = -0.25f; +const float DEFAULT_AVATAR_SUPPORT_BASE_RIGHT = 0.25f; +const float DEFAULT_AVATAR_SUPPORT_BASE_FRONT = -0.20f; +const float DEFAULT_AVATAR_SUPPORT_BASE_BACK = 0.10f; // Used when avatar is missing joints... (avatar space) const glm::quat DEFAULT_AVATAR_MIDDLE_EYE_ROT { Quaternions::Y_180 }; diff --git a/scripts/defaultScripts.js b/scripts/defaultScripts.js index 71755e3abb..eec9d8eda6 100644 --- a/scripts/defaultScripts.js +++ b/scripts/defaultScripts.js @@ -33,7 +33,9 @@ var DEFAULT_SCRIPTS_COMBINED = [ "system/emote.js" ]; var DEFAULT_SCRIPTS_SEPARATE = [ - "system/controllers/controllerScripts.js" + "system/controllers/controllerScripts.js", + //"developer/step.js", + //"developer/cg_lean.js" //"system/chat.js" ]; diff --git a/scripts/developer/cg_lean.js b/scripts/developer/cg_lean.js new file mode 100644 index 0000000000..a4ca56d6d6 --- /dev/null +++ b/scripts/developer/cg_lean.js @@ -0,0 +1,553 @@ + +/* global Script, Vec3, MyAvatar Tablet Messages Quat DebugDraw Mat4 Xform*/ + + +Script.include("/~/system/libraries/Xform.js"); + +var MESSAGE_CHANNEL = "Hifi-Step-Cg"; + +var ANIM_VARS = [ + "isTalking", + "isNotMoving", + "isMovingForward", + "isMovingBackward", + "isMovingRight", + "isMovingLeft", + "isTurningRight", + "isTurningLeft", + "isFlying", + "isTakeoffStand", + "isTakeoffRun", + "isInAirStand", + "isInAirRun", + "hipsPosition", + "hipsRotation", + "hipsType", + "headWeight", + "headType" +]; + +var DEBUGDRAWING; +var YELLOW; +var BLUE; +var GREEN; +var RED; + +var ROT_Y90; +var ROT_Y180; +var FLOOR_Y; +var IDENT_QUAT; + +var TABLET_BUTTON_NAME; +var RECENTER; +var JOINT_MASSES; + +var hipsUnderHead; + +var armsHipRotation; +var hipsPosition; +var filteredHipsPosition; +var hipsRotation; + +var jointList; +var rightFootName; +var leftFootName; +var rightToeName; +var leftToeName; +var leftToeEnd; +var rightToeEnd; +var leftFoot; +var rightFoot; +var base; + +var clampFront; +var clampBack; +var clampLeft; +var clampRight; + +var tablet; +var tabletButton; + +function initCg() { + + DEBUGDRAWING = false; + + YELLOW = { r: 1, g: 1, b: 0, a: 1 }; + BLUE = { r: 0, g: 0, b: 1, a: 1 }; + GREEN = { r: 0, g: 1, b: 0, a: 1 }; + RED = { r: 1, g: 0, b: 0, a: 1 }; + + ROT_Y90 = { x: 0, y: 0.7071067811865475, z: 0, w: 0.7071067811865476 }; + ROT_Y180 = { x: 0, y: 1, z: 0, w: 0 }; + FLOOR_Y = -0.9; + IDENT_QUAT = { x: 0, y: 0, z: 0, w: 1 }; + + JOINT_MASSES = [{ joint: "Head", mass: 20.0, pos: { x: 0, y: 0, z: 0 } }, + { joint: "LeftHand", mass: 2.0, pos: { x: 0, y: 0, z: 0 } }, + { joint: "RightHand", mass: 2.0, pos: { x: 0, y: 0, z: 0 } }]; + + TABLET_BUTTON_NAME = "CG"; + RECENTER = false; + + MyAvatar.hmdLeanRecenterEnabled = RECENTER; + hipsUnderHead; + + armsHipRotation = { x: 0, y: 1, z: 0, w: 0 }; + hipsPosition = MyAvatar.getAbsoluteDefaultJointTranslationInObjectFrame(MyAvatar.getJointIndex("Hips")); + filteredHipsPosition = MyAvatar.position; + hipsRotation = { x: 0, y: 0, z: 0, w: 1 }; + + jointList = MyAvatar.getJointNames(); + // print(JSON.stringify(jointList)); + + rightFootName = null; + leftFootName = null; + rightToeName = null; + leftToeName = null; + leftToeEnd = null; + rightToeEnd = null; + leftFoot; + rightFoot; + + clampFront = -0.10; + clampBack = 0.17; + clampLeft = -0.50; + clampRight = 0.50; + + getFeetAndToeNames(); + base = computeBase(); + mirrorPoints(); + + + tablet = Tablet.getTablet("com.highfidelity.interface.tablet.system"); + + tabletButton = tablet.addButton({ + text: TABLET_BUTTON_NAME, + icon: "icons/tablet-icons/avatar-record-i.svg" + }); + + tabletButton.clicked.connect(function () { + print("recenter is: " + RECENTER); + MyAvatar.hmdLeanRecenterEnabled = RECENTER; + RECENTER = !RECENTER; + + // messageSend("clicked button in cg"); + }); + + + var handlerId = MyAvatar.addAnimationStateHandler(function (props) { + + var result = {}; + + // prevent animations from ever leaving the idle state + result.isTalking = false; + result.isFlying = false; + result.isTakeoffStand = false; + result.isTakeoffRun = false; + result.isInAirStand = false; + result.isInAirRun = false; + result.hipsPosition = hipsPosition; + result.hipsRotation = hipsRotation; + result.hipsType = 0; + result.headWeight = 4; + result.headType = 4; + + return result; + }, ANIM_VARS); + + Messages.subscribe(MESSAGE_CHANNEL); + Messages.messageReceived.connect(messageHandler); + Script.update.connect(update); + MyAvatar.skeletonChanged.connect(function () { + Script.setTimeout(function () { + // stop logic if needed + MyAvatar.clearJointsData(); + // reset logic + }, 200); + }); + HMD.displayModeChanged.connect(function () { + Script.setTimeout(function () { + // stop logic if needed + MyAvatar.clearJointsData(); + // reset logic + }, 200); + }); + + +} + +function messageSend(message) { + Messages.sendLocalMessage(MESSAGE_CHANNEL, message); +} + +function messageHandler(channel, messageString, senderID) { + if (channel !== MESSAGE_CHANNEL) { + return; + } + + var hipquat = JSON.parse(messageString); + armsHipRotation = Quat.multiply(ROT_Y180,hipquat); + +} + +function getFeetAndToeNames() { + + for (var i = 0; i < jointList.length; i++) { + if ((jointList[i].indexOf('Right') !== -1) && (jointList[i].indexOf('Foot') !== -1)) { + print(JSON.stringify(jointList[i])); + rightFootName = jointList[i]; + } + if ((jointList[i].indexOf('Left') !== -1) && (jointList[i].indexOf('Foot') !== -1)) { + print(JSON.stringify(jointList[i])); + leftFootName = jointList[i]; + } + if ((jointList[i].indexOf('Right') !== -1) && (jointList[i].indexOf('Toe') !== -1) && (jointList[i].indexOf('End') !== -1)) { + print(JSON.stringify(jointList[i])); + rightToeName = jointList[i]; + } + if ((jointList[i].indexOf('Left') !== -1) && (jointList[i].indexOf('Toe') !== -1) && (jointList[i].indexOf('End') !== -1)) { + print(JSON.stringify(jointList[i])); + leftToeName = jointList[i]; + } + } +} + +function computeBase() { + + if (rightFootName === null || leftFootName === null) { + // if the feet names aren't found then use our best guess of the base. + leftToeEnd = {x: 0.12, y: 0.0, z: 0.12}; + rightToeEnd = {x: -0.18, y: 0.0, z: 0.12}; + leftFoot = {x: 0.15, y: 0.0, z: -0.17}; + rightFoot = {x: -0.20, y: 0.0, z: -0.17}; + } else { + // else we at least found the feet in the skeleton. + var leftFootIndex = MyAvatar.getJointIndex(leftFootName); + var rightFootIndex = MyAvatar.getJointIndex(rightFootName); + var leftFoot = MyAvatar.getAbsoluteJointTranslationInObjectFrame(leftFootIndex); + var rightFoot = MyAvatar.getAbsoluteJointTranslationInObjectFrame(rightFootIndex); + + if (rightToeName === null || leftToeName === null) { + // the toe ends were not found then we use a guess for the length and width of the feet. + leftToeEnd = {x: (leftFoot.x + 0.02), y: 0.0, z: (leftFoot.z - 0.2)}; + rightToeEnd = {x: (rightFoot.x - 0.02), y: 0.0, z: (rightFoot.z - 0.2)}; + } else { + // else we found the toe ends and now we can really compute the base. + var leftToeIndex = MyAvatar.getJointIndex(leftToeName); + var rightToeIndex = MyAvatar.getJointIndex(rightToeName); + leftToeEnd = MyAvatar.getAbsoluteJointTranslationInObjectFrame(leftToeIndex); + rightToeEnd = MyAvatar.getAbsoluteJointTranslationInObjectFrame(rightToeIndex); + } + + } + + // project each point into the FLOOR plane. + var points = [{x: leftToeEnd.x, y: FLOOR_Y, z: leftToeEnd.z}, + {x: rightToeEnd.x, y: FLOOR_Y, z: rightToeEnd.z}, + {x: rightFoot.x, y: FLOOR_Y, z: rightFoot.z}, + {x: leftFoot.x, y: FLOOR_Y, z: rightFoot.z}]; + + // compute normals for each plane + var normal, normals = []; + var n = points.length; + var next, prev; + for (next = 0, prev = n - 1; next < n; prev = next, next++) { + normal = Vec3.multiplyQbyV(ROT_Y90, Vec3.normalize(Vec3.subtract(points[next], points[prev]))); + normals.push(normal); + } + + var TOE_FORWARD_RADIUS = 0.01; + var TOE_SIDE_RADIUS = 0.05; + var HEEL_FORWARD_RADIUS = 0.01; + var HEEL_SIDE_RADIUS = 0.03; + var radii = [ + TOE_SIDE_RADIUS, TOE_FORWARD_RADIUS, TOE_FORWARD_RADIUS, TOE_SIDE_RADIUS, + HEEL_SIDE_RADIUS, HEEL_FORWARD_RADIUS, HEEL_FORWARD_RADIUS, HEEL_SIDE_RADIUS + ]; + + // subdivide base and extrude by the toe and heel radius. + var newPoints = []; + for (next = 0, prev = n - 1; next < n; prev = next, next++) { + newPoints.push(Vec3.sum(points[next], Vec3.multiply(radii[2 * next], normals[next]))); + newPoints.push(Vec3.sum(points[next], Vec3.multiply(radii[(2 * next) + 1], normals[(next + 1) % n]))); + } + + // compute newNormals + var newNormals = []; + n = newPoints.length; + for (next = 0, prev = n - 1; next < n; prev = next, next++) { + normal = Vec3.multiplyQbyV(ROT_Y90, Vec3.normalize(Vec3.subtract(newPoints[next], newPoints[prev]))); + newNormals.push(normal); + } + + for (var j = 0;j Math.abs(base.points[3].x)) { + base.points[3].x = -base.points[0].x; + base.points[2].x = -base.points[1].x; + } else { + base.points[0].x = -base.points[3].x; + base.points[1].x = -base.points[2].x; + } + + if (Math.abs(base.points[4].x) > Math.abs(base.points[7].x)) { + base.points[7].x = -base.points[4].x; + base.points[6].x = -base.points[5].x; + } else { + base.points[4].x = -base.points[7].x; + base.points[5].x = -base.points[6].x; + } + + if (Math.abs(base.points[0].z) > Math.abs(base.points[0].z)) { + base.points[3].z = base.points[0].z; + base.points[2].z = base.points[1].z; + } else { + base.points[0].z = base.points[3].z; + base.points[1].z = base.points[2].z; + } + + if (Math.abs(base.points[4].z) > Math.abs(base.points[7].z)) { + base.points[7].z = base.points[4].z; + base.points[6].z = base.points[5].z; + } else { + base.points[4].z = base.points[7].z; + base.points[5].z = base.points[6].z; + } + + for (var i = 0; i < base.points.length; i++) { + + print("point: " + i + " " + JSON.stringify(base.points[i])); + } + for (var j = 0; j < base.normals.length; j++) { + print("normal: " + j + " " + JSON.stringify(base.normals[j])); + } +} + + +function drawBase(base) { + // transform corners into world space, for rendering. + var xform = new Xform(MyAvatar.orientation, MyAvatar.position); + var worldPoints = base.points.map(function (point) { + return xform.xformPoint(point); + }); + var worldNormals = base.normals.map(function (normal) { + return xform.xformVector(normal); + }); + + var n = worldPoints.length; + var next, prev; + for (next = 0, prev = n - 1; next < n; prev = next, next++) { + if (DEBUGDRAWING) { + // draw border + DebugDraw.drawRay(worldPoints[prev], worldPoints[next], GREEN); + DebugDraw.drawRay(worldPoints[next], worldPoints[prev], GREEN); + + // draw normal + var midPoint = Vec3.multiply(0.5, Vec3.sum(worldPoints[prev], worldPoints[next])); + DebugDraw.drawRay(midPoint, Vec3.sum(midPoint, worldNormals[next]), YELLOW); + DebugDraw.drawRay(midPoint, Vec3.sum(midPoint, worldNormals[next+1]), YELLOW); + } + } +} + +function computeCg() { + // point mass. + var n = JOINT_MASSES.length; + var moments = {x: 0, y: 0, z: 0}; + var masses = 0; + for (var i = 0; i < n; i++) { + var pos = MyAvatar.getAbsoluteJointTranslationInObjectFrame(MyAvatar.getJointIndex(JOINT_MASSES[i].joint)); + JOINT_MASSES[i].pos = pos; + moments = Vec3.sum(moments, Vec3.multiply(JOINT_MASSES[i].mass, pos)); + masses += JOINT_MASSES[i].mass; + } + return Vec3.multiply(1 / masses, moments); +} + + +function clamp(val, min, max) { + return Math.max(min, Math.min(max, val)); +} + +function distancetoline(p1,p2,cg) { + var numerator = Math.abs((p2.z - p1.z)*(cg.x) - (p2.x - p1.x)*(cg.z) + (p2.x)*(p1.z) - (p2.z)*(p1.x)); + var denominator = Math.sqrt( Math.pow((p2.z - p1.z),2) + Math.pow((p2.x - p1.x),2)); + + return numerator/denominator; +} + +function isLeft(a, b, c) { + return (((b.x - a.x)*(c.z - a.z) - (b.z - a.z)*(c.x - a.x)) > 0); +} + +function slope(num) { + var constant = 1.0; + return 1 - ( 1/(1+constant*num)); +} + +function dampenCgMovement(rawCg) { + + var distanceFromCenterZ = rawCg.z; + var distanceFromCenterX = rawCg.x; + + // clampFront = -0.10; + // clampBack = 0.17; + // clampLeft = -0.50; + // clampRight = 0.50; + + var dampedCg = { x: 0, y: 0, z: 0 }; + + if (rawCg.z < 0.0) { + var inputFront; + inputFront = Math.abs(distanceFromCenterZ / clampFront); + var scaleFrontNew = slope(inputFront); + dampedCg.z = scaleFrontNew * clampFront; + } else { + // cg.z > 0.0 + var inputBack; + inputBack = Math.abs(distanceFromCenterZ / clampBack); + var scaleBackNew = slope(inputBack); + dampedCg.z = scaleBackNew * clampBack; + } + + if (rawCg.x > 0.0) { + var inputRight; + inputRight = Math.abs(distanceFromCenterX / clampRight); + var scaleRightNew = slope(inputRight); + dampedCg.x = scaleRightNew * clampRight; + } else { + // left of center + var inputLeft; + inputLeft = Math.abs(distanceFromCenterX / clampLeft); + var scaleLeftNew = slope(inputLeft); + dampedCg.x = scaleLeftNew * clampLeft; + } + return dampedCg; +} + +function computeCounterBalance(desiredCgPos) { + // compute hips position to maintain desiredCg + var HIPS_MASS = 40; + var totalMass = JOINT_MASSES.reduce(function (accum, obj) { + return accum + obj.mass; + }, 0); + var temp1 = Vec3.subtract(Vec3.multiply(totalMass + HIPS_MASS, desiredCgPos), + Vec3.multiply(JOINT_MASSES[0].mass, JOINT_MASSES[0].pos)); + var temp2 = Vec3.subtract(temp1, + Vec3.multiply(JOINT_MASSES[1].mass, JOINT_MASSES[1].pos)); + var temp3 = Vec3.subtract(temp2, + Vec3.multiply(JOINT_MASSES[2].mass, JOINT_MASSES[2].pos)); + var temp4 = Vec3.multiply(1 / HIPS_MASS, temp3); + + + var currentHead = MyAvatar.getAbsoluteJointTranslationInObjectFrame(MyAvatar.getJointIndex("Head")); + var tposeHead = MyAvatar.getAbsoluteDefaultJointTranslationInObjectFrame(MyAvatar.getJointIndex("Head")); + var tposeHips = MyAvatar.getAbsoluteDefaultJointTranslationInObjectFrame(MyAvatar.getJointIndex("Hips")); + + var xzDiff = { x: (currentHead.x - temp4.x), y: 0, z: (currentHead.z - temp4.z) }; + var headMinusHipXz = Vec3.length(xzDiff); + + var headHipDefault = Vec3.length(Vec3.subtract(tposeHead, tposeHips)); + + var hipHeight = Math.sqrt((headHipDefault * headHipDefault) - (headMinusHipXz * headMinusHipXz)); + + temp4.y = (currentHead.y - hipHeight); + if (temp4.y > tposeHips.y) { + temp4.y = 0.0; + } + return temp4; +} + +function update(dt) { + + var cg = computeCg(); + // print("time elapsed " + dt); + + var desiredCg = { x: 0, y: 0, z: 0 }; + // print("the raw cg " + cg.x + " " + cg.y + " " + cg.z); + + desiredCg.x = cg.x; + desiredCg.y = 0; + desiredCg.z = cg.z; + + desiredCg = dampenCgMovement(cg); + + cg.y = FLOOR_Y; + + // after the dampening above it might be right to clamp the desiredcg to the edge of the base + // of support. + + if (DEBUGDRAWING) { + DebugDraw.addMyAvatarMarker("left toe", IDENT_QUAT, leftToeEnd, BLUE); + DebugDraw.addMyAvatarMarker("right toe", IDENT_QUAT, rightToeEnd, BLUE); + DebugDraw.addMyAvatarMarker("cg", IDENT_QUAT, cg, BLUE); + DebugDraw.addMyAvatarMarker("desiredCg", IDENT_QUAT, desiredCg, GREEN); + drawBase(base); + } + + var currentHeadPos = MyAvatar.getAbsoluteJointTranslationInObjectFrame(MyAvatar.getJointIndex("Head")); + var localHipsPos = computeCounterBalance(desiredCg); + // print("current hips " + cg.x + " " + cg.y + " " + cg.z); + // print("dampened hips " + desiredCg.x + " " + desiredCg.y + " " + desiredCg.z) + + var globalPosRoot = MyAvatar.position; + var globalRotRoot = Quat.normalize(MyAvatar.orientation); + var inverseGlobalRotRoot = Quat.normalize(Quat.inverse(globalRotRoot)); + var globalPosHips = Vec3.sum(globalPosRoot, Vec3.multiplyQbyV(globalRotRoot, localHipsPos)); + var unRotatedHipsPosition; + + if (!MyAvatar.isRecenteringHorizontally()) { + + filteredHipsPosition = Vec3.mix(filteredHipsPosition, globalPosHips, 0.1); + unRotatedHipsPosition = Vec3.multiplyQbyV(inverseGlobalRotRoot, Vec3.subtract(filteredHipsPosition, globalPosRoot)); + hipsPosition = Vec3.multiplyQbyV(ROT_Y180, unRotatedHipsPosition); + } else { + // DebugDraw.addMarker("hipsunder", IDENT_QUAT, hipsUnderHead, GREEN); + filteredHipsPosition = Vec3.mix(filteredHipsPosition, globalPosHips, 0.1); + unRotatedHipsPosition = Vec3.multiplyQbyV(inverseGlobalRotRoot, Vec3.subtract(filteredHipsPosition, globalPosRoot)); + hipsPosition = Vec3.multiplyQbyV(ROT_Y180, unRotatedHipsPosition); + } + + var newYaxisHips = Vec3.normalize(Vec3.subtract(currentHeadPos, unRotatedHipsPosition)); + var forward = { x: 0.0, y: 0.0, z: 1.0 }; + + // arms hip rotation is sent from the step script + var oldZaxisHips = Vec3.normalize(Vec3.multiplyQbyV(armsHipRotation, forward)); + var newXaxisHips = Vec3.normalize(Vec3.cross(newYaxisHips, oldZaxisHips)); + var newZaxisHips = Vec3.normalize(Vec3.cross(newXaxisHips, newYaxisHips)); + + // var beforeHips = MyAvatar.getAbsoluteJointRotationInObjectFrame(MyAvatar.getJointIndex("Hips")); + var left = { x: newXaxisHips.x, y: newXaxisHips.y, z: newXaxisHips.z, w: 0.0 }; + var up = { x: newYaxisHips.x, y: newYaxisHips.y, z: newYaxisHips.z, w: 0.0 }; + var view = { x: newZaxisHips.x, y: newZaxisHips.y, z: newZaxisHips.z, w: 0.0 }; + + var translation = { x: 0.0, y: 0.0, z: 0.0, w: 1.0 }; + var newRotHips = Mat4.createFromColumns(left, up, view, translation); + var finalRot = Mat4.extractRotation(newRotHips); + + hipsRotation = Quat.multiply(ROT_Y180, finalRot); + print("final rot" + finalRot.x + " " + finalRot.y + " " + finalRot.z + " " + finalRot.w); + + if (DEBUGDRAWING) { + DebugDraw.addMyAvatarMarker("hipsPos", IDENT_QUAT, hipsPosition, RED); + } +} + + +Script.setTimeout(initCg, 10); +Script.scriptEnding.connect(function () { + Script.update.disconnect(update); + if (tablet) { + tablet.removeButton(tabletButton); + } + Messages.messageReceived.disconnect(messageHandler); + Messages.unsubscribe(MESSAGE_CHANNEL); + +}); diff --git a/tests/animation/src/data/avatar.json b/tests/animation/src/data/avatar.json index 550a95e980..3b80ff6d77 100644 --- a/tests/animation/src/data/avatar.json +++ b/tests/animation/src/data/avatar.json @@ -363,7 +363,7 @@ { "id": "idle", "interpTarget": 6, - "interpDuration": 6, + "interpDuration": 3, "transitions": [ { "var": "isMovingForward", "state": "walkFwd" }, { "var": "isMovingBackward", "state": "walkBwd" }, From bbf8c7357248cca8c83ab1dd99f1a7bd4b754645 Mon Sep 17 00:00:00 2001 From: amantley Date: Tue, 8 May 2018 10:52:28 -0700 Subject: [PATCH 02/16] code review changes to declare functions static --- interface/src/avatar/MyAvatar.cpp | 39 ++++++++++++++++++------------- 1 file changed, 23 insertions(+), 16 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index c3df9f6143..c78fc1c11b 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -2847,32 +2847,37 @@ glm::mat4 MyAvatar::deriveBodyFromHMDSensor() const { return createMatFromQuatAndPos(headOrientationYawOnly, bodyPos); } -float slope(float num) { - float constantK = 1.0; +// ease in function for dampening cg movement +static float slope(float num) { + const float CURVE_CONSTANT = 1.0f; float ret = 1.0f; if (num > 0.0f) { - ret = 1.0f - (1.0f / (1.0f + constantK * num)); + ret = 1.0f - (1.0f / (1.0f + CURVE_CONSTANT * num)); } return ret; } -glm::vec3 dampenCgMovement(glm::vec3 rawCg, float baseOfSupportScale) { +// This function gives a soft clamp at the edge of the base of support +// dampenCgMovement returns the cg value in Avatar space. +// rawCg is also in Avatar space +// baseOfSupportScale is based on the height of the user +static glm::vec3 dampenCgMovement(glm::vec3 rawCg, float baseOfSupportScale) { float distanceFromCenterZ = rawCg.z; float distanceFromCenterX = rawCg.x; - // The dampening scale factors makes the slope function soft clamp the - // cg at the edge of the base of support of the feet, in the lateral and posterior directions. - // In the forward direction we need a different scale because forward is in - // the direction of the hip extensor joint, which means bending usually happens + // The dampening scale factors makes the slope function soft clamp the + // cg at the edge of the base of support of the feet, in the lateral and posterior directions. + // In the forward direction we need a different scale because forward is in + // the direction of the hip extensor joint, which means bending usually happens // well before reaching the edge of the base of support. // The scale of the base of support reflects the size of the user in real life. - float forwardDampeningFactor = 0.5f; - float lateralAndBackDampeningScaleFactor = 2.0f; - float clampFront = DEFAULT_AVATAR_SUPPORT_BASE_FRONT * forwardDampeningFactor * baseOfSupportScale; + const float forwardDampeningFactor = 0.5f; + const float lateralAndBackDampeningScaleFactor = 2.0f; + const float clampFront = DEFAULT_AVATAR_SUPPORT_BASE_FRONT * forwardDampeningFactor * baseOfSupportScale; float clampBack = DEFAULT_AVATAR_SUPPORT_BASE_BACK * lateralAndBackDampeningScaleFactor * baseOfSupportScale; float clampLeft = DEFAULT_AVATAR_SUPPORT_BASE_LEFT * lateralAndBackDampeningScaleFactor * baseOfSupportScale; float clampRight = DEFAULT_AVATAR_SUPPORT_BASE_RIGHT * lateralAndBackDampeningScaleFactor * baseOfSupportScale; - glm::vec3 dampedCg = {0.0f,0.0f,0.0f}; + glm::vec3 dampedCg(0.0f, 0.0f, 0.0f); // find the damped z coord of the cg if (rawCg.z < 0.0f) { @@ -2906,8 +2911,9 @@ glm::vec3 dampenCgMovement(glm::vec3 rawCg, float baseOfSupportScale) { return dampedCg; } +// computeCounterBalance returns the center of gravity in Avatar space glm::vec3 MyAvatar::computeCounterBalance() const { - struct jointMass { + struct JointMass { QString name; float weight; glm::vec3 position; @@ -2925,14 +2931,14 @@ glm::vec3 MyAvatar::computeCounterBalance() const { // find the current center of gravity position based on head and hand moments float hipsMass = 40.0f; float totalMass = 0.0f; - glm::vec3 sumOfMoments = { 0.0f, 0.0f, 0.0f }; + glm::vec3 sumOfMoments(0.0f, 0.0f, 0.0f); for (int i = 0; i < 3; i++) { const QString jointName = cgMasses[i].name; cgMasses[i].position = getAbsoluteJointTranslationInObjectFrame(_skeletonModel->getRig().indexOfJoint(jointName)); sumOfMoments += cgMasses[i].weight * cgMasses[i].position; totalMass += cgMasses[i].weight; } - glm::vec3 currentCg = (1 / totalMass) * sumOfMoments; + glm::vec3 currentCg = (1.0f / totalMass) * sumOfMoments; currentCg.y = 0.0f; // dampening the center of gravity, in effect, limits the value to the perimeter of the base of support float baseScale = 1.0f; @@ -2969,7 +2975,8 @@ glm::vec3 MyAvatar::computeCounterBalance() const { return counterBalancedCg; } -glm::quat computeNewHipsRotation(glm::quat hipYawRot, glm::vec3 curHead, glm::vec3 hipPos) { +// this function matches the hips rotation to the new cg head axis +static glm::quat computeNewHipsRotation(glm::quat hipYawRot, glm::vec3 curHead, glm::vec3 hipPos) { glm::vec3 spineVec = curHead - hipPos; glm::quat finalRot = Quaternions::IDENTITY; From 544967ef3bbeb5835333c91729a99306b7645853 Mon Sep 17 00:00:00 2001 From: amantley Date: Tue, 8 May 2018 10:55:10 -0700 Subject: [PATCH 03/16] added test function to cancel out roll and pitch --- interface/src/avatar/MyAvatar.cpp | 26 +++++++++++++++++++++++++- 1 file changed, 25 insertions(+), 1 deletion(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index c3df9f6143..b9fa8d2ac6 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -3049,6 +3049,30 @@ void drawBaseOfSupport(float baseOfSupportScale, float footLocal, glm::mat4 avat DebugDraw::getInstance().drawRay(frontLeft, frontRight, rayColor); } +// cancel out roll and pitch test fix +glm::quat cancelOutRollAndPitch2(const glm::quat& q) { + glm::vec3 zAxis = q * glm::vec3(0.0f, 0.0f, 1.0f); + glm::vec3 newZ; + glm::vec3 newX; + glm::vec3 newY; + // cancel out the roll and pitch + if (zAxis.x == 0 && zAxis.z == 0.0f) { + if (fabs(zAxis.y) > 0.0) { + // new z is the up axis, that is the direction the body is pointing + newZ = glm::normalize(q * glm::vec3(0.0f, 1.0f, 0.0f)); + } + newX = glm::cross(vec3(0.0f, 1.0f, 0.0f), newZ); + newY = glm::cross(newZ, newX); + } + else { + newZ = glm::normalize(vec3(zAxis.x, 0.0f, zAxis.z)); + newX = glm::cross(vec3(0.0f, 1.0f, 0.0f), newZ); + newY = glm::cross(newZ, newX); + } + glm::mat4 temp(glm::vec4(newX, 0.0f), glm::vec4(newY, 0.0f), glm::vec4(newZ, 0.0f), glm::vec4(0.0f, 0.0f, 0.0f, 1.0f)); + return glm::quat_cast(temp); +} + glm::mat4 MyAvatar::deriveBodyUsingCgModel() const { glm::mat4 worldToSensorMat = glm::inverse(getSensorToWorldMatrix()); glm::mat4 avatarToWorldMat = getTransform().getMatrix(); @@ -3062,7 +3086,7 @@ glm::mat4 MyAvatar::deriveBodyUsingCgModel() const { // rotate by 180 Y to put the head in same frame as the avatar headOrientation = headPose.rotation * Quaternions::Y_180; } - const glm::quat headOrientationYawOnly = cancelOutRollAndPitch(headOrientation); + const glm::quat headOrientationYawOnly = cancelOutRollAndPitch2(headOrientation); const float MIX_RATIO = 0.15f; // here we mix in some of the head yaw into the hip yaw glm::quat hipYawRot = glm::normalize(glm::lerp(glmExtractRotation(avatarToSensorMat), headOrientationYawOnly, MIX_RATIO)); From 8000bcd272fc907df4ffee73dcd6f93a3e8cfb1f Mon Sep 17 00:00:00 2001 From: amantley Date: Tue, 8 May 2018 18:12:53 -0700 Subject: [PATCH 04/16] cleaned up cg code ie computeNewHipsRotation --- interface/src/avatar/MyAvatar.cpp | 205 ++++++++++--------------- libraries/shared/src/AvatarConstants.h | 7 + 2 files changed, 91 insertions(+), 121 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 5faebb0789..941aecc58b 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -2858,55 +2858,38 @@ static float slope(float num) { } // This function gives a soft clamp at the edge of the base of support -// dampenCgMovement returns the cg value in Avatar space. -// rawCg is also in Avatar space +// dampenCgMovement returns the damped cg value in Avatar space. +// cgUnderHeadHandsAvatarSpace is also in Avatar space // baseOfSupportScale is based on the height of the user -static glm::vec3 dampenCgMovement(glm::vec3 rawCg, float baseOfSupportScale) { - float distanceFromCenterZ = rawCg.z; - float distanceFromCenterX = rawCg.x; +static glm::vec3 dampenCgMovement(glm::vec3 cgUnderHeadHandsAvatarSpace, float baseOfSupportScale) { + float distanceFromCenterZ = cgUnderHeadHandsAvatarSpace.z; + float distanceFromCenterX = cgUnderHeadHandsAvatarSpace.x; - // The dampening scale factors makes the slope function soft clamp the - // cg at the edge of the base of support of the feet, in the lateral and posterior directions. // In the forward direction we need a different scale because forward is in // the direction of the hip extensor joint, which means bending usually happens // well before reaching the edge of the base of support. - // The scale of the base of support reflects the size of the user in real life. - const float forwardDampeningFactor = 0.5f; - const float lateralAndBackDampeningScaleFactor = 2.0f; - const float clampFront = DEFAULT_AVATAR_SUPPORT_BASE_FRONT * forwardDampeningFactor * baseOfSupportScale; - float clampBack = DEFAULT_AVATAR_SUPPORT_BASE_BACK * lateralAndBackDampeningScaleFactor * baseOfSupportScale; - float clampLeft = DEFAULT_AVATAR_SUPPORT_BASE_LEFT * lateralAndBackDampeningScaleFactor * baseOfSupportScale; - float clampRight = DEFAULT_AVATAR_SUPPORT_BASE_RIGHT * lateralAndBackDampeningScaleFactor * baseOfSupportScale; + const float clampFront = DEFAULT_AVATAR_SUPPORT_BASE_FRONT * DEFAULT_AVATAR_FORWARD_DAMPENING_FACTOR * baseOfSupportScale; + float clampBack = DEFAULT_AVATAR_SUPPORT_BASE_BACK * DEFAULT_AVATAR_LATERAL_DAMPENING_FACTOR * baseOfSupportScale; + float clampLeft = DEFAULT_AVATAR_SUPPORT_BASE_LEFT * DEFAULT_AVATAR_LATERAL_DAMPENING_FACTOR * baseOfSupportScale; + float clampRight = DEFAULT_AVATAR_SUPPORT_BASE_RIGHT * DEFAULT_AVATAR_LATERAL_DAMPENING_FACTOR * baseOfSupportScale; glm::vec3 dampedCg(0.0f, 0.0f, 0.0f); // find the damped z coord of the cg - if (rawCg.z < 0.0f) { + if (cgUnderHeadHandsAvatarSpace.z < 0.0f) { // forward displacement - float inputFront; - inputFront = fabs(distanceFromCenterZ / clampFront); - float scaleFrontNew = slope(inputFront); - dampedCg.z = scaleFrontNew * clampFront; + dampedCg.z = slope(fabs(distanceFromCenterZ / clampFront)) * clampFront; } else { // backwards displacement - float inputBack; - inputBack = fabs(distanceFromCenterZ / clampBack); - float scaleBackNew = slope(inputBack); - dampedCg.z = scaleBackNew * clampBack; + dampedCg.z = slope(fabs(distanceFromCenterZ / clampBack)) * clampBack; } // find the damped x coord of the cg - if (rawCg.x > 0.0f) { + if (cgUnderHeadHandsAvatarSpace.x > 0.0f) { // right of center - float inputRight; - inputRight = fabs(distanceFromCenterX / clampRight); - float scaleRightNew = slope(inputRight); - dampedCg.x = scaleRightNew * clampRight; + dampedCg.x = slope(fabs(distanceFromCenterX / clampRight)) * clampRight; } else { // left of center - float inputLeft; - inputLeft = fabs(distanceFromCenterX / clampLeft); - float scaleLeftNew = slope(inputLeft); - dampedCg.x = scaleLeftNew * clampLeft; + dampedCg.x = slope(fabs(distanceFromCenterX / clampLeft)) * clampLeft; } return dampedCg; } @@ -2917,27 +2900,43 @@ glm::vec3 MyAvatar::computeCounterBalance() const { QString name; float weight; glm::vec3 position; - } cgMasses[3]; + JointMass() {}; + JointMass(QString n, float w, glm::vec3 p) { + name = n; + weight = w; + position = p; + } + }; + // init the body part weights - cgMasses[0].name = "Head"; - cgMasses[0].weight = 20.0f; - cgMasses[0].position = { 0.0f, 0.0f, 0.0f }; - cgMasses[1].name = "LeftHand"; - cgMasses[1].weight = 2.0f; - cgMasses[1].position = { 0.0f, 0.0f, 0.0f }; - cgMasses[2].name = "RightHand"; - cgMasses[2].weight = 2.0f; - cgMasses[2].position = { 0.0f, 0.0f, 0.0f }; - // find the current center of gravity position based on head and hand moments - float hipsMass = 40.0f; - float totalMass = 0.0f; - glm::vec3 sumOfMoments(0.0f, 0.0f, 0.0f); - for (int i = 0; i < 3; i++) { - const QString jointName = cgMasses[i].name; - cgMasses[i].position = getAbsoluteJointTranslationInObjectFrame(_skeletonModel->getRig().indexOfJoint(jointName)); - sumOfMoments += cgMasses[i].weight * cgMasses[i].position; - totalMass += cgMasses[i].weight; + JointMass cgHeadMass(QString("Head"), DEFAULT_AVATAR_HEAD_MASS, glm::vec3(0.0f, 0.0f, 0.0f)); + JointMass cgLeftHandMass(QString("LeftHand"), DEFAULT_AVATAR_LEFTHAND_MASS, glm::vec3(0.0f, 0.0f, 0.0f)); + JointMass cgRightHandMass(QString("RightHand"), DEFAULT_AVATAR_RIGHTHAND_MASS, glm::vec3(0.0f, 0.0f, 0.0f)); + glm::vec3 tposeHead = DEFAULT_AVATAR_HEAD_POS; + glm::vec3 tposeHips = glm::vec3(0.0f, 0.0f, 0.0f); + + if (_skeletonModel->getRig().indexOfJoint(cgHeadMass.name) != -1) { + cgHeadMass.position = getAbsoluteJointTranslationInObjectFrame(_skeletonModel->getRig().indexOfJoint(cgHeadMass.name)); + tposeHead = getAbsoluteDefaultJointTranslationInObjectFrame(_skeletonModel->getRig().indexOfJoint(cgHeadMass.name)); } + if (_skeletonModel->getRig().indexOfJoint(cgLeftHandMass.name) != -1) { + cgLeftHandMass.position = getAbsoluteJointTranslationInObjectFrame(_skeletonModel->getRig().indexOfJoint(cgLeftHandMass.name)); + } else { + cgLeftHandMass.position = DEFAULT_AVATAR_LEFTHAND_POS; + } + if (_skeletonModel->getRig().indexOfJoint(cgRightHandMass.name) != -1) { + cgRightHandMass.position = getAbsoluteJointTranslationInObjectFrame(_skeletonModel->getRig().indexOfJoint(cgRightHandMass.name)); + } else { + cgRightHandMass.position = DEFAULT_AVATAR_RIGHTHAND_POS; + } + if (_skeletonModel->getRig().indexOfJoint("Hips") != -1) { + tposeHips = getAbsoluteDefaultJointTranslationInObjectFrame(_skeletonModel->getRig().indexOfJoint("Hips")); + } + + // find the current center of gravity position based on head and hand moments + glm::vec3 sumOfMoments = (cgHeadMass.weight * cgHeadMass.position) + (cgLeftHandMass.weight * cgLeftHandMass.position) + (cgRightHandMass.weight * cgRightHandMass.position); + float totalMass = cgHeadMass.weight + cgLeftHandMass.weight + cgRightHandMass.weight; + glm::vec3 currentCg = (1.0f / totalMass) * sumOfMoments; currentCg.y = 0.0f; // dampening the center of gravity, in effect, limits the value to the perimeter of the base of support @@ -2948,23 +2947,19 @@ glm::vec3 MyAvatar::computeCounterBalance() const { glm::vec3 desiredCg = dampenCgMovement(currentCg, baseScale); // compute hips position to maintain desiredCg - glm::vec3 counterBalancedForHead = ((totalMass + hipsMass) * desiredCg) - (cgMasses[0].position * cgMasses[0].weight); - glm::vec3 counterBalancedForLeftHand = counterBalancedForHead - (cgMasses[1].weight * cgMasses[1].position); - glm::vec3 counterBalancedForRightHand = counterBalancedForLeftHand - (cgMasses[2].weight * cgMasses[2].position); - glm::vec3 counterBalancedCg = (1.0f / hipsMass) * counterBalancedForRightHand; + glm::vec3 counterBalancedForHead = (totalMass + DEFAULT_AVATAR_HIPS_MASS) * desiredCg; + counterBalancedForHead -= sumOfMoments; + glm::vec3 counterBalancedCg = (1.0f / DEFAULT_AVATAR_HIPS_MASS) * counterBalancedForHead; // find the height of the hips - glm::vec3 currentHead = getAbsoluteJointTranslationInObjectFrame(_skeletonModel->getRig().indexOfJoint("Head")); - glm::vec3 tposeHead = getAbsoluteDefaultJointTranslationInObjectFrame(_skeletonModel->getRig().indexOfJoint("Head")); - glm::vec3 tposeHips = getAbsoluteDefaultJointTranslationInObjectFrame(_skeletonModel->getRig().indexOfJoint("Hips")); - glm::vec3 xzDiff = {(currentHead.x - counterBalancedCg.x), 0.0f, (currentHead.z - counterBalancedCg.z)}; + glm::vec3 xzDiff((cgHeadMass.position.x - counterBalancedCg.x), 0.0f, (cgHeadMass.position.z - counterBalancedCg.z)); float headMinusHipXz = glm::length(xzDiff); float headHipDefault = glm::length(tposeHead - tposeHips); float hipHeight = 0.0f; if (headHipDefault > headMinusHipXz) { hipHeight = sqrtf((headHipDefault * headHipDefault) - (headMinusHipXz * headMinusHipXz)); } - counterBalancedCg.y = (currentHead.y - hipHeight); + counterBalancedCg.y = (cgHeadMass.position.y - hipHeight); // this is to be sure that the feet don't lift off the floor. // add 5 centimeters to allow for going up on the toes. @@ -2975,66 +2970,33 @@ glm::vec3 MyAvatar::computeCounterBalance() const { return counterBalancedCg; } -// this function matches the hips rotation to the new cg head axis -static glm::quat computeNewHipsRotation(glm::quat hipYawRot, glm::vec3 curHead, glm::vec3 hipPos) { +// this function matches the hips rotation to the new cghips-head axis +// curHead and hipPos are in Avatar space +// returns the rotation of the hips in Avatar space +static glm::quat computeNewHipsRotation(glm::vec3 curHead, glm::vec3 hipPos) { glm::vec3 spineVec = curHead - hipPos; glm::quat finalRot = Quaternions::IDENTITY; - - if (spineVec.y > 0.0f) { - - glm::vec3 newYaxisHips = glm::normalize(spineVec); - glm::vec3 forward = { 0.0f, 0.0f, 1.0f }; - glm::vec3 oldZaxisHips = glm::normalize(forward); - glm::vec3 newXaxisHips = glm::normalize(glm::cross(newYaxisHips, oldZaxisHips)); - glm::vec3 newZaxisHips = glm::normalize(glm::cross(newXaxisHips, newYaxisHips)); - - // create mat4 with the new axes - glm::vec4 left = { newXaxisHips.x, newXaxisHips.y, newXaxisHips.z, 0.0f }; - glm::vec4 up = { newYaxisHips.x, newYaxisHips.y, newYaxisHips.z, 0.0f }; - glm::vec4 view = { newZaxisHips.x, newZaxisHips.y, newZaxisHips.z, 0.0f }; - glm::vec4 translation = { 0.0f, 0.0f, 0.0f, 1.0f }; - glm::mat4 newRotHips(left, up, view, translation); - finalRot = glm::toQuat(newRotHips); - } else if (spineVec.y < 0.0f) { - - glm::vec3 newYaxisHips = glm::normalize(-spineVec); - glm::vec3 forward = { 0.0f, 0.0f, 1.0f }; - glm::vec3 oldZaxisHips = glm::normalize(forward); - glm::vec3 newXaxisHips = glm::normalize(glm::cross(newYaxisHips, oldZaxisHips)); - glm::vec3 newZaxisHips = glm::normalize(glm::cross(newXaxisHips, newYaxisHips)); - - // create mat4 with the new axes - glm::vec4 left = { newXaxisHips.x, newXaxisHips.y, newXaxisHips.z, 0.0f }; - glm::vec4 up = { newYaxisHips.x, newYaxisHips.y, newYaxisHips.z, 0.0f }; - glm::vec4 view = { newZaxisHips.x, newZaxisHips.y, newZaxisHips.z, 0.0f }; - glm::vec4 translation = { 0.0f, 0.0f, 0.0f, 1.0f }; - glm::mat4 newRotHips(left, up, view, translation); - finalRot = glm::toQuat(newRotHips); - } else { - - //y equals zero. - if (glm::length(spineVec) > 0.0f) { - glm::vec3 newYaxisHips = glm::normalize(spineVec); - glm::vec3 forward = { 0.0f, 1.0f, 0.0f }; - glm::vec3 oldZaxisHips = forward; - glm::vec3 newXaxisHips = glm::normalize(glm::cross(newYaxisHips, oldZaxisHips)); - glm::vec3 newZaxisHips = oldZaxisHips; - - // create mat4 with the new axes - glm::vec4 left = { newXaxisHips.x, newXaxisHips.y, newXaxisHips.z, 0.0f }; - glm::vec4 up = { newYaxisHips.x, newYaxisHips.y, newYaxisHips.z, 0.0f }; - glm::vec4 view = { newZaxisHips.x, newZaxisHips.y, newZaxisHips.z, 0.0f }; - glm::vec4 translation = { 0.0f, 0.0f, 0.0f, 1.0f }; - glm::mat4 newRotHips(left, up, view, translation); - finalRot = glm::toQuat(newRotHips); - } - // otherwise, head and hips are equal so leave finalRot identity + glm::vec3 newYaxisHips = glm::normalize(spineVec); + glm::vec3 forward(0.0f, 0.0f, 1.0f); + if ((fabs(spineVec.y) == 0.0f) && (glm::length(spineVec) > 0.0f)){ + //y equals zero and hips position != head position + forward = glm::vec3(0.0f, 1.0f, 0.0f); } - glm::quat hipsRotation = hipYawRot*finalRot; + glm::vec3 oldZaxisHips = glm::normalize(forward); + glm::vec3 newXaxisHips = glm::normalize(glm::cross(newYaxisHips, oldZaxisHips)); + glm::vec3 newZaxisHips = glm::normalize(glm::cross(newXaxisHips, newYaxisHips)); + // create mat4 with the new axes + glm::vec4 left(newXaxisHips.x, newXaxisHips.y, newXaxisHips.z, 0.0f); + glm::vec4 up(newYaxisHips.x, newYaxisHips.y, newYaxisHips.z, 0.0f); + glm::vec4 view(newZaxisHips.x, newZaxisHips.y, newZaxisHips.z, 0.0f); + glm::vec4 translation(0.0f, 0.0f, 0.0f, 1.0f); + glm::mat4 newRotHips(left, up, view, translation); + finalRot = glm::toQuat(newRotHips); + glm::quat hipsRotation = finalRot; return hipsRotation; } -void drawBaseOfSupport(float baseOfSupportScale, float footLocal, glm::mat4 avatarToWorld) { +static void drawBaseOfSupport(float baseOfSupportScale, float footLocal, glm::mat4 avatarToWorld) { // scale the base of support based on user height float clampFront = DEFAULT_AVATAR_SUPPORT_BASE_FRONT * baseOfSupportScale; float clampBack = DEFAULT_AVATAR_SUPPORT_BASE_BACK * baseOfSupportScale; @@ -3080,6 +3042,9 @@ glm::quat cancelOutRollAndPitch2(const glm::quat& q) { return glm::quat_cast(temp); } +// this function finds the hips position using a center of gravity model that +// balances the head and hands with the hips over the base of support +// returns the rotation and position of the Avatar in Sensor space glm::mat4 MyAvatar::deriveBodyUsingCgModel() const { glm::mat4 worldToSensorMat = glm::inverse(getSensorToWorldMatrix()); glm::mat4 avatarToWorldMat = getTransform().getMatrix(); @@ -3094,12 +3059,10 @@ glm::mat4 MyAvatar::deriveBodyUsingCgModel() const { headOrientation = headPose.rotation * Quaternions::Y_180; } const glm::quat headOrientationYawOnly = cancelOutRollAndPitch2(headOrientation); - const float MIX_RATIO = 0.15f; + const float MIX_RATIO = 0.5f; // here we mix in some of the head yaw into the hip yaw glm::quat hipYawRot = glm::normalize(glm::lerp(glmExtractRotation(avatarToSensorMat), headOrientationYawOnly, MIX_RATIO)); - glm::quat deltaRot = glm::inverse(glmExtractRotation(avatarToSensorMat))*hipYawRot; - glm::vec3 headPositionLocal = getAbsoluteJointTranslationInObjectFrame(_skeletonModel->getRig().indexOfJoint("Head")); - glm::vec3 headLocalAfterDelta = glm::inverse(deltaRot)*headPositionLocal; + glm::vec3 newLocalHeadPos = glm::inverse(hipYawRot) * (headPosition - extractTranslation(avatarToSensorMat)); if (_enableDebugDrawBaseOfSupport) { // default height is ~ 1.64 meters @@ -3112,9 +3075,9 @@ glm::mat4 MyAvatar::deriveBodyUsingCgModel() const { const glm::vec3 cgHipsPosition = computeCounterBalance(); glm::vec3 hipsPositionFinal = transformPoint(avatarToSensorMat, cgHipsPosition); - //find the new hips rotation using the new head-hips axis as the up axis - glm::quat newHipsRotation = computeNewHipsRotation( hipYawRot, headLocalAfterDelta, cgHipsPosition); - return createMatFromQuatAndPos(newHipsRotation, hipsPositionFinal); + // find the new hips rotation using the new head-hips axis as the up axis + glm::quat newHipsRotation = computeNewHipsRotation(newLocalHeadPos, cgHipsPosition); + return createMatFromQuatAndPos(hipYawRot*newHipsRotation, hipsPositionFinal); } float MyAvatar::getUserHeight() const { diff --git a/libraries/shared/src/AvatarConstants.h b/libraries/shared/src/AvatarConstants.h index 9f0c789b9d..bdc54dfeb6 100644 --- a/libraries/shared/src/AvatarConstants.h +++ b/libraries/shared/src/AvatarConstants.h @@ -24,6 +24,13 @@ const float DEFAULT_AVATAR_SUPPORT_BASE_LEFT = -0.25f; const float DEFAULT_AVATAR_SUPPORT_BASE_RIGHT = 0.25f; const float DEFAULT_AVATAR_SUPPORT_BASE_FRONT = -0.20f; const float DEFAULT_AVATAR_SUPPORT_BASE_BACK = 0.10f; +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; +const float DEFAULT_AVATAR_HEAD_MASS = 20.0f; +const float DEFAULT_AVATAR_LEFTHAND_MASS = 2.0f; +const float DEFAULT_AVATAR_RIGHTHAND_MASS = 2.0f; + // Used when avatar is missing joints... (avatar space) const glm::quat DEFAULT_AVATAR_MIDDLE_EYE_ROT { Quaternions::Y_180 }; From 77aedc1512d502bfa9f8e7198722ffba9099b9c8 Mon Sep 17 00:00:00 2001 From: amantley Date: Wed, 9 May 2018 11:14:13 -0700 Subject: [PATCH 05/16] changed lerp to saferLerp in cg code --- interface/src/avatar/MyAvatar.cpp | 19 +- scripts/developer/cg_lean.js | 553 ------------------------------ 2 files changed, 16 insertions(+), 556 deletions(-) delete mode 100644 scripts/developer/cg_lean.js diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 941aecc58b..c73b99d716 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -2978,7 +2978,8 @@ static glm::quat computeNewHipsRotation(glm::vec3 curHead, glm::vec3 hipPos) { glm::quat finalRot = Quaternions::IDENTITY; glm::vec3 newYaxisHips = glm::normalize(spineVec); glm::vec3 forward(0.0f, 0.0f, 1.0f); - if ((fabs(spineVec.y) == 0.0f) && (glm::length(spineVec) > 0.0f)){ + const float EPSILON = 0.0001f; + if ((fabs(spineVec.y) < EPSILON) && (glm::length(spineVec) > 0.0f)) { //y equals zero and hips position != head position forward = glm::vec3(0.0f, 1.0f, 0.0f); } @@ -3042,6 +3043,16 @@ glm::quat cancelOutRollAndPitch2(const glm::quat& q) { return glm::quat_cast(temp); } +static glm::quat saferLerp(const glm::quat& a, const glm::quat& b, float alpha) { + // adjust signs if necessary + glm::quat bTemp = b; + float dot = glm::dot(a, bTemp); + if (dot < 0.0f) { + bTemp = -bTemp; + } + return glm::normalize(glm::lerp(a, bTemp, alpha)); +} + // this function finds the hips position using a center of gravity model that // balances the head and hands with the hips over the base of support // returns the rotation and position of the Avatar in Sensor space @@ -3058,10 +3069,11 @@ glm::mat4 MyAvatar::deriveBodyUsingCgModel() const { // rotate by 180 Y to put the head in same frame as the avatar headOrientation = headPose.rotation * Quaternions::Y_180; } - const glm::quat headOrientationYawOnly = cancelOutRollAndPitch2(headOrientation); + const glm::quat headOrientationYawOnly = cancelOutRollAndPitch(headOrientation); const float MIX_RATIO = 0.5f; // here we mix in some of the head yaw into the hip yaw - glm::quat hipYawRot = glm::normalize(glm::lerp(glmExtractRotation(avatarToSensorMat), headOrientationYawOnly, MIX_RATIO)); + glm::quat hipYawRot = glm::normalize(saferLerp(glmExtractRotation(avatarToSensorMat), headOrientationYawOnly, MIX_RATIO)); + // glm::quat hipYawRot = glmExtractRotation(avatarToSensorMat); glm::vec3 newLocalHeadPos = glm::inverse(hipYawRot) * (headPosition - extractTranslation(avatarToSensorMat)); if (_enableDebugDrawBaseOfSupport) { @@ -3078,6 +3090,7 @@ glm::mat4 MyAvatar::deriveBodyUsingCgModel() const { // find the new hips rotation using the new head-hips axis as the up axis glm::quat newHipsRotation = computeNewHipsRotation(newLocalHeadPos, cgHipsPosition); return createMatFromQuatAndPos(hipYawRot*newHipsRotation, hipsPositionFinal); + // return createMatFromQuatAndPos(hipYawRot, hipsPositionFinal); } float MyAvatar::getUserHeight() const { diff --git a/scripts/developer/cg_lean.js b/scripts/developer/cg_lean.js deleted file mode 100644 index a4ca56d6d6..0000000000 --- a/scripts/developer/cg_lean.js +++ /dev/null @@ -1,553 +0,0 @@ - -/* global Script, Vec3, MyAvatar Tablet Messages Quat DebugDraw Mat4 Xform*/ - - -Script.include("/~/system/libraries/Xform.js"); - -var MESSAGE_CHANNEL = "Hifi-Step-Cg"; - -var ANIM_VARS = [ - "isTalking", - "isNotMoving", - "isMovingForward", - "isMovingBackward", - "isMovingRight", - "isMovingLeft", - "isTurningRight", - "isTurningLeft", - "isFlying", - "isTakeoffStand", - "isTakeoffRun", - "isInAirStand", - "isInAirRun", - "hipsPosition", - "hipsRotation", - "hipsType", - "headWeight", - "headType" -]; - -var DEBUGDRAWING; -var YELLOW; -var BLUE; -var GREEN; -var RED; - -var ROT_Y90; -var ROT_Y180; -var FLOOR_Y; -var IDENT_QUAT; - -var TABLET_BUTTON_NAME; -var RECENTER; -var JOINT_MASSES; - -var hipsUnderHead; - -var armsHipRotation; -var hipsPosition; -var filteredHipsPosition; -var hipsRotation; - -var jointList; -var rightFootName; -var leftFootName; -var rightToeName; -var leftToeName; -var leftToeEnd; -var rightToeEnd; -var leftFoot; -var rightFoot; -var base; - -var clampFront; -var clampBack; -var clampLeft; -var clampRight; - -var tablet; -var tabletButton; - -function initCg() { - - DEBUGDRAWING = false; - - YELLOW = { r: 1, g: 1, b: 0, a: 1 }; - BLUE = { r: 0, g: 0, b: 1, a: 1 }; - GREEN = { r: 0, g: 1, b: 0, a: 1 }; - RED = { r: 1, g: 0, b: 0, a: 1 }; - - ROT_Y90 = { x: 0, y: 0.7071067811865475, z: 0, w: 0.7071067811865476 }; - ROT_Y180 = { x: 0, y: 1, z: 0, w: 0 }; - FLOOR_Y = -0.9; - IDENT_QUAT = { x: 0, y: 0, z: 0, w: 1 }; - - JOINT_MASSES = [{ joint: "Head", mass: 20.0, pos: { x: 0, y: 0, z: 0 } }, - { joint: "LeftHand", mass: 2.0, pos: { x: 0, y: 0, z: 0 } }, - { joint: "RightHand", mass: 2.0, pos: { x: 0, y: 0, z: 0 } }]; - - TABLET_BUTTON_NAME = "CG"; - RECENTER = false; - - MyAvatar.hmdLeanRecenterEnabled = RECENTER; - hipsUnderHead; - - armsHipRotation = { x: 0, y: 1, z: 0, w: 0 }; - hipsPosition = MyAvatar.getAbsoluteDefaultJointTranslationInObjectFrame(MyAvatar.getJointIndex("Hips")); - filteredHipsPosition = MyAvatar.position; - hipsRotation = { x: 0, y: 0, z: 0, w: 1 }; - - jointList = MyAvatar.getJointNames(); - // print(JSON.stringify(jointList)); - - rightFootName = null; - leftFootName = null; - rightToeName = null; - leftToeName = null; - leftToeEnd = null; - rightToeEnd = null; - leftFoot; - rightFoot; - - clampFront = -0.10; - clampBack = 0.17; - clampLeft = -0.50; - clampRight = 0.50; - - getFeetAndToeNames(); - base = computeBase(); - mirrorPoints(); - - - tablet = Tablet.getTablet("com.highfidelity.interface.tablet.system"); - - tabletButton = tablet.addButton({ - text: TABLET_BUTTON_NAME, - icon: "icons/tablet-icons/avatar-record-i.svg" - }); - - tabletButton.clicked.connect(function () { - print("recenter is: " + RECENTER); - MyAvatar.hmdLeanRecenterEnabled = RECENTER; - RECENTER = !RECENTER; - - // messageSend("clicked button in cg"); - }); - - - var handlerId = MyAvatar.addAnimationStateHandler(function (props) { - - var result = {}; - - // prevent animations from ever leaving the idle state - result.isTalking = false; - result.isFlying = false; - result.isTakeoffStand = false; - result.isTakeoffRun = false; - result.isInAirStand = false; - result.isInAirRun = false; - result.hipsPosition = hipsPosition; - result.hipsRotation = hipsRotation; - result.hipsType = 0; - result.headWeight = 4; - result.headType = 4; - - return result; - }, ANIM_VARS); - - Messages.subscribe(MESSAGE_CHANNEL); - Messages.messageReceived.connect(messageHandler); - Script.update.connect(update); - MyAvatar.skeletonChanged.connect(function () { - Script.setTimeout(function () { - // stop logic if needed - MyAvatar.clearJointsData(); - // reset logic - }, 200); - }); - HMD.displayModeChanged.connect(function () { - Script.setTimeout(function () { - // stop logic if needed - MyAvatar.clearJointsData(); - // reset logic - }, 200); - }); - - -} - -function messageSend(message) { - Messages.sendLocalMessage(MESSAGE_CHANNEL, message); -} - -function messageHandler(channel, messageString, senderID) { - if (channel !== MESSAGE_CHANNEL) { - return; - } - - var hipquat = JSON.parse(messageString); - armsHipRotation = Quat.multiply(ROT_Y180,hipquat); - -} - -function getFeetAndToeNames() { - - for (var i = 0; i < jointList.length; i++) { - if ((jointList[i].indexOf('Right') !== -1) && (jointList[i].indexOf('Foot') !== -1)) { - print(JSON.stringify(jointList[i])); - rightFootName = jointList[i]; - } - if ((jointList[i].indexOf('Left') !== -1) && (jointList[i].indexOf('Foot') !== -1)) { - print(JSON.stringify(jointList[i])); - leftFootName = jointList[i]; - } - if ((jointList[i].indexOf('Right') !== -1) && (jointList[i].indexOf('Toe') !== -1) && (jointList[i].indexOf('End') !== -1)) { - print(JSON.stringify(jointList[i])); - rightToeName = jointList[i]; - } - if ((jointList[i].indexOf('Left') !== -1) && (jointList[i].indexOf('Toe') !== -1) && (jointList[i].indexOf('End') !== -1)) { - print(JSON.stringify(jointList[i])); - leftToeName = jointList[i]; - } - } -} - -function computeBase() { - - if (rightFootName === null || leftFootName === null) { - // if the feet names aren't found then use our best guess of the base. - leftToeEnd = {x: 0.12, y: 0.0, z: 0.12}; - rightToeEnd = {x: -0.18, y: 0.0, z: 0.12}; - leftFoot = {x: 0.15, y: 0.0, z: -0.17}; - rightFoot = {x: -0.20, y: 0.0, z: -0.17}; - } else { - // else we at least found the feet in the skeleton. - var leftFootIndex = MyAvatar.getJointIndex(leftFootName); - var rightFootIndex = MyAvatar.getJointIndex(rightFootName); - var leftFoot = MyAvatar.getAbsoluteJointTranslationInObjectFrame(leftFootIndex); - var rightFoot = MyAvatar.getAbsoluteJointTranslationInObjectFrame(rightFootIndex); - - if (rightToeName === null || leftToeName === null) { - // the toe ends were not found then we use a guess for the length and width of the feet. - leftToeEnd = {x: (leftFoot.x + 0.02), y: 0.0, z: (leftFoot.z - 0.2)}; - rightToeEnd = {x: (rightFoot.x - 0.02), y: 0.0, z: (rightFoot.z - 0.2)}; - } else { - // else we found the toe ends and now we can really compute the base. - var leftToeIndex = MyAvatar.getJointIndex(leftToeName); - var rightToeIndex = MyAvatar.getJointIndex(rightToeName); - leftToeEnd = MyAvatar.getAbsoluteJointTranslationInObjectFrame(leftToeIndex); - rightToeEnd = MyAvatar.getAbsoluteJointTranslationInObjectFrame(rightToeIndex); - } - - } - - // project each point into the FLOOR plane. - var points = [{x: leftToeEnd.x, y: FLOOR_Y, z: leftToeEnd.z}, - {x: rightToeEnd.x, y: FLOOR_Y, z: rightToeEnd.z}, - {x: rightFoot.x, y: FLOOR_Y, z: rightFoot.z}, - {x: leftFoot.x, y: FLOOR_Y, z: rightFoot.z}]; - - // compute normals for each plane - var normal, normals = []; - var n = points.length; - var next, prev; - for (next = 0, prev = n - 1; next < n; prev = next, next++) { - normal = Vec3.multiplyQbyV(ROT_Y90, Vec3.normalize(Vec3.subtract(points[next], points[prev]))); - normals.push(normal); - } - - var TOE_FORWARD_RADIUS = 0.01; - var TOE_SIDE_RADIUS = 0.05; - var HEEL_FORWARD_RADIUS = 0.01; - var HEEL_SIDE_RADIUS = 0.03; - var radii = [ - TOE_SIDE_RADIUS, TOE_FORWARD_RADIUS, TOE_FORWARD_RADIUS, TOE_SIDE_RADIUS, - HEEL_SIDE_RADIUS, HEEL_FORWARD_RADIUS, HEEL_FORWARD_RADIUS, HEEL_SIDE_RADIUS - ]; - - // subdivide base and extrude by the toe and heel radius. - var newPoints = []; - for (next = 0, prev = n - 1; next < n; prev = next, next++) { - newPoints.push(Vec3.sum(points[next], Vec3.multiply(radii[2 * next], normals[next]))); - newPoints.push(Vec3.sum(points[next], Vec3.multiply(radii[(2 * next) + 1], normals[(next + 1) % n]))); - } - - // compute newNormals - var newNormals = []; - n = newPoints.length; - for (next = 0, prev = n - 1; next < n; prev = next, next++) { - normal = Vec3.multiplyQbyV(ROT_Y90, Vec3.normalize(Vec3.subtract(newPoints[next], newPoints[prev]))); - newNormals.push(normal); - } - - for (var j = 0;j Math.abs(base.points[3].x)) { - base.points[3].x = -base.points[0].x; - base.points[2].x = -base.points[1].x; - } else { - base.points[0].x = -base.points[3].x; - base.points[1].x = -base.points[2].x; - } - - if (Math.abs(base.points[4].x) > Math.abs(base.points[7].x)) { - base.points[7].x = -base.points[4].x; - base.points[6].x = -base.points[5].x; - } else { - base.points[4].x = -base.points[7].x; - base.points[5].x = -base.points[6].x; - } - - if (Math.abs(base.points[0].z) > Math.abs(base.points[0].z)) { - base.points[3].z = base.points[0].z; - base.points[2].z = base.points[1].z; - } else { - base.points[0].z = base.points[3].z; - base.points[1].z = base.points[2].z; - } - - if (Math.abs(base.points[4].z) > Math.abs(base.points[7].z)) { - base.points[7].z = base.points[4].z; - base.points[6].z = base.points[5].z; - } else { - base.points[4].z = base.points[7].z; - base.points[5].z = base.points[6].z; - } - - for (var i = 0; i < base.points.length; i++) { - - print("point: " + i + " " + JSON.stringify(base.points[i])); - } - for (var j = 0; j < base.normals.length; j++) { - print("normal: " + j + " " + JSON.stringify(base.normals[j])); - } -} - - -function drawBase(base) { - // transform corners into world space, for rendering. - var xform = new Xform(MyAvatar.orientation, MyAvatar.position); - var worldPoints = base.points.map(function (point) { - return xform.xformPoint(point); - }); - var worldNormals = base.normals.map(function (normal) { - return xform.xformVector(normal); - }); - - var n = worldPoints.length; - var next, prev; - for (next = 0, prev = n - 1; next < n; prev = next, next++) { - if (DEBUGDRAWING) { - // draw border - DebugDraw.drawRay(worldPoints[prev], worldPoints[next], GREEN); - DebugDraw.drawRay(worldPoints[next], worldPoints[prev], GREEN); - - // draw normal - var midPoint = Vec3.multiply(0.5, Vec3.sum(worldPoints[prev], worldPoints[next])); - DebugDraw.drawRay(midPoint, Vec3.sum(midPoint, worldNormals[next]), YELLOW); - DebugDraw.drawRay(midPoint, Vec3.sum(midPoint, worldNormals[next+1]), YELLOW); - } - } -} - -function computeCg() { - // point mass. - var n = JOINT_MASSES.length; - var moments = {x: 0, y: 0, z: 0}; - var masses = 0; - for (var i = 0; i < n; i++) { - var pos = MyAvatar.getAbsoluteJointTranslationInObjectFrame(MyAvatar.getJointIndex(JOINT_MASSES[i].joint)); - JOINT_MASSES[i].pos = pos; - moments = Vec3.sum(moments, Vec3.multiply(JOINT_MASSES[i].mass, pos)); - masses += JOINT_MASSES[i].mass; - } - return Vec3.multiply(1 / masses, moments); -} - - -function clamp(val, min, max) { - return Math.max(min, Math.min(max, val)); -} - -function distancetoline(p1,p2,cg) { - var numerator = Math.abs((p2.z - p1.z)*(cg.x) - (p2.x - p1.x)*(cg.z) + (p2.x)*(p1.z) - (p2.z)*(p1.x)); - var denominator = Math.sqrt( Math.pow((p2.z - p1.z),2) + Math.pow((p2.x - p1.x),2)); - - return numerator/denominator; -} - -function isLeft(a, b, c) { - return (((b.x - a.x)*(c.z - a.z) - (b.z - a.z)*(c.x - a.x)) > 0); -} - -function slope(num) { - var constant = 1.0; - return 1 - ( 1/(1+constant*num)); -} - -function dampenCgMovement(rawCg) { - - var distanceFromCenterZ = rawCg.z; - var distanceFromCenterX = rawCg.x; - - // clampFront = -0.10; - // clampBack = 0.17; - // clampLeft = -0.50; - // clampRight = 0.50; - - var dampedCg = { x: 0, y: 0, z: 0 }; - - if (rawCg.z < 0.0) { - var inputFront; - inputFront = Math.abs(distanceFromCenterZ / clampFront); - var scaleFrontNew = slope(inputFront); - dampedCg.z = scaleFrontNew * clampFront; - } else { - // cg.z > 0.0 - var inputBack; - inputBack = Math.abs(distanceFromCenterZ / clampBack); - var scaleBackNew = slope(inputBack); - dampedCg.z = scaleBackNew * clampBack; - } - - if (rawCg.x > 0.0) { - var inputRight; - inputRight = Math.abs(distanceFromCenterX / clampRight); - var scaleRightNew = slope(inputRight); - dampedCg.x = scaleRightNew * clampRight; - } else { - // left of center - var inputLeft; - inputLeft = Math.abs(distanceFromCenterX / clampLeft); - var scaleLeftNew = slope(inputLeft); - dampedCg.x = scaleLeftNew * clampLeft; - } - return dampedCg; -} - -function computeCounterBalance(desiredCgPos) { - // compute hips position to maintain desiredCg - var HIPS_MASS = 40; - var totalMass = JOINT_MASSES.reduce(function (accum, obj) { - return accum + obj.mass; - }, 0); - var temp1 = Vec3.subtract(Vec3.multiply(totalMass + HIPS_MASS, desiredCgPos), - Vec3.multiply(JOINT_MASSES[0].mass, JOINT_MASSES[0].pos)); - var temp2 = Vec3.subtract(temp1, - Vec3.multiply(JOINT_MASSES[1].mass, JOINT_MASSES[1].pos)); - var temp3 = Vec3.subtract(temp2, - Vec3.multiply(JOINT_MASSES[2].mass, JOINT_MASSES[2].pos)); - var temp4 = Vec3.multiply(1 / HIPS_MASS, temp3); - - - var currentHead = MyAvatar.getAbsoluteJointTranslationInObjectFrame(MyAvatar.getJointIndex("Head")); - var tposeHead = MyAvatar.getAbsoluteDefaultJointTranslationInObjectFrame(MyAvatar.getJointIndex("Head")); - var tposeHips = MyAvatar.getAbsoluteDefaultJointTranslationInObjectFrame(MyAvatar.getJointIndex("Hips")); - - var xzDiff = { x: (currentHead.x - temp4.x), y: 0, z: (currentHead.z - temp4.z) }; - var headMinusHipXz = Vec3.length(xzDiff); - - var headHipDefault = Vec3.length(Vec3.subtract(tposeHead, tposeHips)); - - var hipHeight = Math.sqrt((headHipDefault * headHipDefault) - (headMinusHipXz * headMinusHipXz)); - - temp4.y = (currentHead.y - hipHeight); - if (temp4.y > tposeHips.y) { - temp4.y = 0.0; - } - return temp4; -} - -function update(dt) { - - var cg = computeCg(); - // print("time elapsed " + dt); - - var desiredCg = { x: 0, y: 0, z: 0 }; - // print("the raw cg " + cg.x + " " + cg.y + " " + cg.z); - - desiredCg.x = cg.x; - desiredCg.y = 0; - desiredCg.z = cg.z; - - desiredCg = dampenCgMovement(cg); - - cg.y = FLOOR_Y; - - // after the dampening above it might be right to clamp the desiredcg to the edge of the base - // of support. - - if (DEBUGDRAWING) { - DebugDraw.addMyAvatarMarker("left toe", IDENT_QUAT, leftToeEnd, BLUE); - DebugDraw.addMyAvatarMarker("right toe", IDENT_QUAT, rightToeEnd, BLUE); - DebugDraw.addMyAvatarMarker("cg", IDENT_QUAT, cg, BLUE); - DebugDraw.addMyAvatarMarker("desiredCg", IDENT_QUAT, desiredCg, GREEN); - drawBase(base); - } - - var currentHeadPos = MyAvatar.getAbsoluteJointTranslationInObjectFrame(MyAvatar.getJointIndex("Head")); - var localHipsPos = computeCounterBalance(desiredCg); - // print("current hips " + cg.x + " " + cg.y + " " + cg.z); - // print("dampened hips " + desiredCg.x + " " + desiredCg.y + " " + desiredCg.z) - - var globalPosRoot = MyAvatar.position; - var globalRotRoot = Quat.normalize(MyAvatar.orientation); - var inverseGlobalRotRoot = Quat.normalize(Quat.inverse(globalRotRoot)); - var globalPosHips = Vec3.sum(globalPosRoot, Vec3.multiplyQbyV(globalRotRoot, localHipsPos)); - var unRotatedHipsPosition; - - if (!MyAvatar.isRecenteringHorizontally()) { - - filteredHipsPosition = Vec3.mix(filteredHipsPosition, globalPosHips, 0.1); - unRotatedHipsPosition = Vec3.multiplyQbyV(inverseGlobalRotRoot, Vec3.subtract(filteredHipsPosition, globalPosRoot)); - hipsPosition = Vec3.multiplyQbyV(ROT_Y180, unRotatedHipsPosition); - } else { - // DebugDraw.addMarker("hipsunder", IDENT_QUAT, hipsUnderHead, GREEN); - filteredHipsPosition = Vec3.mix(filteredHipsPosition, globalPosHips, 0.1); - unRotatedHipsPosition = Vec3.multiplyQbyV(inverseGlobalRotRoot, Vec3.subtract(filteredHipsPosition, globalPosRoot)); - hipsPosition = Vec3.multiplyQbyV(ROT_Y180, unRotatedHipsPosition); - } - - var newYaxisHips = Vec3.normalize(Vec3.subtract(currentHeadPos, unRotatedHipsPosition)); - var forward = { x: 0.0, y: 0.0, z: 1.0 }; - - // arms hip rotation is sent from the step script - var oldZaxisHips = Vec3.normalize(Vec3.multiplyQbyV(armsHipRotation, forward)); - var newXaxisHips = Vec3.normalize(Vec3.cross(newYaxisHips, oldZaxisHips)); - var newZaxisHips = Vec3.normalize(Vec3.cross(newXaxisHips, newYaxisHips)); - - // var beforeHips = MyAvatar.getAbsoluteJointRotationInObjectFrame(MyAvatar.getJointIndex("Hips")); - var left = { x: newXaxisHips.x, y: newXaxisHips.y, z: newXaxisHips.z, w: 0.0 }; - var up = { x: newYaxisHips.x, y: newYaxisHips.y, z: newYaxisHips.z, w: 0.0 }; - var view = { x: newZaxisHips.x, y: newZaxisHips.y, z: newZaxisHips.z, w: 0.0 }; - - var translation = { x: 0.0, y: 0.0, z: 0.0, w: 1.0 }; - var newRotHips = Mat4.createFromColumns(left, up, view, translation); - var finalRot = Mat4.extractRotation(newRotHips); - - hipsRotation = Quat.multiply(ROT_Y180, finalRot); - print("final rot" + finalRot.x + " " + finalRot.y + " " + finalRot.z + " " + finalRot.w); - - if (DEBUGDRAWING) { - DebugDraw.addMyAvatarMarker("hipsPos", IDENT_QUAT, hipsPosition, RED); - } -} - - -Script.setTimeout(initCg, 10); -Script.scriptEnding.connect(function () { - Script.update.disconnect(update); - if (tablet) { - tablet.removeButton(tabletButton); - } - Messages.messageReceived.disconnect(messageHandler); - Messages.unsubscribe(MESSAGE_CHANNEL); - -}); From a7955b642a791bf138e5f4e19cb7a9192c310e92 Mon Sep 17 00:00:00 2001 From: amantley Date: Wed, 9 May 2018 11:27:04 -0700 Subject: [PATCH 06/16] removed the follow hips code from pre physics update --- interface/src/avatar/MyAvatar.cpp | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index c73b99d716..9a30113ca4 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -3341,13 +3341,8 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat followWorldPose.scale() = glm::vec3(1.0f); if (isActive(Rotation)) { - if (getToggleHipsFollowing()) { //use the hmd reading for the hips follow - followWorldPose.rot() = glmExtractRotation(desiredWorldMatrix); - } else { - //use the hips as changed by the arms azimuth for the hips to follow. - followWorldPose.rot() = resultingTwistInWorld; - } + followWorldPose.rot() = glmExtractRotation(desiredWorldMatrix); } if (isActive(Horizontal)) { glm::vec3 desiredTranslation = extractTranslation(desiredWorldMatrix); From 50f225777af00698b9f1c34262356716d54c6399 Mon Sep 17 00:00:00 2001 From: amantley Date: Wed, 9 May 2018 11:31:19 -0700 Subject: [PATCH 07/16] removed debug code from cg code in MyAvatar.cpp --- interface/src/avatar/MyAvatar.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 9a30113ca4..778332f65b 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -3073,7 +3073,6 @@ glm::mat4 MyAvatar::deriveBodyUsingCgModel() const { const float MIX_RATIO = 0.5f; // here we mix in some of the head yaw into the hip yaw glm::quat hipYawRot = glm::normalize(saferLerp(glmExtractRotation(avatarToSensorMat), headOrientationYawOnly, MIX_RATIO)); - // glm::quat hipYawRot = glmExtractRotation(avatarToSensorMat); glm::vec3 newLocalHeadPos = glm::inverse(hipYawRot) * (headPosition - extractTranslation(avatarToSensorMat)); if (_enableDebugDrawBaseOfSupport) { @@ -3090,7 +3089,6 @@ glm::mat4 MyAvatar::deriveBodyUsingCgModel() const { // find the new hips rotation using the new head-hips axis as the up axis glm::quat newHipsRotation = computeNewHipsRotation(newLocalHeadPos, cgHipsPosition); return createMatFromQuatAndPos(hipYawRot*newHipsRotation, hipsPositionFinal); - // return createMatFromQuatAndPos(hipYawRot, hipsPositionFinal); } float MyAvatar::getUserHeight() const { From eff84de99f12fcf7b0462ae3f363ab2b302a54d0 Mon Sep 17 00:00:00 2001 From: amantley Date: Wed, 9 May 2018 14:55:10 -0700 Subject: [PATCH 08/16] added more straight forward computation of the hips rotation inMyAvatar.cpp contributed by AJT --- interface/src/avatar/MyAvatar.cpp | 84 +++++++++++-------------------- 1 file changed, 29 insertions(+), 55 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 778332f65b..35247bca20 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -52,6 +52,7 @@ #include "MyHead.h" #include "MySkeletonModel.h" +#include "AnimUtil.h" #include "Application.h" #include "AvatarManager.h" #include "AvatarActionHold.h" @@ -2971,30 +2972,21 @@ glm::vec3 MyAvatar::computeCounterBalance() const { } // this function matches the hips rotation to the new cghips-head axis -// curHead and hipPos are in Avatar space -// returns the rotation of the hips in Avatar space -static glm::quat computeNewHipsRotation(glm::vec3 curHead, glm::vec3 hipPos) { - glm::vec3 spineVec = curHead - hipPos; - glm::quat finalRot = Quaternions::IDENTITY; - glm::vec3 newYaxisHips = glm::normalize(spineVec); - glm::vec3 forward(0.0f, 0.0f, 1.0f); - const float EPSILON = 0.0001f; - if ((fabs(spineVec.y) < EPSILON) && (glm::length(spineVec) > 0.0f)) { - //y equals zero and hips position != head position - forward = glm::vec3(0.0f, 1.0f, 0.0f); - } - glm::vec3 oldZaxisHips = glm::normalize(forward); - glm::vec3 newXaxisHips = glm::normalize(glm::cross(newYaxisHips, oldZaxisHips)); - glm::vec3 newZaxisHips = glm::normalize(glm::cross(newXaxisHips, newYaxisHips)); - // create mat4 with the new axes - glm::vec4 left(newXaxisHips.x, newXaxisHips.y, newXaxisHips.z, 0.0f); - glm::vec4 up(newYaxisHips.x, newYaxisHips.y, newYaxisHips.z, 0.0f); - glm::vec4 view(newZaxisHips.x, newZaxisHips.y, newZaxisHips.z, 0.0f); - glm::vec4 translation(0.0f, 0.0f, 0.0f, 1.0f); - glm::mat4 newRotHips(left, up, view, translation); - finalRot = glm::toQuat(newRotHips); - glm::quat hipsRotation = finalRot; - return hipsRotation; +// headOrientation, headPosition and hipsPosition are in avatar space +// returns the matrix of the hips in Avatar space +static glm::mat4 computeNewHipsMatrix(glm::quat headOrientation, glm::vec3 headPosition, glm::vec3 hipsPosition) { + glm::quat headOrientationYawOnly = cancelOutRollAndPitch(headOrientation); + const float MIX_RATIO = 0.5f; + glm::quat hipsRot = safeLerp(Quaternions::IDENTITY, headOrientationYawOnly, MIX_RATIO); + glm::vec3 hipsFacing = hipsRot * Vectors::UNIT_Z; + + glm::vec3 spineVec = headPosition - hipsPosition; + glm::vec3 u, v, w; + generateBasisVectors(glm::normalize(spineVec), hipsFacing, u, v, w); + return glm::mat4(glm::vec4(w, 0.0f), + glm::vec4(u, 0.0f), + glm::vec4(v, 0.0f), + glm::vec4(hipsPosition, 1.0f)); } static void drawBaseOfSupport(float baseOfSupportScale, float footLocal, glm::mat4 avatarToWorld) { @@ -3043,40 +3035,21 @@ glm::quat cancelOutRollAndPitch2(const glm::quat& q) { return glm::quat_cast(temp); } -static glm::quat saferLerp(const glm::quat& a, const glm::quat& b, float alpha) { - // adjust signs if necessary - glm::quat bTemp = b; - float dot = glm::dot(a, bTemp); - if (dot < 0.0f) { - bTemp = -bTemp; - } - return glm::normalize(glm::lerp(a, bTemp, alpha)); -} - -// this function finds the hips position using a center of gravity model that +// this function finds the hips position using a center of gravity model that // balances the head and hands with the hips over the base of support // returns the rotation and position of the Avatar in Sensor space glm::mat4 MyAvatar::deriveBodyUsingCgModel() const { - glm::mat4 worldToSensorMat = glm::inverse(getSensorToWorldMatrix()); - glm::mat4 avatarToWorldMat = getTransform().getMatrix(); - glm::mat4 avatarToSensorMat = worldToSensorMat * avatarToWorldMat; - - glm::vec3 headPosition; - glm::quat headOrientation; + glm::mat4 sensorToWorldMat = getSensorToWorldMatrix(); + glm::mat4 worldToSensorMat = glm::inverse(sensorToWorldMat); auto headPose = getControllerPoseInSensorFrame(controller::Action::HEAD); - if (headPose.isValid()) { - headPosition = headPose.translation; - // rotate by 180 Y to put the head in same frame as the avatar - headOrientation = headPose.rotation * Quaternions::Y_180; - } - const glm::quat headOrientationYawOnly = cancelOutRollAndPitch(headOrientation); - const float MIX_RATIO = 0.5f; - // here we mix in some of the head yaw into the hip yaw - glm::quat hipYawRot = glm::normalize(saferLerp(glmExtractRotation(avatarToSensorMat), headOrientationYawOnly, MIX_RATIO)); - glm::vec3 newLocalHeadPos = glm::inverse(hipYawRot) * (headPosition - extractTranslation(avatarToSensorMat)); + + // the Y_180 is to flip the controller pose from -z forward to the head joint which is +z forward. + glm::mat4 sensorHeadMat = createMatFromQuatAndPos(headPose.rotation * Quaternions::Y_180, headPose.translation); + // convert into avatar space + glm::mat4 avatarToWorldMat = getTransform().getMatrix(); + glm::mat4 avatarHeadMat = glm::inverse(avatarToWorldMat) * sensorToWorldMat * sensorHeadMat; if (_enableDebugDrawBaseOfSupport) { - // default height is ~ 1.64 meters float scaleBaseOfSupport = getUserEyeHeight() / DEFAULT_AVATAR_EYE_HEIGHT; glm::vec3 rightFootPositionLocal = getAbsoluteJointTranslationInObjectFrame(_skeletonModel->getRig().indexOfJoint("RightFoot")); drawBaseOfSupport(scaleBaseOfSupport, rightFootPositionLocal.y, avatarToWorldMat); @@ -3084,11 +3057,12 @@ glm::mat4 MyAvatar::deriveBodyUsingCgModel() const { // get the new center of gravity const glm::vec3 cgHipsPosition = computeCounterBalance(); - glm::vec3 hipsPositionFinal = transformPoint(avatarToSensorMat, cgHipsPosition); // find the new hips rotation using the new head-hips axis as the up axis - glm::quat newHipsRotation = computeNewHipsRotation(newLocalHeadPos, cgHipsPosition); - return createMatFromQuatAndPos(hipYawRot*newHipsRotation, hipsPositionFinal); + glm::mat4 avatarHipsMat = computeNewHipsMatrix(glmExtractRotation(avatarHeadMat), extractTranslation(avatarHeadMat), cgHipsPosition); + + // convert hips from avatar to sensor space + return worldToSensorMat * avatarToWorldMat * avatarHipsMat; } float MyAvatar::getUserHeight() const { From 20c960c0da5406db331c600eb3af93284241c00d Mon Sep 17 00:00:00 2001 From: amantley Date: Wed, 9 May 2018 15:00:20 -0700 Subject: [PATCH 09/16] removed white space in MyAvatar.cpp --- interface/src/avatar/MyAvatar.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 35247bca20..72bb5cc500 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -3022,11 +3022,10 @@ glm::quat cancelOutRollAndPitch2(const glm::quat& q) { if (fabs(zAxis.y) > 0.0) { // new z is the up axis, that is the direction the body is pointing newZ = glm::normalize(q * glm::vec3(0.0f, 1.0f, 0.0f)); - } + } newX = glm::cross(vec3(0.0f, 1.0f, 0.0f), newZ); newY = glm::cross(newZ, newX); - } - else { + } else { newZ = glm::normalize(vec3(zAxis.x, 0.0f, zAxis.z)); newX = glm::cross(vec3(0.0f, 1.0f, 0.0f), newZ); newY = glm::cross(newZ, newX); @@ -3057,10 +3056,10 @@ glm::mat4 MyAvatar::deriveBodyUsingCgModel() const { // get the new center of gravity const glm::vec3 cgHipsPosition = computeCounterBalance(); - + // find the new hips rotation using the new head-hips axis as the up axis glm::mat4 avatarHipsMat = computeNewHipsMatrix(glmExtractRotation(avatarHeadMat), extractTranslation(avatarHeadMat), cgHipsPosition); - + // convert hips from avatar to sensor space return worldToSensorMat * avatarToWorldMat * avatarHipsMat; } From 1cc7eb635fb8a955a0cc211b925aa1f4c3cab61b Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 14 May 2018 09:13:26 -0700 Subject: [PATCH 10/16] Fix ubuntu warning by removing cancelOutRollAndPitch2() --- interface/src/avatar/MyAvatar.cpp | 23 ----------------------- 1 file changed, 23 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 11395182fa..1e87dca89b 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -3011,29 +3011,6 @@ static void drawBaseOfSupport(float baseOfSupportScale, float footLocal, glm::ma DebugDraw::getInstance().drawRay(frontLeft, frontRight, rayColor); } -// cancel out roll and pitch test fix -glm::quat cancelOutRollAndPitch2(const glm::quat& q) { - glm::vec3 zAxis = q * glm::vec3(0.0f, 0.0f, 1.0f); - glm::vec3 newZ; - glm::vec3 newX; - glm::vec3 newY; - // cancel out the roll and pitch - if (zAxis.x == 0 && zAxis.z == 0.0f) { - if (fabs(zAxis.y) > 0.0) { - // new z is the up axis, that is the direction the body is pointing - newZ = glm::normalize(q * glm::vec3(0.0f, 1.0f, 0.0f)); - } - newX = glm::cross(vec3(0.0f, 1.0f, 0.0f), newZ); - newY = glm::cross(newZ, newX); - } else { - newZ = glm::normalize(vec3(zAxis.x, 0.0f, zAxis.z)); - newX = glm::cross(vec3(0.0f, 1.0f, 0.0f), newZ); - newY = glm::cross(newZ, newX); - } - glm::mat4 temp(glm::vec4(newX, 0.0f), glm::vec4(newY, 0.0f), glm::vec4(newZ, 0.0f), glm::vec4(0.0f, 0.0f, 0.0f, 1.0f)); - return glm::quat_cast(temp); -} - // this function finds the hips position using a center of gravity model that // balances the head and hands with the hips over the base of support // returns the rotation and position of the Avatar in Sensor space From ec638d9d91716dd966cd071af66df19ed57c80b3 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 14 May 2018 13:48:04 -0700 Subject: [PATCH 11/16] Avatar can now look straight down without spinning 180. Also, come code clean up and removal of unnecessary Y_180 flips. --- interface/src/avatar/MyAvatar.cpp | 33 ++++++++++--------- interface/src/avatar/MyAvatar.h | 8 ++--- interface/src/avatar/MySkeletonModel.cpp | 1 + libraries/animation/src/AnimUtil.cpp | 40 +++++++++++++++++++++++- libraries/animation/src/AnimUtil.h | 5 +++ libraries/shared/src/GLMHelpers.cpp | 6 ++-- libraries/shared/src/GLMHelpers.h | 3 ++ 7 files changed, 75 insertions(+), 21 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 1e87dca89b..f017ba0527 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -423,12 +423,12 @@ void MyAvatar::update(float deltaTime) { } #ifdef DEBUG_DRAW_HMD_MOVING_AVERAGE - glm::vec3 p = transformPoint(getSensorToWorldMatrix(), getControllerPoseInAvatarFrame(controller::Pose::HEAD) * - glm::vec3(_headControllerFacingMovingAverage.x, 0.0f, _headControllerFacingMovingAverage.y)); - DebugDraw::getInstance().addMarker("facing-avg", getOrientation(), p, glm::vec4(1.0f)); - p = transformPoint(getSensorToWorldMatrix(), getHMDSensorPosition() + - glm::vec3(_headControllerFacing.x, 0.0f, _headControllerFacing.y)); - DebugDraw::getInstance().addMarker("facing", getOrientation(), p, glm::vec4(1.0f)); + auto sensorHeadPose = getControllerPoseInSensorFrame(controller::Action::HEAD); + glm::vec3 worldHeadPos = transformPoint(getSensorToWorldMatrix(), sensorHeadPose.getTranslation()); + glm::vec3 worldFacingAverage = transformVectorFast(getSensorToWorldMatrix(), glm::vec3(_headControllerFacingMovingAverage.x, 0.0f, _headControllerFacingMovingAverage.y)); + glm::vec3 worldFacing = transformVectorFast(getSensorToWorldMatrix(), glm::vec3(_headControllerFacing.x, 0.0f, _headControllerFacing.y)); + DebugDraw::getInstance().drawRay(worldHeadPos, worldHeadPos + worldFacing, glm::vec4(0.0f, 1.0f, 0.0f, 1.0f)); + DebugDraw::getInstance().drawRay(worldHeadPos, worldHeadPos + worldFacingAverage, glm::vec4(0.0f, 0.0f, 1.0f, 1.0f)); #endif if (_goToPending) { @@ -702,7 +702,8 @@ void MyAvatar::updateFromHMDSensorMatrix(const glm::mat4& hmdSensorMatrix) { _hmdSensorOrientation = glmExtractRotation(hmdSensorMatrix); auto headPose = getControllerPoseInSensorFrame(controller::Action::HEAD); if (headPose.isValid()) { - _headControllerFacing = getFacingDir2D(headPose.rotation); + glm::quat bodyOrientation = computeBodyFacingFromHead(headPose.rotation, Vectors::UNIT_Y); + _headControllerFacing = getFacingDir2D(bodyOrientation); } else { _headControllerFacing = glm::vec2(1.0f, 0.0f); } @@ -2817,6 +2818,7 @@ glm::mat4 MyAvatar::deriveBodyFromHMDSensor() const { auto headPose = getControllerPoseInSensorFrame(controller::Action::HEAD); if (headPose.isValid()) { headPosition = headPose.translation; + // AJT: TODO: can remove this Y_180 headOrientation = headPose.rotation * Quaternions::Y_180; } const glm::quat headOrientationYawOnly = cancelOutRollAndPitch(headOrientation); @@ -2839,6 +2841,8 @@ glm::mat4 MyAvatar::deriveBodyFromHMDSensor() const { // eyeToNeck offset is relative full HMD orientation. // while neckToRoot offset is only relative to HMDs yaw. // Y_180 is necessary because rig is z forward and hmdOrientation is -z forward + + // AJT: TODO: can remove this Y_180, if we remove the higher level one. glm::vec3 headToNeck = headOrientation * Quaternions::Y_180 * (localNeck - localHead); glm::vec3 neckToRoot = headOrientationYawOnly * Quaternions::Y_180 * -localNeck; @@ -2975,9 +2979,11 @@ glm::vec3 MyAvatar::computeCounterBalance() const { // headOrientation, headPosition and hipsPosition are in avatar space // returns the matrix of the hips in Avatar space static glm::mat4 computeNewHipsMatrix(glm::quat headOrientation, glm::vec3 headPosition, glm::vec3 hipsPosition) { - glm::quat headOrientationYawOnly = cancelOutRollAndPitch(headOrientation); - const float MIX_RATIO = 0.5f; - glm::quat hipsRot = safeLerp(Quaternions::IDENTITY, headOrientationYawOnly, MIX_RATIO); + + glm::quat bodyOrientation = computeBodyFacingFromHead(headOrientation, Vectors::UNIT_Y); + + const float MIX_RATIO = 0.3f; + glm::quat hipsRot = safeLerp(Quaternions::IDENTITY, bodyOrientation, MIX_RATIO); glm::vec3 hipsFacing = hipsRot * Vectors::UNIT_Z; glm::vec3 spineVec = headPosition - hipsPosition; @@ -3013,14 +3019,14 @@ static void drawBaseOfSupport(float baseOfSupportScale, float footLocal, glm::ma // this function finds the hips position using a center of gravity model that // balances the head and hands with the hips over the base of support -// returns the rotation and position of the Avatar in Sensor space +// returns the rotation (-z forward) and position of the Avatar in Sensor space glm::mat4 MyAvatar::deriveBodyUsingCgModel() const { glm::mat4 sensorToWorldMat = getSensorToWorldMatrix(); glm::mat4 worldToSensorMat = glm::inverse(sensorToWorldMat); auto headPose = getControllerPoseInSensorFrame(controller::Action::HEAD); - // the Y_180 is to flip the controller pose from -z forward to the head joint which is +z forward. glm::mat4 sensorHeadMat = createMatFromQuatAndPos(headPose.rotation * Quaternions::Y_180, headPose.translation); + // convert into avatar space glm::mat4 avatarToWorldMat = getTransform().getMatrix(); glm::mat4 avatarHeadMat = glm::inverse(avatarToWorldMat) * sensorToWorldMat * sensorHeadMat; @@ -3038,6 +3044,7 @@ glm::mat4 MyAvatar::deriveBodyUsingCgModel() const { glm::mat4 avatarHipsMat = computeNewHipsMatrix(glmExtractRotation(avatarHeadMat), extractTranslation(avatarHeadMat), cgHipsPosition); // convert hips from avatar to sensor space + // The Y_180 is to convert from z forward to -z forward. return worldToSensorMat * avatarToWorldMat * avatarHipsMat; } @@ -3205,9 +3212,7 @@ void MyAvatar::FollowHelper::decrementTimeRemaining(float dt) { bool MyAvatar::FollowHelper::shouldActivateRotation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) const { const float FOLLOW_ROTATION_THRESHOLD = cosf(PI / 6.0f); // 30 degrees glm::vec2 bodyFacing = getFacingDir2D(currentBodyMatrix); - return glm::dot(-myAvatar.getHeadControllerFacingMovingAverage(), 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 c7b09f7bc6..0d9fd860b7 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -978,14 +978,14 @@ public: // derive avatar body position and orientation from the current HMD Sensor location. - // results are in HMD frame + // results are in sensor frame (-z forward) glm::mat4 deriveBodyFromHMDSensor() const; glm::vec3 computeCounterBalance() const; // derive avatar body position and orientation from using the current HMD Sensor location in relation to the previous // location of the base of support of the avatar. - // results are in HMD frame + // results are in sensor frame (-z foward) glm::mat4 deriveBodyUsingCgModel() const; /**jsdoc @@ -1495,8 +1495,8 @@ private: glm::quat _hmdSensorOrientation; glm::vec3 _hmdSensorPosition; // cache head controller pose in sensor space - glm::vec2 _headControllerFacing; // facing vector in xz plane - glm::vec2 _headControllerFacingMovingAverage { 0, 0 }; // facing vector in xz plane + glm::vec2 _headControllerFacing; // facing vector in xz plane (sensor space) + glm::vec2 _headControllerFacingMovingAverage { 0.0f, 0.0f }; // facing vector in xz plane (sensor space) // cache of the current body position and orientation of the avatar's body, // in sensor space. diff --git a/interface/src/avatar/MySkeletonModel.cpp b/interface/src/avatar/MySkeletonModel.cpp index f7f55db369..4a7c203f11 100644 --- a/interface/src/avatar/MySkeletonModel.cpp +++ b/interface/src/avatar/MySkeletonModel.cpp @@ -83,6 +83,7 @@ static AnimPose computeHipsInSensorFrame(MyAvatar* myAvatar, bool isFlying) { hipsPos = headPos + tiltRot * (hipsPos - headPos); } + // AJT: TODO can we remove this? return AnimPose(hipsRot * Quaternions::Y_180, hipsPos); } diff --git a/libraries/animation/src/AnimUtil.cpp b/libraries/animation/src/AnimUtil.cpp index 65c605b5ba..ad11623a25 100644 --- a/libraries/animation/src/AnimUtil.cpp +++ b/libraries/animation/src/AnimUtil.cpp @@ -9,7 +9,9 @@ // #include "AnimUtil.h" -#include "GLMHelpers.h" +#include +#include +#include // TODO: use restrict keyword // TODO: excellent candidate for simd vectorization. @@ -107,3 +109,39 @@ AnimPose boneLookAt(const glm::vec3& target, const AnimPose& bone) { glm::vec4(bone.trans(), 1.0f)); return AnimPose(lookAt); } + +// This will attempt to determine the proper body facing of a characters body +// assumes headRot is z-forward and y-up. +// and returns a bodyRot that is also z-forward and y-up +glm::quat computeBodyFacingFromHead(const glm::quat& headRot, const glm::vec3& up) { + + glm::vec3 bodyUp = glm::normalize(up); + + // initially take the body facing from the head. + glm::vec3 headUp = headRot * Vectors::UNIT_Y; + glm::vec3 headForward = headRot * Vectors::UNIT_Z; + const float THRESHOLD = cosf(glm::radians(30.0f)); + + glm::vec3 bodyForward = headForward; + + float dot = glm::dot(headForward, bodyUp); + + if (dot < -THRESHOLD) { // head is looking down + // the body should face in the same direction as the top the head. + bodyForward = headUp; + } else if (dot > THRESHOLD) { // head is looking upward + // the body should face away from the top of the head. + bodyForward = -headUp; + } + + // cancel out upward component + bodyForward = glm::normalize(bodyForward - dot * bodyUp); + + glm::vec3 u, v, w; + generateBasisVectors(bodyForward, bodyUp, u, v, w); + + // create matrix from orthogonal basis vectors + glm::mat4 bodyMat(glm::vec4(w, 0.0f), glm::vec4(v, 0.0f), glm::vec4(u, 0.0f), glm::vec4(0.0f, 0.0f, 0.0f, 1.0f)); + + return glmExtractRotation(bodyMat); +} diff --git a/libraries/animation/src/AnimUtil.h b/libraries/animation/src/AnimUtil.h index f2cceb361b..3cd7f4b6fb 100644 --- a/libraries/animation/src/AnimUtil.h +++ b/libraries/animation/src/AnimUtil.h @@ -33,4 +33,9 @@ inline glm::quat safeLerp(const glm::quat& a, const glm::quat& b, float alpha) { AnimPose boneLookAt(const glm::vec3& target, const AnimPose& bone); +// This will attempt to determine the proper body facing of a characters body +// assumes headRot is z-forward and y-up. +// and returns a bodyRot that is also z-forward and y-up +glm::quat computeBodyFacingFromHead(const glm::quat& headRot, const glm::vec3& up); + #endif diff --git a/libraries/shared/src/GLMHelpers.cpp b/libraries/shared/src/GLMHelpers.cpp index 75446754d5..4be8ad0e41 100644 --- a/libraries/shared/src/GLMHelpers.cpp +++ b/libraries/shared/src/GLMHelpers.cpp @@ -574,8 +574,9 @@ void generateBasisVectors(const glm::vec3& primaryAxis, const glm::vec3& seconda vAxisOut = glm::cross(wAxisOut, uAxisOut); } +// assumes z-forward and y-up glm::vec2 getFacingDir2D(const glm::quat& rot) { - glm::vec3 facing3D = rot * Vectors::UNIT_NEG_Z; + glm::vec3 facing3D = rot * Vectors::UNIT_Z; glm::vec2 facing2D(facing3D.x, facing3D.z); const float ALMOST_ZERO = 0.0001f; if (glm::length(facing2D) < ALMOST_ZERO) { @@ -585,8 +586,9 @@ glm::vec2 getFacingDir2D(const glm::quat& rot) { } } +// assumes z-forward and y-up glm::vec2 getFacingDir2D(const glm::mat4& m) { - glm::vec3 facing3D = transformVectorFast(m, Vectors::UNIT_NEG_Z); + glm::vec3 facing3D = transformVectorFast(m, Vectors::UNIT_Z); glm::vec2 facing2D(facing3D.x, facing3D.z); const float ALMOST_ZERO = 0.0001f; if (glm::length(facing2D) < ALMOST_ZERO) { diff --git a/libraries/shared/src/GLMHelpers.h b/libraries/shared/src/GLMHelpers.h index 0e1af27cd2..7e6ef4cb28 100644 --- a/libraries/shared/src/GLMHelpers.h +++ b/libraries/shared/src/GLMHelpers.h @@ -250,7 +250,10 @@ glm::vec3 transformVectorFull(const glm::mat4& m, const glm::vec3& v); void generateBasisVectors(const glm::vec3& primaryAxis, const glm::vec3& secondaryAxis, glm::vec3& uAxisOut, glm::vec3& vAxisOut, glm::vec3& wAxisOut); +// assumes z-forward and y-up glm::vec2 getFacingDir2D(const glm::quat& rot); + +// assumes z-forward and y-up glm::vec2 getFacingDir2D(const glm::mat4& m); inline bool isNaN(const glm::vec3& value) { return isNaN(value.x) || isNaN(value.y) || isNaN(value.z); } From 558711f906425ac23bb0b2f362192b4e261fb8a8 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 4 Jun 2018 16:57:41 -0700 Subject: [PATCH 12/16] Smooth hips transition between idle and fly. Also, take dt into account for the critically damped spring lerp calculation. --- interface/src/avatar/MySkeletonModel.cpp | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/interface/src/avatar/MySkeletonModel.cpp b/interface/src/avatar/MySkeletonModel.cpp index 87a22396cd..d8ceb09b5f 100644 --- a/interface/src/avatar/MySkeletonModel.cpp +++ b/interface/src/avatar/MySkeletonModel.cpp @@ -192,14 +192,20 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { AnimPose hips = computeHipsInSensorFrame(myAvatar, isFlying); + // timescale in seconds + const float TRANS_HORIZ_TIMESCALE = 0.25f; + const float TRANS_VERT_TIMESCALE = 0.01f; // We want the vertical component of the hips to follow quickly to prevent spine squash/stretch. + const float ROT_TIMESCALE = 0.15f; + + float transHorizAlpha = glm::min(deltaTime / TRANS_HORIZ_TIMESCALE, 1.0f); + float transVertAlpha = glm::min(deltaTime / TRANS_VERT_TIMESCALE, 1.0f); + float rotAlpha = glm::min(deltaTime / ROT_TIMESCALE, 1.0f); + // smootly lerp hips, in sensorframe, with different coeff for horiz and vertical translation. - const float ROT_ALPHA = 0.9f; - const float TRANS_HORIZ_ALPHA = 0.9f; - const float TRANS_VERT_ALPHA = 0.1f; float hipsY = hips.trans().y; - hips.trans() = lerp(hips.trans(), _prevHips.trans(), TRANS_HORIZ_ALPHA); - hips.trans().y = lerp(hipsY, _prevHips.trans().y, TRANS_VERT_ALPHA); - hips.rot() = safeLerp(hips.rot(), _prevHips.rot(), ROT_ALPHA); + hips.trans() = lerp(_prevHips.trans(), hips.trans(), transHorizAlpha); + hips.trans().y = lerp(_prevHips.trans().y, hipsY, transVertAlpha); + hips.rot() = safeLerp(_prevHips.rot(), hips.rot(), rotAlpha); _prevHips = hips; _prevHipsValid = true; From 94c39bc4af1ac912bba9c2ba6e0b46c5db2db56c Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Tue, 5 Jun 2018 14:43:33 -0700 Subject: [PATCH 13/16] Better fix for looking down at your own body. --- libraries/animation/src/AnimUtil.cpp | 23 ++++++++++++++--------- 1 file changed, 14 insertions(+), 9 deletions(-) diff --git a/libraries/animation/src/AnimUtil.cpp b/libraries/animation/src/AnimUtil.cpp index ad11623a25..acb90126fc 100644 --- a/libraries/animation/src/AnimUtil.cpp +++ b/libraries/animation/src/AnimUtil.cpp @@ -120,22 +120,27 @@ glm::quat computeBodyFacingFromHead(const glm::quat& headRot, const glm::vec3& u // initially take the body facing from the head. glm::vec3 headUp = headRot * Vectors::UNIT_Y; glm::vec3 headForward = headRot * Vectors::UNIT_Z; - const float THRESHOLD = cosf(glm::radians(30.0f)); + glm::vec3 headLeft = headRot * Vectors::UNIT_X; + const float NOD_THRESHOLD = cosf(glm::radians(45.0f)); + const float TILT_THRESHOLD = cosf(glm::radians(30.0f)); glm::vec3 bodyForward = headForward; - float dot = glm::dot(headForward, bodyUp); + float nodDot = glm::dot(headForward, bodyUp); + float tiltDot = glm::dot(headLeft, bodyUp); - if (dot < -THRESHOLD) { // head is looking down - // the body should face in the same direction as the top the head. - bodyForward = headUp; - } else if (dot > THRESHOLD) { // head is looking upward - // the body should face away from the top of the head. - bodyForward = -headUp; + if (fabsf(tiltDot) < TILT_THRESHOLD) { // if we are not tilting too much + if (nodDot < -NOD_THRESHOLD) { // head is looking downward + // the body should face in the same direction as the top the head. + bodyForward = headUp; + } else if (nodDot > NOD_THRESHOLD) { // head is looking upward + // the body should face away from the top of the head. + bodyForward = -headUp; + } } // cancel out upward component - bodyForward = glm::normalize(bodyForward - dot * bodyUp); + bodyForward = glm::normalize(bodyForward - nodDot * bodyUp); glm::vec3 u, v, w; generateBasisVectors(bodyForward, bodyUp, u, v, w); From 993d035d3767ba4ee649e322bec5e8ee81585292 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Tue, 5 Jun 2018 15:45:19 -0700 Subject: [PATCH 14/16] Smooth out fly to idle transition, to prevent hip thrusting and pops. --- interface/src/avatar/MySkeletonModel.cpp | 23 ++++++++++++++++++++--- interface/src/avatar/MySkeletonModel.h | 2 ++ 2 files changed, 22 insertions(+), 3 deletions(-) diff --git a/interface/src/avatar/MySkeletonModel.cpp b/interface/src/avatar/MySkeletonModel.cpp index d8ceb09b5f..c23c6da8d3 100644 --- a/interface/src/avatar/MySkeletonModel.cpp +++ b/interface/src/avatar/MySkeletonModel.cpp @@ -181,6 +181,15 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { } } + bool isFlying = (myAvatar->getCharacterController()->getState() == CharacterController::State::Hover || myAvatar->getCharacterController()->computeCollisionGroup() == BULLET_COLLISION_GROUP_COLLISIONLESS); + if (isFlying != _prevIsFlying) { + const float FLY_TO_IDLE_HIPS_TRANSITION_TIME = 0.5f; + _flyIdleTimer = FLY_TO_IDLE_HIPS_TRANSITION_TIME; + } else { + _flyIdleTimer -= deltaTime; + } + _prevIsFlying = isFlying; + // if hips are not under direct control, estimate the hips position. if (avatarHeadPose.isValid() && !(params.primaryControllerFlags[Rig::PrimaryControllerType_Hips] & (uint8_t)Rig::ControllerFlags::Enabled)) { bool isFlying = (myAvatar->getCharacterController()->getState() == CharacterController::State::Hover || myAvatar->getCharacterController()->computeCollisionGroup() == BULLET_COLLISION_GROUP_COLLISIONLESS); @@ -196,10 +205,18 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { const float TRANS_HORIZ_TIMESCALE = 0.25f; const float TRANS_VERT_TIMESCALE = 0.01f; // We want the vertical component of the hips to follow quickly to prevent spine squash/stretch. const float ROT_TIMESCALE = 0.15f; + const float FLY_IDLE_TRANSITION_TIMESCALE = 0.3f; - float transHorizAlpha = glm::min(deltaTime / TRANS_HORIZ_TIMESCALE, 1.0f); - float transVertAlpha = glm::min(deltaTime / TRANS_VERT_TIMESCALE, 1.0f); - float rotAlpha = glm::min(deltaTime / ROT_TIMESCALE, 1.0f); + float transHorizAlpha, transVertAlpha, rotAlpha; + if (_flyIdleTimer < 0.0f) { + transHorizAlpha = glm::min(deltaTime / TRANS_HORIZ_TIMESCALE, 1.0f); + transVertAlpha = glm::min(deltaTime / TRANS_VERT_TIMESCALE, 1.0f); + rotAlpha = glm::min(deltaTime / ROT_TIMESCALE, 1.0f); + } else { + transHorizAlpha = glm::min(deltaTime / FLY_IDLE_TRANSITION_TIMESCALE, 1.0f); + transVertAlpha = glm::min(deltaTime / FLY_IDLE_TRANSITION_TIMESCALE, 1.0f); + rotAlpha = glm::min(deltaTime / FLY_IDLE_TRANSITION_TIMESCALE, 1.0f); + } // smootly lerp hips, in sensorframe, with different coeff for horiz and vertical translation. float hipsY = hips.trans().y; diff --git a/interface/src/avatar/MySkeletonModel.h b/interface/src/avatar/MySkeletonModel.h index 252b6c293b..ebef9796a4 100644 --- a/interface/src/avatar/MySkeletonModel.h +++ b/interface/src/avatar/MySkeletonModel.h @@ -28,6 +28,8 @@ private: AnimPose _prevHips; // sensor frame bool _prevHipsValid { false }; + bool _prevIsFlying { false }; + float _flyIdleTimer { 0.0f }; std::map _jointRotationFrameOffsetMap; }; From b5a3b25adce366c83788b479eb41df61870cbdf3 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Tue, 5 Jun 2018 16:07:54 -0700 Subject: [PATCH 15/16] Tuned hip smoothing times. --- interface/src/avatar/MySkeletonModel.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/interface/src/avatar/MySkeletonModel.cpp b/interface/src/avatar/MySkeletonModel.cpp index c23c6da8d3..c15b00ca19 100644 --- a/interface/src/avatar/MySkeletonModel.cpp +++ b/interface/src/avatar/MySkeletonModel.cpp @@ -202,10 +202,10 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { AnimPose hips = computeHipsInSensorFrame(myAvatar, isFlying); // timescale in seconds - const float TRANS_HORIZ_TIMESCALE = 0.25f; + const float TRANS_HORIZ_TIMESCALE = 0.15f; const float TRANS_VERT_TIMESCALE = 0.01f; // We want the vertical component of the hips to follow quickly to prevent spine squash/stretch. const float ROT_TIMESCALE = 0.15f; - const float FLY_IDLE_TRANSITION_TIMESCALE = 0.3f; + const float FLY_IDLE_TRANSITION_TIMESCALE = 0.25f; float transHorizAlpha, transVertAlpha, rotAlpha; if (_flyIdleTimer < 0.0f) { From c3da4e74de328e7fec004f162089730dfa82022c Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Fri, 8 Jun 2018 16:22:00 -0700 Subject: [PATCH 16/16] Code review feedback --- libraries/shared/src/AvatarConstants.h | 1 - scripts/defaultScripts.js | 2 -- tests/animation/src/data/avatar.json | 2 +- 3 files changed, 1 insertion(+), 4 deletions(-) diff --git a/libraries/shared/src/AvatarConstants.h b/libraries/shared/src/AvatarConstants.h index bdc54dfeb6..e90e25d5b0 100644 --- a/libraries/shared/src/AvatarConstants.h +++ b/libraries/shared/src/AvatarConstants.h @@ -31,7 +31,6 @@ const float DEFAULT_AVATAR_HEAD_MASS = 20.0f; const float DEFAULT_AVATAR_LEFTHAND_MASS = 2.0f; const float DEFAULT_AVATAR_RIGHTHAND_MASS = 2.0f; - // Used when avatar is missing joints... (avatar space) const glm::quat DEFAULT_AVATAR_MIDDLE_EYE_ROT { Quaternions::Y_180 }; const glm::vec3 DEFAULT_AVATAR_MIDDLE_EYE_POS { 0.0f, 0.6f, 0.0f }; diff --git a/scripts/defaultScripts.js b/scripts/defaultScripts.js index 27589547c6..15c5a575fd 100644 --- a/scripts/defaultScripts.js +++ b/scripts/defaultScripts.js @@ -33,8 +33,6 @@ var DEFAULT_SCRIPTS_COMBINED = [ ]; var DEFAULT_SCRIPTS_SEPARATE = [ "system/controllers/controllerScripts.js", - //"developer/step.js", - //"developer/cg_lean.js" //"system/chat.js" ]; diff --git a/tests/animation/src/data/avatar.json b/tests/animation/src/data/avatar.json index 3b80ff6d77..550a95e980 100644 --- a/tests/animation/src/data/avatar.json +++ b/tests/animation/src/data/avatar.json @@ -363,7 +363,7 @@ { "id": "idle", "interpTarget": 6, - "interpDuration": 3, + "interpDuration": 6, "transitions": [ { "var": "isMovingForward", "state": "walkFwd" }, { "var": "isMovingBackward", "state": "walkBwd" },