From e4cb6e2b87a639bcdae4662bf406002222aeab55 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 17 May 2016 13:57:04 -0700 Subject: [PATCH] order-independent CharacterController motors --- interface/src/avatar/MyAvatar.cpp | 263 ++++++++---------- interface/src/avatar/MyAvatar.h | 6 +- libraries/physics/src/CharacterController.cpp | 188 +++++++++---- libraries/physics/src/CharacterController.h | 26 +- 4 files changed, 284 insertions(+), 199 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index ddc0407f14..24c41c9ea5 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -59,9 +59,10 @@ using namespace std; const glm::vec3 DEFAULT_UP_DIRECTION(0.0f, 1.0f, 0.0f); const float DEFAULT_REAL_WORLD_FIELD_OF_VIEW_DEGREES = 30.0f; -const float MAX_WALKING_SPEED = 2.5f; // human walking speed +const float MAX_WALKING_SPEED = 2.6f; // human walking speed const float MAX_BOOST_SPEED = 0.5f * MAX_WALKING_SPEED; // keyboard motor gets additive boost below this speed -const float MIN_AVATAR_SPEED = 0.05f; // speed is set to zero below this +const float MIN_AVATAR_SPEED = 0.05f; +const float MIN_AVATAR_SPEED_SQUARED = MIN_AVATAR_SPEED * MIN_AVATAR_SPEED; // speed is set to zero below this const float YAW_SPEED_DEFAULT = 120.0f; // degrees/sec const float PITCH_SPEED_DEFAULT = 90.0f; // degrees/sec @@ -70,7 +71,6 @@ const float PITCH_SPEED_DEFAULT = 90.0f; // degrees/sec // to properly follow avatar size. float MAX_AVATAR_SPEED = 30.0f; float MAX_KEYBOARD_MOTOR_SPEED = MAX_AVATAR_SPEED; -float DEFAULT_KEYBOARD_MOTOR_TIMESCALE = 0.25f; float MIN_SCRIPTED_MOTOR_TIMESCALE = 0.005f; float DEFAULT_SCRIPTED_MOTOR_TIMESCALE = 1.0e6f; const int SCRIPTED_MOTOR_CAMERA_FRAME = 0; @@ -86,13 +86,13 @@ MyAvatar::MyAvatar(RigPointer rig) : Avatar(rig), _wasPushing(false), _isPushing(false), + _isBeingPushed(false), _isBraking(false), _boomLength(ZOOM_DEFAULT), _yawSpeed(YAW_SPEED_DEFAULT), _pitchSpeed(PITCH_SPEED_DEFAULT), _thrust(0.0f), _keyboardMotorVelocity(0.0f), - _keyboardMotorTimescale(DEFAULT_KEYBOARD_MOTOR_TIMESCALE), _scriptedMotorVelocity(0.0f), _scriptedMotorTimescale(DEFAULT_SCRIPTED_MOTOR_TIMESCALE), _scriptedMotorFrame(SCRIPTED_MOTOR_CAMERA_FRAME), @@ -246,7 +246,6 @@ void MyAvatar::reset(bool andRecenter) { _follow.deactivate(); _skeletonModel->reset(); getHead()->reset(); - _targetVelocity = glm::vec3(0.0f); setThrust(glm::vec3(0.0f)); if (andRecenter) { @@ -1166,8 +1165,47 @@ controller::Pose MyAvatar::getRightHandControllerPoseInAvatarFrame() const { return getRightHandControllerPoseInWorldFrame().transform(invAvatarMatrix); } +void MyAvatar::updateMotors() { + _characterController.clearMotors(); + glm::quat motorRotation; + if (_motionBehaviors & AVATAR_MOTION_KEYBOARD_MOTOR_ENABLED) { + if (_characterController.getState() == CharacterController::State::Hover) { + motorRotation = getHead()->getCameraOrientation(); + } else { + motorRotation = getOrientation(); + } + //const float DEFAULT_MOTOR_TIMESCALE = 0.7f; + const float DEFAULT_MOTOR_TIMESCALE = 0.2f; + const float INVALID_MOTOR_TIMESCALE = 1.0e6f; + if (_isPushing || _isBraking || !_isBeingPushed) { + _characterController.addMotor(_keyboardMotorVelocity, motorRotation, DEFAULT_MOTOR_TIMESCALE, INVALID_MOTOR_TIMESCALE); + } else { + // _isBeingPushed must be true --> disable keyboard motor by giving it a long timescale, + // otherwise it's attempt to "stand in in place" could defeat scripted motor/thrusts + _characterController.addMotor(_keyboardMotorVelocity, motorRotation, INVALID_MOTOR_TIMESCALE); + } + } + if (_motionBehaviors & AVATAR_MOTION_SCRIPTED_MOTOR_ENABLED) { + if (_scriptedMotorFrame == SCRIPTED_MOTOR_CAMERA_FRAME) { + motorRotation = getHead()->getCameraOrientation() * glm::angleAxis(PI, Vectors::UNIT_Y); + } else if (_scriptedMotorFrame == SCRIPTED_MOTOR_AVATAR_FRAME) { + motorRotation = getOrientation() * glm::angleAxis(PI, Vectors::UNIT_Y); + } else { + // world-frame + motorRotation = glm::quat(); + } + _characterController.addMotor(_scriptedMotorVelocity, motorRotation, _scriptedMotorTimescale); + } + + // legacy support for 'MyAvatar::applyThrust()', which has always been implemented as a + // short-lived linearAcceleration + _characterController.setLinearAcceleration(_thrust); + _thrust = Vectors::ZERO; +} + void MyAvatar::prepareForPhysicsSimulation() { relayDriveKeysToCharacterController(); + updateMotors(); bool success; glm::vec3 parentVelocity = getParentVelocity(success); @@ -1177,7 +1215,6 @@ void MyAvatar::prepareForPhysicsSimulation() { } _characterController.setParentVelocity(parentVelocity); - _characterController.setTargetVelocity(getTargetVelocity()); _characterController.setPositionAndOrientation(getPosition(), getOrientation()); if (qApp->isHMDMode()) { bool hasDriveInput = fabsf(_driveKeys[TRANSLATE_X]) > 0.0f || fabsf(_driveKeys[TRANSLATE_Z]) > 0.0f; @@ -1190,11 +1227,17 @@ void MyAvatar::prepareForPhysicsSimulation() { void MyAvatar::harvestResultsFromPhysicsSimulation(float deltaTime) { glm::vec3 position = getPosition(); glm::quat orientation = getOrientation(); - _characterController.getPositionAndOrientation(position, orientation); + if (_characterController.isEnabled()) { + _characterController.getPositionAndOrientation(position, orientation); + } nextAttitude(position, orientation); _bodySensorMatrix = _follow.postPhysicsUpdate(*this, _bodySensorMatrix); - setVelocity(_characterController.getLinearVelocity() + _characterController.getFollowVelocity()); + if (_characterController.isEnabled()) { + setVelocity(_characterController.getLinearVelocity() + _characterController.getFollowVelocity()); + } else { + setVelocity(getVelocity() + _characterController.getFollowVelocity()); + } } QString MyAvatar::getScriptedMotorFrame() const { @@ -1234,7 +1277,7 @@ void MyAvatar::setScriptedMotorFrame(QString frame) { } void MyAvatar::clearScriptableSettings() { - _scriptedMotorVelocity = glm::vec3(0.0f); + _scriptedMotorVelocity = Vectors::ZERO; _scriptedMotorTimescale = DEFAULT_SCRIPTED_MOTOR_TIMESCALE; } @@ -1499,163 +1542,97 @@ void MyAvatar::updateOrientation(float deltaTime) { } } -glm::vec3 MyAvatar::applyKeyboardMotor(float deltaTime, const glm::vec3& localVelocity, bool isHovering) { - if (! (_motionBehaviors & AVATAR_MOTION_KEYBOARD_MOTOR_ENABLED)) { - return localVelocity; - } - // compute motor efficiency - // The timescale of the motor is the approximate time it takes for the motor to - // accomplish its intended localVelocity. A short timescale makes the motor strong, - // and a long timescale makes it weak. The value of timescale to use depends - // on what the motor is doing: - // - // (1) braking --> short timescale (aggressive motor assertion) - // (2) pushing --> medium timescale (mild motor assertion) - // (3) inactive --> long timescale (gentle friction for low speeds) - const float MIN_KEYBOARD_MOTOR_TIMESCALE = 0.125f; - const float MAX_KEYBOARD_MOTOR_TIMESCALE = 0.4f; - const float MIN_KEYBOARD_BRAKE_SPEED = 0.3f; - float timescale = MAX_KEYBOARD_MOTOR_TIMESCALE; - bool isThrust = (glm::length2(_thrust) > EPSILON); - if (_isPushing || isThrust || - (_scriptedMotorTimescale < MAX_KEYBOARD_MOTOR_TIMESCALE && - (_motionBehaviors & AVATAR_MOTION_SCRIPTED_MOTOR_ENABLED))) { - // we don't want to brake if something is pushing the avatar around - timescale = _keyboardMotorTimescale; +void MyAvatar::updateKeyboardMotor(float deltaTime) { + bool thrustIsPushing = (glm::length2(_thrust) > EPSILON); + bool scriptedMotorIsPushing = (_motionBehaviors & AVATAR_MOTION_SCRIPTED_MOTOR_ENABLED) + && _scriptedMotorTimescale < MAX_CHARACTER_MOTOR_TIMESCALE; + _isBeingPushed = thrustIsPushing || scriptedMotorIsPushing; + if (_isPushing || _isBeingPushed) { + // we don't want the keyboard to brake if a script is pushing the avatar around + // (we assume the avatar is driving itself via script) _isBraking = false; } else { - float speed = glm::length(localVelocity); + float speed = glm::length(_keyboardMotorVelocity); + const float MIN_KEYBOARD_BRAKE_SPEED = 0.1f; _isBraking = _wasPushing || (_isBraking && speed > MIN_KEYBOARD_BRAKE_SPEED); - if (_isBraking) { - timescale = MIN_KEYBOARD_MOTOR_TIMESCALE; - } } - _wasPushing = _isPushing || isThrust; - _isPushing = false; - float motorEfficiency = glm::clamp(deltaTime / timescale, 0.0f, 1.0f); - glm::vec3 newLocalVelocity = localVelocity; + // compute keyboard input + glm::vec3 front = (_driveKeys[TRANSLATE_Z]) * IDENTITY_FRONT; + glm::vec3 right = (_driveKeys[TRANSLATE_X]) * IDENTITY_RIGHT; - // FIXME how do I implement step translation as well? - - - float keyboardInput = fabsf(_driveKeys[TRANSLATE_Z]) + fabsf(_driveKeys[TRANSLATE_X]) + fabsf(_driveKeys[TRANSLATE_Y]); - if (keyboardInput) { - // Compute keyboard input - glm::vec3 front = (_driveKeys[TRANSLATE_Z]) * IDENTITY_FRONT; - glm::vec3 right = (_driveKeys[TRANSLATE_X]) * IDENTITY_RIGHT; + glm::vec3 direction = front + right; + CharacterController::State state = _characterController.getState(); + if (state == CharacterController::State::Hover) { + // we're flying --> support vertical motion glm::vec3 up = (_driveKeys[TRANSLATE_Y]) * IDENTITY_UP; + direction += up; + } - glm::vec3 direction = front + right + up; - float directionLength = glm::length(direction); + _wasPushing = _isPushing; + float directionLength = glm::length(direction); + _isPushing = directionLength > EPSILON; - //qCDebug(interfaceapp, "direction = (%.5f, %.5f, %.5f)", direction.x, direction.y, direction.z); - - // Compute motor magnitude - if (directionLength > EPSILON) { - direction /= directionLength; - - if (isHovering) { - // we're flying --> complex acceleration curve with high max speed - float motorSpeed = glm::length(_keyboardMotorVelocity); - float finalMaxMotorSpeed = getUniformScale() * MAX_KEYBOARD_MOTOR_SPEED; - float speedGrowthTimescale = 2.0f; - float speedIncreaseFactor = 1.8f; - motorSpeed *= 1.0f + glm::clamp(deltaTime / speedGrowthTimescale , 0.0f, 1.0f) * speedIncreaseFactor; - const float maxBoostSpeed = getUniformScale() * MAX_BOOST_SPEED; - - if (motorSpeed < maxBoostSpeed) { - // an active keyboard motor should never be slower than this - float boostCoefficient = (maxBoostSpeed - motorSpeed) / maxBoostSpeed; - motorSpeed += MIN_AVATAR_SPEED * boostCoefficient; - motorEfficiency += (1.0f - motorEfficiency) * boostCoefficient; - } else if (motorSpeed > finalMaxMotorSpeed) { - motorSpeed = finalMaxMotorSpeed; - } - _keyboardMotorVelocity = motorSpeed * direction; - - } else { - // we're using a floor --> simple exponential decay toward target walk speed - const float WALK_ACCELERATION_TIMESCALE = 0.7f; // seconds to decrease delta to 1/e - _keyboardMotorVelocity = MAX_WALKING_SPEED * direction; - motorEfficiency = glm::clamp(deltaTime / WALK_ACCELERATION_TIMESCALE, 0.0f, 1.0f); - } - _isPushing = true; - } - newLocalVelocity = localVelocity + motorEfficiency * (_keyboardMotorVelocity - localVelocity); + // normalize direction + if (_isPushing) { + direction /= directionLength; } else { - _keyboardMotorVelocity = glm::vec3(0.0f); - newLocalVelocity = (1.0f - motorEfficiency) * localVelocity; - if (!isHovering && !_wasPushing) { - float speed = glm::length(newLocalVelocity); - if (speed > MIN_AVATAR_SPEED) { - // add small constant friction to help avatar drift to a stop sooner at low speeds - const float CONSTANT_FRICTION_DECELERATION = MIN_AVATAR_SPEED / 0.20f; - newLocalVelocity *= (speed - timescale * CONSTANT_FRICTION_DECELERATION) / speed; + direction = Vectors::ZERO; + } + + if (state == CharacterController::State::Hover) { + // we're flying --> complex acceleration curve that builds on top of current motor speed and caps at some max speed + float motorSpeed = glm::length(_keyboardMotorVelocity); + float finalMaxMotorSpeed = getUniformScale() * MAX_KEYBOARD_MOTOR_SPEED; + float speedGrowthTimescale = 2.0f; + float speedIncreaseFactor = 1.8f; + motorSpeed *= 1.0f + glm::clamp(deltaTime / speedGrowthTimescale , 0.0f, 1.0f) * speedIncreaseFactor; + const float maxBoostSpeed = getUniformScale() * MAX_BOOST_SPEED; + + if (_isPushing) { + if (motorSpeed < maxBoostSpeed) { + // an active keyboard motor should never be slower than this + float boostCoefficient = (maxBoostSpeed - motorSpeed) / maxBoostSpeed; + motorSpeed += MIN_AVATAR_SPEED * boostCoefficient; + } else if (motorSpeed > finalMaxMotorSpeed) { + motorSpeed = finalMaxMotorSpeed; } } + _keyboardMotorVelocity = motorSpeed * direction; + } else { + // we're interacting with a floor --> simple horizontal speed and exponential decay + _keyboardMotorVelocity = MAX_WALKING_SPEED * direction; } float boomChange = _driveKeys[ZOOM]; _boomLength += 2.0f * _boomLength * boomChange + boomChange * boomChange; _boomLength = glm::clamp(_boomLength, ZOOM_MIN, ZOOM_MAX); - - return newLocalVelocity; -} - -glm::vec3 MyAvatar::applyScriptedMotor(float deltaTime, const glm::vec3& localVelocity) { - // NOTE: localVelocity is in camera-frame because that's the frame of the default avatar motor - if (! (_motionBehaviors & AVATAR_MOTION_SCRIPTED_MOTOR_ENABLED)) { - return localVelocity; - } - glm::vec3 deltaVelocity(0.0f); - if (_scriptedMotorFrame == SCRIPTED_MOTOR_CAMERA_FRAME) { - // camera frame - deltaVelocity = _scriptedMotorVelocity - localVelocity; - } else if (_scriptedMotorFrame == SCRIPTED_MOTOR_AVATAR_FRAME) { - // avatar frame - glm::quat rotation = glm::inverse(getHead()->getCameraOrientation()) * getOrientation(); - deltaVelocity = rotation * _scriptedMotorVelocity - localVelocity; - } else { - // world-frame - glm::quat rotation = glm::inverse(getHead()->getCameraOrientation()); - deltaVelocity = rotation * _scriptedMotorVelocity - localVelocity; - } - float motorEfficiency = glm::clamp(deltaTime / _scriptedMotorTimescale, 0.0f, 1.0f); - return localVelocity + motorEfficiency * deltaVelocity; } void MyAvatar::updatePosition(float deltaTime) { - // rotate velocity into camera frame - glm::quat rotation = getHead()->getCameraOrientation(); - glm::vec3 localVelocity = glm::inverse(rotation) * _targetVelocity; - bool isHovering = _characterController.getState() == CharacterController::State::Hover; - glm::vec3 newLocalVelocity = applyKeyboardMotor(deltaTime, localVelocity, isHovering); - newLocalVelocity = applyScriptedMotor(deltaTime, newLocalVelocity); - - // rotate back into world-frame - _targetVelocity = rotation * newLocalVelocity; - - _targetVelocity += _thrust * deltaTime; - _thrust = glm::vec3(0.0f); - - // cap avatar speed - float speed = glm::length(_targetVelocity); - if (speed > MAX_AVATAR_SPEED) { - _targetVelocity *= MAX_AVATAR_SPEED / speed; - speed = MAX_AVATAR_SPEED; + if (_motionBehaviors & AVATAR_MOTION_KEYBOARD_MOTOR_ENABLED) { + updateKeyboardMotor(deltaTime); } - if (speed > MIN_AVATAR_SPEED && !_characterController.isEnabled()) { - // update position ourselves - applyPositionDelta(deltaTime * _targetVelocity); + vec3 velocity = getVelocity(); + const float MOVING_SPEED_THRESHOLD_SQUARED = 0.0001f; // 0.01 m/s + if (!_characterController.isEnabled()) { + // _characterController is not in physics simulation but it can still compute its target velocity + updateMotors(); + _characterController.computeNewVelocity(deltaTime, velocity); + + float speed2 = glm::length2(velocity); + if (speed2 > MIN_AVATAR_SPEED_SQUARED) { + // update position ourselves + applyPositionDelta(deltaTime * velocity); + } measureMotionDerivatives(deltaTime); - } // else physics will move avatar later - - // update _moving flag based on speed - const float MOVING_SPEED_THRESHOLD = 0.01f; - _moving = speed > MOVING_SPEED_THRESHOLD; - + _moving = speed2 > MOVING_SPEED_THRESHOLD_SQUARED; + } else { + // physics physics simulation updated elsewhere + float speed2 = glm::length2(velocity); + _moving = speed2 > MOVING_SPEED_THRESHOLD_SQUARED; + } // capture the head rotation, in sensor space, when the user first indicates they would like to move/fly. if (!_hoverReferenceCameraFacingIsCaptured && (fabs(_driveKeys[TRANSLATE_Z]) > 0.1f || fabs(_driveKeys[TRANSLATE_X]) > 0.1f)) { diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 66e6af340e..68f5becdf7 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -218,6 +218,7 @@ public: MyCharacterController* getCharacterController() { return &_characterController; } const MyCharacterController* getCharacterController() const { return &_characterController; } + void updateMotors(); void prepareForPhysicsSimulation(); void harvestResultsFromPhysicsSimulation(float deltaTime); @@ -350,6 +351,7 @@ private: float _driveKeys[MAX_DRIVE_KEYS]; bool _wasPushing; bool _isPushing; + bool _isBeingPushed; bool _isBraking; float _boomLength; @@ -359,7 +361,6 @@ private: glm::vec3 _thrust; // impulse accumulator for outside sources glm::vec3 _keyboardMotorVelocity; // target local-frame velocity of avatar (keyboard) - float _keyboardMotorTimescale; // timescale for avatar to achieve its target velocity glm::vec3 _scriptedMotorVelocity; // target local-frame velocity of avatar (script) float _scriptedMotorTimescale; // timescale for avatar to achieve its target velocity int _scriptedMotorFrame; @@ -384,8 +385,7 @@ private: // private methods void updateOrientation(float deltaTime); - glm::vec3 applyKeyboardMotor(float deltaTime, const glm::vec3& velocity, bool isHovering); - glm::vec3 applyScriptedMotor(float deltaTime, const glm::vec3& velocity); + void updateKeyboardMotor(float deltaTime); void updatePosition(float deltaTime); void updateCollisionSound(const glm::vec3& penetration, float deltaTime, float frequency); void initHeadBones(); diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 39710b63ab..7ba266d279 100644 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -46,6 +46,20 @@ protected: btRigidBody* _me; }; +CharacterController::CharacterMotor::CharacterMotor(const glm::vec3& vel, const glm::quat& rot, float horizTimescale, float vertTimescale) { + velocity = glmToBullet(vel); + rotation = glmToBullet(rot); + hTimescale = horizTimescale; + if (hTimescale < MIN_CHARACTER_MOTOR_TIMESCALE) { + hTimescale = MIN_CHARACTER_MOTOR_TIMESCALE; + } + vTimescale = vertTimescale; + if (vTimescale < 0.0f) { + vTimescale = hTimescale; + } else if (vTimescale < MIN_CHARACTER_MOTOR_TIMESCALE) { + vTimescale = MIN_CHARACTER_MOTOR_TIMESCALE; + } +} CharacterController::CharacterController() { _halfHeight = 1.0f; @@ -177,52 +191,12 @@ const btScalar MIN_TARGET_SPEED = 0.001f; const btScalar MIN_TARGET_SPEED_SQUARED = MIN_TARGET_SPEED * MIN_TARGET_SPEED; void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) { - - btVector3 actualVelocity = _rigidBody->getLinearVelocity() - _parentVelocity; - if (actualVelocity.length2() < MIN_TARGET_SPEED_SQUARED) { - actualVelocity = btVector3(0.0f, 0.0f, 0.0f); - } - - btVector3 desiredVelocity = _targetVelocity; - if (desiredVelocity.length2() < MIN_TARGET_SPEED_SQUARED) { - desiredVelocity = btVector3(0.0f, 0.0f, 0.0f); - } - - - btVector3 finalVelocity; - const btScalar FLY_ACCELERATION_TIMESCALE = 0.2f; - - if (_state == State::Hover) { - // simple case - btScalar tau = dt / FLY_ACCELERATION_TIMESCALE; - finalVelocity = tau * desiredVelocity + (1.0f - tau) * actualVelocity; - } else { - // ground states require special handling of horizontal and vertical components - btVector3 actualVertVelocity = actualVelocity.dot(_currentUp) * _currentUp; - btVector3 actualHorizVelocity = actualVelocity - actualVertVelocity; - btVector3 desiredVertVelocity = desiredVelocity.dot(_currentUp) * _currentUp; - btVector3 desiredHorizVelocity = desiredVelocity - desiredVertVelocity; - - // horizontal part - const btScalar WALK_ACCELERATION_TIMESCALE = 0.1f; - const btScalar IN_AIR_ACCELERATION_TIMESCALE = 2.0f; - btScalar timescale = (_state == State::InAir) ? IN_AIR_ACCELERATION_TIMESCALE : WALK_ACCELERATION_TIMESCALE; - btScalar tau = dt / timescale; - finalVelocity = tau * desiredHorizVelocity + (1.0f - tau) * actualHorizVelocity; - - // only blend vertical part if the target velocity has non-zero vertical component - if (desiredVertVelocity.length2() > MIN_TARGET_SPEED_SQUARED) { - tau = dt / FLY_ACCELERATION_TIMESCALE; - finalVelocity += tau * desiredVertVelocity + (1.0f - tau) * actualVertVelocity; - } else { - finalVelocity += actualVertVelocity; - } - } - - _rigidBody->setLinearVelocity(finalVelocity + _parentVelocity); + btVector3 velocity = _rigidBody->getLinearVelocity() - _parentVelocity; + computeNewVelocity(dt, velocity); + _rigidBody->setLinearVelocity(velocity + _parentVelocity); // Dynamicaly compute a follow velocity to move this body toward the _followDesiredBodyTransform. - // Rather then add this velocity to velocity the RigidBody, we explicitly teleport the RigidBody towards its goal. + // Rather than add this velocity to velocity the RigidBody, we explicitly teleport the RigidBody towards its goal. // This mirrors the computation done in MyAvatar::FollowHelper::postPhysicsUpdate(). const float MINIMUM_TIME_REMAINING = 0.005f; @@ -389,10 +363,6 @@ void CharacterController::getPositionAndOrientation(glm::vec3& position, glm::qu } } -void CharacterController::setTargetVelocity(const glm::vec3& velocity) { - _targetVelocity = glmToBullet(velocity); -} - void CharacterController::setParentVelocity(const glm::vec3& velocity) { _parentVelocity = glmToBullet(velocity); } @@ -434,6 +404,123 @@ glm::vec3 CharacterController::getVelocityChange() const { return velocity; } +void CharacterController::clearMotors() { + _motors.clear(); +} + +void CharacterController::addMotor(const glm::vec3& velocity, const glm::quat& rotation, float horizTimescale, float vertTimescale) { + _motors.push_back(CharacterController::CharacterMotor(velocity, rotation, horizTimescale, vertTimescale)); +} + +void CharacterController::applyMotor(int index, btScalar dt, btVector3& worldVelocity, std::vector& velocities, std::vector& weights) { + assert(index < (int)(_motors.size())); + CharacterController::CharacterMotor& motor = _motors[index]; + if (motor.hTimescale >= MAX_CHARACTER_MOTOR_TIMESCALE && motor.vTimescale >= MAX_CHARACTER_MOTOR_TIMESCALE) { + // nothing to do + return; + } + + // rotate into motor-frame + btVector3 axis = motor.rotation.getAxis(); + btScalar angle = motor.rotation.getAngle(); + btVector3 velocity = worldVelocity.rotate(axis, -angle); + btVector3 oldVelocity = velocity; + + if (_state == State::Hover || motor.hTimescale == motor.vTimescale) { + // modify velocity + btScalar tau = dt / motor.hTimescale; + if (tau > 1.0f) { + tau = 1.0f; + } + velocity += (motor.velocity - velocity) * tau; + + // rotate back into world-frame + velocity = velocity.rotate(axis, angle); + + // store the velocity and weight + velocities.push_back(velocity); + weights.push_back(tau); + } else { + // compute local UP + btVector3 up = _currentUp.rotate(axis, -angle); + + // split velocity into horizontal and vertical components + btVector3 vVelocity = velocity.dot(up) * up; + btVector3 hVelocity = velocity - vVelocity; + btVector3 vTargetVelocity = motor.velocity.dot(up) * up; + btVector3 hTargetVelocity = motor.velocity - vTargetVelocity; + + // modify each component separately + btScalar maxTau = 0.0f; + if (motor.hTimescale < MAX_CHARACTER_MOTOR_TIMESCALE) { + btScalar tau = dt / motor.hTimescale; + if (tau > 1.0f) { + tau = 1.0f; + } + maxTau = tau; + hVelocity += (hTargetVelocity - hVelocity) * tau; + } + if (motor.vTimescale < MAX_CHARACTER_MOTOR_TIMESCALE) { + btScalar tau = dt / motor.vTimescale; + if (tau > 1.0f) { + tau = 1.0f; + } + if (tau > maxTau) { + maxTau = tau; + } + vVelocity += (vTargetVelocity - vVelocity) * tau; + } + + // add components back together and rotate into world-frame + velocity = (hVelocity + vVelocity).rotate(axis, angle); + + // store velocity and weights + velocities.push_back(velocity); + weights.push_back(maxTau); + } +} + +void CharacterController::computeNewVelocity(btScalar dt, btVector3& velocity) { + if (velocity.length2() < MIN_TARGET_SPEED_SQUARED) { + velocity = btVector3(0.0f, 0.0f, 0.0f); + } + + // measure velocity changes and their weights + std::vector velocities; + velocities.reserve(_motors.size()); + std::vector weights; + weights.reserve(_motors.size()); + for (size_t i = 0; i < _motors.size(); ++i) { + applyMotor(i, dt, velocity, velocities, weights); + } + assert(velocities.size() == weights.size()); + + // blend velocity changes according to relative weights + btScalar totalWeight = 0.0f; + for (size_t i = 0; i < weights.size(); ++i) { + totalWeight += weights[i]; + } + if (totalWeight > 0.0f) { + velocity = btVector3(0.0f, 0.0f, 0.0f); + for (size_t i = 0; i < velocities.size(); ++i) { + velocity += (weights[i] / totalWeight) * velocities[i]; + } + } + if (velocity.length2() < MIN_TARGET_SPEED_SQUARED) { + velocity = btVector3(0.0f, 0.0f, 0.0f); + } + + // 'thrust' is applied at the very end + velocity += dt * _linearAcceleration; + _targetVelocity = velocity; +} + +void CharacterController::computeNewVelocity(btScalar dt, glm::vec3& velocity) { + btVector3 btVelocity = glmToBullet(velocity); + computeNewVelocity(dt, btVelocity); + velocity = bulletToGLM(btVelocity); +} + void CharacterController::preSimulation() { if (_enabled && _dynamicsWorld) { quint64 now = usecTimestampNow(); @@ -443,9 +530,6 @@ void CharacterController::preSimulation() { btVector3 velocity = _rigidBody->getLinearVelocity(); _preSimulationVelocity = velocity; - btVector3 actualVertVelocity = velocity.dot(_currentUp) * _currentUp; - btVector3 actualHorizVelocity = velocity - actualVertVelocity; - // scan for distant floor // rayStart is at center of bottom sphere btVector3 rayStart = _characterBodyTransform.getOrigin() - _halfHeight * _currentUp; @@ -483,6 +567,8 @@ void CharacterController::preSimulation() { } bool jumpButtonHeld = _pendingFlags & PENDING_FLAG_JUMP; + + btVector3 actualHorizVelocity = velocity - velocity.dot(_currentUp) * _currentUp; bool flyingFast = _state == State::Hover && actualHorizVelocity.length() > (MAX_WALKING_SPEED * 0.75f); switch (_state) { diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h index a54ab97a14..ba82f07c97 100644 --- a/libraries/physics/src/CharacterController.h +++ b/libraries/physics/src/CharacterController.h @@ -31,7 +31,10 @@ class btRigidBody; class btCollisionWorld; class btDynamicsWorld; -//#define DEBUG_STATE_CHANGE +#define DEBUG_STATE_CHANGE + +const btScalar MAX_CHARACTER_MOTOR_TIMESCALE = 60.0f; // one minute +const btScalar MIN_CHARACTER_MOTOR_TIMESCALE = 0.05f; class CharacterController : public btCharacterControllerInterface { public: @@ -62,13 +65,21 @@ public: virtual void jump() override; virtual bool onGround() const override; + void clearMotors(); + void addMotor(const glm::vec3& velocity, const glm::quat& rotation, float horizTimescale, float vertTimescale = -1.0f); + void applyMotor(int index, btScalar dt, btVector3& worldVelocity, std::vector& velocities, std::vector& weights); + void computeNewVelocity(btScalar dt, btVector3& velocity); + void computeNewVelocity(btScalar dt, glm::vec3& velocity); + + // HACK for legacy 'thrust' feature + void setLinearAcceleration(const glm::vec3& acceleration) { _linearAcceleration = glmToBullet(acceleration); } + void preSimulation(); void postSimulation(); void setPositionAndOrientation( const glm::vec3& position, const glm::quat& orientation); void getPositionAndOrientation(glm::vec3& position, glm::quat& rotation) const; - void setTargetVelocity(const glm::vec3& velocity); void setParentVelocity(const glm::vec3& parentVelocity); void setFollowParameters(const glm::mat4& desiredWorldMatrix, float timeRemaining); float getFollowTime() const { return _followTime; } @@ -110,6 +121,16 @@ protected: bool checkForSupport(btCollisionWorld* collisionWorld) const; protected: + struct CharacterMotor { + CharacterMotor(const glm::vec3& vel, const glm::quat& rot, float horizTimescale, float vertTimescale = -1.0f); + + btVector3 velocity { btVector3(0.0f, 0.0f, 0.0f) }; // local-frame + btQuaternion rotation; // local-to-world + btScalar hTimescale { MAX_CHARACTER_MOTOR_TIMESCALE }; // horizontal + btScalar vTimescale { MAX_CHARACTER_MOTOR_TIMESCALE }; // vertical + }; + + std::vector _motors; btVector3 _currentUp; btVector3 _targetVelocity; btVector3 _parentVelocity; @@ -141,6 +162,7 @@ protected: btScalar _followTime; btVector3 _followLinearDisplacement; btQuaternion _followAngularDisplacement; + btVector3 _linearAcceleration; bool _enabled; State _state;