diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 40e350dcb7..2e27706820 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -45,7 +45,10 @@ const float PITCH_SPEED = 100.0f; // degrees/sec const float COLLISION_RADIUS_SCALAR = 1.2f; // pertains to avatar-to-avatar collisions const float COLLISION_RADIUS_SCALE = 0.125f; -const float DATA_SERVER_LOCATION_CHANGE_UPDATE_MSECS = 5 * 1000; +const float DATA_SERVER_LOCATION_CHANGE_UPDATE_MSECS = 5.0f * 1000.0f; + +float DEFAULT_MOTOR_TIMESCALE = 0.25f; +float DEFAULT_MAX_MOTOR_SPEED = 300.0f; // TODO: normalize this based on avatar height MyAvatar::MyAvatar() : Avatar(), @@ -60,7 +63,10 @@ MyAvatar::MyAvatar() : _thrust(0.0f), _isThrustOn(false), _thrustMultiplier(1.0f), - _motionBehaviors(0), + _motorVelocity(0.0f), + _motorTimescale(DEFAULT_MOTOR_TIMESCALE), + _maxMotorSpeed(DEFAULT_MAX_MOTOR_SPEED), + _motionBehaviors(AVATAR_MOTION_DEFAULTS), _lastBodyPenetration(0.0f), _lookAtTargetAvatar(), _shouldRender(true), @@ -134,17 +140,12 @@ void MyAvatar::update(float deltaTime) { void MyAvatar::simulate(float deltaTime) { - glm::quat orientation = getOrientation(); - if (_scale != _targetScale) { float scale = (1.0f - SMOOTHING_RATIO) * _scale + SMOOTHING_RATIO * _targetScale; setScale(scale); Application::getInstance()->getCamera()->setScale(scale); } - // Collect thrust forces from keyboard and devices - updateThrust(deltaTime); - // update the movement of the hand and process handshaking with other avatars... updateHandMovementAndTouching(deltaTime); @@ -156,105 +157,19 @@ void MyAvatar::simulate(float deltaTime) { _velocity += _scale * _gravity * (GRAVITY_EARTH * deltaTime); } - // add thrust to velocity - _velocity += _thrust * deltaTime; + updateOrientation(deltaTime); - // update body yaw by body yaw delta - orientation = orientation * glm::quat(glm::radians( - glm::vec3(_bodyPitchDelta, _bodyYawDelta, _bodyRollDelta) * deltaTime)); - // decay body rotation momentum + rampMotor(deltaTime); + applyMotor(deltaTime); - const float BODY_SPIN_FRICTION = 7.5f; - float bodySpinMomentum = 1.0f - BODY_SPIN_FRICTION * deltaTime; - if (bodySpinMomentum < 0.0f) { bodySpinMomentum = 0.0f; } - _bodyPitchDelta *= bodySpinMomentum; - _bodyYawDelta *= bodySpinMomentum; - _bodyRollDelta *= bodySpinMomentum; + // zero thrust so we don't pile up thrust from other sources + _thrust = glm::vec3(0.0f); - float MINIMUM_ROTATION_RATE = 2.0f; - if (fabs(_bodyYawDelta) < MINIMUM_ROTATION_RATE) { _bodyYawDelta = 0.0f; } - if (fabs(_bodyRollDelta) < MINIMUM_ROTATION_RATE) { _bodyRollDelta = 0.0f; } - if (fabs(_bodyPitchDelta) < MINIMUM_ROTATION_RATE) { _bodyPitchDelta = 0.0f; } - - const float MAX_STATIC_FRICTION_SPEED = 0.5f; - const float STATIC_FRICTION_STRENGTH = _scale * 20.0f; - applyStaticFriction(deltaTime, _velocity, MAX_STATIC_FRICTION_SPEED, STATIC_FRICTION_STRENGTH); - - // Damp avatar velocity - const float LINEAR_DAMPING_STRENGTH = 0.5f; - const float SPEED_BRAKE_POWER = _scale * 10.0f; - const float SQUARED_DAMPING_STRENGTH = 0.007f; - - const float SLOW_NEAR_RADIUS = 5.0f; - float linearDamping = LINEAR_DAMPING_STRENGTH; - const float NEAR_AVATAR_DAMPING_FACTOR = 50.0f; - if (_distanceToNearestAvatar < _scale * SLOW_NEAR_RADIUS) { - linearDamping *= 1.0f + NEAR_AVATAR_DAMPING_FACTOR * - ((SLOW_NEAR_RADIUS - _distanceToNearestAvatar) / SLOW_NEAR_RADIUS); - } - if (_speedBrakes) { - applyDamping(deltaTime, _velocity, linearDamping * SPEED_BRAKE_POWER, SQUARED_DAMPING_STRENGTH * SPEED_BRAKE_POWER); - } else { - applyDamping(deltaTime, _velocity, linearDamping, SQUARED_DAMPING_STRENGTH); - } - - if (OculusManager::isConnected()) { - // these angles will be in radians - float yaw, pitch, roll; - OculusManager::getEulerAngles(yaw, pitch, roll); - // ... so they need to be converted to degrees before we do math... - - // The neck is limited in how much it can yaw, so we check its relative - // yaw from the body and yaw the body if necessary. - yaw *= DEGREES_PER_RADIAN; - float bodyToHeadYaw = yaw - _oculusYawOffset; - const float MAX_NECK_YAW = 85.0f; // degrees - if ((fabs(bodyToHeadYaw) > 2.0f * MAX_NECK_YAW) && (yaw * _oculusYawOffset < 0.0f)) { - // We've wrapped around the range for yaw so adjust - // the measured yaw to be relative to _oculusYawOffset. - if (yaw > 0.0f) { - yaw -= 360.0f; - } else { - yaw += 360.0f; - } - bodyToHeadYaw = yaw - _oculusYawOffset; - } - - float delta = fabs(bodyToHeadYaw) - MAX_NECK_YAW; - if (delta > 0.0f) { - yaw = MAX_NECK_YAW; - if (bodyToHeadYaw < 0.0f) { - delta *= -1.0f; - bodyToHeadYaw = -MAX_NECK_YAW; - } else { - bodyToHeadYaw = MAX_NECK_YAW; - } - // constrain _oculusYawOffset to be within range [-180,180] - _oculusYawOffset = fmod((_oculusYawOffset + delta) + 180.0f, 360.0f) - 180.0f; - - // We must adjust the body orientation using a delta rotation (rather than - // doing yaw math) because the body's yaw ranges are not the same - // as what the Oculus API provides. - glm::vec3 UP_AXIS = glm::vec3(0.0f, 1.0f, 0.0f); - glm::quat bodyCorrection = glm::angleAxis(glm::radians(delta), UP_AXIS); - orientation = orientation * bodyCorrection; - } - Head* head = getHead(); - head->setBaseYaw(bodyToHeadYaw); - - head->setBasePitch(pitch * DEGREES_PER_RADIAN); - head->setBaseRoll(roll * DEGREES_PER_RADIAN); - } - - // update the euler angles - setOrientation(orientation); + updateChatCircle(deltaTime); // update moving flag based on speed const float MOVING_SPEED_THRESHOLD = 0.01f; - float speed = glm::length(_velocity); - _moving = speed > MOVING_SPEED_THRESHOLD; - - updateChatCircle(deltaTime); + _moving = glm::length(_velocity) > MOVING_SPEED_THRESHOLD; _position += _velocity * deltaTime; @@ -280,9 +195,6 @@ void MyAvatar::simulate(float deltaTime) { head->setScale(_scale); head->simulate(deltaTime, true); - // Zero thrust out now that we've added it to velocity in this frame - _thrust *= glm::vec3(0.0f); - // now that we're done stepping the avatar forward in time, compute new collisions if (_collisionGroups != 0) { Camera* myCamera = Application::getInstance()->getCamera(); @@ -643,6 +555,167 @@ bool MyAvatar::shouldRenderHead(const glm::vec3& cameraPosition, RenderMode rend (glm::length(cameraPosition - head->calculateAverageEyePosition()) > RENDER_HEAD_CUTOFF_DISTANCE * _scale); } +void MyAvatar::updateOrientation(float deltaTime) { + // Gather rotation information from keyboard + _bodyYawDelta -= _driveKeys[ROT_RIGHT] * YAW_SPEED * deltaTime; + _bodyYawDelta += _driveKeys[ROT_LEFT] * YAW_SPEED * deltaTime; + getHead()->setBasePitch(getHead()->getBasePitch() + (_driveKeys[ROT_UP] - _driveKeys[ROT_DOWN]) * PITCH_SPEED * deltaTime); + + // update body yaw by body yaw delta + glm::quat orientation = getOrientation() * glm::quat(glm::radians( + glm::vec3(_bodyPitchDelta, _bodyYawDelta, _bodyRollDelta) * deltaTime)); + + // decay body rotation momentum + const float BODY_SPIN_FRICTION = 7.5f; + float bodySpinMomentum = 1.0f - BODY_SPIN_FRICTION * deltaTime; + if (bodySpinMomentum < 0.0f) { bodySpinMomentum = 0.0f; } + _bodyPitchDelta *= bodySpinMomentum; + _bodyYawDelta *= bodySpinMomentum; + _bodyRollDelta *= bodySpinMomentum; + + float MINIMUM_ROTATION_RATE = 2.0f; + if (fabs(_bodyYawDelta) < MINIMUM_ROTATION_RATE) { _bodyYawDelta = 0.0f; } + if (fabs(_bodyRollDelta) < MINIMUM_ROTATION_RATE) { _bodyRollDelta = 0.0f; } + if (fabs(_bodyPitchDelta) < MINIMUM_ROTATION_RATE) { _bodyPitchDelta = 0.0f; } + + if (OculusManager::isConnected()) { + // these angles will be in radians + float yaw, pitch, roll; + OculusManager::getEulerAngles(yaw, pitch, roll); + // ... so they need to be converted to degrees before we do math... + + // The neck is limited in how much it can yaw, so we check its relative + // yaw from the body and yaw the body if necessary. + yaw *= DEGREES_PER_RADIAN; + float bodyToHeadYaw = yaw - _oculusYawOffset; + const float MAX_NECK_YAW = 85.0f; // degrees + if ((fabs(bodyToHeadYaw) > 2.0f * MAX_NECK_YAW) && (yaw * _oculusYawOffset < 0.0f)) { + // We've wrapped around the range for yaw so adjust + // the measured yaw to be relative to _oculusYawOffset. + if (yaw > 0.0f) { + yaw -= 360.0f; + } else { + yaw += 360.0f; + } + bodyToHeadYaw = yaw - _oculusYawOffset; + } + + float delta = fabs(bodyToHeadYaw) - MAX_NECK_YAW; + if (delta > 0.0f) { + yaw = MAX_NECK_YAW; + if (bodyToHeadYaw < 0.0f) { + delta *= -1.0f; + bodyToHeadYaw = -MAX_NECK_YAW; + } else { + bodyToHeadYaw = MAX_NECK_YAW; + } + // constrain _oculusYawOffset to be within range [-180,180] + _oculusYawOffset = fmod((_oculusYawOffset + delta) + 180.0f, 360.0f) - 180.0f; + + // We must adjust the body orientation using a delta rotation (rather than + // doing yaw math) because the body's yaw ranges are not the same + // as what the Oculus API provides. + glm::vec3 UP_AXIS = glm::vec3(0.0f, 1.0f, 0.0f); + glm::quat bodyCorrection = glm::angleAxis(glm::radians(delta), UP_AXIS); + orientation = orientation * bodyCorrection; + } + Head* head = getHead(); + head->setBaseYaw(bodyToHeadYaw); + + head->setBasePitch(pitch * DEGREES_PER_RADIAN); + head->setBaseRoll(roll * DEGREES_PER_RADIAN); + } + + // update the euler angles + setOrientation(orientation); +} + +void MyAvatar::rampMotor(float deltaTime) { + // Increase motor velocity until its length is equal to _maxMotorSpeed. + if (!(_motionBehaviors & AVATAR_MOTION_MOTOR_RAMP_AND_BRAKES_ENABLED)) { + // nothing to do + return; + } + + glm::vec3 localVelocity = _velocity; + if (_motionBehaviors & AVATAR_MOTION_MOTOR_USE_LOCAL_FRAME) { + glm::quat orientation = getHead()->getCameraOrientation(); + localVelocity = glm::inverse(orientation) * _velocity; + } + + // Compute keyboard input + glm::vec3 front = (_driveKeys[FWD] - _driveKeys[BACK]) * IDENTITY_FRONT; + glm::vec3 right = (_driveKeys[RIGHT] - _driveKeys[LEFT]) * IDENTITY_RIGHT; + glm::vec3 up = (_driveKeys[UP] - _driveKeys[DOWN]) * IDENTITY_UP; + + glm::vec3 direction = front + right + up; + float directionLength = glm::length(direction); + + // Compute motor magnitude + if (directionLength > EPSILON) { + direction /= directionLength; + const float MIN_WALKING_SPEED = 2.0f; + float motorLength = glm::length(_motorVelocity); + if (motorLength < MIN_WALKING_SPEED) { + _motorVelocity = MIN_WALKING_SPEED * direction; + } else { + float MOTOR_LENGTH_TIMESCALE = 1.5f; + float tau = glm::clamp(deltaTime / MOTOR_LENGTH_TIMESCALE, 0.0f, 1.0f); + float INCREASE_FACTOR = 2.0f; + //_motorVelocity *= 1.0f + tau * INCREASE_FACTOR; + motorLength *= 1.0f + tau * INCREASE_FACTOR; + if (motorLength > _maxMotorSpeed) { + motorLength = _maxMotorSpeed; + } + _motorVelocity = motorLength * direction; + } + _speedBrakes = false; + } else { + // apply brakes + _speedBrakes = true; + _motorVelocity = - localVelocity; + } +} + +void MyAvatar::applyMotor(float deltaTime) { + if (!( _motionBehaviors & AVATAR_MOTION_MOTOR_ENABLED)) { + // nothing to do --> early exit + return; + } + glm::vec3 targetVelocity = _motorVelocity; + if (_motionBehaviors & AVATAR_MOTION_MOTOR_USE_LOCAL_FRAME) { + // rotate _motorVelocity into world frame + glm::quat rotation = getOrientation(); + targetVelocity = rotation * _motorVelocity; + } + + glm::vec3 targetDirection(0.f); + if (glm::length2(targetVelocity) > EPSILON) { + targetDirection = glm::normalize(targetVelocity); + } + glm::vec3 deltaVelocity = targetVelocity - _velocity; + + if (_motionBehaviors & AVATAR_MOTION_MOTOR_COLLISION_SURFACE_ONLY && glm::length2(_gravity) > EPSILON) { + // For now we subtract the component parallel to gravity but what we need to do is: + // TODO: subtract the component perp to the local surface normal (motor only pushes in surface plane). + glm::vec3 gravityDirection = glm::normalize(_gravity); + glm::vec3 parallelDelta = glm::dot(deltaVelocity, gravityDirection) * gravityDirection; + if (glm::dot(targetVelocity, _velocity) > 0.0f) { + // remove parallel part from deltaVelocity + deltaVelocity -= parallelDelta; + } + } + + float MIN_MOTOR_TIMESCALE = 0.25f; + float timescale = _speedBrakes ? MIN_MOTOR_TIMESCALE : _motorTimescale; + + // simple critical damping + float tau = glm::clamp(deltaTime / timescale, 0.0f, 1.0f); + _velocity += tau * deltaVelocity; +} + +/* Keep this code for the short term as reference in case we need to further tune + *the new velocity model to achieve similar movement response. void MyAvatar::updateThrust(float deltaTime) { // // Gather thrust information from keyboard and sensors to apply to avatar motion @@ -688,10 +761,6 @@ void MyAvatar::updateThrust(float deltaTime) { } _lastBodyPenetration = glm::vec3(0.0f); - _bodyYawDelta -= _driveKeys[ROT_RIGHT] * YAW_SPEED * deltaTime; - _bodyYawDelta += _driveKeys[ROT_LEFT] * YAW_SPEED * deltaTime; - getHead()->setBasePitch(getHead()->getBasePitch() + (_driveKeys[ROT_UP] - _driveKeys[ROT_DOWN]) * PITCH_SPEED * deltaTime); - // If thrust keys are being held down, slowly increase thrust to allow reaching great speeds if (_driveKeys[FWD] || _driveKeys[BACK] || _driveKeys[RIGHT] || _driveKeys[LEFT] || _driveKeys[UP] || _driveKeys[DOWN]) { const float THRUST_INCREASE_RATE = 1.05f; @@ -722,8 +791,34 @@ void MyAvatar::updateThrust(float deltaTime) { if (_isThrustOn || (_speedBrakes && (glm::length(_velocity) < MIN_SPEED_BRAKE_VELOCITY))) { _speedBrakes = false; } + _velocity += _thrust * deltaTime; + // Zero thrust out now that we've added it to velocity in this frame + _thrust = glm::vec3(0.0f); + + // apply linear damping + const float MAX_STATIC_FRICTION_SPEED = 0.5f; + const float STATIC_FRICTION_STRENGTH = _scale * 20.0f; + applyStaticFriction(deltaTime, _velocity, MAX_STATIC_FRICTION_SPEED, STATIC_FRICTION_STRENGTH); + + const float LINEAR_DAMPING_STRENGTH = 0.5f; + const float SPEED_BRAKE_POWER = _scale * 10.0f; + const float SQUARED_DAMPING_STRENGTH = 0.007f; + + const float SLOW_NEAR_RADIUS = 5.0f; + float linearDamping = LINEAR_DAMPING_STRENGTH; + const float NEAR_AVATAR_DAMPING_FACTOR = 50.0f; + if (_distanceToNearestAvatar < _scale * SLOW_NEAR_RADIUS) { + linearDamping *= 1.0f + NEAR_AVATAR_DAMPING_FACTOR * + ((SLOW_NEAR_RADIUS - _distanceToNearestAvatar) / SLOW_NEAR_RADIUS); + } + if (_speedBrakes) { + applyDamping(deltaTime, _velocity, linearDamping * SPEED_BRAKE_POWER, SQUARED_DAMPING_STRENGTH * SPEED_BRAKE_POWER); + } else { + applyDamping(deltaTime, _velocity, linearDamping, SQUARED_DAMPING_STRENGTH); + } } +*/ void MyAvatar::updateHandMovementAndTouching(float deltaTime) { glm::quat orientation = getOrientation(); @@ -1151,8 +1246,7 @@ void MyAvatar::goToLocationFromResponse(const QJsonObject& jsonObject) { } } -void MyAvatar::updateMotionBehaviors() { - _motionBehaviors = 0; +void MyAvatar::updateMotionBehaviorsFromMenu() { if (Menu::getInstance()->isOptionChecked(MenuOption::ObeyEnvironmentalGravity)) { _motionBehaviors |= AVATAR_MOTION_OBEY_ENVIRONMENTAL_GRAVITY; // Environmental and Local gravities are incompatible. Environmental setting trumps local. @@ -1172,8 +1266,14 @@ void MyAvatar::setCollisionGroups(quint32 collisionGroups) { menu->setIsOptionChecked(MenuOption::CollideWithParticles, (bool)(_collisionGroups & COLLISION_GROUP_PARTICLES)); } -void MyAvatar::setMotionBehaviors(quint32 flags) { - _motionBehaviors = flags; +void MyAvatar::setMotionBehaviorsByScript(quint32 flags) { + // start with the defaults + _motionBehaviors = AVATAR_MOTION_DEFAULTS; + + // add the set scriptable bits + _motionBehaviors += flags & AVATAR_MOTION_SCRIPTABLE_BITS; + + // reconcile incompatible settings from menu (if any) Menu* menu = Menu::getInstance(); menu->setIsOptionChecked(MenuOption::ObeyEnvironmentalGravity, (bool)(_motionBehaviors & AVATAR_MOTION_OBEY_ENVIRONMENTAL_GRAVITY)); // Environmental and Local gravities are incompatible. Environmental setting trumps local. diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index a5312b0016..bfafffdc02 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -28,7 +28,7 @@ enum AvatarHandState class MyAvatar : public Avatar { Q_OBJECT Q_PROPERTY(bool shouldRenderLocally READ getShouldRenderLocally WRITE setShouldRenderLocally) - Q_PROPERTY(quint32 motionBehaviors READ getMotionBehaviors WRITE setMotionBehaviors) + Q_PROPERTY(quint32 motionBehaviors READ getMotionBehaviorsForScript WRITE setMotionBehaviorsByScript) Q_PROPERTY(glm::vec3 gravity READ getGravity WRITE setLocalGravity) public: @@ -90,8 +90,9 @@ public: virtual void setSkeletonModelURL(const QUrl& skeletonModelURL); virtual void setCollisionGroups(quint32 collisionGroups); - void setMotionBehaviors(quint32 flags); - quint32 getMotionBehaviors() const { return _motionBehaviors; } + + void setMotionBehaviorsByScript(quint32 flags); + quint32 getMotionBehaviorsForScript() const { return _motionBehaviors & AVATAR_MOTION_SCRIPTABLE_BITS; } void applyCollision(const glm::vec3& contactPoint, const glm::vec3& penetration); @@ -109,7 +110,7 @@ public slots: glm::vec3 getThrust() { return _thrust; }; void setThrust(glm::vec3 newThrust) { _thrust = newThrust; } - void updateMotionBehaviors(); + void updateMotionBehaviorsFromMenu(); signals: void transformChanged(); @@ -124,13 +125,17 @@ private: glm::vec3 _environmentGravity; float _distanceToNearestAvatar; // How close is the nearest avatar? - // motion stuff + // old motion stuff glm::vec3 _lastCollisionPosition; bool _speedBrakes; - glm::vec3 _thrust; // final acceleration for the current frame + glm::vec3 _thrust; // final acceleration from outside sources for the current frame bool _isThrustOn; float _thrustMultiplier; + // new motion stuff + glm::vec3 _motorVelocity; // intended velocity of avatar motion + float _motorTimescale; // timescale for avatar motion to kick in + float _maxMotorSpeed; quint32 _motionBehaviors; glm::vec3 _lastBodyPenetration; @@ -141,7 +146,9 @@ private: float _oculusYawOffset; // private methods - void updateThrust(float deltaTime); + void updateOrientation(float deltaTime); + void rampMotor(float deltaTime); + void applyMotor(float deltaTime); void updateHandMovementAndTouching(float deltaTime); void updateCollisionWithAvatars(float deltaTime); void updateCollisionWithEnvironment(float deltaTime, float radius); diff --git a/libraries/avatars/src/AvatarData.h b/libraries/avatars/src/AvatarData.h index be47aed1ba..3c1aa9963d 100755 --- a/libraries/avatars/src/AvatarData.h +++ b/libraries/avatars/src/AvatarData.h @@ -51,8 +51,24 @@ typedef unsigned long long quint64; #include "HandData.h" // avatar motion behaviors -const quint32 AVATAR_MOTION_OBEY_ENVIRONMENTAL_GRAVITY = 1U << 0; -const quint32 AVATAR_MOTION_OBEY_LOCAL_GRAVITY = 1U << 1; +const quint32 AVATAR_MOTION_MOTOR_ENABLED = 1U << 0; +const quint32 AVATAR_MOTION_MOTOR_RAMP_AND_BRAKES_ENABLED = 1U << 1; +const quint32 AVATAR_MOTION_MOTOR_USE_LOCAL_FRAME = 1U << 2; +const quint32 AVATAR_MOTION_MOTOR_COLLISION_SURFACE_ONLY = 1U << 3; + +const quint32 AVATAR_MOTION_OBEY_ENVIRONMENTAL_GRAVITY = 1U << 4; +const quint32 AVATAR_MOTION_OBEY_LOCAL_GRAVITY = 1U << 5; + +const quint32 AVATAR_MOTION_DEFAULTS = + AVATAR_MOTION_MOTOR_ENABLED | + AVATAR_MOTION_MOTOR_RAMP_AND_BRAKES_ENABLED | + AVATAR_MOTION_MOTOR_USE_LOCAL_FRAME; + +// these bits will be expanded as features are exposed +const quint32 AVATAR_MOTION_SCRIPTABLE_BITS = + AVATAR_MOTION_OBEY_ENVIRONMENTAL_GRAVITY | + AVATAR_MOTION_OBEY_LOCAL_GRAVITY; + // First bitset const int KEY_STATE_START_BIT = 0; // 1st and 2nd bits