Merge pull request #13641 from amantley/rotateFilterChange

Hand Azimuth Controls Shoulder Rotation
This commit is contained in:
John Conklin II 2018-08-30 13:59:00 -07:00 committed by GitHub
commit 8fa4a2855e
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
4 changed files with 151 additions and 17 deletions

View file

@ -91,8 +91,6 @@ const float MIN_SCALE_CHANGED_DELTA = 0.001f;
const int MODE_READINGS_RING_BUFFER_SIZE = 500;
const float CENTIMETERS_PER_METER = 100.0f;
//#define DEBUG_DRAW_HMD_MOVING_AVERAGE
MyAvatar::MyAvatar(QThread* thread) :
Avatar(thread),
_yawSpeed(YAW_SPEED_DEFAULT),
@ -447,9 +445,27 @@ void MyAvatar::reset(bool andRecenter, bool andReload, bool andHead) {
void MyAvatar::update(float deltaTime) {
// update moving average of HMD facing in xz plane.
const float HMD_FACING_TIMESCALE = getRotationRecenterFilterLength();
const float PERCENTAGE_WEIGHT_HEAD_VS_SHOULDERS_AZIMUTH = 0.0f; // 100 percent shoulders
float tau = deltaTime / HMD_FACING_TIMESCALE;
_headControllerFacingMovingAverage = lerp(_headControllerFacingMovingAverage, _headControllerFacing, tau);
setHipToHandController(computeHandAzimuth());
// put the average hand azimuth into sensor space.
// then mix it with head facing direction to determine rotation recenter
if (getControllerPoseInAvatarFrame(controller::Action::LEFT_HAND).isValid() && getControllerPoseInAvatarFrame(controller::Action::RIGHT_HAND).isValid()) {
glm::vec3 handHipAzimuthWorldSpace = transformVectorFast(getTransform().getMatrix(), glm::vec3(_hipToHandController.x, 0.0f, _hipToHandController.y));
glm::mat4 sensorToWorldMat = getSensorToWorldMatrix();
glm::mat4 worldToSensorMat = glm::inverse(sensorToWorldMat);
glm::vec3 handHipAzimuthSensorSpace = transformVectorFast(worldToSensorMat, handHipAzimuthWorldSpace);
glm::vec2 normedHandHipAzimuthSensorSpace(0.0f, 1.0f);
if (glm::length(glm::vec2(handHipAzimuthSensorSpace.x, handHipAzimuthSensorSpace.z)) > 0.0f) {
normedHandHipAzimuthSensorSpace = glm::normalize(glm::vec2(handHipAzimuthSensorSpace.x, handHipAzimuthSensorSpace.z));
}
glm::vec2 headFacingPlusHandHipAzimuthMix = lerp(normedHandHipAzimuthSensorSpace, _headControllerFacing, PERCENTAGE_WEIGHT_HEAD_VS_SHOULDERS_AZIMUTH);
_headControllerFacingMovingAverage = lerp(_headControllerFacingMovingAverage, headFacingPlusHandHipAzimuthMix, tau);
} else {
_headControllerFacingMovingAverage = lerp(_headControllerFacingMovingAverage, _headControllerFacing, tau);
}
if (_smoothOrientationTimer < SMOOTH_TIME_ORIENTATION) {
_rotationChanged = usecTimestampNow();
@ -462,19 +478,23 @@ void MyAvatar::update(float deltaTime) {
setCurrentStandingHeight(computeStandingHeightMode(getControllerPoseInAvatarFrame(controller::Action::HEAD)));
setAverageHeadRotation(computeAverageHeadRotation(getControllerPoseInAvatarFrame(controller::Action::HEAD)));
#ifdef DEBUG_DRAW_HMD_MOVING_AVERAGE
auto sensorHeadPose = getControllerPoseInSensorFrame(controller::Action::HEAD);
glm::vec3 worldHeadPos = transformPoint(getSensorToWorldMatrix(), sensorHeadPose.getTranslation());
glm::vec3 worldFacingAverage = transformVectorFast(getSensorToWorldMatrix(), glm::vec3(_headControllerFacingMovingAverage.x, 0.0f, _headControllerFacingMovingAverage.y));
glm::vec3 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 (_drawAverageFacingEnabled) {
auto sensorHeadPose = getControllerPoseInSensorFrame(controller::Action::HEAD);
glm::vec3 worldHeadPos = transformPoint(getSensorToWorldMatrix(), sensorHeadPose.getTranslation());
glm::vec3 worldFacingAverage = transformVectorFast(getSensorToWorldMatrix(), glm::vec3(_headControllerFacingMovingAverage.x, 0.0f, _headControllerFacingMovingAverage.y));
glm::vec3 worldFacing = transformVectorFast(getSensorToWorldMatrix(), glm::vec3(_headControllerFacing.x, 0.0f, _headControllerFacing.y));
DebugDraw::getInstance().drawRay(worldHeadPos, worldHeadPos + worldFacing, glm::vec4(0.0f, 1.0f, 0.0f, 1.0f));
DebugDraw::getInstance().drawRay(worldHeadPos, worldHeadPos + worldFacingAverage, glm::vec4(0.0f, 0.0f, 1.0f, 1.0f));
// draw hand azimuth vector
glm::vec3 handAzimuthMidpoint = transformPoint(getTransform().getMatrix(), glm::vec3(_hipToHandController.x, 0.0f, _hipToHandController.y));
DebugDraw::getInstance().drawRay(getWorldPosition(), handAzimuthMidpoint, glm::vec4(0.0f, 1.0f, 1.0f, 1.0f));
}
if (_goToPending) {
setWorldPosition(_goToPosition);
setWorldOrientation(_goToOrientation);
_headControllerFacingMovingAverage = _headControllerFacing; // reset moving average
_headControllerFacingMovingAverage = _headControllerFacing; // reset moving average
_goToPending = false;
// updateFromHMDSensorMatrix (called from paintGL) expects that the sensorToWorldMatrix is updated for any position changes
// that happen between render and Application::update (which calls updateSensorToWorldMatrix to do so).
@ -796,6 +816,47 @@ void MyAvatar::updateFromHMDSensorMatrix(const glm::mat4& hmdSensorMatrix) {
}
}
// Find the vector halfway between the hip to hand azimuth vectors
// This midpoint hand azimuth is in Avatar space
glm::vec2 MyAvatar::computeHandAzimuth() const {
controller::Pose leftHandPoseAvatarSpace = getLeftHandPose();
controller::Pose rightHandPoseAvatarSpace = getRightHandPose();
controller::Pose headPoseAvatarSpace = getControllerPoseInAvatarFrame(controller::Action::HEAD);
const float HALFWAY = 0.50f;
glm::vec2 latestHipToHandController = _hipToHandController;
if (leftHandPoseAvatarSpace.isValid() && rightHandPoseAvatarSpace.isValid() && headPoseAvatarSpace.isValid()) {
// we need the old azimuth reading to prevent flipping the facing direction 180
// in the case where the hands go from being slightly less than 180 apart to slightly more than 180 apart.
glm::vec2 oldAzimuthReading = _hipToHandController;
if ((glm::length(glm::vec2(rightHandPoseAvatarSpace.translation.x, rightHandPoseAvatarSpace.translation.z)) > 0.0f) && (glm::length(glm::vec2(leftHandPoseAvatarSpace.translation.x, leftHandPoseAvatarSpace.translation.z)) > 0.0f)) {
latestHipToHandController = lerp(glm::normalize(glm::vec2(rightHandPoseAvatarSpace.translation.x, rightHandPoseAvatarSpace.translation.z)), glm::normalize(glm::vec2(leftHandPoseAvatarSpace.translation.x, leftHandPoseAvatarSpace.translation.z)), HALFWAY);
} else {
latestHipToHandController = glm::vec2(0.0f, -1.0f);
}
glm::vec3 headLookAtAvatarSpace = transformVectorFast(headPoseAvatarSpace.getMatrix(), glm::vec3(0.0f, 0.0f, 1.0f));
glm::vec2 headAzimuthAvatarSpace = glm::vec2(headLookAtAvatarSpace.x, headLookAtAvatarSpace.z);
if (glm::length(headAzimuthAvatarSpace) > 0.0f) {
headAzimuthAvatarSpace = glm::normalize(headAzimuthAvatarSpace);
} else {
headAzimuthAvatarSpace = -latestHipToHandController;
}
// check the angular distance from forward and back
float cosForwardAngle = glm::dot(latestHipToHandController, oldAzimuthReading);
float cosHeadShoulder = glm::dot(-latestHipToHandController, headAzimuthAvatarSpace);
// if we are now closer to the 180 flip of the previous chest forward
// then we negate our computed latestHipToHandController to keep the chest from flipping.
// also check the head to shoulder azimuth difference if we negate.
// don't negate the chest azimuth if this is greater than 100 degrees.
if ((cosForwardAngle < 0.0f) && !(cosHeadShoulder < -0.2f)) {
latestHipToHandController = -latestHipToHandController;
}
}
return latestHipToHandController;
}
void MyAvatar::updateJointFromController(controller::Action poseKey, ThreadSafeValueCache<glm::mat4>& matrixCache) {
assert(QThread::currentThread() == thread());
auto userInputMapper = DependencyManager::get<UserInputMapper>();
@ -3353,6 +3414,24 @@ glm::mat4 MyAvatar::deriveBodyFromHMDSensor() const {
return createMatFromQuatAndPos(headOrientationYawOnly, bodyPos);
}
glm::mat4 MyAvatar::getSpine2RotationRigSpace() const {
// static const glm::quat RIG_CHANGE_OF_BASIS = Quaternions::Y_180;
// RIG_CHANGE_OF_BASIS * AVATAR_TO_RIG_ROTATION * inverse(RIG_CHANGE_OF_BASIS) = Quaternions::Y_180; //avatar Space;
const glm::quat AVATAR_TO_RIG_ROTATION = Quaternions::Y_180;
glm::vec3 hipToHandRigSpace = AVATAR_TO_RIG_ROTATION * glm::vec3(_hipToHandController.x, 0.0f, _hipToHandController.y);
glm::vec3 u, v, w;
if (glm::length(hipToHandRigSpace) > 0.0f) {
hipToHandRigSpace = glm::normalize(hipToHandRigSpace);
} else {
hipToHandRigSpace = glm::vec3(0.0f, 0.0f, 1.0f);
}
generateBasisVectors(glm::vec3(0.0f,1.0f,0.0f), hipToHandRigSpace, u, v, w);
glm::mat4 spine2RigSpace(glm::vec4(w, 0.0f), glm::vec4(u, 0.0f), glm::vec4(v, 0.0f), glm::vec4(glm::vec3(0.0f, 0.0f, 0.0f), 1.0f));
return spine2RigSpace;
}
// ease in function for dampening cg movement
static float slope(float num) {
const float CURVE_CONSTANT = 1.0f;
@ -3936,7 +4015,7 @@ bool MyAvatar::FollowHelper::shouldActivateHorizontalCG(MyAvatar& myAvatar) cons
glm::vec3 currentHeadPosition = currentHeadPose.getTranslation();
float anatomicalHeadToHipsDistance = glm::length(defaultHeadPosition - defaultHipsPosition);
if (!isActive(Horizontal) &&
(glm::length(currentHeadPosition - defaultHipsPosition) > (anatomicalHeadToHipsDistance + DEFAULT_AVATAR_SPINE_STRETCH_LIMIT))) {
(glm::length(currentHeadPosition - defaultHipsPosition) > (anatomicalHeadToHipsDistance + (DEFAULT_AVATAR_SPINE_STRETCH_LIMIT * anatomicalHeadToHipsDistance)))) {
myAvatar.setResetMode(true);
stepDetected = true;
}
@ -3960,25 +4039,32 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat
qApp->getCamera().getMode() != CAMERA_MODE_MIRROR) {
if (!isActive(Rotation) && (shouldActivateRotation(myAvatar, desiredBodyMatrix, currentBodyMatrix) || hasDriveInput)) {
activate(Rotation);
myAvatar.setHeadControllerFacingMovingAverage(myAvatar._headControllerFacing);
myAvatar.setHeadControllerFacingMovingAverage(myAvatar.getHeadControllerFacing());
}
if (myAvatar.getCenterOfGravityModelEnabled()) {
if (!isActive(Horizontal) && (shouldActivateHorizontalCG(myAvatar) || hasDriveInput)) {
activate(Horizontal);
if (myAvatar.getEnableStepResetRotation()) {
activate(Rotation);
myAvatar.setHeadControllerFacingMovingAverage(myAvatar.getHeadControllerFacing());
}
}
} else {
if (!isActive(Horizontal) && (shouldActivateHorizontal(myAvatar, desiredBodyMatrix, currentBodyMatrix) || hasDriveInput)) {
activate(Horizontal);
if (myAvatar.getEnableStepResetRotation()) {
activate(Rotation);
myAvatar.setHeadControllerFacingMovingAverage(myAvatar.getHeadControllerFacing());
}
}
}
if (!isActive(Vertical) && (shouldActivateVertical(myAvatar, desiredBodyMatrix, currentBodyMatrix) || hasDriveInput)) {
activate(Vertical);
}
} else {
if (!isActive(Rotation) && getForceActivateRotation()) {
activate(Rotation);
myAvatar.setHeadControllerFacingMovingAverage(myAvatar._headControllerFacing);
myAvatar.setHeadControllerFacingMovingAverage(myAvatar.getHeadControllerFacing());
setForceActivateRotation(false);
}
if (!isActive(Horizontal) && getForceActivateHorizontal()) {

View file

@ -201,6 +201,8 @@ class MyAvatar : public Avatar {
Q_PROPERTY(bool hasAudioEnabledFaceMovement READ getHasAudioEnabledFaceMovement WRITE setHasAudioEnabledFaceMovement)
Q_PROPERTY(float rotationRecenterFilterLength READ getRotationRecenterFilterLength WRITE setRotationRecenterFilterLength)
Q_PROPERTY(float rotationThreshold READ getRotationThreshold WRITE setRotationThreshold)
Q_PROPERTY(bool enableStepResetRotation READ getEnableStepResetRotation WRITE setEnableStepResetRotation)
Q_PROPERTY(bool enableDrawAverageFacing READ getEnableDrawAverageFacing WRITE setEnableDrawAverageFacing)
//TODO: make gravity feature work Q_PROPERTY(glm::vec3 gravity READ getGravity WRITE setGravity)
Q_PROPERTY(glm::vec3 leftHandPosition READ getLeftHandPosition)
@ -318,6 +320,9 @@ public:
// as it moves through the world.
void updateFromHMDSensorMatrix(const glm::mat4& hmdSensorMatrix);
// compute the hip to hand average azimuth.
glm::vec2 computeHandAzimuth() const;
// read the location of a hand controller and save the transform
void updateJointFromController(controller::Action poseKey, ThreadSafeValueCache<glm::mat4>& matrixCache);
@ -909,6 +914,10 @@ public:
virtual void rebuildCollisionShape() override;
const glm::vec2& getHipToHandController() const { return _hipToHandController; }
void setHipToHandController(glm::vec2 currentHipToHandController) { _hipToHandController = currentHipToHandController; }
const glm::vec2& getHeadControllerFacing() const { return _headControllerFacing; }
void setHeadControllerFacing(glm::vec2 currentHeadControllerFacing) { _headControllerFacing = currentHeadControllerFacing; }
const glm::vec2& getHeadControllerFacingMovingAverage() const { return _headControllerFacingMovingAverage; }
void setHeadControllerFacingMovingAverage(glm::vec2 currentHeadControllerFacing) { _headControllerFacingMovingAverage = currentHeadControllerFacing; }
float getCurrentStandingHeight() const { return _currentStandingHeight; }
@ -1046,6 +1055,8 @@ public:
// results are in sensor frame (-z forward)
glm::mat4 deriveBodyFromHMDSensor() const;
glm::mat4 getSpine2RotationRigSpace() const;
glm::vec3 computeCounterBalance();
// derive avatar body position and orientation from using the current HMD Sensor location in relation to the previous
@ -1516,6 +1527,10 @@ private:
float getRotationRecenterFilterLength() const { return _rotationRecenterFilterLength; }
void setRotationThreshold(float angleRadians);
float getRotationThreshold() const { return _rotationThreshold; }
void setEnableStepResetRotation(bool stepReset) { _stepResetRotationEnabled = stepReset; }
bool getEnableStepResetRotation() const { return _stepResetRotationEnabled; }
void setEnableDrawAverageFacing(bool drawAverage) { _drawAverageFacingEnabled = drawAverage; }
bool getEnableDrawAverageFacing() const { return _drawAverageFacingEnabled; }
bool isMyAvatar() const override { return true; }
virtual int parseDataFromBuffer(const QByteArray& buffer) override;
virtual glm::vec3 getSkeletonPosition() const override;
@ -1638,6 +1653,8 @@ private:
std::atomic<bool> _hasScriptedBlendShapes { false };
std::atomic<float> _rotationRecenterFilterLength { 4.0f };
std::atomic<float> _rotationThreshold { 0.5235f }; // 30 degrees in radians
std::atomic<bool> _stepResetRotationEnabled { true };
std::atomic<bool> _drawAverageFacingEnabled { false };
// working copy -- see AvatarData for thread-safe _sensorToWorldMatrixCache, used for outward facing access
glm::mat4 _sensorToWorldMatrix { glm::mat4() };
@ -1651,6 +1668,8 @@ private:
glm::vec2 _headControllerFacingMovingAverage { 0.0f, 0.0f }; // facing vector in xz plane (sensor space)
glm::quat _averageHeadRotation { 0.0f, 0.0f, 0.0f, 1.0f };
glm::vec2 _hipToHandController { 0.0f, -1.0f }; // spine2 facing vector in xz plane (avatar space)
float _currentStandingHeight { 0.0f };
bool _resetMode { true };
RingBufferHistory<int> _recentModeReadings;

View file

@ -238,6 +238,35 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
params.primaryControllerPoses[Rig::PrimaryControllerType_Hips] = sensorToRigPose * hips;
params.primaryControllerFlags[Rig::PrimaryControllerType_Hips] = (uint8_t)Rig::ControllerFlags::Enabled | (uint8_t)Rig::ControllerFlags::Estimated;
// set spine2 if we have hand controllers
if (myAvatar->getControllerPoseInAvatarFrame(controller::Action::RIGHT_HAND).isValid() &&
myAvatar->getControllerPoseInAvatarFrame(controller::Action::LEFT_HAND).isValid() &&
!(params.primaryControllerFlags[Rig::PrimaryControllerType_Spine2] & (uint8_t)Rig::ControllerFlags::Enabled)) {
AnimPose currentSpine2Pose;
AnimPose currentHeadPose;
AnimPose currentHipsPose;
bool spine2Exists = _rig.getAbsoluteJointPoseInRigFrame(_rig.indexOfJoint("Spine2"), currentSpine2Pose);
bool headExists = _rig.getAbsoluteJointPoseInRigFrame(_rig.indexOfJoint("Head"), currentHeadPose);
bool hipsExists = _rig.getAbsoluteJointPoseInRigFrame(_rig.indexOfJoint("Hips"), currentHipsPose);
if (spine2Exists && headExists && hipsExists) {
AnimPose rigSpaceYaw(myAvatar->getSpine2RotationRigSpace());
glm::vec3 u, v, w;
glm::vec3 fwd = rigSpaceYaw.rot() * glm::vec3(0.0f, 0.0f, 1.0f);
glm::vec3 up = currentHeadPose.trans() - currentHipsPose.trans();
if (glm::length(up) > 0.0f) {
up = glm::normalize(up);
} else {
up = glm::vec3(0.0f, 1.0f, 0.0f);
}
generateBasisVectors(up, fwd, u, v, w);
AnimPose newSpinePose(glm::mat4(glm::vec4(w, 0.0f), glm::vec4(u, 0.0f), glm::vec4(v, 0.0f), glm::vec4(glm::vec3(0.0f, 0.0f, 0.0f), 1.0f)));
currentSpine2Pose.rot() = newSpinePose.rot();
params.primaryControllerPoses[Rig::PrimaryControllerType_Spine2] = currentSpine2Pose;
params.primaryControllerFlags[Rig::PrimaryControllerType_Spine2] = (uint8_t)Rig::ControllerFlags::Enabled | (uint8_t)Rig::ControllerFlags::Estimated;
}
}
} else {
_prevHipsValid = false;
}

View file

@ -34,7 +34,7 @@ const float DEFAULT_HANDS_ANGULAR_VELOCITY_STEPPING_THRESHOLD = 3.3f;
const float DEFAULT_HEAD_VELOCITY_STEPPING_THRESHOLD = 0.18f;
const float DEFAULT_HEAD_PITCH_STEPPING_TOLERANCE = 7.0f;
const float DEFAULT_HEAD_ROLL_STEPPING_TOLERANCE = 7.0f;
const float DEFAULT_AVATAR_SPINE_STRETCH_LIMIT = 0.07f;
const float DEFAULT_AVATAR_SPINE_STRETCH_LIMIT = 0.04f;
const float DEFAULT_AVATAR_FORWARD_DAMPENING_FACTOR = 0.5f;
const float DEFAULT_AVATAR_LATERAL_DAMPENING_FACTOR = 2.0f;
const float DEFAULT_AVATAR_HIPS_MASS = 40.0f;