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
This commit is contained in:
amantley 2018-04-19 14:04:54 -07:00
parent 6ac43c34c2
commit 3a10cacec0
9 changed files with 902 additions and 11 deletions

View file

@ -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,

View file

@ -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";

View file

@ -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<const MyHead*>(getHead());
}

View file

@ -105,6 +105,9 @@ class MyAvatar : public Avatar {
* by 30cm. <em>Read-only.</em>
* @property {Pose} rightHandTipPose - The pose of the right hand as determined by the hand controllers, with the position
* by 30cm. <em>Read-only.</em>
* @property {boolean} centerOfGravityModelEnabled=true - If <code>true</code> 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 <code>false</code>
* will result in the default behaviour where the hips are placed under the head.
* @property {boolean} hmdLeanRecenterEnabled=true - If <code>true</code> 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 <code>false</code> 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<bool> _forceActivateRotation{ false };
std::atomic<bool> _forceActivateVertical{ false };
std::atomic<bool> _forceActivateHorizontal{ false };
bool getToggleHipsFollowing() const;
void setToggleHipsFollowing(bool followHead);
std::atomic<bool> _forceActivateRotation { false };
std::atomic<bool> _forceActivateVertical { false };
std::atomic<bool> _forceActivateHorizontal { false };
std::atomic<bool> _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<controller::Action, controller::Pose> _controllerPoseMap;
mutable std::mutex _controllerPoseMapMutex;
bool _centerOfGravityModelEnabled { true };
bool _hmdLeanRecenterEnabled { true };
bool _sprint { false };

View file

@ -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.

View file

@ -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 };

View file

@ -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"
];

View file

@ -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<points.length;j++) {
print(JSON.stringify(points[j]));
}
return {points: newPoints, normals: newNormals};
}
function mirrorPoints() {
if (Math.abs(base.points[0].x) > 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);
});

View file

@ -363,7 +363,7 @@
{
"id": "idle",
"interpTarget": 6,
"interpDuration": 6,
"interpDuration": 3,
"transitions": [
{ "var": "isMovingForward", "state": "walkFwd" },
{ "var": "isMovingBackward", "state": "walkBwd" },