From bc56df0be18c85e6735d96623800742100ac8aa5 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 8 Sep 2016 14:31:31 -0700 Subject: [PATCH 001/101] out-of-body with reduced recovery speeds --- interface/src/avatar/MyAvatar.cpp | 164 ++++++++---------- interface/src/avatar/MyAvatar.h | 13 +- libraries/physics/src/CharacterController.cpp | 34 ++-- libraries/physics/src/CharacterController.h | 5 +- 4 files changed, 98 insertions(+), 118 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 24dbc62318..a3371e6caa 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -416,10 +416,6 @@ void MyAvatar::simulate(float deltaTime) { updatePosition(deltaTime); } - // update sensorToWorldMatrix for camera and hand controllers - // before we perform rig animations and IK. - updateSensorToWorldMatrix(); - { PerformanceTimer perfTimer("skeleton"); _skeletonModel->simulate(deltaTime); @@ -1305,9 +1301,10 @@ void MyAvatar::prepareForPhysicsSimulation() { _characterController.setPositionAndOrientation(getPosition(), getOrientation()); if (qApp->isHMDMode()) { - _follow.prePhysicsUpdate(*this, deriveBodyFromHMDSensor(), _bodySensorMatrix, hasDriveInput()); + _follow.prePhysicsUpdate(*this, deriveBodyFromHMDSensor(), _bodySensorMatrix); } else { _follow.deactivate(); + getCharacterController()->disableFollow(); } } @@ -1318,7 +1315,10 @@ void MyAvatar::harvestResultsFromPhysicsSimulation(float deltaTime) { _characterController.getPositionAndOrientation(position, orientation); } nextAttitude(position, orientation); - _bodySensorMatrix = _follow.postPhysicsUpdate(*this, _bodySensorMatrix); + + // compute new _bodyToSensorMatrix + glm::mat4 bodyToWorldMatrix = createMatFromQuatAndPos(orientation, position); + _bodySensorMatrix = glm::inverse(_sensorToWorldMatrix) * bodyToWorldMatrix; if (_characterController.isEnabledAndReady()) { setVelocity(_characterController.getLinearVelocity() + _characterController.getFollowVelocity()); @@ -2050,64 +2050,47 @@ MyAvatar::FollowHelper::FollowHelper() { } void MyAvatar::FollowHelper::deactivate() { - for (int i = 0; i < NumFollowTypes; i++) { - deactivate((FollowType)i); - } + _activeBits = 0; } void MyAvatar::FollowHelper::deactivate(FollowType type) { assert(type >= 0 && type < NumFollowTypes); - _timeRemaining[(int)type] = 0.0f; + _activeBits &= ~(uint8_t)(0x01 << (int)type); } void MyAvatar::FollowHelper::activate(FollowType type) { assert(type >= 0 && type < NumFollowTypes); - // TODO: Perhaps, the follow time should be proportional to the displacement. - _timeRemaining[(int)type] = FOLLOW_TIME; + _activeBits |= (uint8_t)(0x01 << (int)type); } bool MyAvatar::FollowHelper::isActive(FollowType type) const { assert(type >= 0 && type < NumFollowTypes); - return _timeRemaining[(int)type] > 0.0f; + return (bool)(_activeBits & (uint8_t)(0x01 << (int)type)); } bool MyAvatar::FollowHelper::isActive() const { - for (int i = 0; i < NumFollowTypes; i++) { - if (isActive((FollowType)i)) { - return true; - } - } - return false; + return (bool)_activeBits; } -float MyAvatar::FollowHelper::getMaxTimeRemaining() const { - float max = 0.0f; - for (int i = 0; i < NumFollowTypes; i++) { - if (_timeRemaining[i] > max) { - max = _timeRemaining[i]; - } - } - return max; -} - -void MyAvatar::FollowHelper::decrementTimeRemaining(float dt) { - for (int i = 0; i < NumFollowTypes; i++) { - _timeRemaining[i] -= dt; - } -} - -bool MyAvatar::FollowHelper::shouldActivateRotation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) const { +void MyAvatar::FollowHelper::updateRotationActivation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) { auto cameraMode = qApp->getCamera()->getMode(); if (cameraMode == CAMERA_MODE_THIRD_PERSON) { - return false; + deactivate(Rotation); } else { const float FOLLOW_ROTATION_THRESHOLD = cosf(PI / 6.0f); // 30 degrees + const float STOP_FOLLOW_ROTATION_THRESHOLD = 0.99f; glm::vec2 bodyFacing = getFacingDir2D(currentBodyMatrix); - return glm::dot(myAvatar.getHMDSensorFacingMovingAverage(), bodyFacing) < FOLLOW_ROTATION_THRESHOLD; + if (isActive(Rotation)) { + if (glm::dot(myAvatar.getHMDSensorFacingMovingAverage(), bodyFacing) > STOP_FOLLOW_ROTATION_THRESHOLD) { + deactivate(Rotation); + } + } else if (glm::dot(myAvatar.getHMDSensorFacingMovingAverage(), bodyFacing) < FOLLOW_ROTATION_THRESHOLD) { + activate(Rotation); + } } } -bool MyAvatar::FollowHelper::shouldActivateHorizontal(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) const { +void MyAvatar::FollowHelper::updateHorizontalActivation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) { // -z axis of currentBodyMatrix in world space. glm::vec3 forward = glm::normalize(glm::vec3(-currentBodyMatrix[0][2], -currentBodyMatrix[1][2], -currentBodyMatrix[2][2])); @@ -2116,85 +2099,74 @@ bool MyAvatar::FollowHelper::shouldActivateHorizontal(const MyAvatar& myAvatar, glm::vec3 offset = extractTranslation(desiredBodyMatrix) - extractTranslation(currentBodyMatrix); float forwardLeanAmount = glm::dot(forward, offset); - float lateralLeanAmount = glm::dot(right, offset); + float lateralLeanAmount = fabsf(glm::dot(right, offset)); const float MAX_LATERAL_LEAN = 0.3f; const float MAX_FORWARD_LEAN = 0.15f; const float MAX_BACKWARD_LEAN = 0.1f; + const float MIN_LEAN = 0.02f; - if (forwardLeanAmount > 0 && forwardLeanAmount > MAX_FORWARD_LEAN) { - return true; - } else if (forwardLeanAmount < 0 && forwardLeanAmount < -MAX_BACKWARD_LEAN) { - return true; + if (isActive(Horizontal)) { + if (fabsf(forwardLeanAmount) < MIN_LEAN && lateralLeanAmount < MIN_LEAN) { + deactivate(Horizontal); + } + } else { + if (forwardLeanAmount > MAX_FORWARD_LEAN || + forwardLeanAmount < -MAX_BACKWARD_LEAN || + lateralLeanAmount > MAX_LATERAL_LEAN) { + activate(Horizontal); + } } - - return fabs(lateralLeanAmount) > MAX_LATERAL_LEAN; } -bool MyAvatar::FollowHelper::shouldActivateVertical(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) const { - +void MyAvatar::FollowHelper::updateVerticalActivation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) { const float CYLINDER_TOP = 0.1f; const float CYLINDER_BOTTOM = -1.5f; + const float MIN_VERTICAL_OFFSET = 0.02f; glm::vec3 offset = extractTranslation(desiredBodyMatrix) - extractTranslation(currentBodyMatrix); - return (offset.y > CYLINDER_TOP) || (offset.y < CYLINDER_BOTTOM); + if (isActive(Vertical)) { + if (fabsf(offset.y) < MIN_VERTICAL_OFFSET) { + deactivate(Vertical); + } + } else if (offset.y > CYLINDER_TOP || offset.y < CYLINDER_BOTTOM) { + activate(Vertical); + } } -void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix, bool hasDriveInput) { +void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) { _desiredBodyMatrix = desiredBodyMatrix; if (myAvatar.getHMDLeanRecenterEnabled()) { - if (!isActive(Rotation) && shouldActivateRotation(myAvatar, desiredBodyMatrix, currentBodyMatrix)) { - activate(Rotation); + updateRotationActivation(myAvatar, desiredBodyMatrix, currentBodyMatrix); + updateHorizontalActivation(myAvatar, desiredBodyMatrix, currentBodyMatrix); + updateVerticalActivation(myAvatar, desiredBodyMatrix, currentBodyMatrix); + + glm::mat4 desiredWorldMatrix = myAvatar.getSensorToWorldMatrix() * _desiredBodyMatrix; + glm::mat4 currentWorldMatrix = createMatFromQuatAndPos(myAvatar.getOrientation(), myAvatar.getPosition()); + + AnimPose followWorldPose(currentWorldMatrix); + if (isActive(Rotation)) { + followWorldPose.rot = glmExtractRotation(desiredWorldMatrix); } - if (!isActive(Horizontal) && shouldActivateHorizontal(myAvatar, desiredBodyMatrix, currentBodyMatrix)) { - activate(Horizontal); + if (isActive(Horizontal)) { + glm::vec3 desiredTranslation = extractTranslation(desiredWorldMatrix); + followWorldPose.trans.x = desiredTranslation.x; + followWorldPose.trans.z = desiredTranslation.z; } - if (!isActive(Vertical) && (shouldActivateVertical(myAvatar, desiredBodyMatrix, currentBodyMatrix) || hasDriveInput)) { - activate(Vertical); + if (isActive(Vertical)) { + glm::vec3 desiredTranslation = extractTranslation(desiredWorldMatrix); + followWorldPose.trans.y = desiredTranslation.y; } - } - - glm::mat4 desiredWorldMatrix = myAvatar.getSensorToWorldMatrix() * _desiredBodyMatrix; - glm::mat4 currentWorldMatrix = myAvatar.getSensorToWorldMatrix() * currentBodyMatrix; - - AnimPose followWorldPose(currentWorldMatrix); - if (isActive(Rotation)) { - followWorldPose.rot = glmExtractRotation(desiredWorldMatrix); - } - if (isActive(Horizontal)) { - glm::vec3 desiredTranslation = extractTranslation(desiredWorldMatrix); - followWorldPose.trans.x = desiredTranslation.x; - followWorldPose.trans.z = desiredTranslation.z; - } - if (isActive(Vertical)) { - glm::vec3 desiredTranslation = extractTranslation(desiredWorldMatrix); - followWorldPose.trans.y = desiredTranslation.y; - } - - myAvatar.getCharacterController()->setFollowParameters(followWorldPose, getMaxTimeRemaining()); -} - -glm::mat4 MyAvatar::FollowHelper::postPhysicsUpdate(const MyAvatar& myAvatar, const glm::mat4& currentBodyMatrix) { - if (isActive()) { - float dt = myAvatar.getCharacterController()->getFollowTime(); - decrementTimeRemaining(dt); - - // apply follow displacement to the body matrix. - glm::vec3 worldLinearDisplacement = myAvatar.getCharacterController()->getFollowLinearDisplacement(); - glm::quat worldAngularDisplacement = myAvatar.getCharacterController()->getFollowAngularDisplacement(); - glm::quat sensorToWorld = glmExtractRotation(myAvatar.getSensorToWorldMatrix()); - glm::quat worldToSensor = glm::inverse(sensorToWorld); - - glm::vec3 sensorLinearDisplacement = worldToSensor * worldLinearDisplacement; - glm::quat sensorAngularDisplacement = worldToSensor * worldAngularDisplacement * sensorToWorld; - - glm::mat4 newBodyMat = createMatFromQuatAndPos(sensorAngularDisplacement * glmExtractRotation(currentBodyMatrix), - sensorLinearDisplacement + extractTranslation(currentBodyMatrix)); - return newBodyMat; + if (isActive()) { + myAvatar.getCharacterController()->setFollowParameters(followWorldPose); + } else { + myAvatar.getCharacterController()->disableFollow(); + } } else { - return currentBodyMatrix; + deactivate(); + myAvatar.getCharacterController()->disableFollow(); } } diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index c4ffc08cbc..9614bf9bf8 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -442,7 +442,7 @@ private: NumFollowTypes }; glm::mat4 _desiredBodyMatrix; - float _timeRemaining[NumFollowTypes]; + uint8_t _activeBits; void deactivate(); void deactivate(FollowType type); @@ -450,13 +450,10 @@ private: void activate(FollowType type); bool isActive() const; bool isActive(FollowType followType) const; - float getMaxTimeRemaining() const; - void decrementTimeRemaining(float dt); - bool shouldActivateRotation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) const; - bool shouldActivateVertical(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) const; - bool shouldActivateHorizontal(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) const; - void prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat4& bodySensorMatrix, const glm::mat4& currentBodyMatrix, bool hasDriveInput); - glm::mat4 postPhysicsUpdate(const MyAvatar& myAvatar, const glm::mat4& currentBodyMatrix); + void updateRotationActivation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix); + void updateHorizontalActivation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix); + void updateVerticalActivation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix); + void prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat4& bodySensorMatrix, const glm::mat4& currentBodyMatrix); }; FollowHelper _follow; diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 19207f13ed..af983deea9 100644 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -70,7 +70,6 @@ CharacterController::CharacterController() { _targetVelocity.setValue(0.0f, 0.0f, 0.0f); _followDesiredBodyTransform.setIdentity(); - _followTimeRemaining = 0.0f; _jumpSpeed = JUMP_SPEED; _state = State::Hover; _isPushingUp = false; @@ -199,16 +198,24 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) { // 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; - const float MAX_DISPLACEMENT = 0.5f * _radius; - _followTimeRemaining -= dt; - if (_followTimeRemaining >= MINIMUM_TIME_REMAINING) { + if (_following) { + // HACK these copied form elsewhere + const float NORMAL_WALKING_SPEED = 0.5f; + const float FOLLOW_TIME = 0.8f; + const float FOLLOW_ROTATION_THRESHOLD = cosf(PI / 6.0f); + + const float MAX_ANGULAR_SPEED = FOLLOW_ROTATION_THRESHOLD / FOLLOW_TIME; + btTransform bodyTransform = _rigidBody->getWorldTransform(); btVector3 startPos = bodyTransform.getOrigin(); btVector3 deltaPos = _followDesiredBodyTransform.getOrigin() - startPos; - btVector3 vel = deltaPos / _followTimeRemaining; - btVector3 linearDisplacement = clampLength(vel * dt, MAX_DISPLACEMENT); // clamp displacement to prevent tunneling. + btVector3 vel = deltaPos * (0.5f / dt); + btScalar speed = vel.length(); + if (speed > NORMAL_WALKING_SPEED) { + vel *= NORMAL_WALKING_SPEED / speed; + } + btVector3 linearDisplacement = vel * dt; btVector3 endPos = startPos + linearDisplacement; btQuaternion startRot = bodyTransform.getRotation(); @@ -216,7 +223,10 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) { glm::vec2 currentRight(currentFacing.y, -currentFacing.x); glm::vec2 desiredFacing = getFacingDir2D(bulletToGLM(_followDesiredBodyTransform.getRotation())); float deltaAngle = acosf(glm::clamp(glm::dot(currentFacing, desiredFacing), -1.0f, 1.0f)); - float angularSpeed = deltaAngle / _followTimeRemaining; + float angularSpeed = 0.5f * deltaAngle / dt; + if (angularSpeed > MAX_ANGULAR_SPEED) { + angularSpeed *= MAX_ANGULAR_SPEED / angularSpeed; + } float sign = copysignf(1.0f, glm::dot(desiredFacing, currentRight)); btQuaternion angularDisplacement = btQuaternion(btVector3(0.0f, 1.0f, 0.0f), sign * angularSpeed * dt); btQuaternion endRot = angularDisplacement * startRot; @@ -229,8 +239,8 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) { _followAngularDisplacement = angularDisplacement * _followAngularDisplacement; _rigidBody->setWorldTransform(btTransform(endRot, endPos)); + _followTime += dt; } - _followTime += dt; } void CharacterController::jump() { @@ -371,9 +381,9 @@ void CharacterController::setParentVelocity(const glm::vec3& velocity) { _parentVelocity = glmToBullet(velocity); } -void CharacterController::setFollowParameters(const glm::mat4& desiredWorldBodyMatrix, float timeRemaining) { - _followTimeRemaining = timeRemaining; +void CharacterController::setFollowParameters(const glm::mat4& desiredWorldBodyMatrix) { _followDesiredBodyTransform = glmToBullet(desiredWorldBodyMatrix) * btTransform(btQuaternion::getIdentity(), glmToBullet(_shapeLocalOffset)); + _following = true; } glm::vec3 CharacterController::getFollowLinearDisplacement() const { @@ -626,7 +636,7 @@ void CharacterController::preSimulation() { _pendingFlags &= ~PENDING_FLAG_JUMP; _followTime = 0.0f; - _followLinearDisplacement = btVector3(0, 0, 0); + _followLinearDisplacement = btVector3(0.0f, 0.0f, 0.0f); _followAngularDisplacement = btQuaternion::getIdentity(); } diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h index 586ea175e6..26ad481c46 100644 --- a/libraries/physics/src/CharacterController.h +++ b/libraries/physics/src/CharacterController.h @@ -82,7 +82,8 @@ public: void getPositionAndOrientation(glm::vec3& position, glm::quat& rotation) const; void setParentVelocity(const glm::vec3& parentVelocity); - void setFollowParameters(const glm::mat4& desiredWorldMatrix, float timeRemaining); + void setFollowParameters(const glm::mat4& desiredWorldBodyMatrix); + void disableFollow() { _following = false; } float getFollowTime() const { return _followTime; } glm::vec3 getFollowLinearDisplacement() const; glm::quat getFollowAngularDisplacement() const; @@ -142,7 +143,6 @@ protected: btVector3 _preSimulationVelocity; btVector3 _velocityChange; btTransform _followDesiredBodyTransform; - btScalar _followTimeRemaining; btTransform _characterBodyTransform; glm::vec3 _shapeLocalOffset; @@ -168,6 +168,7 @@ protected: btVector3 _followLinearDisplacement; btQuaternion _followAngularDisplacement; btVector3 _linearAcceleration; + bool _following { false }; std::atomic_bool _enabled; State _state; From 7e7803c648313c7667e2944c220383c7eb03e5cc Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Fri, 9 Sep 2016 12:50:34 -0700 Subject: [PATCH 002/101] Added developer option to draw IK targets in world. --- interface/src/Menu.cpp | 2 ++ interface/src/Menu.h | 1 + interface/src/avatar/MyAvatar.cpp | 9 ++++++++ interface/src/avatar/MyAvatar.h | 2 ++ .../animation/src/AnimInverseKinematics.cpp | 21 +++++++++++++++++++ libraries/animation/src/Rig.cpp | 4 ++++ 6 files changed, 39 insertions(+) diff --git a/interface/src/Menu.cpp b/interface/src/Menu.cpp index 50dc748461..f58a593395 100644 --- a/interface/src/Menu.cpp +++ b/interface/src/Menu.cpp @@ -491,6 +491,8 @@ Menu::Menu() { avatar, SLOT(setEnableInverseKinematics(bool))); addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::RenderSensorToWorldMatrix, 0, false, avatar, SLOT(setEnableDebugDrawSensorToWorldMatrix(bool))); + addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::RenderIKTargets, 0, false, + avatar, SLOT(setEnableDebugDrawIKTargets(bool))); addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::ActionMotorControl, Qt::CTRL | Qt::SHIFT | Qt::Key_K, true, avatar, SLOT(updateMotionBehaviorFromMenu()), diff --git a/interface/src/Menu.h b/interface/src/Menu.h index ee00644746..9b916467d0 100644 --- a/interface/src/Menu.h +++ b/interface/src/Menu.h @@ -161,6 +161,7 @@ namespace MenuOption { const QString RenderResolutionThird = "1/3"; const QString RenderResolutionQuarter = "1/4"; const QString RenderSensorToWorldMatrix = "Show SensorToWorld Matrix"; + const QString RenderIKTargets = "Show IK Targets"; const QString ResetAvatarSize = "Reset Avatar Size"; const QString ResetSensors = "Reset Sensors"; const QString RunningScripts = "Running Scripts..."; diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index a3371e6caa..fc4a4febb3 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -82,6 +82,8 @@ const float MyAvatar::ZOOM_MIN = 0.5f; const float MyAvatar::ZOOM_MAX = 25.0f; const float MyAvatar::ZOOM_DEFAULT = 1.5f; +extern bool HACKY_GLOBAL_ENABLE_DEBUG_DRAW_IK_TARGETS; + MyAvatar::MyAvatar(RigPointer rig) : Avatar(rig), _wasPushing(false), @@ -830,6 +832,13 @@ void MyAvatar::setEnableDebugDrawSensorToWorldMatrix(bool isEnabled) { } } +void MyAvatar::setEnableDebugDrawIKTargets(bool isEnabled) { + _enableDebugDrawIKTargets = isEnabled; + + HACKY_GLOBAL_ENABLE_DEBUG_DRAW_IK_TARGETS = isEnabled; +} + + void MyAvatar::setEnableMeshVisible(bool isEnabled) { render::ScenePointer scene = qApp->getMain3DScene(); _skeletonModel->setVisibleInScene(isEnabled, scene); diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 9614bf9bf8..3a838505d1 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -299,6 +299,7 @@ public slots: void setEnableDebugDrawPosition(bool isEnabled); void setEnableDebugDrawHandControllers(bool isEnabled); void setEnableDebugDrawSensorToWorldMatrix(bool isEnabled); + void setEnableDebugDrawIKTargets(bool isEnabled); bool getEnableMeshVisible() const { return _skeletonModel->isVisible(); } void setEnableMeshVisible(bool isEnabled); void setUseAnimPreAndPostRotations(bool isEnabled); @@ -469,6 +470,7 @@ private: bool _enableDebugDrawAnimPose { false }; bool _enableDebugDrawHandControllers { false }; bool _enableDebugDrawSensorToWorldMatrix { false }; + bool _enableDebugDrawIKTargets { false }; AudioListenerMode _audioListenerMode; glm::vec3 _customListenPosition; diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 7985251002..42eafa80fd 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -14,11 +14,16 @@ #include #include #include +#include +#include "Rig.h" #include "ElbowConstraint.h" #include "SwingTwistConstraint.h" #include "AnimationLogging.h" +bool HACKY_GLOBAL_ENABLE_DEBUG_DRAW_IK_TARGETS = false; +Rig* HACKY_GLOBAL_RIG_POINTER = nullptr; + AnimInverseKinematics::AnimInverseKinematics(const QString& id) : AnimNode(AnimNode::Type::InverseKinematics, id) { } @@ -437,6 +442,22 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars computeTargets(animVars, targets, underPoses); } + // AJT: HACK + if (HACKY_GLOBAL_ENABLE_DEBUG_DRAW_IK_TARGETS && HACKY_GLOBAL_RIG_POINTER) { + const float CM_TO_M = 0.01f; + const vec4 WHITE(1.0f); + glm::mat4 geomToRigMat = HACKY_GLOBAL_RIG_POINTER->getGeometryToRigTransform(); + glm::mat4 rigToAvatarMat = createMatFromQuatAndPos(Quaternions::Y_180, glm::vec3()); + + for (auto& target : targets) { + glm::mat4 geomTargetMat = createMatFromQuatAndPos(target.getRotation(), target.getTranslation()); + glm::mat4 avatarTargetMat = rigToAvatarMat * geomToRigMat * geomTargetMat; + + std::string name = "ikTarget" + std::to_string(target.getIndex()); + DebugDraw::getInstance().addMyAvatarMarker(name, glmExtractRotation(avatarTargetMat), extractTranslation(avatarTargetMat), WHITE); + } + } + if (targets.empty()) { // no IK targets but still need to enforce constraints std::map::iterator constraintItr = _constraints.begin(); diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index d6ec0487f0..150c96e18f 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -47,6 +47,8 @@ const glm::vec3 DEFAULT_LEFT_EYE_POS(0.3f, 0.9f, 0.0f); const glm::vec3 DEFAULT_HEAD_POS(0.0f, 0.75f, 0.0f); const glm::vec3 DEFAULT_NECK_POS(0.0f, 0.70f, 0.0f); +extern Rig* HACKY_GLOBAL_RIG_POINTER; + void Rig::overrideAnimation(const QString& url, float fps, bool loop, float firstFrame, float lastFrame) { UserAnimState::ClipNodeEnum clipNodeEnum; @@ -875,6 +877,7 @@ void Rig::updateAnimations(float deltaTime, glm::mat4 rootTransform) { _animVars.setRigToGeometryTransform(_rigToGeometryTransform); // evaluate the animation + HACKY_GLOBAL_RIG_POINTER = this; AnimNode::Triggers triggersOut; _internalPoseSet._relativePoses = _animNode->evaluate(_animVars, deltaTime, triggersOut); if ((int)_internalPoseSet._relativePoses.size() != _animSkeleton->getNumJoints()) { @@ -885,6 +888,7 @@ void Rig::updateAnimations(float deltaTime, glm::mat4 rootTransform) { for (auto& trigger : triggersOut) { _animVars.setTrigger(trigger); } + HACKY_GLOBAL_RIG_POINTER = nullptr; } applyOverridePoses(); From 0f248c45409218fec5cfe635ebeaa672641c16a9 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 9 Sep 2016 16:31:36 -0700 Subject: [PATCH 003/101] expose MyAvatar::setSensorToWorldMatrix() --- interface/src/avatar/MyAvatar.cpp | 8 ++++++-- interface/src/avatar/MyAvatar.h | 2 ++ 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index fc4a4febb3..dc0a009ef7 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -547,8 +547,12 @@ void MyAvatar::updateJointFromController(controller::Action poseKey, ThreadSafeV void MyAvatar::updateSensorToWorldMatrix() { // update the sensor mat so that the body position will end up in the desired // position when driven from the head. - glm::mat4 desiredMat = createMatFromQuatAndPos(getOrientation(), getPosition()); - _sensorToWorldMatrix = desiredMat * glm::inverse(_bodySensorMatrix); + glm::mat4 bodyToWorld = createMatFromQuatAndPos(getOrientation(), getPosition()); + setSensorToWorldMatrix(bodyToWorld * glm::inverse(_bodySensorMatrix)); +} + +void MyAvatar::setSensorToWorldMatrix(const glm::mat4& sensorToWorld) { + _sensorToWorldMatrix = sensorToWorld; lateUpdatePalms(); diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 3a838505d1..65d0de3801 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -126,6 +126,8 @@ public: // This is so the correct camera can be used for rendering. void updateSensorToWorldMatrix(); + void setSensorToWorldMatrix(const glm::mat4& sensorToWorld); + void setRealWorldFieldOfView(float realWorldFov) { _realWorldFieldOfView.set(realWorldFov); } Q_INVOKABLE glm::vec3 getDefaultEyePosition() const; From 97623f489cd6255fdcae2a2ff7426a639125adb2 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 9 Sep 2016 16:32:57 -0700 Subject: [PATCH 004/101] expose setting HMD sensorToWorld from JS script --- interface/src/scripting/HMDScriptingInterface.cpp | 15 +++++++++++++++ interface/src/scripting/HMDScriptingInterface.h | 9 +++++++-- 2 files changed, 22 insertions(+), 2 deletions(-) diff --git a/interface/src/scripting/HMDScriptingInterface.cpp b/interface/src/scripting/HMDScriptingInterface.cpp index b031e05789..aa1b046962 100644 --- a/interface/src/scripting/HMDScriptingInterface.cpp +++ b/interface/src/scripting/HMDScriptingInterface.cpp @@ -90,6 +90,16 @@ glm::vec3 HMDScriptingInterface::getPosition() const { return glm::vec3(); } +void HMDScriptingInterface::setPosition(const glm::vec3& position) { + MyAvatar* myAvatar = DependencyManager::get()->getMyAvatar(); + glm::mat4 hmdToSensor = myAvatar->getHMDSensorMatrix(); + glm::mat4 sensorToWorld = myAvatar->getSensorToWorldMatrix(); + glm::mat4 hmdToWorld = sensorToWorld * hmdToSensor; + setTranslation(hmdToWorld, position); + sensorToWorld = hmdToWorld * glm::inverse(hmdToSensor); + myAvatar->setSensorToWorldMatrix(sensorToWorld); +} + glm::quat HMDScriptingInterface::getOrientation() const { if (qApp->getActiveDisplayPlugin()->isHmd()) { return glm::normalize(glm::quat_cast(getWorldHMDMatrix())); @@ -139,3 +149,8 @@ bool HMDScriptingInterface::isKeyboardVisible() { void HMDScriptingInterface::centerUI() { QMetaObject::invokeMethod(qApp, "centerUI", Qt::QueuedConnection); } + +void HMDScriptingInterface::snapToAvatar() { + MyAvatar* myAvatar = DependencyManager::get()->getMyAvatar(); + myAvatar->updateSensorToWorldMatrix(); +} diff --git a/interface/src/scripting/HMDScriptingInterface.h b/interface/src/scripting/HMDScriptingInterface.h index 2fbdb76198..45952b3a5e 100644 --- a/interface/src/scripting/HMDScriptingInterface.h +++ b/interface/src/scripting/HMDScriptingInterface.h @@ -25,7 +25,7 @@ class QScriptEngine; class HMDScriptingInterface : public AbstractHMDScriptingInterface, public Dependency { Q_OBJECT - Q_PROPERTY(glm::vec3 position READ getPosition) + Q_PROPERTY(glm::vec3 position READ getPosition WRITE setPosition) Q_PROPERTY(glm::quat orientation READ getOrientation) Q_PROPERTY(bool mounted READ isMounted) @@ -58,6 +58,8 @@ public: // rotate the overlay UI sphere so that it is centered about the the current HMD position and orientation Q_INVOKABLE void centerUI(); + // snap HMD to align with Avatar's current position in world-frame + Q_INVOKABLE void snapToAvatar(); public: HMDScriptingInterface(); static QScriptValue getHUDLookAtPosition2D(QScriptContext* context, QScriptEngine* engine); @@ -68,7 +70,10 @@ public: private: // Get the position of the HMD glm::vec3 getPosition() const; - + + // Set the position of the HMD + void setPosition(const glm::vec3& position); + // Get the orientation of the HMD glm::quat getOrientation() const; From 6b9cdb440bd21ef3a1e141c61d253e96489b311b Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 9 Sep 2016 16:33:13 -0700 Subject: [PATCH 005/101] update teleport.js to teleport HMD before avatar (in theory, not tested) --- scripts/system/controllers/teleport.js | 40 +++++++++++++++++++++----- 1 file changed, 33 insertions(+), 7 deletions(-) diff --git a/scripts/system/controllers/teleport.js b/scripts/system/controllers/teleport.js index b4a8eefcd2..c5a7a5e81a 100644 --- a/scripts/system/controllers/teleport.js +++ b/scripts/system/controllers/teleport.js @@ -44,6 +44,7 @@ var COLORS_TELEPORT_TOO_CLOSE = { var TELEPORT_CANCEL_RANGE = 1; var USE_COOL_IN = true; var COOL_IN_DURATION = 500; +var MAX_HMD_AVATAR_SEPARATION = 10.0f; function ThumbPad(hand) { this.hand = hand; @@ -88,6 +89,8 @@ function Teleporter() { this.updateConnected = null; this.smoothArrivalInterval = null; this.teleportHand = null; + this.distance = 0.0; + this.teleportMode = "HMDAndAvatarTogether"; this.tooClose = false; this.inCoolIn = false; @@ -459,7 +462,8 @@ function Teleporter() { z: intersection.intersection.z }; - this.tooClose = isTooCloseToTeleport(position); + this.distance = Vec3.distance(MyAvatar.position, position); + this.tooClose = this.distance <= TELEPORT_CANCEL_RANGE; var towardUs = Quat.fromPitchYawRollDegrees(0, euler.y, 0); Overlays.editOverlay(this.targetOverlay, { @@ -480,7 +484,8 @@ function Teleporter() { z: intersection.intersection.z }; - this.tooClose = isTooCloseToTeleport(position); + this.distance = Vec3.distance(MyAvatar.position, position); + this.tooClose = this.distance <= TELEPORT_CANCEL_RANGE; var towardUs = Quat.fromPitchYawRollDegrees(0, euler.y, 0); Overlays.editOverlay(this.cancelOverlay, { @@ -509,6 +514,15 @@ function Teleporter() { var offset = getAvatarFootOffset(); this.intersection.intersection.y += offset; this.exitTeleportMode(); + if (MyAvatar.hmdLeanRecenterEnabled) { + if (this.distance > MAX_HMD_AVATAR_SEPARATION) { + this.teleportMode = "HMDAndAvatarTogether"; + } else { + this.teleportMode = "HMDFirstAvatarWillFollow"; + } + } else { + this.teleportMode = "AvatarOnly"; + } this.smoothArrival(); } }; @@ -538,6 +552,22 @@ function Teleporter() { return arrivalPoints; }; + this.teleportTo = function(landingPoint) { + if (this.teleportMode === "AvatarOnly") { + MyAvatar.position = landingPoint; + } else if (this.teleportMode === "HMDAndAvatarTogether") { + var leanEnabled = MyAvatar.hmdLeanRecenterEnabled; + MyAvatar.setHMDLeanRecenterEnabled(false); + MyAvatar.position = landingPoint; + HMD.snapToAvatar(); + MyAvatar.hmdLeanRecenterEnabled = leanEnabled; + } else if (this.teleportMode === "HMDFirstAvatarWillFollow") { + var eyeOffset = Vec3.subtract(MyAvatar.getEyePosition(), MyAvatar.position); + landingPoint = Vec3.sum(landingPoint, eyeOffset); + HMD.setPosition(landingPoint); + } + } + this.smoothArrival = function() { _this.arrivalPoints = _this.getArrivalPoints(MyAvatar.position, _this.intersection.intersection); @@ -548,7 +578,7 @@ function Teleporter() { return; } var landingPoint = _this.arrivalPoints.shift(); - MyAvatar.position = landingPoint; + _this.teleportTo(landingPoint); if (_this.arrivalPoints.length === 1 || _this.arrivalPoints.length === 0) { _this.deleteTargetOverlay(); @@ -627,10 +657,6 @@ function isMoving() { } }; -function isTooCloseToTeleport(position) { - return Vec3.distance(MyAvatar.position, position) <= TELEPORT_CANCEL_RANGE; -}; - function registerMappings() { mappingName = 'Hifi-Teleporter-Dev-' + Math.random(); teleportMapping = Controller.newMapping(mappingName); From 82522fbbb12bc29db497195063621c73e32e9fb7 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 9 Sep 2016 16:40:07 -0700 Subject: [PATCH 006/101] fix C++ism typo in JS script --- scripts/system/controllers/teleport.js | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/scripts/system/controllers/teleport.js b/scripts/system/controllers/teleport.js index c5a7a5e81a..21aee70cc0 100644 --- a/scripts/system/controllers/teleport.js +++ b/scripts/system/controllers/teleport.js @@ -44,7 +44,7 @@ var COLORS_TELEPORT_TOO_CLOSE = { var TELEPORT_CANCEL_RANGE = 1; var USE_COOL_IN = true; var COOL_IN_DURATION = 500; -var MAX_HMD_AVATAR_SEPARATION = 10.0f; +var MAX_HMD_AVATAR_SEPARATION = 10.0; function ThumbPad(hand) { this.hand = hand; From 86351ca21cfe1b43efef5642356814b8f8b1eed7 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Fri, 9 Sep 2016 17:31:11 -0700 Subject: [PATCH 007/101] Teleport bugs * made HMD.setPosition thread safe * changed script to use HMD.position = foo; --- .../src/scripting/HMDScriptingInterface.cpp | 19 ++++++++++++------- .../src/scripting/HMDScriptingInterface.h | 2 +- scripts/system/controllers/teleport.js | 2 +- 3 files changed, 14 insertions(+), 9 deletions(-) diff --git a/interface/src/scripting/HMDScriptingInterface.cpp b/interface/src/scripting/HMDScriptingInterface.cpp index aa1b046962..474126d51b 100644 --- a/interface/src/scripting/HMDScriptingInterface.cpp +++ b/interface/src/scripting/HMDScriptingInterface.cpp @@ -91,13 +91,18 @@ glm::vec3 HMDScriptingInterface::getPosition() const { } void HMDScriptingInterface::setPosition(const glm::vec3& position) { - MyAvatar* myAvatar = DependencyManager::get()->getMyAvatar(); - glm::mat4 hmdToSensor = myAvatar->getHMDSensorMatrix(); - glm::mat4 sensorToWorld = myAvatar->getSensorToWorldMatrix(); - glm::mat4 hmdToWorld = sensorToWorld * hmdToSensor; - setTranslation(hmdToWorld, position); - sensorToWorld = hmdToWorld * glm::inverse(hmdToSensor); - myAvatar->setSensorToWorldMatrix(sensorToWorld); + if (QThread::currentThread() != thread()) { + QMetaObject::invokeMethod(this, "setPosition", Qt::QueuedConnection, Q_ARG(const glm::vec3&, position)); + return; + } else { + MyAvatar* myAvatar = DependencyManager::get()->getMyAvatar(); + glm::mat4 hmdToSensor = myAvatar->getHMDSensorMatrix(); + glm::mat4 sensorToWorld = myAvatar->getSensorToWorldMatrix(); + glm::mat4 hmdToWorld = sensorToWorld * hmdToSensor; + setTranslation(hmdToWorld, position); + sensorToWorld = hmdToWorld * glm::inverse(hmdToSensor); + myAvatar->setSensorToWorldMatrix(sensorToWorld); + } } glm::quat HMDScriptingInterface::getOrientation() const { diff --git a/interface/src/scripting/HMDScriptingInterface.h b/interface/src/scripting/HMDScriptingInterface.h index 45952b3a5e..9d553c393f 100644 --- a/interface/src/scripting/HMDScriptingInterface.h +++ b/interface/src/scripting/HMDScriptingInterface.h @@ -72,7 +72,7 @@ private: glm::vec3 getPosition() const; // Set the position of the HMD - void setPosition(const glm::vec3& position); + Q_INVOKABLE void setPosition(const glm::vec3& position); // Get the orientation of the HMD glm::quat getOrientation() const; diff --git a/scripts/system/controllers/teleport.js b/scripts/system/controllers/teleport.js index 21aee70cc0..3d4bf244a5 100644 --- a/scripts/system/controllers/teleport.js +++ b/scripts/system/controllers/teleport.js @@ -564,7 +564,7 @@ function Teleporter() { } else if (this.teleportMode === "HMDFirstAvatarWillFollow") { var eyeOffset = Vec3.subtract(MyAvatar.getEyePosition(), MyAvatar.position); landingPoint = Vec3.sum(landingPoint, eyeOffset); - HMD.setPosition(landingPoint); + HMD.position = landingPoint; } } From 49cac31b6cad5edd5c92e286904a88467974e884 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Fri, 9 Sep 2016 17:33:02 -0700 Subject: [PATCH 008/101] Truncate IK Targets --- libraries/animation/src/Rig.cpp | 79 +++++++++++++++++++++++++++------ libraries/animation/src/Rig.h | 2 + 2 files changed, 67 insertions(+), 14 deletions(-) diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 150c96e18f..22486b5172 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -47,6 +47,10 @@ const glm::vec3 DEFAULT_LEFT_EYE_POS(0.3f, 0.9f, 0.0f); const glm::vec3 DEFAULT_HEAD_POS(0.0f, 0.75f, 0.0f); const glm::vec3 DEFAULT_NECK_POS(0.0f, 0.70f, 0.0f); +const glm::vec3 TRUNCATE_IK_CAPSULE_POSITION(0.0f, 0.0f, 0.0f); +float TRUNCATE_IK_CAPSULE_LENGTH = 1000.0; +float TRUNCATE_IK_CAPSULE_RADIUS = 0.5; + extern Rig* HACKY_GLOBAL_RIG_POINTER; void Rig::overrideAnimation(const QString& url, float fps, bool loop, float firstFrame, float lastFrame) { @@ -999,6 +1003,37 @@ void Rig::computeHeadNeckAnimVars(const AnimPose& hmdPose, glm::vec3& headPositi neckOrientationOut = safeMix(hmdOrientation, _animSkeleton->getRelativeDefaultPose(neckIndex).rot, 0.5f); } +static bool pointIsInsideCapsule(const glm::vec3& point, const glm::vec3& capsulePosition, float capsuleLength, float capsuleRadius) { + glm::vec3 top = capsulePosition.y + glm::vec3(0.0f, capsuleLength / 2.0f, 0.0f); + glm::vec3 bottom = capsulePosition.y - glm::vec3(0.0f, capsuleLength / 2.0f, 0.0f); + if (point.y > top.y + capsuleRadius) { + return false; + } else if (point.y > top.y) { + return glm::length(point - top) < capsuleRadius; + } else if (point.y < bottom.y - capsuleRadius) { + return false; + } else if (point.y < bottom.y) { + return glm::length(point - bottom) < capsuleRadius; + } else { + return glm::length(glm::vec2(point.x, point.z) - glm::vec2(capsulePosition.x, capsulePosition.z)) < capsuleRadius; + } +} + +static glm::vec3 projectPointOntoCapsule(const glm::vec3& point, const glm::vec3& capsulePosition, float capsuleLength, float capsuleRadius) { + glm::vec3 top = capsulePosition.y + glm::vec3(0.0f, capsuleLength / 2.0f, 0.0f); + glm::vec3 bottom = capsulePosition.y - glm::vec3(0.0f, capsuleLength / 2.0f, 0.0f); + if (point.y > top.y) { + return capsuleRadius * glm::normalize(point - top) + top; + } else if (point.y < bottom.y) { + return capsuleRadius * glm::normalize(point - bottom) + bottom; + } else { + glm::vec2 capsulePosition2D(capsulePosition.x, capsulePosition.z); + glm::vec2 point2D(point.x, point.z); + glm::vec2 projectedPoint2D = capsuleRadius * glm::normalize(point2D - capsulePosition2D) + capsulePosition2D; + return glm::vec3(projectedPoint2D.x, point.y, projectedPoint2D.y); + } +} + void Rig::updateNeckJoint(int index, const HeadParameters& params) { if (_animSkeleton && index >= 0 && index < _animSkeleton->getNumJoints()) { glm::quat yFlip180 = glm::angleAxis(PI, glm::vec3(0.0f, 1.0f, 0.0f)); @@ -1009,19 +1044,19 @@ void Rig::updateNeckJoint(int index, const HeadParameters& params) { AnimPose hmdPose(glm::vec3(1.0f), params.rigHeadOrientation * yFlip180, params.rigHeadPosition); computeHeadNeckAnimVars(hmdPose, headPos, headRot, neckPos, neckRot); - // debug rendering -#ifdef DEBUG_RENDERING - const glm::vec4 red(1.0f, 0.0f, 0.0f, 1.0f); - const glm::vec4 green(0.0f, 1.0f, 0.0f, 1.0f); + // decide if we SHOULD truncate IK targets + if (!pointIsInsideCapsule(params.rigHeadPosition, TRUNCATE_IK_CAPSULE_POSITION, TRUNCATE_IK_CAPSULE_LENGTH, TRUNCATE_IK_CAPSULE_RADIUS)) { + _desiredRigHeadPosition = headPos; + _truncateIKTargets = true; + } else { + _truncateIKTargets = false; + } - // transform from bone into avatar space - AnimPose headPose(glm::vec3(1), headRot, headPos); - DebugDraw::getInstance().addMyAvatarMarker("headTarget", headPose.rot, headPose.trans, red); - - // transform from bone into avatar space - AnimPose neckPose(glm::vec3(1), neckRot, neckPos); - DebugDraw::getInstance().addMyAvatarMarker("neckTarget", neckPose.rot, neckPose.trans, green); -#endif + // truncate head IK target. + if (_truncateIKTargets) { + headPos = projectPointOntoCapsule(_desiredRigHeadPosition, TRUNCATE_IK_CAPSULE_POSITION, TRUNCATE_IK_CAPSULE_LENGTH, TRUNCATE_IK_CAPSULE_RADIUS); + neckPos = (neckPos - _desiredRigHeadPosition) + headPos; + } _animVars.set("headPosition", headPos); _animVars.set("headRotation", headRot); @@ -1095,8 +1130,16 @@ void Rig::updateFromHandParameters(const HandParameters& params, float dt) { if (params.isLeftEnabled) { - // prevent the hand IK targets from intersecting the body capsule glm::vec3 handPosition = params.leftPosition; + + // truncate hand IK target + if (_truncateIKTargets) { + glm::vec3 offset = handPosition - _desiredRigHeadPosition; + glm::vec3 headPos = projectPointOntoCapsule(_desiredRigHeadPosition, TRUNCATE_IK_CAPSULE_POSITION, TRUNCATE_IK_CAPSULE_LENGTH, TRUNCATE_IK_CAPSULE_RADIUS); + handPosition = headPos + offset; + } + + // prevent the hand IK targets from intersecting the body capsule glm::vec3 displacement(glm::vec3::_null); if (findSphereCapsulePenetration(handPosition, HAND_RADIUS, bodyCapsuleStart, bodyCapsuleEnd, bodyCapsuleRadius, displacement)) { handPosition -= displacement; @@ -1113,8 +1156,16 @@ void Rig::updateFromHandParameters(const HandParameters& params, float dt) { if (params.isRightEnabled) { - // prevent the hand IK targets from intersecting the body capsule glm::vec3 handPosition = params.rightPosition; + + // truncate hand IK target + if (_truncateIKTargets) { + glm::vec3 offset = handPosition - _desiredRigHeadPosition; + glm::vec3 headPos = projectPointOntoCapsule(_desiredRigHeadPosition, TRUNCATE_IK_CAPSULE_POSITION, TRUNCATE_IK_CAPSULE_LENGTH, TRUNCATE_IK_CAPSULE_RADIUS); + handPosition = headPos + offset; + } + + // prevent the hand IK targets from intersecting the body capsule glm::vec3 displacement(glm::vec3::_null); if (findSphereCapsulePenetration(handPosition, HAND_RADIUS, bodyCapsuleStart, bodyCapsuleEnd, bodyCapsuleRadius, displacement)) { handPosition -= displacement; diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index 86b8dadd85..7eb0316889 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -312,6 +312,8 @@ protected: bool _enableInverseKinematics { true }; mutable uint32_t _jointNameWarningCount { 0 }; + glm::vec3 _desiredRigHeadPosition; + bool _truncateIKTargets { false }; private: QMap _stateHandlers; From 28d1faa1c0f83492d0a914eeb7abf030ff3738f6 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 12 Sep 2016 09:10:11 -0700 Subject: [PATCH 009/101] remove unused variable --- libraries/animation/src/AnimInverseKinematics.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 42eafa80fd..93ba3bd689 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -444,7 +444,6 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars // AJT: HACK if (HACKY_GLOBAL_ENABLE_DEBUG_DRAW_IK_TARGETS && HACKY_GLOBAL_RIG_POINTER) { - const float CM_TO_M = 0.01f; const vec4 WHITE(1.0f); glm::mat4 geomToRigMat = HACKY_GLOBAL_RIG_POINTER->getGeometryToRigTransform(); glm::mat4 rigToAvatarMat = createMatFromQuatAndPos(Quaternions::Y_180, glm::vec3()); From 5e0c2286ec38bb3b0b7f7b14f52bf0d785e67771 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 12 Sep 2016 10:58:06 -0700 Subject: [PATCH 010/101] Use OUTOFBODY_HACK as searchable token --- interface/src/avatar/MyAvatar.cpp | 4 ++-- libraries/animation/src/AnimInverseKinematics.cpp | 9 ++++----- libraries/animation/src/Rig.cpp | 6 +++--- 3 files changed, 9 insertions(+), 10 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index dc0a009ef7..53f37635f3 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -82,7 +82,7 @@ const float MyAvatar::ZOOM_MIN = 0.5f; const float MyAvatar::ZOOM_MAX = 25.0f; const float MyAvatar::ZOOM_DEFAULT = 1.5f; -extern bool HACKY_GLOBAL_ENABLE_DEBUG_DRAW_IK_TARGETS; +extern bool OUTOFBODY_HACK_ENABLE_DEBUG_DRAW_IK_TARGETS; MyAvatar::MyAvatar(RigPointer rig) : Avatar(rig), @@ -839,7 +839,7 @@ void MyAvatar::setEnableDebugDrawSensorToWorldMatrix(bool isEnabled) { void MyAvatar::setEnableDebugDrawIKTargets(bool isEnabled) { _enableDebugDrawIKTargets = isEnabled; - HACKY_GLOBAL_ENABLE_DEBUG_DRAW_IK_TARGETS = isEnabled; + OUTOFBODY_HACK_ENABLE_DEBUG_DRAW_IK_TARGETS = isEnabled; } diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 93ba3bd689..27300699c4 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -21,8 +21,8 @@ #include "SwingTwistConstraint.h" #include "AnimationLogging.h" -bool HACKY_GLOBAL_ENABLE_DEBUG_DRAW_IK_TARGETS = false; -Rig* HACKY_GLOBAL_RIG_POINTER = nullptr; +bool OUTOFBODY_HACK_ENABLE_DEBUG_DRAW_IK_TARGETS = false; +Rig* OUTOFBODY_HACK_RIG_POINTER = nullptr; AnimInverseKinematics::AnimInverseKinematics(const QString& id) : AnimNode(AnimNode::Type::InverseKinematics, id) { } @@ -442,10 +442,9 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars computeTargets(animVars, targets, underPoses); } - // AJT: HACK - if (HACKY_GLOBAL_ENABLE_DEBUG_DRAW_IK_TARGETS && HACKY_GLOBAL_RIG_POINTER) { + if (OUTOFBODY_HACK_ENABLE_DEBUG_DRAW_IK_TARGETS && OUTOFBODY_HACK_RIG_POINTER) { const vec4 WHITE(1.0f); - glm::mat4 geomToRigMat = HACKY_GLOBAL_RIG_POINTER->getGeometryToRigTransform(); + glm::mat4 geomToRigMat = OUTOFBODY_HACK_RIG_POINTER->getGeometryToRigTransform(); glm::mat4 rigToAvatarMat = createMatFromQuatAndPos(Quaternions::Y_180, glm::vec3()); for (auto& target : targets) { diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 22486b5172..f0c7f85011 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -51,7 +51,7 @@ const glm::vec3 TRUNCATE_IK_CAPSULE_POSITION(0.0f, 0.0f, 0.0f); float TRUNCATE_IK_CAPSULE_LENGTH = 1000.0; float TRUNCATE_IK_CAPSULE_RADIUS = 0.5; -extern Rig* HACKY_GLOBAL_RIG_POINTER; +extern Rig* OUTOFBODY_HACK_RIG_POINTER; void Rig::overrideAnimation(const QString& url, float fps, bool loop, float firstFrame, float lastFrame) { @@ -881,7 +881,7 @@ void Rig::updateAnimations(float deltaTime, glm::mat4 rootTransform) { _animVars.setRigToGeometryTransform(_rigToGeometryTransform); // evaluate the animation - HACKY_GLOBAL_RIG_POINTER = this; + OUTOFBODY_HACK_RIG_POINTER = this; AnimNode::Triggers triggersOut; _internalPoseSet._relativePoses = _animNode->evaluate(_animVars, deltaTime, triggersOut); if ((int)_internalPoseSet._relativePoses.size() != _animSkeleton->getNumJoints()) { @@ -892,7 +892,7 @@ void Rig::updateAnimations(float deltaTime, glm::mat4 rootTransform) { for (auto& trigger : triggersOut) { _animVars.setTrigger(trigger); } - HACKY_GLOBAL_RIG_POINTER = nullptr; + OUTOFBODY_HACK_RIG_POINTER = nullptr; } applyOverridePoses(); From baee5180c1a74e5825565beaf99282d9eb9758a3 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 12 Sep 2016 11:52:27 -0700 Subject: [PATCH 011/101] Moved outOfBody detection into MyAvatar::FollowHelper in preparation for different follow behavior for in-body and out-of-body. --- interface/src/avatar/MyAvatar.cpp | 21 +++++++++ interface/src/avatar/MyAvatar.h | 6 ++- interface/src/avatar/SkeletonModel.cpp | 33 +++++++++++++- libraries/animation/src/Rig.cpp | 63 -------------------------- libraries/shared/src/GeometryUtil.cpp | 31 +++++++++++++ libraries/shared/src/GeometryUtil.h | 3 ++ 6 files changed, 91 insertions(+), 66 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 53f37635f3..94b6c2f93c 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -82,8 +82,14 @@ const float MyAvatar::ZOOM_MIN = 0.5f; const float MyAvatar::ZOOM_MAX = 25.0f; const float MyAvatar::ZOOM_DEFAULT = 1.5f; +// OUTOFBODY_HACK defined in Rig.cpp extern bool OUTOFBODY_HACK_ENABLE_DEBUG_DRAW_IK_TARGETS; +// OUTOFBODY_HACK defined in SkeletonModel.cpp +extern const glm::vec3 TRUNCATE_IK_CAPSULE_POSITION; +extern const float TRUNCATE_IK_CAPSULE_LENGTH; +extern const float TRUNCATE_IK_CAPSULE_RADIUS; + MyAvatar::MyAvatar(RigPointer rig) : Avatar(rig), _wasPushing(false), @@ -1338,6 +1344,8 @@ void MyAvatar::harvestResultsFromPhysicsSimulation(float deltaTime) { } else { setVelocity(getVelocity() + _characterController.getFollowVelocity()); } + + _follow.postPhysicsUpdate(*this); } QString MyAvatar::getScriptedMotorFrame() const { @@ -2183,6 +2191,19 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat } } +void MyAvatar::FollowHelper::postPhysicsUpdate(MyAvatar& myAvatar) { + + // get HMD position from sensor space into world space, and back into rig space + glm::mat4 worldHMDMat = myAvatar.getSensorToWorldMatrix() * myAvatar.getHMDSensorMatrix(); + glm::mat4 rigToWorld = createMatFromQuatAndPos(myAvatar.getRotation() * Quaternions::Y_180, myAvatar.getPosition()); + glm::mat4 worldToRig = glm::inverse(rigToWorld); + glm::mat4 rigHMDMat = worldToRig * worldHMDMat; + glm::vec3 rigHMDPosition = extractTranslation(rigHMDMat); + + // detect if the rig head position is too far from the avatar's position. + _isOutOfBody = !pointIsInsideCapsule(rigHMDPosition, TRUNCATE_IK_CAPSULE_POSITION, TRUNCATE_IK_CAPSULE_LENGTH, TRUNCATE_IK_CAPSULE_RADIUS); +} + float MyAvatar::getAccelerationEnergy() { glm::vec3 velocity = getVelocity(); int changeInVelocity = abs(velocity.length() - priorVelocity.length()); diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 65d0de3801..f6833c6f33 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -315,6 +315,8 @@ public slots: glm::vec3 getPositionForAudio(); glm::quat getOrientationForAudio(); + bool isOutOfBody() const { return _follow._isOutOfBody; } + signals: void audioListenerModeChanged(); void transformChanged(); @@ -445,7 +447,8 @@ private: NumFollowTypes }; glm::mat4 _desiredBodyMatrix; - uint8_t _activeBits; + uint8_t _activeBits { 0 }; + bool _isOutOfBody { false }; void deactivate(); void deactivate(FollowType type); @@ -457,6 +460,7 @@ private: void updateHorizontalActivation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix); void updateVerticalActivation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix); void prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat4& bodySensorMatrix, const glm::mat4& currentBodyMatrix); + void postPhysicsUpdate(MyAvatar& myAvatar); }; FollowHelper _follow; diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index 889f0ef36b..be1d071f6b 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -23,6 +23,10 @@ #include "InterfaceLogging.h" #include "AnimDebugDraw.h" +const glm::vec3 TRUNCATE_IK_CAPSULE_POSITION(0.0f, 0.0f, 0.0f); +const float TRUNCATE_IK_CAPSULE_LENGTH = 1000.0f; +const float TRUNCATE_IK_CAPSULE_RADIUS = 0.5f; + SkeletonModel::SkeletonModel(Avatar* owningAvatar, QObject* parent, RigPointer rig) : Model(rig, parent), _owningAvatar(owningAvatar), @@ -86,7 +90,6 @@ Rig::CharacterControllerState convertCharacterControllerState(CharacterControlle }; } - // Called within Model::simulate call, below. void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { const FBXGeometry& geometry = getFBXGeometry(); @@ -107,6 +110,9 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { Rig::HeadParameters headParams; + glm::vec3 hmdPositionInRigSpace; + glm::vec3 truncatedHMDPositionInRigSpace; + if (qApp->isHMDMode()) { headParams.isInHMD = true; @@ -116,9 +122,20 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { glm::mat4 worldToRig = glm::inverse(rigToWorld); glm::mat4 rigHMDMat = worldToRig * worldHMDMat; - headParams.rigHeadPosition = extractTranslation(rigHMDMat); + hmdPositionInRigSpace = extractTranslation(rigHMDMat); + + // truncate head IK target if it's out of body + if (myAvatar->isOutOfBody()) { + truncatedHMDPositionInRigSpace = projectPointOntoCapsule(hmdPositionInRigSpace, TRUNCATE_IK_CAPSULE_POSITION, + TRUNCATE_IK_CAPSULE_LENGTH, TRUNCATE_IK_CAPSULE_RADIUS); + } else { + truncatedHMDPositionInRigSpace = hmdPositionInRigSpace; + } + + headParams.rigHeadPosition = truncatedHMDPositionInRigSpace; headParams.rigHeadOrientation = extractRotation(rigHMDMat); headParams.worldHeadOrientation = extractRotation(worldHMDMat); + } else { headParams.isInHMD = false; @@ -139,6 +156,12 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { handParams.isLeftEnabled = true; handParams.leftPosition = Quaternions::Y_180 * leftPose.getTranslation(); handParams.leftOrientation = Quaternions::Y_180 * leftPose.getRotation(); + + // truncate hand target + if (myAvatar->isOutOfBody() && qApp->isHMDMode()) { + glm::vec3 offset = handParams.leftPosition - hmdPositionInRigSpace; + handParams.leftPosition = truncatedHMDPositionInRigSpace + offset; + } } else { handParams.isLeftEnabled = false; } @@ -148,6 +171,12 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { handParams.isRightEnabled = true; handParams.rightPosition = Quaternions::Y_180 * rightPose.getTranslation(); handParams.rightOrientation = Quaternions::Y_180 * rightPose.getRotation(); + + // truncate hand target + if (myAvatar->isOutOfBody() && qApp->isHMDMode()) { + glm::vec3 offset = handParams.rightPosition - hmdPositionInRigSpace; + handParams.rightPosition = truncatedHMDPositionInRigSpace + offset; + } } else { handParams.isRightEnabled = false; } diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index f0c7f85011..2d11a1e17f 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -47,10 +47,6 @@ const glm::vec3 DEFAULT_LEFT_EYE_POS(0.3f, 0.9f, 0.0f); const glm::vec3 DEFAULT_HEAD_POS(0.0f, 0.75f, 0.0f); const glm::vec3 DEFAULT_NECK_POS(0.0f, 0.70f, 0.0f); -const glm::vec3 TRUNCATE_IK_CAPSULE_POSITION(0.0f, 0.0f, 0.0f); -float TRUNCATE_IK_CAPSULE_LENGTH = 1000.0; -float TRUNCATE_IK_CAPSULE_RADIUS = 0.5; - extern Rig* OUTOFBODY_HACK_RIG_POINTER; void Rig::overrideAnimation(const QString& url, float fps, bool loop, float firstFrame, float lastFrame) { @@ -1003,37 +999,6 @@ void Rig::computeHeadNeckAnimVars(const AnimPose& hmdPose, glm::vec3& headPositi neckOrientationOut = safeMix(hmdOrientation, _animSkeleton->getRelativeDefaultPose(neckIndex).rot, 0.5f); } -static bool pointIsInsideCapsule(const glm::vec3& point, const glm::vec3& capsulePosition, float capsuleLength, float capsuleRadius) { - glm::vec3 top = capsulePosition.y + glm::vec3(0.0f, capsuleLength / 2.0f, 0.0f); - glm::vec3 bottom = capsulePosition.y - glm::vec3(0.0f, capsuleLength / 2.0f, 0.0f); - if (point.y > top.y + capsuleRadius) { - return false; - } else if (point.y > top.y) { - return glm::length(point - top) < capsuleRadius; - } else if (point.y < bottom.y - capsuleRadius) { - return false; - } else if (point.y < bottom.y) { - return glm::length(point - bottom) < capsuleRadius; - } else { - return glm::length(glm::vec2(point.x, point.z) - glm::vec2(capsulePosition.x, capsulePosition.z)) < capsuleRadius; - } -} - -static glm::vec3 projectPointOntoCapsule(const glm::vec3& point, const glm::vec3& capsulePosition, float capsuleLength, float capsuleRadius) { - glm::vec3 top = capsulePosition.y + glm::vec3(0.0f, capsuleLength / 2.0f, 0.0f); - glm::vec3 bottom = capsulePosition.y - glm::vec3(0.0f, capsuleLength / 2.0f, 0.0f); - if (point.y > top.y) { - return capsuleRadius * glm::normalize(point - top) + top; - } else if (point.y < bottom.y) { - return capsuleRadius * glm::normalize(point - bottom) + bottom; - } else { - glm::vec2 capsulePosition2D(capsulePosition.x, capsulePosition.z); - glm::vec2 point2D(point.x, point.z); - glm::vec2 projectedPoint2D = capsuleRadius * glm::normalize(point2D - capsulePosition2D) + capsulePosition2D; - return glm::vec3(projectedPoint2D.x, point.y, projectedPoint2D.y); - } -} - void Rig::updateNeckJoint(int index, const HeadParameters& params) { if (_animSkeleton && index >= 0 && index < _animSkeleton->getNumJoints()) { glm::quat yFlip180 = glm::angleAxis(PI, glm::vec3(0.0f, 1.0f, 0.0f)); @@ -1044,20 +1009,6 @@ void Rig::updateNeckJoint(int index, const HeadParameters& params) { AnimPose hmdPose(glm::vec3(1.0f), params.rigHeadOrientation * yFlip180, params.rigHeadPosition); computeHeadNeckAnimVars(hmdPose, headPos, headRot, neckPos, neckRot); - // decide if we SHOULD truncate IK targets - if (!pointIsInsideCapsule(params.rigHeadPosition, TRUNCATE_IK_CAPSULE_POSITION, TRUNCATE_IK_CAPSULE_LENGTH, TRUNCATE_IK_CAPSULE_RADIUS)) { - _desiredRigHeadPosition = headPos; - _truncateIKTargets = true; - } else { - _truncateIKTargets = false; - } - - // truncate head IK target. - if (_truncateIKTargets) { - headPos = projectPointOntoCapsule(_desiredRigHeadPosition, TRUNCATE_IK_CAPSULE_POSITION, TRUNCATE_IK_CAPSULE_LENGTH, TRUNCATE_IK_CAPSULE_RADIUS); - neckPos = (neckPos - _desiredRigHeadPosition) + headPos; - } - _animVars.set("headPosition", headPos); _animVars.set("headRotation", headRot); _animVars.set("headType", (int)IKTarget::Type::HmdHead); @@ -1132,13 +1083,6 @@ void Rig::updateFromHandParameters(const HandParameters& params, float dt) { glm::vec3 handPosition = params.leftPosition; - // truncate hand IK target - if (_truncateIKTargets) { - glm::vec3 offset = handPosition - _desiredRigHeadPosition; - glm::vec3 headPos = projectPointOntoCapsule(_desiredRigHeadPosition, TRUNCATE_IK_CAPSULE_POSITION, TRUNCATE_IK_CAPSULE_LENGTH, TRUNCATE_IK_CAPSULE_RADIUS); - handPosition = headPos + offset; - } - // prevent the hand IK targets from intersecting the body capsule glm::vec3 displacement(glm::vec3::_null); if (findSphereCapsulePenetration(handPosition, HAND_RADIUS, bodyCapsuleStart, bodyCapsuleEnd, bodyCapsuleRadius, displacement)) { @@ -1158,13 +1102,6 @@ void Rig::updateFromHandParameters(const HandParameters& params, float dt) { glm::vec3 handPosition = params.rightPosition; - // truncate hand IK target - if (_truncateIKTargets) { - glm::vec3 offset = handPosition - _desiredRigHeadPosition; - glm::vec3 headPos = projectPointOntoCapsule(_desiredRigHeadPosition, TRUNCATE_IK_CAPSULE_POSITION, TRUNCATE_IK_CAPSULE_LENGTH, TRUNCATE_IK_CAPSULE_RADIUS); - handPosition = headPos + offset; - } - // prevent the hand IK targets from intersecting the body capsule glm::vec3 displacement(glm::vec3::_null); if (findSphereCapsulePenetration(handPosition, HAND_RADIUS, bodyCapsuleStart, bodyCapsuleEnd, bodyCapsuleRadius, displacement)) { diff --git a/libraries/shared/src/GeometryUtil.cpp b/libraries/shared/src/GeometryUtil.cpp index 92fe138021..bc4dc7cb9a 100644 --- a/libraries/shared/src/GeometryUtil.cpp +++ b/libraries/shared/src/GeometryUtil.cpp @@ -578,3 +578,34 @@ float coneSphereAngle(const glm::vec3& coneCenter, const glm::vec3& coneDirectio return glm::max(0.0f, theta - phi); } + +bool pointIsInsideCapsule(const glm::vec3& point, const glm::vec3& capsulePosition, float capsuleLength, float capsuleRadius) { + glm::vec3 top = capsulePosition.y + glm::vec3(0.0f, capsuleLength / 2.0f, 0.0f); + glm::vec3 bottom = capsulePosition.y - glm::vec3(0.0f, capsuleLength / 2.0f, 0.0f); + if (point.y > top.y + capsuleRadius) { + return false; + } else if (point.y > top.y) { + return glm::length(point - top) < capsuleRadius; + } else if (point.y < bottom.y - capsuleRadius) { + return false; + } else if (point.y < bottom.y) { + return glm::length(point - bottom) < capsuleRadius; + } else { + return glm::length(glm::vec2(point.x, point.z) - glm::vec2(capsulePosition.x, capsulePosition.z)) < capsuleRadius; + } +} + +glm::vec3 projectPointOntoCapsule(const glm::vec3& point, const glm::vec3& capsulePosition, float capsuleLength, float capsuleRadius) { + glm::vec3 top = capsulePosition.y + glm::vec3(0.0f, capsuleLength / 2.0f, 0.0f); + glm::vec3 bottom = capsulePosition.y - glm::vec3(0.0f, capsuleLength / 2.0f, 0.0f); + if (point.y > top.y) { + return capsuleRadius * glm::normalize(point - top) + top; + } else if (point.y < bottom.y) { + return capsuleRadius * glm::normalize(point - bottom) + bottom; + } else { + glm::vec2 capsulePosition2D(capsulePosition.x, capsulePosition.z); + glm::vec2 point2D(point.x, point.z); + glm::vec2 projectedPoint2D = capsuleRadius * glm::normalize(point2D - capsulePosition2D) + capsulePosition2D; + return glm::vec3(projectedPoint2D.x, point.y, projectedPoint2D.y); + } +} diff --git a/libraries/shared/src/GeometryUtil.h b/libraries/shared/src/GeometryUtil.h index 1c951ca09a..01459e51a9 100644 --- a/libraries/shared/src/GeometryUtil.h +++ b/libraries/shared/src/GeometryUtil.h @@ -160,5 +160,8 @@ private: static void copyCleanArray(int& lengthA, glm::vec2* vertexArrayA, int& lengthB, glm::vec2* vertexArrayB); }; +// vertical capsule +bool pointIsInsideCapsule(const glm::vec3& point, const glm::vec3& capsulePosition, float capsuleLength, float capsuleRadius); +glm::vec3 projectPointOntoCapsule(const glm::vec3& point, const glm::vec3& capsulePosition, float capsuleLength, float capsuleRadius); #endif // hifi_GeometryUtil_h From ffb1bae1f08ac2edbbd9fe9ade1c8d39ac68f295 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 12 Sep 2016 16:20:13 -0700 Subject: [PATCH 012/101] When out-of-body have your avatar face toward your HMD. --- interface/src/avatar/MyAvatar.cpp | 74 +++++++++++++++++++------- interface/src/avatar/MyAvatar.h | 7 ++- interface/src/avatar/SkeletonModel.cpp | 40 ++++++++------ 3 files changed, 82 insertions(+), 39 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 94b6c2f93c..c37fb0d972 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -86,9 +86,9 @@ const float MyAvatar::ZOOM_DEFAULT = 1.5f; extern bool OUTOFBODY_HACK_ENABLE_DEBUG_DRAW_IK_TARGETS; // OUTOFBODY_HACK defined in SkeletonModel.cpp -extern const glm::vec3 TRUNCATE_IK_CAPSULE_POSITION; -extern const float TRUNCATE_IK_CAPSULE_LENGTH; -extern const float TRUNCATE_IK_CAPSULE_RADIUS; +extern glm::vec3 TRUNCATE_IK_CAPSULE_POSITION; +extern float TRUNCATE_IK_CAPSULE_LENGTH; +extern float TRUNCATE_IK_CAPSULE_RADIUS; MyAvatar::MyAvatar(RigPointer rig) : Avatar(rig), @@ -2159,31 +2159,65 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat _desiredBodyMatrix = desiredBodyMatrix; if (myAvatar.getHMDLeanRecenterEnabled()) { - updateRotationActivation(myAvatar, desiredBodyMatrix, currentBodyMatrix); - updateHorizontalActivation(myAvatar, desiredBodyMatrix, currentBodyMatrix); - updateVerticalActivation(myAvatar, desiredBodyMatrix, currentBodyMatrix); - glm::mat4 desiredWorldMatrix = myAvatar.getSensorToWorldMatrix() * _desiredBodyMatrix; - glm::mat4 currentWorldMatrix = createMatFromQuatAndPos(myAvatar.getOrientation(), myAvatar.getPosition()); + if (_isOutOfBody) { - AnimPose followWorldPose(currentWorldMatrix); - if (isActive(Rotation)) { - followWorldPose.rot = glmExtractRotation(desiredWorldMatrix); - } - if (isActive(Horizontal)) { + glm::mat4 desiredWorldMatrix = myAvatar.getSensorToWorldMatrix() * _desiredBodyMatrix; + glm::mat4 currentWorldMatrix = createMatFromQuatAndPos(myAvatar.getOrientation(), myAvatar.getPosition()); + AnimPose followWorldPose(currentWorldMatrix); + + // OUTOFBODY_HACK, only takes horizontal movement into account. + + // horizontal follow glm::vec3 desiredTranslation = extractTranslation(desiredWorldMatrix); followWorldPose.trans.x = desiredTranslation.x; followWorldPose.trans.z = desiredTranslation.z; - } - if (isActive(Vertical)) { - glm::vec3 desiredTranslation = extractTranslation(desiredWorldMatrix); - followWorldPose.trans.y = desiredTranslation.y; - } - if (isActive()) { + // rotation follow + // face the HMD + glm::vec3 hmdWorldPosition = extractTranslation(myAvatar.getSensorToWorldMatrix() * myAvatar.getHMDSensorMatrix()); + glm::vec3 facing = myAvatar.getPosition() - hmdWorldPosition; + facing.y = 0.0f; + if (glm::length(facing) > EPSILON) { + // turn to face the hmd + followWorldPose.rot = glm::angleAxis(atan2(facing.x, facing.z), Vectors::UNIT_Y); + } else { + followWorldPose.rot = glmExtractRotation(desiredWorldMatrix); + } + myAvatar.getCharacterController()->setFollowParameters(followWorldPose); + } else { - myAvatar.getCharacterController()->disableFollow(); + updateRotationActivation(myAvatar, desiredBodyMatrix, currentBodyMatrix); + updateHorizontalActivation(myAvatar, desiredBodyMatrix, currentBodyMatrix); + updateVerticalActivation(myAvatar, desiredBodyMatrix, currentBodyMatrix); + + glm::mat4 desiredWorldMatrix = myAvatar.getSensorToWorldMatrix() * _desiredBodyMatrix; + glm::mat4 currentWorldMatrix = createMatFromQuatAndPos(myAvatar.getOrientation(), myAvatar.getPosition()); + + AnimPose followWorldPose(currentWorldMatrix); + if (isActive(Rotation)) { + followWorldPose.rot = glmExtractRotation(desiredWorldMatrix); + } + if (isActive(Horizontal)) { + glm::vec3 desiredTranslation = extractTranslation(desiredWorldMatrix); + followWorldPose.trans.x = desiredTranslation.x; + followWorldPose.trans.z = desiredTranslation.z; + } + if (isActive(Vertical)) { + glm::vec3 desiredTranslation = extractTranslation(desiredWorldMatrix); + followWorldPose.trans.y = desiredTranslation.y; + } + + if (isActive()) { + myAvatar.getCharacterController()->setFollowParameters(followWorldPose); + } else { + myAvatar.getCharacterController()->disableFollow(); + } + + glm::mat4 currentWorldMatrixY180 = createMatFromQuatAndPos(myAvatar.getOrientation() * Quaternions::Y_180, myAvatar.getPosition()); + _prevInBodyHMDMatInAvatarSpace = _inBodyHMDMatInAvatarSpace; + _inBodyHMDMatInAvatarSpace = glm::inverse(currentWorldMatrixY180) * myAvatar.getSensorToWorldMatrix() * myAvatar.getHMDSensorMatrix(); } } else { deactivate(); diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index f6833c6f33..85a2b72939 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -279,6 +279,9 @@ public: virtual glm::quat getAbsoluteJointRotationInObjectFrame(int index) const override; virtual glm::vec3 getAbsoluteJointTranslationInObjectFrame(int index) const override; + bool isOutOfBody() const { return _follow._isOutOfBody; } + glm::mat4 getInBodyHMDMatInAvatarSpace() const { return _follow._prevInBodyHMDMatInAvatarSpace; } + public slots: void increaseSize(); void decreaseSize(); @@ -315,8 +318,6 @@ public slots: glm::vec3 getPositionForAudio(); glm::quat getOrientationForAudio(); - bool isOutOfBody() const { return _follow._isOutOfBody; } - signals: void audioListenerModeChanged(); void transformChanged(); @@ -449,6 +450,8 @@ private: glm::mat4 _desiredBodyMatrix; uint8_t _activeBits { 0 }; bool _isOutOfBody { false }; + glm::mat4 _prevInBodyHMDMatInAvatarSpace; + glm::mat4 _inBodyHMDMatInAvatarSpace; void deactivate(); void deactivate(FollowType type); diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index be1d071f6b..2bbda480e0 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -23,9 +23,9 @@ #include "InterfaceLogging.h" #include "AnimDebugDraw.h" -const glm::vec3 TRUNCATE_IK_CAPSULE_POSITION(0.0f, 0.0f, 0.0f); -const float TRUNCATE_IK_CAPSULE_LENGTH = 1000.0f; -const float TRUNCATE_IK_CAPSULE_RADIUS = 0.5f; +glm::vec3 TRUNCATE_IK_CAPSULE_POSITION(0.0f, 0.0f, 0.0f); +float TRUNCATE_IK_CAPSULE_LENGTH = 1000.0f; +float TRUNCATE_IK_CAPSULE_RADIUS = 0.5f; SkeletonModel::SkeletonModel(Avatar* owningAvatar, QObject* parent, RigPointer rig) : Model(rig, parent), @@ -110,8 +110,8 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { Rig::HeadParameters headParams; - glm::vec3 hmdPositionInRigSpace; - glm::vec3 truncatedHMDPositionInRigSpace; + glm::mat4 desiredHMDMat; // rig space + glm::mat4 actualHMDMat; // rig space if (qApp->isHMDMode()) { headParams.isInHMD = true; @@ -120,20 +120,20 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { glm::mat4 worldHMDMat = myAvatar->getSensorToWorldMatrix() * myAvatar->getHMDSensorMatrix(); glm::mat4 rigToWorld = createMatFromQuatAndPos(getRotation(), getTranslation()); glm::mat4 worldToRig = glm::inverse(rigToWorld); - glm::mat4 rigHMDMat = worldToRig * worldHMDMat; + desiredHMDMat = worldToRig * worldHMDMat; - hmdPositionInRigSpace = extractTranslation(rigHMDMat); - // truncate head IK target if it's out of body if (myAvatar->isOutOfBody()) { - truncatedHMDPositionInRigSpace = projectPointOntoCapsule(hmdPositionInRigSpace, TRUNCATE_IK_CAPSULE_POSITION, - TRUNCATE_IK_CAPSULE_LENGTH, TRUNCATE_IK_CAPSULE_RADIUS); + // use the last valid in-body hmd matrix for the head yaw. + glm::mat4 hmdYaw = cancelOutRollAndPitch(myAvatar->getInBodyHMDMatInAvatarSpace()); + glm::mat4 hmdPitchAndRoll = glm::inverse(cancelOutRollAndPitch(desiredHMDMat)) * desiredHMDMat; + actualHMDMat = hmdYaw * hmdPitchAndRoll; } else { - truncatedHMDPositionInRigSpace = hmdPositionInRigSpace; + actualHMDMat = desiredHMDMat; } - headParams.rigHeadPosition = truncatedHMDPositionInRigSpace; - headParams.rigHeadOrientation = extractRotation(rigHMDMat); + headParams.rigHeadPosition = extractTranslation(actualHMDMat); + headParams.rigHeadOrientation = extractRotation(actualHMDMat); headParams.worldHeadOrientation = extractRotation(worldHMDMat); } else { @@ -159,8 +159,11 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { // truncate hand target if (myAvatar->isOutOfBody() && qApp->isHMDMode()) { - glm::vec3 offset = handParams.leftPosition - hmdPositionInRigSpace; - handParams.leftPosition = truncatedHMDPositionInRigSpace + offset; + glm::mat4 handMat = createMatFromQuatAndPos(handParams.leftOrientation, handParams.leftPosition); + glm::mat4 offset = glm::inverse(desiredHMDMat) * handMat; + handMat = actualHMDMat * offset; + handParams.leftPosition = extractTranslation(handMat); + handParams.leftOrientation = glmExtractRotation(handMat); } } else { handParams.isLeftEnabled = false; @@ -174,8 +177,11 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { // truncate hand target if (myAvatar->isOutOfBody() && qApp->isHMDMode()) { - glm::vec3 offset = handParams.rightPosition - hmdPositionInRigSpace; - handParams.rightPosition = truncatedHMDPositionInRigSpace + offset; + glm::mat4 handMat = createMatFromQuatAndPos(handParams.rightOrientation, handParams.rightPosition); + glm::mat4 offset = glm::inverse(desiredHMDMat) * handMat; + handMat = actualHMDMat * offset; + handParams.rightPosition = extractTranslation(handMat); + handParams.rightOrientation = glmExtractRotation(handMat); } } else { handParams.isRightEnabled = false; From 954d690b8a22523f1b4362c0147916c6b9356b20 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 12 Sep 2016 13:12:06 -0700 Subject: [PATCH 013/101] expose setting avatar collisionless --- interface/src/Application.cpp | 7 - interface/src/Menu.cpp | 2 +- interface/src/Menu.h | 2 +- interface/src/avatar/MyAvatar.cpp | 26 ++- interface/src/avatar/MyAvatar.h | 6 +- libraries/physics/src/CharacterController.cpp | 180 ++++++++++-------- libraries/physics/src/CharacterController.h | 13 +- 7 files changed, 127 insertions(+), 109 deletions(-) diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index 8620b384ec..03fdd52585 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -3667,13 +3667,6 @@ void Application::update(float deltaTime) { if (nearbyEntitiesAreReadyForPhysics()) { _physicsEnabled = true; getMyAvatar()->updateMotionBehaviorFromMenu(); - } else { - auto characterController = getMyAvatar()->getCharacterController(); - if (characterController) { - // if we have a character controller, disable it here so the avatar doesn't get stuck due to - // a non-loading collision hull. - characterController->setEnabled(false); - } } } } diff --git a/interface/src/Menu.cpp b/interface/src/Menu.cpp index f58a593395..84d1c2cac6 100644 --- a/interface/src/Menu.cpp +++ b/interface/src/Menu.cpp @@ -502,7 +502,7 @@ Menu::Menu() { avatar, SLOT(updateMotionBehaviorFromMenu()), UNSPECIFIED_POSITION, "Developer"); - addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::EnableCharacterController, 0, true, + addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::EnableAvatarCollisions, 0, true, avatar, SLOT(updateMotionBehaviorFromMenu()), UNSPECIFIED_POSITION, "Developer"); diff --git a/interface/src/Menu.h b/interface/src/Menu.h index 9b916467d0..5e213f3974 100644 --- a/interface/src/Menu.h +++ b/interface/src/Menu.h @@ -97,7 +97,7 @@ namespace MenuOption { const QString DontRenderEntitiesAsScene = "Don't Render Entities as Scene"; const QString EchoLocalAudio = "Echo Local Audio"; const QString EchoServerAudio = "Echo Server Audio"; - const QString EnableCharacterController = "Enable avatar collisions"; + const QString EnableAvatarCollisions = "Enable Avatar Collisions"; const QString EnableInverseKinematics = "Enable Inverse Kinematics"; const QString ExpandMyAvatarSimulateTiming = "Expand /myAvatar/simulation"; const QString ExpandMyAvatarTiming = "Expand /myAvatar"; diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index c37fb0d972..df9e07ee78 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -137,8 +137,6 @@ MyAvatar::MyAvatar(RigPointer rig) : connect(DependencyManager::get().data(), &AddressManager::locationChangeRequired, this, static_cast(&MyAvatar::goToLocation)); - _characterController.setEnabled(true); - _bodySensorMatrix = deriveBodyFromHMDSensor(); using namespace recording; @@ -506,8 +504,8 @@ void MyAvatar::simulate(float deltaTime) { } }); _characterController.setFlyingAllowed(flyingAllowed); - if (!_characterController.isEnabled() && !ghostingAllowed) { - _characterController.setEnabled(true); + if (!ghostingAllowed && _characterController.getCollisionGroup() == BULLET_COLLISION_GROUP_COLLISIONLESS) { + _characterController.setCollisionGroup(BULLET_COLLISION_GROUP_MY_AVATAR); } } @@ -1270,7 +1268,8 @@ void MyAvatar::updateMotors() { _characterController.clearMotors(); glm::quat motorRotation; if (_motionBehaviors & AVATAR_MOTION_ACTION_MOTOR_ENABLED) { - if (_characterController.getState() == CharacterController::State::Hover) { + if (_characterController.getState() == CharacterController::State::Hover || + _characterController.getCollisionGroup() == BULLET_COLLISION_GROUP_COLLISIONLESS) { motorRotation = getHead()->getCameraOrientation(); } else { // non-hovering = walking: follow camera twist about vertical but not lift @@ -1316,6 +1315,7 @@ void MyAvatar::prepareForPhysicsSimulation() { qDebug() << "Warning: getParentVelocity failed" << getID(); parentVelocity = glm::vec3(); } + _characterController.handleChangedCollisionGroup(); _characterController.setParentVelocity(parentVelocity); _characterController.setPositionAndOrientation(getPosition(), getOrientation()); @@ -1933,13 +1933,13 @@ void MyAvatar::updateMotionBehaviorFromMenu() { _motionBehaviors &= ~AVATAR_MOTION_SCRIPTED_MOTOR_ENABLED; } - setCharacterControllerEnabled(menu->isOptionChecked(MenuOption::EnableCharacterController)); + setAvatarCollisionsEnabled(menu->isOptionChecked(MenuOption::EnableAvatarCollisions)); } -void MyAvatar::setCharacterControllerEnabled(bool enabled) { +void MyAvatar::setAvatarCollisionsEnabled(bool enabled) { if (QThread::currentThread() != thread()) { - QMetaObject::invokeMethod(this, "setCharacterControllerEnabled", Q_ARG(bool, enabled)); + QMetaObject::invokeMethod(this, "setAvatarCollisionsEnabled", Q_ARG(bool, enabled)); return; } @@ -1951,11 +1951,12 @@ void MyAvatar::setCharacterControllerEnabled(bool enabled) { ghostingAllowed = zone->getGhostingAllowed(); } } - _characterController.setEnabled(ghostingAllowed ? enabled : true); + int16_t group = enabled || !ghostingAllowed ? BULLET_COLLISION_GROUP_MY_AVATAR : BULLET_COLLISION_GROUP_COLLISIONLESS; + _characterController.setCollisionGroup(group); } -bool MyAvatar::getCharacterControllerEnabled() { - return _characterController.isEnabled(); +bool MyAvatar::getAvatarCollisionsEnabled() { + return _characterController.getCollisionGroup() != BULLET_COLLISION_GROUP_COLLISIONLESS; } void MyAvatar::clearDriveKeys() { @@ -2063,9 +2064,6 @@ void MyAvatar::lateUpdatePalms() { Avatar::updatePalms(); } - -static const float FOLLOW_TIME = 0.5f; - MyAvatar::FollowHelper::FollowHelper() { deactivate(); } diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 85a2b72939..4b76c0defa 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -83,7 +83,7 @@ class MyAvatar : public Avatar { Q_PROPERTY(float energy READ getEnergy WRITE setEnergy) Q_PROPERTY(bool hmdLeanRecenterEnabled READ getHMDLeanRecenterEnabled WRITE setHMDLeanRecenterEnabled) - Q_PROPERTY(bool characterControllerEnabled READ getCharacterControllerEnabled WRITE setCharacterControllerEnabled) + Q_PROPERTY(bool avatarCollisionsEnabled READ getAvatarCollisionsEnabled WRITE setAvatarCollisionsEnabled) public: explicit MyAvatar(RigPointer rig); @@ -273,8 +273,8 @@ public: bool hasDriveInput() const; - Q_INVOKABLE void setCharacterControllerEnabled(bool enabled); - Q_INVOKABLE bool getCharacterControllerEnabled(); + Q_INVOKABLE void setAvatarCollisionsEnabled(bool enabled); + Q_INVOKABLE bool getAvatarCollisionsEnabled(); virtual glm::quat getAbsoluteJointRotationInObjectFrame(int index) const override; virtual glm::vec3 getAbsoluteJointTranslationInObjectFrame(int index) const override; diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index af983deea9..331be5c9da 100644 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -13,7 +13,6 @@ #include -#include "PhysicsCollisionGroups.h" #include "ObjectMotionState.h" #include "PhysicsLogging.h" @@ -62,10 +61,7 @@ CharacterController::CharacterMotor::CharacterMotor(const glm::vec3& vel, const } CharacterController::CharacterController() { - _halfHeight = 1.0f; - - _enabled = false; - + _halfHeight = 1.0f; _floorDistance = MAX_FALL_HEIGHT; _targetVelocity.setValue(0.0f, 0.0f, 0.0f); @@ -119,7 +115,7 @@ void CharacterController::setDynamicsWorld(btDynamicsWorld* world) { // Before adding the RigidBody to the world we must save its oldGravity to the side // because adding an object to the world will overwrite it with the default gravity. btVector3 oldGravity = _rigidBody->getGravity(); - _dynamicsWorld->addRigidBody(_rigidBody, BULLET_COLLISION_GROUP_MY_AVATAR, BULLET_COLLISION_MASK_MY_AVATAR); + _dynamicsWorld->addRigidBody(_rigidBody, _collisionGroup, BULLET_COLLISION_MASK_MY_AVATAR); _dynamicsWorld->addAction(this); // restore gravity settings _rigidBody->setGravity(oldGravity); @@ -199,7 +195,7 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) { // This mirrors the computation done in MyAvatar::FollowHelper::postPhysicsUpdate(). if (_following) { - // HACK these copied form elsewhere + // OUTOFBODY_HACK -- these consts were copied from elsewhere, and then tuned const float NORMAL_WALKING_SPEED = 0.5f; const float FOLLOW_TIME = 0.8f; const float FOLLOW_ROTATION_THRESHOLD = cosf(PI / 6.0f); @@ -282,15 +278,17 @@ void CharacterController::setState(State desiredState) { #ifdef DEBUG_STATE_CHANGE qCDebug(physics) << "CharacterController::setState" << stateToStr(desiredState) << "from" << stateToStr(_state) << "," << reason; #endif - if (desiredState == State::Hover && _state != State::Hover) { - // hover enter - if (_rigidBody) { + if (_rigidBody) { + if (desiredState == State::Hover && _state != State::Hover) { + // hover enter _rigidBody->setGravity(btVector3(0.0f, 0.0f, 0.0f)); - } - } else if (_state == State::Hover && desiredState != State::Hover) { - // hover exit - if (_rigidBody) { - _rigidBody->setGravity(DEFAULT_CHARACTER_GRAVITY * _currentUp); + } else if (_state == State::Hover && desiredState != State::Hover) { + // hover exit + if (_collisionGroup == BULLET_COLLISION_GROUP_COLLISIONLESS) { + _rigidBody->setGravity(btVector3(0.0f, 0.0f, 0.0f)); + } else { + _rigidBody->setGravity(DEFAULT_CHARACTER_GRAVITY * _currentUp); + } } } _state = desiredState; @@ -320,39 +318,45 @@ void CharacterController::setLocalBoundingBox(const glm::vec3& corner, const glm _pendingFlags |= PENDING_FLAG_REMOVE_FROM_SIMULATION; } _pendingFlags |= PENDING_FLAG_UPDATE_SHAPE; - // only need to ADD back when we happen to be enabled - if (_enabled) { - _pendingFlags |= PENDING_FLAG_ADD_TO_SIMULATION; - } + _pendingFlags |= PENDING_FLAG_ADD_TO_SIMULATION; } // it's ok to change offset immediately -- there are no thread safety issues here _shapeLocalOffset = corner + 0.5f * _boxScale; } -void CharacterController::setEnabled(bool enabled) { - if (enabled != _enabled) { - if (enabled) { - // Don't bother clearing REMOVE bit since it might be paired with an UPDATE_SHAPE bit. - // Setting the ADD bit here works for all cases so we don't even bother checking other bits. - _pendingFlags |= PENDING_FLAG_ADD_TO_SIMULATION; - } else { - if (_dynamicsWorld) { - _pendingFlags |= PENDING_FLAG_REMOVE_FROM_SIMULATION; - } - _pendingFlags &= ~ PENDING_FLAG_ADD_TO_SIMULATION; +void CharacterController::setCollisionGroup(int16_t group) { + if (_collisionGroup != group) { + _collisionGroup = group; + _pendingFlags |= PENDING_FLAG_UPDATE_COLLISION_GROUP; + } +} + +void CharacterController::handleChangedCollisionGroup() { + if (_pendingFlags & PENDING_FLAG_UPDATE_COLLISION_GROUP) { + // ATM the easiest way to update collision groups is to remove/re-add the RigidBody + if (_dynamicsWorld) { + _dynamicsWorld->removeRigidBody(_rigidBody); + _dynamicsWorld->addRigidBody(_rigidBody, _collisionGroup, BULLET_COLLISION_MASK_MY_AVATAR); + } + _pendingFlags &= ~PENDING_FLAG_UPDATE_COLLISION_GROUP; + + if (_state != State::Hover && _rigidBody) { + if (_collisionGroup == BULLET_COLLISION_GROUP_COLLISIONLESS) { + _rigidBody->setGravity(btVector3(0.0f, 0.0f, 0.0f)); + } else { + _rigidBody->setGravity(DEFAULT_CHARACTER_GRAVITY * _currentUp); + } } - SET_STATE(State::Hover, "setEnabled"); - _enabled = enabled; } } void CharacterController::updateUpAxis(const glm::quat& rotation) { - btVector3 oldUp = _currentUp; _currentUp = quatRotate(glmToBullet(rotation), LOCAL_UP_AXIS); - if (_state != State::Hover) { - const btScalar MIN_UP_ERROR = 0.01f; - if (oldUp.distance(_currentUp) > MIN_UP_ERROR) { + if (_state != State::Hover && _rigidBody) { + if (_collisionGroup == BULLET_COLLISION_GROUP_COLLISIONLESS) { + _rigidBody->setGravity(btVector3(0.0f, 0.0f, 0.0f)); + } else { _rigidBody->setGravity(DEFAULT_CHARACTER_GRAVITY * _currentUp); } } @@ -370,7 +374,7 @@ void CharacterController::setPositionAndOrientation( } void CharacterController::getPositionAndOrientation(glm::vec3& position, glm::quat& rotation) const { - if (_enabled && _rigidBody) { + if (_rigidBody) { const btTransform& avatarTransform = _rigidBody->getWorldTransform(); rotation = bulletToGLM(avatarTransform.getRotation()); position = bulletToGLM(avatarTransform.getOrigin()) - rotation * _shapeLocalOffset; @@ -438,7 +442,8 @@ void CharacterController::applyMotor(int index, btScalar dt, btVector3& worldVel btScalar angle = motor.rotation.getAngle(); btVector3 velocity = worldVelocity.rotate(axis, -angle); - if (_state == State::Hover || motor.hTimescale == motor.vTimescale) { + if (_collisionGroup == BULLET_COLLISION_GROUP_COLLISIONLESS || + _state == State::Hover || motor.hTimescale == motor.vTimescale) { // modify velocity btScalar tau = dt / motor.hTimescale; if (tau > 1.0f) { @@ -534,7 +539,7 @@ void CharacterController::computeNewVelocity(btScalar dt, glm::vec3& velocity) { } void CharacterController::preSimulation() { - if (_enabled && _dynamicsWorld) { + if (_dynamicsWorld) { quint64 now = usecTimestampNow(); // slam body to where it is supposed to be @@ -584,51 +589,61 @@ void CharacterController::preSimulation() { btVector3 actualHorizVelocity = velocity - velocity.dot(_currentUp) * _currentUp; bool flyingFast = _state == State::Hover && actualHorizVelocity.length() > (MAX_WALKING_SPEED * 0.75f); - switch (_state) { - case State::Ground: - if (!rayHasHit && !_hasSupport) { - SET_STATE(State::Hover, "no ground detected"); - } else if (_pendingFlags & PENDING_FLAG_JUMP && _jumpButtonDownCount != _takeoffJumpButtonID) { - _takeoffJumpButtonID = _jumpButtonDownCount; - _takeoffToInAirStartTime = now; - SET_STATE(State::Takeoff, "jump pressed"); - } else if (rayHasHit && !_hasSupport && _floorDistance > GROUND_TO_FLY_THRESHOLD) { - SET_STATE(State::InAir, "falling"); + // OUTOFBODY_HACK -- disable normal state transitions while collisionless + if (_collisionGroup == BULLET_COLLISION_GROUP_MY_AVATAR) { + switch (_state) { + case State::Ground: + if (!rayHasHit && !_hasSupport) { + SET_STATE(State::Hover, "no ground detected"); + } else if (_pendingFlags & PENDING_FLAG_JUMP && _jumpButtonDownCount != _takeoffJumpButtonID) { + _takeoffJumpButtonID = _jumpButtonDownCount; + _takeoffToInAirStartTime = now; + SET_STATE(State::Takeoff, "jump pressed"); + } else if (rayHasHit && !_hasSupport && _floorDistance > GROUND_TO_FLY_THRESHOLD) { + SET_STATE(State::InAir, "falling"); + } + break; + case State::Takeoff: + if (!rayHasHit && !_hasSupport) { + SET_STATE(State::Hover, "no ground"); + } else if ((now - _takeoffToInAirStartTime) > TAKE_OFF_TO_IN_AIR_PERIOD) { + SET_STATE(State::InAir, "takeoff done"); + velocity += _jumpSpeed * _currentUp; + _rigidBody->setLinearVelocity(velocity); + } + break; + case State::InAir: { + if ((velocity.dot(_currentUp) <= (JUMP_SPEED / 2.0f)) && ((_floorDistance < FLY_TO_GROUND_THRESHOLD) || _hasSupport)) { + SET_STATE(State::Ground, "hit ground"); + } else { + btVector3 desiredVelocity = _targetVelocity; + if (desiredVelocity.length2() < MIN_TARGET_SPEED_SQUARED) { + desiredVelocity = btVector3(0.0f, 0.0f, 0.0f); + } + bool vertTargetSpeedIsNonZero = desiredVelocity.dot(_currentUp) > MIN_TARGET_SPEED; + if ((jumpButtonHeld || vertTargetSpeedIsNonZero) && (_takeoffJumpButtonID != _jumpButtonDownCount)) { + SET_STATE(State::Hover, "double jump button"); + } else if ((jumpButtonHeld || vertTargetSpeedIsNonZero) && (now - _jumpButtonDownStartTime) > JUMP_TO_HOVER_PERIOD) { + SET_STATE(State::Hover, "jump button held"); + } + } + break; } - break; - case State::Takeoff: - if (!rayHasHit && !_hasSupport) { - SET_STATE(State::Hover, "no ground"); - } else if ((now - _takeoffToInAirStartTime) > TAKE_OFF_TO_IN_AIR_PERIOD) { - SET_STATE(State::InAir, "takeoff done"); - velocity += _jumpSpeed * _currentUp; - _rigidBody->setLinearVelocity(velocity); + case State::Hover: + if ((_floorDistance < MIN_HOVER_HEIGHT) && !jumpButtonHeld && !flyingFast) { + SET_STATE(State::InAir, "near ground"); + } else if (((_floorDistance < FLY_TO_GROUND_THRESHOLD) || _hasSupport) && !flyingFast) { + SET_STATE(State::Ground, "touching ground"); + } + break; } - break; - case State::InAir: { - if ((velocity.dot(_currentUp) <= (JUMP_SPEED / 2.0f)) && ((_floorDistance < FLY_TO_GROUND_THRESHOLD) || _hasSupport)) { - SET_STATE(State::Ground, "hit ground"); + } else { + // OUTOFBODY_HACK -- in collisionless state switch between Ground and Hover states + if (rayHasHit) { + SET_STATE(State::Ground, "collisionless above ground"); } else { - btVector3 desiredVelocity = _targetVelocity; - if (desiredVelocity.length2() < MIN_TARGET_SPEED_SQUARED) { - desiredVelocity = btVector3(0.0f, 0.0f, 0.0f); - } - bool vertTargetSpeedIsNonZero = desiredVelocity.dot(_currentUp) > MIN_TARGET_SPEED; - if ((jumpButtonHeld || vertTargetSpeedIsNonZero) && (_takeoffJumpButtonID != _jumpButtonDownCount)) { - SET_STATE(State::Hover, "double jump button"); - } else if ((jumpButtonHeld || vertTargetSpeedIsNonZero) && (now - _jumpButtonDownStartTime) > JUMP_TO_HOVER_PERIOD) { - SET_STATE(State::Hover, "jump button held"); - } + SET_STATE(State::Hover, "collisionless in air"); } - break; - } - case State::Hover: - if ((_floorDistance < MIN_HOVER_HEIGHT) && !jumpButtonHeld && !flyingFast) { - SET_STATE(State::InAir, "near ground"); - } else if (((_floorDistance < FLY_TO_GROUND_THRESHOLD) || _hasSupport) && !flyingFast) { - SET_STATE(State::Ground, "touching ground"); - } - break; } } @@ -664,7 +679,10 @@ void CharacterController::setFlyingAllowed(bool value) { _flyingAllowed = value; if (!_flyingAllowed && _state == State::Hover) { - SET_STATE(State::InAir, "flying not allowed"); + // OUTOFBODY_HACK -- disable normal state transitions while collisionless + if (_collisionGroup == BULLET_COLLISION_GROUP_MY_AVATAR) { + SET_STATE(State::InAir, "flying not allowed"); + } } } } diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h index 26ad481c46..2d8cf489de 100644 --- a/libraries/physics/src/CharacterController.h +++ b/libraries/physics/src/CharacterController.h @@ -19,12 +19,15 @@ #include #include +#include + #include "BulletUtil.h" const uint32_t PENDING_FLAG_ADD_TO_SIMULATION = 1U << 0; const uint32_t PENDING_FLAG_REMOVE_FROM_SIMULATION = 1U << 1; const uint32_t PENDING_FLAG_UPDATE_SHAPE = 1U << 2; const uint32_t PENDING_FLAG_JUMP = 1U << 3; +const uint32_t PENDING_FLAG_UPDATE_COLLISION_GROUP = 1U << 4; const float DEFAULT_CHARACTER_GRAVITY = -5.0f; @@ -107,9 +110,15 @@ public: void setLocalBoundingBox(const glm::vec3& corner, const glm::vec3& scale); + /* bool isEnabled() const { return _enabled; } // thread-safe void setEnabled(bool enabled); - bool isEnabledAndReady() const { return _enabled && _dynamicsWorld; } + */ + bool isEnabledAndReady() const { return _dynamicsWorld; } + + void setCollisionGroup(int16_t group); + int16_t getCollisionGroup() const { return _collisionGroup; } + void handleChangedCollisionGroup(); bool getRigidBodyLocation(glm::vec3& avatarRigidBodyPosition, glm::quat& avatarRigidBodyRotation); @@ -170,7 +179,6 @@ protected: btVector3 _linearAcceleration; bool _following { false }; - std::atomic_bool _enabled; State _state; bool _isPushingUp; @@ -180,6 +188,7 @@ protected: uint32_t _previousFlags { 0 }; bool _flyingAllowed { true }; + int16_t _collisionGroup { BULLET_COLLISION_GROUP_MY_AVATAR }; }; #endif // hifi_CharacterControllerInterface_h From 0d300c3a47e015275a2627855817cf9a82eb7ec5 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 12 Sep 2016 15:57:57 -0700 Subject: [PATCH 014/101] teleport.js drags avatar along --- scripts/system/controllers/teleport.js | 74 ++++++++++++++++---------- 1 file changed, 45 insertions(+), 29 deletions(-) diff --git a/scripts/system/controllers/teleport.js b/scripts/system/controllers/teleport.js index 3d4bf244a5..6c2478bd9d 100644 --- a/scripts/system/controllers/teleport.js +++ b/scripts/system/controllers/teleport.js @@ -13,7 +13,10 @@ var inTeleportMode = false; var SMOOTH_ARRIVAL_SPACING = 33; -var NUMBER_OF_STEPS = 6; +var NUMBER_OF_STEPS_FOR_TELEPORT = 6; + +var AVATAR_DRAG_SPACING = 33; +var NUMBER_OF_STEPS_FOR_AVATAR_DRAG = 12; var TARGET_MODEL_URL = Script.resolvePath("../assets/models/teleport-destination.fbx"); var TOO_CLOSE_MODEL_URL = Script.resolvePath("../assets/models/teleport-cancel.fbx"); @@ -88,6 +91,8 @@ function Teleporter() { this.cancelOverlay = null; this.updateConnected = null; this.smoothArrivalInterval = null; + this.dragAvatarInterval = null; + this.oldAvatarCollisionsEnabled = MyAvatar.avatarCollisionsEnabled; this.teleportHand = null; this.distance = 0.0; this.teleportMode = "HMDAndAvatarTogether"; @@ -132,6 +137,10 @@ function Teleporter() { if (this.smoothArrivalInterval !== null) { Script.clearInterval(this.smoothArrivalInterval); } + if (this.dragAvatarInterval !== null) { + Script.clearInterval(this.dragAvatarInterval); + MyAvatar.avatarCollisionsEnabled = _this.oldAvatarCollisionsEnabled; + } if (activationTimeout !== null) { Script.clearInterval(activationTimeout); } @@ -524,32 +533,27 @@ function Teleporter() { this.teleportMode = "AvatarOnly"; } this.smoothArrival(); - } - }; - - this.findMidpoint = function(start, end) { - var xy = Vec3.sum(start, end); - var midpoint = Vec3.multiply(0.5, xy); - return midpoint - }; - - this.getArrivalPoints = function(startPoint, endPoint) { - var arrivalPoints = []; - var i; - var lastPoint; - - for (i = 0; i < NUMBER_OF_STEPS; i++) { - if (i === 0) { - lastPoint = startPoint; + if (this.teleportMode === "HMDFirstAvatarWillFollow") { + this.dragAvatarCollisionlessly(); } - var newPoint = _this.findMidpoint(lastPoint, endPoint); - lastPoint = newPoint; - arrivalPoints.push(newPoint); } + }; - arrivalPoints.push(endPoint); + this.getWayPoints = function(startPoint, endPoint, numberOfSteps) { + var travel = Vec3.subtract(endPoint - startPoint); + var distance = Vec3.length(travel); + if (distance > 1.0) { + var base = Math.exp(log(distance + 1.0) / numberOfSteps); + var wayPoints = []; + var i; - return arrivalPoints; + for (i = 0; i < numberOfSteps - 1; i++) { + var backFraction = (1.0 - Math.exp((numberOfSteps - 1 - i) * Math.log(base))) / distance; + wayPoints.push(Vec3.sum(endPoint, Vec3.multiply(backFraction, travel)); + } + } + wayPoints.push(endPoint); + return wayPoints; }; this.teleportTo = function(landingPoint) { @@ -569,25 +573,37 @@ function Teleporter() { } this.smoothArrival = function() { - - _this.arrivalPoints = _this.getArrivalPoints(MyAvatar.position, _this.intersection.intersection); + _this.teleportPoints = _this.getWayPoints(MyAvatar.position, _this.intersection.intersection, NUMBER_OF_STEPS_FOR_TELEPORT); _this.smoothArrivalInterval = Script.setInterval(function() { - if (_this.arrivalPoints.length === 0) { + if (_this.teleportPoints.length === 0) { Script.clearInterval(_this.smoothArrivalInterval); HMD.centerUI(); return; } - var landingPoint = _this.arrivalPoints.shift(); + var landingPoint = _this.teleportPoints.shift(); _this.teleportTo(landingPoint); - if (_this.arrivalPoints.length === 1 || _this.arrivalPoints.length === 0) { + if (_this.teleportPoints.length === 1 || _this.teleportPoints.length === 0) { _this.deleteTargetOverlay(); _this.deleteCancelOverlay(); } }, SMOOTH_ARRIVAL_SPACING); + } - + this.dragAvatarCollisionlessly = function() { + _this.oldAvatarCollisionsEnabled = MyAvatar.avatarCollisionsEnabled; + MyAvatar.avatarCollisionsEnabled = false; + _this.dragPoints = _this.getWayPoints(MyAvatar.position, _this.intersection.intersection, NUMBER_OF_STEPS_FOR_AVATAR_DRAG); + _this.dragAvatarInterval = Script.setInterval(function() { + if (_this.dragPoints.length === 0) { + Script.clearInterval(_this.dragAvatarInterval); + MyAvatar.avatarCollisionsEnabled = _this.oldAvatarCollisionsEnabled; + return; + } + var landingPoint = _this.dragPoints.shift(); + MyAvatar.position = landingPoint; + }, AVATAR_DRAG_SPACING); } } From 9728cf6904a64669cffb85ba7bbd881abb0c5ac0 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 12 Sep 2016 16:22:56 -0700 Subject: [PATCH 015/101] fix typos in teleport.js --- scripts/system/controllers/teleport.js | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/scripts/system/controllers/teleport.js b/scripts/system/controllers/teleport.js index 6c2478bd9d..ea3535bf64 100644 --- a/scripts/system/controllers/teleport.js +++ b/scripts/system/controllers/teleport.js @@ -540,16 +540,16 @@ function Teleporter() { }; this.getWayPoints = function(startPoint, endPoint, numberOfSteps) { - var travel = Vec3.subtract(endPoint - startPoint); + var travel = Vec3.subtract(endPoint, startPoint); var distance = Vec3.length(travel); if (distance > 1.0) { - var base = Math.exp(log(distance + 1.0) / numberOfSteps); + var base = Math.exp(Math.log(distance + 1.0) / numberOfSteps); var wayPoints = []; var i; for (i = 0; i < numberOfSteps - 1; i++) { var backFraction = (1.0 - Math.exp((numberOfSteps - 1 - i) * Math.log(base))) / distance; - wayPoints.push(Vec3.sum(endPoint, Vec3.multiply(backFraction, travel)); + wayPoints.push(Vec3.sum(endPoint, Vec3.multiply(backFraction, travel))); } } wayPoints.push(endPoint); From 09c4720665335d8da582627019d41d998f7a97b4 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 12 Sep 2016 16:35:52 -0700 Subject: [PATCH 016/101] fix typo in teleport.js --- scripts/system/controllers/teleport.js | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/scripts/system/controllers/teleport.js b/scripts/system/controllers/teleport.js index ea3535bf64..d077b1090b 100644 --- a/scripts/system/controllers/teleport.js +++ b/scripts/system/controllers/teleport.js @@ -542,9 +542,9 @@ function Teleporter() { this.getWayPoints = function(startPoint, endPoint, numberOfSteps) { var travel = Vec3.subtract(endPoint, startPoint); var distance = Vec3.length(travel); + var wayPoints = []; if (distance > 1.0) { var base = Math.exp(Math.log(distance + 1.0) / numberOfSteps); - var wayPoints = []; var i; for (i = 0; i < numberOfSteps - 1; i++) { From 362e185098ea39fccf7f9c3feaf234254743c0a0 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Tue, 13 Sep 2016 09:49:43 -0700 Subject: [PATCH 017/101] Revert "When out-of-body have your avatar face toward your HMD." This reverts commit ffb1bae1f08ac2edbbd9fe9ade1c8d39ac68f295. --- interface/src/avatar/MyAvatar.cpp | 74 +++++++------------------- interface/src/avatar/MyAvatar.h | 7 +-- interface/src/avatar/SkeletonModel.cpp | 40 ++++++-------- 3 files changed, 39 insertions(+), 82 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index df9e07ee78..01e749b7b2 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -86,9 +86,9 @@ const float MyAvatar::ZOOM_DEFAULT = 1.5f; extern bool OUTOFBODY_HACK_ENABLE_DEBUG_DRAW_IK_TARGETS; // OUTOFBODY_HACK defined in SkeletonModel.cpp -extern glm::vec3 TRUNCATE_IK_CAPSULE_POSITION; -extern float TRUNCATE_IK_CAPSULE_LENGTH; -extern float TRUNCATE_IK_CAPSULE_RADIUS; +extern const glm::vec3 TRUNCATE_IK_CAPSULE_POSITION; +extern const float TRUNCATE_IK_CAPSULE_LENGTH; +extern const float TRUNCATE_IK_CAPSULE_RADIUS; MyAvatar::MyAvatar(RigPointer rig) : Avatar(rig), @@ -2157,65 +2157,31 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat _desiredBodyMatrix = desiredBodyMatrix; if (myAvatar.getHMDLeanRecenterEnabled()) { + updateRotationActivation(myAvatar, desiredBodyMatrix, currentBodyMatrix); + updateHorizontalActivation(myAvatar, desiredBodyMatrix, currentBodyMatrix); + updateVerticalActivation(myAvatar, desiredBodyMatrix, currentBodyMatrix); - if (_isOutOfBody) { + glm::mat4 desiredWorldMatrix = myAvatar.getSensorToWorldMatrix() * _desiredBodyMatrix; + glm::mat4 currentWorldMatrix = createMatFromQuatAndPos(myAvatar.getOrientation(), myAvatar.getPosition()); - glm::mat4 desiredWorldMatrix = myAvatar.getSensorToWorldMatrix() * _desiredBodyMatrix; - glm::mat4 currentWorldMatrix = createMatFromQuatAndPos(myAvatar.getOrientation(), myAvatar.getPosition()); - AnimPose followWorldPose(currentWorldMatrix); - - // OUTOFBODY_HACK, only takes horizontal movement into account. - - // horizontal follow + AnimPose followWorldPose(currentWorldMatrix); + if (isActive(Rotation)) { + followWorldPose.rot = glmExtractRotation(desiredWorldMatrix); + } + if (isActive(Horizontal)) { glm::vec3 desiredTranslation = extractTranslation(desiredWorldMatrix); followWorldPose.trans.x = desiredTranslation.x; followWorldPose.trans.z = desiredTranslation.z; + } + if (isActive(Vertical)) { + glm::vec3 desiredTranslation = extractTranslation(desiredWorldMatrix); + followWorldPose.trans.y = desiredTranslation.y; + } - // rotation follow - // face the HMD - glm::vec3 hmdWorldPosition = extractTranslation(myAvatar.getSensorToWorldMatrix() * myAvatar.getHMDSensorMatrix()); - glm::vec3 facing = myAvatar.getPosition() - hmdWorldPosition; - facing.y = 0.0f; - if (glm::length(facing) > EPSILON) { - // turn to face the hmd - followWorldPose.rot = glm::angleAxis(atan2(facing.x, facing.z), Vectors::UNIT_Y); - } else { - followWorldPose.rot = glmExtractRotation(desiredWorldMatrix); - } - + if (isActive()) { myAvatar.getCharacterController()->setFollowParameters(followWorldPose); - } else { - updateRotationActivation(myAvatar, desiredBodyMatrix, currentBodyMatrix); - updateHorizontalActivation(myAvatar, desiredBodyMatrix, currentBodyMatrix); - updateVerticalActivation(myAvatar, desiredBodyMatrix, currentBodyMatrix); - - glm::mat4 desiredWorldMatrix = myAvatar.getSensorToWorldMatrix() * _desiredBodyMatrix; - glm::mat4 currentWorldMatrix = createMatFromQuatAndPos(myAvatar.getOrientation(), myAvatar.getPosition()); - - AnimPose followWorldPose(currentWorldMatrix); - if (isActive(Rotation)) { - followWorldPose.rot = glmExtractRotation(desiredWorldMatrix); - } - if (isActive(Horizontal)) { - glm::vec3 desiredTranslation = extractTranslation(desiredWorldMatrix); - followWorldPose.trans.x = desiredTranslation.x; - followWorldPose.trans.z = desiredTranslation.z; - } - if (isActive(Vertical)) { - glm::vec3 desiredTranslation = extractTranslation(desiredWorldMatrix); - followWorldPose.trans.y = desiredTranslation.y; - } - - if (isActive()) { - myAvatar.getCharacterController()->setFollowParameters(followWorldPose); - } else { - myAvatar.getCharacterController()->disableFollow(); - } - - glm::mat4 currentWorldMatrixY180 = createMatFromQuatAndPos(myAvatar.getOrientation() * Quaternions::Y_180, myAvatar.getPosition()); - _prevInBodyHMDMatInAvatarSpace = _inBodyHMDMatInAvatarSpace; - _inBodyHMDMatInAvatarSpace = glm::inverse(currentWorldMatrixY180) * myAvatar.getSensorToWorldMatrix() * myAvatar.getHMDSensorMatrix(); + myAvatar.getCharacterController()->disableFollow(); } } else { deactivate(); diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 4b76c0defa..59a363c483 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -279,9 +279,6 @@ public: virtual glm::quat getAbsoluteJointRotationInObjectFrame(int index) const override; virtual glm::vec3 getAbsoluteJointTranslationInObjectFrame(int index) const override; - bool isOutOfBody() const { return _follow._isOutOfBody; } - glm::mat4 getInBodyHMDMatInAvatarSpace() const { return _follow._prevInBodyHMDMatInAvatarSpace; } - public slots: void increaseSize(); void decreaseSize(); @@ -318,6 +315,8 @@ public slots: glm::vec3 getPositionForAudio(); glm::quat getOrientationForAudio(); + bool isOutOfBody() const { return _follow._isOutOfBody; } + signals: void audioListenerModeChanged(); void transformChanged(); @@ -450,8 +449,6 @@ private: glm::mat4 _desiredBodyMatrix; uint8_t _activeBits { 0 }; bool _isOutOfBody { false }; - glm::mat4 _prevInBodyHMDMatInAvatarSpace; - glm::mat4 _inBodyHMDMatInAvatarSpace; void deactivate(); void deactivate(FollowType type); diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index 2bbda480e0..be1d071f6b 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -23,9 +23,9 @@ #include "InterfaceLogging.h" #include "AnimDebugDraw.h" -glm::vec3 TRUNCATE_IK_CAPSULE_POSITION(0.0f, 0.0f, 0.0f); -float TRUNCATE_IK_CAPSULE_LENGTH = 1000.0f; -float TRUNCATE_IK_CAPSULE_RADIUS = 0.5f; +const glm::vec3 TRUNCATE_IK_CAPSULE_POSITION(0.0f, 0.0f, 0.0f); +const float TRUNCATE_IK_CAPSULE_LENGTH = 1000.0f; +const float TRUNCATE_IK_CAPSULE_RADIUS = 0.5f; SkeletonModel::SkeletonModel(Avatar* owningAvatar, QObject* parent, RigPointer rig) : Model(rig, parent), @@ -110,8 +110,8 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { Rig::HeadParameters headParams; - glm::mat4 desiredHMDMat; // rig space - glm::mat4 actualHMDMat; // rig space + glm::vec3 hmdPositionInRigSpace; + glm::vec3 truncatedHMDPositionInRigSpace; if (qApp->isHMDMode()) { headParams.isInHMD = true; @@ -120,20 +120,20 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { glm::mat4 worldHMDMat = myAvatar->getSensorToWorldMatrix() * myAvatar->getHMDSensorMatrix(); glm::mat4 rigToWorld = createMatFromQuatAndPos(getRotation(), getTranslation()); glm::mat4 worldToRig = glm::inverse(rigToWorld); - desiredHMDMat = worldToRig * worldHMDMat; + glm::mat4 rigHMDMat = worldToRig * worldHMDMat; + hmdPositionInRigSpace = extractTranslation(rigHMDMat); + // truncate head IK target if it's out of body if (myAvatar->isOutOfBody()) { - // use the last valid in-body hmd matrix for the head yaw. - glm::mat4 hmdYaw = cancelOutRollAndPitch(myAvatar->getInBodyHMDMatInAvatarSpace()); - glm::mat4 hmdPitchAndRoll = glm::inverse(cancelOutRollAndPitch(desiredHMDMat)) * desiredHMDMat; - actualHMDMat = hmdYaw * hmdPitchAndRoll; + truncatedHMDPositionInRigSpace = projectPointOntoCapsule(hmdPositionInRigSpace, TRUNCATE_IK_CAPSULE_POSITION, + TRUNCATE_IK_CAPSULE_LENGTH, TRUNCATE_IK_CAPSULE_RADIUS); } else { - actualHMDMat = desiredHMDMat; + truncatedHMDPositionInRigSpace = hmdPositionInRigSpace; } - headParams.rigHeadPosition = extractTranslation(actualHMDMat); - headParams.rigHeadOrientation = extractRotation(actualHMDMat); + headParams.rigHeadPosition = truncatedHMDPositionInRigSpace; + headParams.rigHeadOrientation = extractRotation(rigHMDMat); headParams.worldHeadOrientation = extractRotation(worldHMDMat); } else { @@ -159,11 +159,8 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { // truncate hand target if (myAvatar->isOutOfBody() && qApp->isHMDMode()) { - glm::mat4 handMat = createMatFromQuatAndPos(handParams.leftOrientation, handParams.leftPosition); - glm::mat4 offset = glm::inverse(desiredHMDMat) * handMat; - handMat = actualHMDMat * offset; - handParams.leftPosition = extractTranslation(handMat); - handParams.leftOrientation = glmExtractRotation(handMat); + glm::vec3 offset = handParams.leftPosition - hmdPositionInRigSpace; + handParams.leftPosition = truncatedHMDPositionInRigSpace + offset; } } else { handParams.isLeftEnabled = false; @@ -177,11 +174,8 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { // truncate hand target if (myAvatar->isOutOfBody() && qApp->isHMDMode()) { - glm::mat4 handMat = createMatFromQuatAndPos(handParams.rightOrientation, handParams.rightPosition); - glm::mat4 offset = glm::inverse(desiredHMDMat) * handMat; - handMat = actualHMDMat * offset; - handParams.rightPosition = extractTranslation(handMat); - handParams.rightOrientation = glmExtractRotation(handMat); + glm::vec3 offset = handParams.rightPosition - hmdPositionInRigSpace; + handParams.rightPosition = truncatedHMDPositionInRigSpace + offset; } } else { handParams.isRightEnabled = false; From 0fde3a2d76f59bbbedfc6574d5ccf52ab3ffa9db Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Tue, 13 Sep 2016 10:26:38 -0700 Subject: [PATCH 018/101] Fix for TRUNCATE_IK_CAPSULE globals --- interface/src/avatar/MyAvatar.cpp | 6 +++--- interface/src/avatar/SkeletonModel.cpp | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 01e749b7b2..38d84fdd53 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -86,9 +86,9 @@ const float MyAvatar::ZOOM_DEFAULT = 1.5f; extern bool OUTOFBODY_HACK_ENABLE_DEBUG_DRAW_IK_TARGETS; // OUTOFBODY_HACK defined in SkeletonModel.cpp -extern const glm::vec3 TRUNCATE_IK_CAPSULE_POSITION; -extern const float TRUNCATE_IK_CAPSULE_LENGTH; -extern const float TRUNCATE_IK_CAPSULE_RADIUS; +extern glm::vec3 TRUNCATE_IK_CAPSULE_POSITION; +extern float TRUNCATE_IK_CAPSULE_LENGTH; +extern float TRUNCATE_IK_CAPSULE_RADIUS; MyAvatar::MyAvatar(RigPointer rig) : Avatar(rig), diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index be1d071f6b..b36da765e5 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -23,9 +23,9 @@ #include "InterfaceLogging.h" #include "AnimDebugDraw.h" -const glm::vec3 TRUNCATE_IK_CAPSULE_POSITION(0.0f, 0.0f, 0.0f); -const float TRUNCATE_IK_CAPSULE_LENGTH = 1000.0f; -const float TRUNCATE_IK_CAPSULE_RADIUS = 0.5f; +glm::vec3 TRUNCATE_IK_CAPSULE_POSITION(0.0f, 0.0f, 0.0f); +float TRUNCATE_IK_CAPSULE_LENGTH = 1000.0f; +float TRUNCATE_IK_CAPSULE_RADIUS = 0.5f; SkeletonModel::SkeletonModel(Avatar* owningAvatar, QObject* parent, RigPointer rig) : Model(rig, parent), From bc130eaf26aef76237780909319d251046a33b46 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Tue, 13 Sep 2016 16:26:10 -0700 Subject: [PATCH 019/101] ramp support with comfort mode option --- interface/src/Menu.cpp | 3 ++ interface/src/Menu.h | 1 + interface/src/avatar/MyAvatar.cpp | 43 ++++++++++++++++++++------ interface/src/avatar/MyAvatar.h | 9 +++++- interface/src/avatar/SkeletonModel.cpp | 2 +- 5 files changed, 47 insertions(+), 11 deletions(-) diff --git a/interface/src/Menu.cpp b/interface/src/Menu.cpp index 84d1c2cac6..276b07a0a6 100644 --- a/interface/src/Menu.cpp +++ b/interface/src/Menu.cpp @@ -506,6 +506,9 @@ Menu::Menu() { avatar, SLOT(updateMotionBehaviorFromMenu()), UNSPECIFIED_POSITION, "Developer"); + addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::EnableVerticalComfortMode, 0, false, + avatar, SLOT(setEnableVerticalComfortMode(bool))); + // Developer > Hands >>> MenuWrapper* handOptionsMenu = developerMenu->addMenu("Hands"); addCheckableActionToQMenuAndActionHash(handOptionsMenu, MenuOption::DisplayHandTargets, 0, false, diff --git a/interface/src/Menu.h b/interface/src/Menu.h index 5e213f3974..949bd298d9 100644 --- a/interface/src/Menu.h +++ b/interface/src/Menu.h @@ -99,6 +99,7 @@ namespace MenuOption { const QString EchoServerAudio = "Echo Server Audio"; const QString EnableAvatarCollisions = "Enable Avatar Collisions"; const QString EnableInverseKinematics = "Enable Inverse Kinematics"; + const QString EnableVerticalComfortMode = "Enable Vertical Comfort Mode"; const QString ExpandMyAvatarSimulateTiming = "Expand /myAvatar/simulation"; const QString ExpandMyAvatarTiming = "Expand /myAvatar"; const QString ExpandOtherAvatarTiming = "Expand /otherAvatar"; diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 38d84fdd53..f15b4c8d42 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -422,6 +422,11 @@ void MyAvatar::simulate(float deltaTime) { updatePosition(deltaTime); } + // update sensorToWorldMatrix for camera and hand controllers + // before we perform rig animations and IK. + + updateSensorToWorldMatrix(_enableVerticalComfortMode ? SensorToWorldUpdateMode::VerticalComfort : SensorToWorldUpdateMode::Vertical); + { PerformanceTimer perfTimer("skeleton"); _skeletonModel->simulate(deltaTime); @@ -548,11 +553,23 @@ void MyAvatar::updateJointFromController(controller::Action poseKey, ThreadSafeV // best called at end of main loop, after physics. // update sensor to world matrix from current body position and hmd sensor. // This is so the correct camera can be used for rendering. -void MyAvatar::updateSensorToWorldMatrix() { - // update the sensor mat so that the body position will end up in the desired - // position when driven from the head. - glm::mat4 bodyToWorld = createMatFromQuatAndPos(getOrientation(), getPosition()); - setSensorToWorldMatrix(bodyToWorld * glm::inverse(_bodySensorMatrix)); +void MyAvatar::updateSensorToWorldMatrix(SensorToWorldUpdateMode mode) { + if (mode == SensorToWorldUpdateMode::Full) { + glm::mat4 bodyToWorld = createMatFromQuatAndPos(getOrientation(), getPosition()); + setSensorToWorldMatrix(bodyToWorld * glm::inverse(_bodySensorMatrix)); + } else if (mode == SensorToWorldUpdateMode::Vertical || + mode == SensorToWorldUpdateMode::VerticalComfort) { + glm::mat4 bodyToWorld = createMatFromQuatAndPos(getOrientation(), getPosition()); + glm::mat4 newSensorToWorldMat = bodyToWorld * glm::inverse(_bodySensorMatrix); + glm::mat4 sensorToWorldMat = _sensorToWorldMatrix; + sensorToWorldMat[3][1] = newSensorToWorldMat[3][1]; + if (mode == SensorToWorldUpdateMode::VerticalComfort && + fabsf(_sensorToWorldMatrix[3][1] - newSensorToWorldMat[3][1]) > 0.1f) { + setSensorToWorldMatrix(sensorToWorldMat); + } else if (mode == SensorToWorldUpdateMode::Vertical) { + setSensorToWorldMatrix(sensorToWorldMat); + } + } } void MyAvatar::setSensorToWorldMatrix(const glm::mat4& sensorToWorld) { @@ -861,6 +878,10 @@ void MyAvatar::setEnableInverseKinematics(bool isEnabled) { _rig->setEnableInverseKinematics(isEnabled); } +void MyAvatar::setEnableVerticalComfortMode(bool isEnabled) { + _enableVerticalComfortMode = isEnabled; +} + void MyAvatar::loadData() { Settings settings; settings.beginGroup("Avatar"); @@ -1318,9 +1339,14 @@ void MyAvatar::prepareForPhysicsSimulation() { _characterController.handleChangedCollisionGroup(); _characterController.setParentVelocity(parentVelocity); - _characterController.setPositionAndOrientation(getPosition(), getOrientation()); + glm::vec3 position = getPosition(); + glm::quat orientation = getOrientation(); + + _characterController.setPositionAndOrientation(position, orientation); if (qApp->isHMDMode()) { - _follow.prePhysicsUpdate(*this, deriveBodyFromHMDSensor(), _bodySensorMatrix); + glm::mat4 bodyToWorldMatrix = createMatFromQuatAndPos(orientation, position); + glm::mat4 currentBodySensorMatrix = glm::inverse(_sensorToWorldMatrix) * bodyToWorldMatrix; + _follow.prePhysicsUpdate(*this, deriveBodyFromHMDSensor(), currentBodySensorMatrix); } else { _follow.deactivate(); getCharacterController()->disableFollow(); @@ -1336,8 +1362,7 @@ void MyAvatar::harvestResultsFromPhysicsSimulation(float deltaTime) { nextAttitude(position, orientation); // compute new _bodyToSensorMatrix - glm::mat4 bodyToWorldMatrix = createMatFromQuatAndPos(orientation, position); - _bodySensorMatrix = glm::inverse(_sensorToWorldMatrix) * bodyToWorldMatrix; + //_bodySensorMatrix = deriveBodyFromHMDSensor(); if (_characterController.isEnabledAndReady()) { setVelocity(_characterController.getLinearVelocity() + _characterController.getFollowVelocity()); diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 59a363c483..0972bc561c 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -124,7 +124,12 @@ public: // best called at end of main loop, just before rendering. // update sensor to world matrix from current body position and hmd sensor. // This is so the correct camera can be used for rendering. - void updateSensorToWorldMatrix(); + enum class SensorToWorldUpdateMode { + Full = 0, + Vertical, + VerticalComfort + }; + void updateSensorToWorldMatrix(SensorToWorldUpdateMode mode = SensorToWorldUpdateMode::Full); void setSensorToWorldMatrix(const glm::mat4& sensorToWorld); @@ -306,6 +311,7 @@ public slots: void setEnableMeshVisible(bool isEnabled); void setUseAnimPreAndPostRotations(bool isEnabled); void setEnableInverseKinematics(bool isEnabled); + void setEnableVerticalComfortMode(bool isEnabled); QUrl getAnimGraphOverrideUrl() const; // thread-safe void setAnimGraphOverrideUrl(QUrl value); // thread-safe @@ -477,6 +483,7 @@ private: bool _enableDebugDrawHandControllers { false }; bool _enableDebugDrawSensorToWorldMatrix { false }; bool _enableDebugDrawIKTargets { false }; + bool _enableVerticalComfortMode { false }; AudioListenerMode _audioListenerMode; glm::vec3 _customListenPosition; diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index b36da765e5..36f74b8784 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -25,7 +25,7 @@ glm::vec3 TRUNCATE_IK_CAPSULE_POSITION(0.0f, 0.0f, 0.0f); float TRUNCATE_IK_CAPSULE_LENGTH = 1000.0f; -float TRUNCATE_IK_CAPSULE_RADIUS = 0.5f; +float TRUNCATE_IK_CAPSULE_RADIUS = 0.25f; SkeletonModel::SkeletonModel(Avatar* owningAvatar, QObject* parent, RigPointer rig) : Model(rig, parent), From f5c5c68937095baeac58d3c61e4d47c58e7073d9 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 14 Sep 2016 11:43:52 -0700 Subject: [PATCH 020/101] adding kinematic character controller demo --- interface/src/Menu.cpp | 5 + interface/src/Menu.h | 1 + interface/src/avatar/MyAvatar.cpp | 4 + interface/src/avatar/MyAvatar.h | 1 + .../src/avatar/MyCharacterController.cpp | 10 +- libraries/physics/src/CharacterController.cpp | 123 ++++-- libraries/physics/src/CharacterController.h | 16 +- .../physics/src/CharacterGhostObject.cpp | 390 ++++++++++++++++++ libraries/physics/src/CharacterGhostObject.h | 83 ++++ libraries/physics/src/CharacterRayResult.cpp | 29 ++ libraries/physics/src/CharacterRayResult.h | 44 ++ .../physics/src/CharacterSweepResult.cpp | 40 ++ libraries/physics/src/CharacterSweepResult.h | 45 ++ 13 files changed, 741 insertions(+), 50 deletions(-) create mode 100644 libraries/physics/src/CharacterGhostObject.cpp create mode 100644 libraries/physics/src/CharacterGhostObject.h create mode 100644 libraries/physics/src/CharacterRayResult.cpp create mode 100644 libraries/physics/src/CharacterRayResult.h create mode 100644 libraries/physics/src/CharacterSweepResult.cpp create mode 100644 libraries/physics/src/CharacterSweepResult.h diff --git a/interface/src/Menu.cpp b/interface/src/Menu.cpp index 84d1c2cac6..52a812f538 100644 --- a/interface/src/Menu.cpp +++ b/interface/src/Menu.cpp @@ -506,6 +506,11 @@ Menu::Menu() { avatar, SLOT(updateMotionBehaviorFromMenu()), UNSPECIFIED_POSITION, "Developer"); + // KINEMATIC_CONTROLLER_HACK + addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::MoveKinematically, 0, false, + avatar, SLOT(updateMotionBehaviorFromMenu()), + UNSPECIFIED_POSITION, "Developer"); + // Developer > Hands >>> MenuWrapper* handOptionsMenu = developerMenu->addMenu("Hands"); addCheckableActionToQMenuAndActionHash(handOptionsMenu, MenuOption::DisplayHandTargets, 0, false, diff --git a/interface/src/Menu.h b/interface/src/Menu.h index 5e213f3974..c706d50adf 100644 --- a/interface/src/Menu.h +++ b/interface/src/Menu.h @@ -189,6 +189,7 @@ namespace MenuOption { const QString UseAudioForMouth = "Use Audio for Mouth"; const QString UseCamera = "Use Camera"; const QString UseAnimPreAndPostRotations = "Use Anim Pre and Post Rotations"; + const QString MoveKinematically = "Move Kinematically"; // KINEMATIC_CONTROLLER_HACK const QString VelocityFilter = "Velocity Filter"; const QString VisibleToEveryone = "Everyone"; const QString VisibleToFriends = "Friends"; diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 38d84fdd53..8d5ec30424 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -1933,6 +1933,10 @@ void MyAvatar::updateMotionBehaviorFromMenu() { _motionBehaviors &= ~AVATAR_MOTION_SCRIPTED_MOTOR_ENABLED; } + // KINEMATIC_CONTROLLER_HACK + bool moveKinematically = menu->isOptionChecked(MenuOption::MoveKinematically); + _characterController.setMoveKinematically(moveKinematically); + setAvatarCollisionsEnabled(menu->isOptionChecked(MenuOption::EnableAvatarCollisions)); } diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 59a363c483..0a3b217a66 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -492,6 +492,7 @@ private: ThreadSafeValueCache _rightHandControllerPoseInSensorFrameCache { controller::Pose() }; bool _hmdLeanRecenterEnabled = true; + bool _moveKinematically { false }; // KINEMATIC_CONTROLLER_HACK float AVATAR_MOVEMENT_ENERGY_CONSTANT { 0.001f }; float AUDIO_ENERGY_CONSTANT { 0.000001f }; diff --git a/interface/src/avatar/MyCharacterController.cpp b/interface/src/avatar/MyCharacterController.cpp index 6e52f4a949..ef0c2d1cac 100644 --- a/interface/src/avatar/MyCharacterController.cpp +++ b/interface/src/avatar/MyCharacterController.cpp @@ -37,7 +37,7 @@ void MyCharacterController::updateShapeIfNecessary() { // compute new dimensions from avatar's bounding box float x = _boxScale.x; float z = _boxScale.z; - _radius = 0.5f * sqrtf(0.5f * (x * x + z * z)); + setCapsuleRadius(0.5f * sqrtf(0.5f * (x * x + z * z))); _halfHeight = 0.5f * _boxScale.y - _radius; float MIN_HALF_HEIGHT = 0.1f; if (_halfHeight < MIN_HALF_HEIGHT) { @@ -74,7 +74,13 @@ void MyCharacterController::updateShapeIfNecessary() { } else { _rigidBody->setGravity(DEFAULT_CHARACTER_GRAVITY * _currentUp); } - //_rigidBody->setCollisionFlags(btCollisionObject::CF_CHARACTER_OBJECT); + // KINEMATIC_CONTROLLER_HACK + if (_moveKinematically) { + _rigidBody->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT); + } else { + _rigidBody->setCollisionFlags(_rigidBody->getCollisionFlags() & + ~(btCollisionObject::CF_KINEMATIC_OBJECT | btCollisionObject::CF_STATIC_OBJECT)); + } } else { // TODO: handle this failure case } diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 331be5c9da..81772c59b3 100644 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -102,6 +102,7 @@ bool CharacterController::needsAddition() const { void CharacterController::setDynamicsWorld(btDynamicsWorld* world) { if (_dynamicsWorld != world) { + // remove from old world if (_dynamicsWorld) { if (_rigidBody) { _dynamicsWorld->removeRigidBody(_rigidBody); @@ -110,6 +111,7 @@ void CharacterController::setDynamicsWorld(btDynamicsWorld* world) { _dynamicsWorld = nullptr; } if (world && _rigidBody) { + // add to new world _dynamicsWorld = world; _pendingFlags &= ~PENDING_FLAG_JUMP; // Before adding the RigidBody to the world we must save its oldGravity to the side @@ -119,7 +121,18 @@ void CharacterController::setDynamicsWorld(btDynamicsWorld* world) { _dynamicsWorld->addAction(this); // restore gravity settings _rigidBody->setGravity(oldGravity); + _ghost.setCollisionShape(_rigidBody->getCollisionShape()); // KINEMATIC_CONTROLLER_HACK } + // KINEMATIC_CONTROLLER_HACK + int16_t group = BULLET_COLLISION_GROUP_MY_AVATAR; + int16_t mask = BULLET_COLLISION_MASK_MY_AVATAR & (~ group); + _ghost.setCollisionGroupAndMask(group, mask); + _ghost.setCollisionWorld(_dynamicsWorld); + _ghost.setDistanceToFeet(_radius + _halfHeight); + _ghost.setMaxStepHeight(0.75f * (_radius + _halfHeight)); // HACK + _ghost.setMinWallAngle(PI / 4.0f); // HACK + _ghost.setUpDirection(_currentUp); + _ghost.setGravity(DEFAULT_CHARACTER_GRAVITY); } if (_dynamicsWorld) { if (_pendingFlags & PENDING_FLAG_UPDATE_SHAPE) { @@ -188,54 +201,67 @@ const btScalar MIN_TARGET_SPEED_SQUARED = MIN_TARGET_SPEED * MIN_TARGET_SPEED; void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) { 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 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(). + if (_moveKinematically) { + // KINEMATIC_CONTROLLER_HACK + btTransform transform = _rigidBody->getWorldTransform(); + transform.setOrigin(_ghost.getWorldTransform().getOrigin()); + _ghost.setWorldTransform(transform); + _ghost.setMotorVelocity(_simpleMotorVelocity); + float overshoot = 1.0f * _radius; + _ghost.move(dt, overshoot); + _rigidBody->setWorldTransform(_ghost.getWorldTransform()); + _rigidBody->setLinearVelocity(_ghost.getLinearVelocity()); + } else { + // Dynamicaly compute a follow velocity to move this body toward the _followDesiredBodyTransform. + // 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(). - if (_following) { - // OUTOFBODY_HACK -- these consts were copied from elsewhere, and then tuned - const float NORMAL_WALKING_SPEED = 0.5f; - const float FOLLOW_TIME = 0.8f; - const float FOLLOW_ROTATION_THRESHOLD = cosf(PI / 6.0f); + _rigidBody->setLinearVelocity(velocity + _parentVelocity); + if (_following) { + // OUTOFBODY_HACK -- these consts were copied from elsewhere, and then tuned + const float NORMAL_WALKING_SPEED = 0.5f; + const float FOLLOW_TIME = 0.8f; + const float FOLLOW_ROTATION_THRESHOLD = cosf(PI / 6.0f); - const float MAX_ANGULAR_SPEED = FOLLOW_ROTATION_THRESHOLD / FOLLOW_TIME; + const float MAX_ANGULAR_SPEED = FOLLOW_ROTATION_THRESHOLD / FOLLOW_TIME; - btTransform bodyTransform = _rigidBody->getWorldTransform(); + btTransform bodyTransform = _rigidBody->getWorldTransform(); - btVector3 startPos = bodyTransform.getOrigin(); - btVector3 deltaPos = _followDesiredBodyTransform.getOrigin() - startPos; - btVector3 vel = deltaPos * (0.5f / dt); - btScalar speed = vel.length(); - if (speed > NORMAL_WALKING_SPEED) { - vel *= NORMAL_WALKING_SPEED / speed; + btVector3 startPos = bodyTransform.getOrigin(); + btVector3 deltaPos = _followDesiredBodyTransform.getOrigin() - startPos; + btVector3 vel = deltaPos * (0.5f / dt); + btScalar speed = vel.length(); + if (speed > NORMAL_WALKING_SPEED) { + vel *= NORMAL_WALKING_SPEED / speed; + } + btVector3 linearDisplacement = vel * dt; + btVector3 endPos = startPos + linearDisplacement; + + btQuaternion startRot = bodyTransform.getRotation(); + glm::vec2 currentFacing = getFacingDir2D(bulletToGLM(startRot)); + glm::vec2 currentRight(currentFacing.y, -currentFacing.x); + glm::vec2 desiredFacing = getFacingDir2D(bulletToGLM(_followDesiredBodyTransform.getRotation())); + float deltaAngle = acosf(glm::clamp(glm::dot(currentFacing, desiredFacing), -1.0f, 1.0f)); + float angularSpeed = 0.5f * deltaAngle / dt; + if (angularSpeed > MAX_ANGULAR_SPEED) { + angularSpeed *= MAX_ANGULAR_SPEED / angularSpeed; + } + float sign = copysignf(1.0f, glm::dot(desiredFacing, currentRight)); + btQuaternion angularDisplacement = btQuaternion(btVector3(0.0f, 1.0f, 0.0f), sign * angularSpeed * dt); + btQuaternion endRot = angularDisplacement * startRot; + + // in order to accumulate displacement of avatar position, we need to take _shapeLocalOffset into account. + btVector3 shapeLocalOffset = glmToBullet(_shapeLocalOffset); + btVector3 swingDisplacement = rotateVector(endRot, -shapeLocalOffset) - rotateVector(startRot, -shapeLocalOffset); + + _followLinearDisplacement = linearDisplacement + swingDisplacement + _followLinearDisplacement; + _followAngularDisplacement = angularDisplacement * _followAngularDisplacement; + + _rigidBody->setWorldTransform(btTransform(endRot, endPos)); } - btVector3 linearDisplacement = vel * dt; - btVector3 endPos = startPos + linearDisplacement; - - btQuaternion startRot = bodyTransform.getRotation(); - glm::vec2 currentFacing = getFacingDir2D(bulletToGLM(startRot)); - glm::vec2 currentRight(currentFacing.y, -currentFacing.x); - glm::vec2 desiredFacing = getFacingDir2D(bulletToGLM(_followDesiredBodyTransform.getRotation())); - float deltaAngle = acosf(glm::clamp(glm::dot(currentFacing, desiredFacing), -1.0f, 1.0f)); - float angularSpeed = 0.5f * deltaAngle / dt; - if (angularSpeed > MAX_ANGULAR_SPEED) { - angularSpeed *= MAX_ANGULAR_SPEED / angularSpeed; - } - float sign = copysignf(1.0f, glm::dot(desiredFacing, currentRight)); - btQuaternion angularDisplacement = btQuaternion(btVector3(0.0f, 1.0f, 0.0f), sign * angularSpeed * dt); - btQuaternion endRot = angularDisplacement * startRot; - - // in order to accumulate displacement of avatar position, we need to take _shapeLocalOffset into account. - btVector3 shapeLocalOffset = glmToBullet(_shapeLocalOffset); - btVector3 swingDisplacement = rotateVector(endRot, -shapeLocalOffset) - rotateVector(startRot, -shapeLocalOffset); - - _followLinearDisplacement = linearDisplacement + swingDisplacement + _followLinearDisplacement; - _followAngularDisplacement = angularDisplacement * _followAngularDisplacement; - - _rigidBody->setWorldTransform(btTransform(endRot, endPos)); _followTime += dt; + _ghost.setWorldTransform(_rigidBody->getWorldTransform()); } } @@ -353,6 +379,7 @@ void CharacterController::handleChangedCollisionGroup() { void CharacterController::updateUpAxis(const glm::quat& rotation) { _currentUp = quatRotate(glmToBullet(rotation), LOCAL_UP_AXIS); + _ghost.setUpDirection(_currentUp); if (_state != State::Hover && _rigidBody) { if (_collisionGroup == BULLET_COLLISION_GROUP_COLLISIONLESS) { _rigidBody->setGravity(btVector3(0.0f, 0.0f, 0.0f)); @@ -414,6 +441,10 @@ glm::vec3 CharacterController::getLinearVelocity() const { return velocity; } +void CharacterController::setCapsuleRadius(float radius) { + _radius = radius; +} + glm::vec3 CharacterController::getVelocityChange() const { if (_rigidBody) { return bulletToGLM(_velocityChange); @@ -490,6 +521,7 @@ void CharacterController::applyMotor(int index, btScalar dt, btVector3& worldVel // add components back together and rotate into world-frame velocity = (hVelocity + vVelocity).rotate(axis, angle); + _simpleMotorVelocity += maxTau * (hTargetVelocity + vTargetVelocity).rotate(axis, angle); // store velocity and weights velocities.push_back(velocity); @@ -507,6 +539,7 @@ void CharacterController::computeNewVelocity(btScalar dt, btVector3& velocity) { velocities.reserve(_motors.size()); std::vector weights; weights.reserve(_motors.size()); + _simpleMotorVelocity = btVector3(0.0f, 0.0f, 0.0f); for (int i = 0; i < (int)_motors.size(); ++i) { applyMotor(i, dt, velocity, velocities, weights); } @@ -522,6 +555,7 @@ void CharacterController::computeNewVelocity(btScalar dt, btVector3& velocity) { for (size_t i = 0; i < velocities.size(); ++i) { velocity += (weights[i] / totalWeight) * velocities[i]; } + _simpleMotorVelocity /= totalWeight; } if (velocity.length2() < MIN_TARGET_SPEED_SQUARED) { velocity = btVector3(0.0f, 0.0f, 0.0f); @@ -686,3 +720,10 @@ void CharacterController::setFlyingAllowed(bool value) { } } } + +void CharacterController::setMoveKinematically(bool kinematic) { + if (kinematic != _moveKinematically) { + _moveKinematically = kinematic; + _pendingFlags |= PENDING_FLAG_UPDATE_SHAPE; + } +} diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h index 2d8cf489de..3b1ec6e945 100644 --- a/libraries/physics/src/CharacterController.h +++ b/libraries/physics/src/CharacterController.h @@ -9,8 +9,8 @@ // See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html // -#ifndef hifi_CharacterControllerInterface_h -#define hifi_CharacterControllerInterface_h +#ifndef hifi_CharacterController_h +#define hifi_CharacterController_h #include #include @@ -22,6 +22,7 @@ #include #include "BulletUtil.h" +#include "CharacterGhostObject.h" const uint32_t PENDING_FLAG_ADD_TO_SIMULATION = 1U << 0; const uint32_t PENDING_FLAG_REMOVE_FROM_SIMULATION = 1U << 1; @@ -95,6 +96,7 @@ public: glm::vec3 getLinearVelocity() const; glm::vec3 getVelocityChange() const; + virtual void setCapsuleRadius(float radius); float getCapsuleRadius() const { return _radius; } float getCapsuleHalfHeight() const { return _halfHeight; } glm::vec3 getCapsuleLocalOffset() const { return _shapeLocalOffset; } @@ -110,10 +112,6 @@ public: void setLocalBoundingBox(const glm::vec3& corner, const glm::vec3& scale); - /* - bool isEnabled() const { return _enabled; } // thread-safe - void setEnabled(bool enabled); - */ bool isEnabledAndReady() const { return _dynamicsWorld; } void setCollisionGroup(int16_t group); @@ -124,6 +122,7 @@ public: void setFlyingAllowed(bool value); + void setMoveKinematically(bool kinematic); // KINEMATIC_CONTROLLER_HACK protected: #ifdef DEBUG_STATE_CHANGE @@ -146,11 +145,13 @@ protected: }; std::vector _motors; + CharacterGhostObject _ghost; // KINEMATIC_CONTROLLER_HACK btVector3 _currentUp; btVector3 _targetVelocity; btVector3 _parentVelocity; btVector3 _preSimulationVelocity; btVector3 _velocityChange; + btVector3 _simpleMotorVelocity; // KINEMATIC_CONTROLLER_HACK btTransform _followDesiredBodyTransform; btTransform _characterBodyTransform; @@ -188,7 +189,8 @@ protected: uint32_t _previousFlags { 0 }; bool _flyingAllowed { true }; + bool _moveKinematically { false }; // KINEMATIC_CONTROLLER_HACK int16_t _collisionGroup { BULLET_COLLISION_GROUP_MY_AVATAR }; }; -#endif // hifi_CharacterControllerInterface_h +#endif // hifi_CharacterController_h diff --git a/libraries/physics/src/CharacterGhostObject.cpp b/libraries/physics/src/CharacterGhostObject.cpp new file mode 100644 index 0000000000..f3e1da3585 --- /dev/null +++ b/libraries/physics/src/CharacterGhostObject.cpp @@ -0,0 +1,390 @@ +// +// CharacterGhostObject.cpp +// libraries/physcis/src +// +// Created by Andrew Meadows 2016.08.26 +// Copyright 2016 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#include "CharacterGhostObject.h" + +#include + +#include "CharacterRayResult.h" + +const btScalar DEFAULT_STEP_UP_HEIGHT = 0.5f; + + +CharacterGhostObject::~CharacterGhostObject() { + removeFromWorld(); + setCollisionShape(nullptr); +} + +void CharacterGhostObject::setCollisionGroupAndMask(int16_t group, int16_t mask) { + _collisionFilterGroup = group; + _collisionFilterMask = mask; + // TODO: if this probe is in the world reset ghostObject overlap cache +} + +void CharacterGhostObject::getCollisionGroupAndMask(int16_t& group, int16_t& mask) const { + group = _collisionFilterGroup; + mask = _collisionFilterMask; +} + + +void CharacterGhostObject::setUpDirection(const btVector3& up) { + btScalar length = up.length(); + if (length > FLT_EPSILON) { + _upDirection /= length; + } else { + _upDirection = btVector3(0.0f, 1.0f, 0.0f); + } +} + +void CharacterGhostObject::setMotorVelocity(const btVector3& velocity) { + _motorVelocity = velocity; + _motorSpeed = _motorVelocity.length(); +} + +// override of btCollisionObject::setCollisionShape() +void CharacterGhostObject::setCollisionShape(btCollisionShape* shape) { + assert(!shape || shape->isConvex()); // if shape is valid then please make it convex + if (shape != getCollisionShape()) { + bool wasInWorld = _inWorld; + removeFromWorld(); + btCollisionObject::setCollisionShape(shape); + if (wasInWorld) { + assert(shape); // please remove from world before setting null shape + addToWorld(); + } + } +} + +void CharacterGhostObject::setCollisionWorld(btCollisionWorld* world) { + if (world != _world) { + removeFromWorld(); + _world = world; + addToWorld(); + } +} + +void CharacterGhostObject::move(btScalar dt, btScalar overshoot) { + _onFloor = false; + assert(_world && _inWorld); + updateVelocity(dt); + + // resolve any penetrations before sweeping + int32_t MAX_LOOPS = 4; + int32_t numExtractions = 0; + btVector3 totalPosition(0.0f, 0.0f, 0.0f); + while (numExtractions < MAX_LOOPS) { + if (resolvePenetration(numExtractions)) { + numExtractions = 0; + break; + } + totalPosition += getWorldTransform().getOrigin(); + ++numExtractions; + } + if (numExtractions > 1) { + // penetration resolution was probably oscillating between opposing objects + // so we use the average of the solutions + totalPosition /= btScalar(numExtractions); + btTransform transform = getWorldTransform(); + transform.setOrigin(totalPosition); + setWorldTransform(transform); + + // TODO: figure out how to untrap character + } + if (_onFloor) { + // a floor was identified during resolvePenetration() + _hovering = false; + updateTraction(); + } + + btVector3 forwardSweep = dt * _linearVelocity; + btScalar stepDistance = forwardSweep.length(); + btScalar MIN_SWEEP_DISTANCE = 0.0001f; + if (stepDistance < MIN_SWEEP_DISTANCE) { + // not moving, no need to sweep + updateHoverState(getWorldTransform()); + return; + } + + const btCollisionShape* shape = getCollisionShape(); + assert(shape->isConvex()); + const btConvexShape* convexShape= static_cast(shape); + + // augment forwardSweep to help slow moving sweeps get over steppable ledges + btScalar margin = shape->getMargin(); + if (overshoot < margin) { + overshoot = margin; + } + btScalar longSweepDistance = stepDistance + overshoot; + forwardSweep *= longSweepDistance / stepDistance; + + // expand this object's Aabb in the broadphase and + // update the pairCache for the sweepTests we intend to do + btVector3 minAabb, maxAabb; + getCollisionShape()->getAabb(getWorldTransform(), minAabb, maxAabb); + minAabb.setMin(minAabb - btVector3(margin, margin, margin)); + maxAabb.setMax(maxAabb + btVector3(margin, margin, margin)); + minAabb.setMin(minAabb + forwardSweep); + maxAabb.setMax(maxAabb + forwardSweep); + minAabb.setMin(minAabb + _maxStepHeight * _upDirection); + maxAabb.setMax(maxAabb + _maxStepHeight * _upDirection); + + // this updates both pairCaches: world broadphase and ghostobject + _world->getBroadphase()->setAabb(getBroadphaseHandle(), minAabb, maxAabb, _world->getDispatcher()); + + // step forward + CharacterSweepResult result(this); + btTransform startTransform = getWorldTransform(); + btTransform transform = startTransform; + btTransform nextTransform = transform; + nextTransform.setOrigin(transform.getOrigin() + forwardSweep); + sweepTest(convexShape, transform, nextTransform, result); // forward + + if (!result.hasHit()) { + nextTransform.setOrigin(transform.getOrigin() + (stepDistance / longSweepDistance) * forwardSweep); + setWorldTransform(nextTransform); + updateHoverState(nextTransform); + updateTraction(); + return; + } + + // check if this hit is obviously unsteppable + btVector3 hitFromBase = result.m_hitPointWorld - (transform.getOrigin() - (_distanceToFeet * _upDirection)); + btScalar hitHeight = hitFromBase.dot(_upDirection); + if (hitHeight > _maxStepHeight) { + // capsule can't step over the obstacle so move forward as much as possible before we bail + btVector3 forwardTranslation = result.m_closestHitFraction * forwardSweep; + btScalar forwardDistance = forwardTranslation.length(); + if (forwardDistance > stepDistance) { + forwardTranslation *= stepDistance / forwardDistance; + } + transform.setOrigin(transform.getOrigin() + forwardTranslation); + setWorldTransform(transform); + return; + } + // if we get here then we hit something that might be steppable + + // remember the forward sweep hit fraction for later + btScalar forwardSweepHitFraction = result.m_closestHitFraction; + + // figure out how high we can step up + btScalar availableStepHeight = measureAvailableStepHeight(); + + // raise by availableStepHeight before sweeping forward + result.resetHitHistory(); + transform.setOrigin(startTransform.getOrigin() + availableStepHeight * _upDirection); + nextTransform.setOrigin(transform.getOrigin() + forwardSweep); + sweepTest(convexShape, transform, nextTransform, result); + if (result.hasHit()) { + transform.setOrigin(transform.getOrigin() + result.m_closestHitFraction * forwardSweep); + } else { + transform = nextTransform; + } + + // sweep down in search of future landing spot + result.resetHitHistory(); + btVector3 downSweep = (dt * _linearVelocity.dot(_upDirection) - availableStepHeight) * _upDirection; + nextTransform.setOrigin(transform.getOrigin() + downSweep); + sweepTest(convexShape, transform, nextTransform, result); + if (result.hasHit() && result.m_hitNormalWorld.dot(_upDirection) > _maxWallNormalUpComponent) { + // can stand on future landing spot, so we interpolate toward it + _floorNormal = result.m_hitNormalWorld; + _onFloor = true; + _hovering = false; + nextTransform.setOrigin(transform.getOrigin() + result.m_closestHitFraction * downSweep); + btVector3 totalStep = nextTransform.getOrigin() - startTransform.getOrigin(); + transform.setOrigin(startTransform.getOrigin() + (stepDistance / totalStep.length()) * totalStep); + } else { + // either there is no future landing spot, or there is but we can't stand on it + // in any case: we go forward as much as possible + transform.setOrigin(startTransform.getOrigin() + forwardSweepHitFraction * (stepDistance / longSweepDistance) * forwardSweep); + } + setWorldTransform(transform); + updateTraction(); +} + +void CharacterGhostObject::removeFromWorld() { + if (_world && _inWorld) { + _world->removeCollisionObject(this); + _inWorld = false; + } +} + +void CharacterGhostObject::addToWorld() { + if (_world && !_inWorld) { + assert(getCollisionShape()); + setCollisionFlags(getCollisionFlags() | btCollisionObject::CF_NO_CONTACT_RESPONSE); + //assert(getBroadphaseHandle()); + //int16_t group = getBroadphaseHandle()->m_collisionFilterGroup; + //int16_t mask = getBroadphaseHandle()->m_collisionFilterMask; + _world->addCollisionObject(this, _collisionFilterGroup, _collisionFilterMask); + _inWorld = true; + } +} + +bool CharacterGhostObject::sweepTest( + const btConvexShape* shape, + const btTransform& start, + const btTransform& end, + CharacterSweepResult& result) const { + if (_world && _inWorld) { + assert(shape); + + btScalar allowedPenetration = _world->getDispatchInfo().m_allowedCcdPenetration; + convexSweepTest(shape, start, end, result, allowedPenetration); + + if (result.hasHit()) { + return true; + } + } + return false; +} + +bool CharacterGhostObject::rayTest(const btVector3& start, + const btVector3& end, + CharacterRayResult& result) const { + if (_world && _inWorld) { + _world->rayTest(start, end, result); + } + return result.hasHit(); +} + +bool CharacterGhostObject::resolvePenetration(int numTries) { + assert(_world); + // We refresh the overlapping pairCache because any previous movement may have pushed us + // into an overlap that was not in the cache. + refreshOverlappingPairCache(); + + // compute collision details + btHashedOverlappingPairCache* pairCache = getOverlappingPairCache(); + _world->getDispatcher()->dispatchAllCollisionPairs(pairCache, _world->getDispatchInfo(), _world->getDispatcher()); + + // loop over contact manifolds + btTransform transform = getWorldTransform(); + btVector3 position = transform.getOrigin(); + btVector3 minBox =btVector3(0.0f, 0.0f, 0.0f); + btVector3 maxBox = btVector3(0.0f, 0.0f, 0.0f); + btManifoldArray manifoldArray; + const btScalar PENETRATION_RESOLUTION_FUDGE_FACTOR = 0.0001f; // speeds up resolvation + + int numPairs = pairCache->getNumOverlappingPairs(); + for (int i = 0; i < numPairs; i++) { + manifoldArray.resize(0); + btBroadphasePair* collisionPair = &(pairCache->getOverlappingPairArray()[i]); + + btCollisionObject* obj0 = static_cast(collisionPair->m_pProxy0->m_clientObject); + btCollisionObject* obj1 = static_cast(collisionPair->m_pProxy1->m_clientObject); + + if ((obj0 && !obj0->hasContactResponse()) && (obj1 && !obj1->hasContactResponse())) { + // we know this probe has no contact response + // but neither does the other object so skip this manifold + continue; + } + + if (!collisionPair->m_algorithm) { + // null m_algorithm means the two shape types don't know how to collide! + // shouldn't fall in here but just in case + continue; + } + + btScalar mostFloorPenetration = 0.0f; + collisionPair->m_algorithm->getAllContactManifolds(manifoldArray); + for (int j = 0;j < manifoldArray.size(); j++) { + btPersistentManifold* manifold = manifoldArray[j]; + btScalar directionSign = (manifold->getBody0() == this) ? btScalar(1.0) : btScalar(-1.0); + for (int p = 0; p < manifold->getNumContacts(); p++) { + const btManifoldPoint& pt = manifold->getContactPoint(p); + if (pt.getDistance() > 0.0f) { + continue; + } + + // normal always points from object to character + btVector3 normal = directionSign * pt.m_normalWorldOnB; + + btScalar penetrationDepth = pt.getDistance(); + if (penetrationDepth < mostFloorPenetration) { // remember penetrationDepth is negative + btScalar normalDotUp = normal.dot(_upDirection); + if (normalDotUp > _maxWallNormalUpComponent) { + mostFloorPenetration = penetrationDepth; + _floorNormal = normal; + _onFloor = true; + } + } + + btVector3 penetration = (-penetrationDepth + PENETRATION_RESOLUTION_FUDGE_FACTOR) * normal; + minBox.setMin(penetration); + maxBox.setMax(penetration); + } + } + } + + btVector3 restore = maxBox + minBox; + if (restore.length2() > 0.0f) { + transform.setOrigin(position + restore); + setWorldTransform(transform); + return false; + } + return true; +} + +void CharacterGhostObject::refreshOverlappingPairCache() { + assert(_world && _inWorld); + btVector3 minAabb, maxAabb; + getCollisionShape()->getAabb(getWorldTransform(), minAabb, maxAabb); + + // this updates both pairCaches: world broadphase and ghostobject + _world->getBroadphase()->setAabb(getBroadphaseHandle(), minAabb, maxAabb, _world->getDispatcher()); +} + +void CharacterGhostObject::updateVelocity(btScalar dt) { + if (_hovering) { + _linearVelocity *= 0.99f; // HACK damping + } else { + _linearVelocity += (dt * _gravity) * _upDirection; + } +} + +void CharacterGhostObject::updateTraction() { + if (_hovering) { + _linearVelocity = _motorVelocity; + } else if (_onFloor) { + btVector3 pathDirection = _floorNormal.cross(_motorVelocity).cross(_floorNormal); + btScalar pathLength = pathDirection.length(); + if (pathLength > FLT_EPSILON) { + _linearVelocity = (_motorSpeed / pathLength) * pathDirection; + } else { + _linearVelocity = btVector3(0.0f, 0.0f, 0.0f); + } + } +} + +btScalar CharacterGhostObject::measureAvailableStepHeight() const { + const btCollisionShape* shape = getCollisionShape(); + assert(shape->isConvex()); + const btConvexShape* convexShape= static_cast(shape); + + CharacterSweepResult result(this); + btTransform transform = getWorldTransform(); + btTransform nextTransform = transform; + nextTransform.setOrigin(transform.getOrigin() + _maxStepHeight * _upDirection); + sweepTest(convexShape, transform, nextTransform, result); + return result.m_closestHitFraction * _maxStepHeight; +} + +void CharacterGhostObject::updateHoverState(const btTransform& transform) { + // cast a ray down looking for floor support + CharacterRayResult rayResult(this); + btVector3 startPos = transform.getOrigin() - ((_distanceToFeet - 0.1f) * _upDirection); // 0.1 HACK to make ray hit + btVector3 endPos = startPos - (2.0f * _distanceToFeet) * _upDirection; + rayTest(startPos, endPos, rayResult); + // we're hovering if the ray didn't hit an object we can stand on + _hovering = !(rayResult.hasHit() && rayResult.m_hitNormalWorld.dot(_upDirection) > _maxWallNormalUpComponent); +} + diff --git a/libraries/physics/src/CharacterGhostObject.h b/libraries/physics/src/CharacterGhostObject.h new file mode 100644 index 0000000000..df569181b9 --- /dev/null +++ b/libraries/physics/src/CharacterGhostObject.h @@ -0,0 +1,83 @@ +// +// CharacterGhostObject.h +// libraries/physcis/src +// +// Created by Andrew Meadows 2016.08.26 +// Copyright 2016 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#ifndef hifi_CharacterGhostObject_h +#define hifi_CharacterGhostObject_h + +#include +#include + +#include "CharacterSweepResult.h" +#include "CharacterRayResult.h" + + +class CharacterGhostObject : public btPairCachingGhostObject { +public: + CharacterGhostObject() { } + ~CharacterGhostObject(); + + void setCollisionGroupAndMask(int16_t group, int16_t mask); + void getCollisionGroupAndMask(int16_t& group, int16_t& mask) const; + + void setDistanceToFeet(btScalar distance) { _distanceToFeet = distance; } + void setUpDirection(const btVector3& up); + void setMotorVelocity(const btVector3& velocity); + void setGravity(btScalar gravity) { _gravity = gravity; } // NOTE: we expect _gravity to be negative (in _upDirection) + void setMinWallAngle(btScalar angle) { _maxWallNormalUpComponent = cosf(angle); } + void setMaxStepHeight(btScalar height) { _maxStepHeight = height; } + + const btVector3& getLinearVelocity() const { return _linearVelocity; } + + void setCollisionShape(btCollisionShape* shape) override; + + void setCollisionWorld(btCollisionWorld* world); + + void move(btScalar dt, btScalar overshoot); + +protected: + void removeFromWorld(); + void addToWorld(); + + bool sweepTest(const btConvexShape* shape, + const btTransform& start, + const btTransform& end, + CharacterSweepResult& result) const; + bool rayTest(const btVector3& start, + const btVector3& end, + CharacterRayResult& result) const; + + bool resolvePenetration(int numTries); + void refreshOverlappingPairCache(); + void updateVelocity(btScalar dt); + void updateTraction(); + btScalar measureAvailableStepHeight() const; + void updateHoverState(const btTransform& transform); + +protected: + btVector3 _upDirection { 0.0f, 1.0f, 0.0f }; // input, up in world-frame + btVector3 _motorVelocity { 0.0f, 0.0f, 0.0f }; // input, velocity character is trying to achieve + btVector3 _linearVelocity { 0.0f, 0.0f, 0.0f }; // internal, actual character velocity + btVector3 _floorNormal { 0.0f, 0.0f, 0.0f }; // internal, probable floor normal + btCollisionWorld* _world { nullptr }; // input, pointer to world + btScalar _distanceToFeet { 0.0f }; // input, distance from object center to lowest point on shape + btScalar _motorSpeed { 0.0f }; // internal, cached for speed + btScalar _gravity { 0.0f }; // input, amplitude of gravity along _upDirection (should be negative) + btScalar _maxWallNormalUpComponent { 0.0f }; // input: max vertical component of wall normal + btScalar _maxStepHeight { 0.0f }; // input, max step height the character can climb + int16_t _collisionFilterGroup { 0 }; + int16_t _collisionFilterMask { 0 }; + bool _inWorld { false }; // internal, was added to world + bool _hovering { false }; // internal, + bool _onFloor { false }; // output, is actually standing on floor + bool _hasFloor { false }; // output, has floor underneath to fall on +}; + +#endif // hifi_CharacterGhostObject_h diff --git a/libraries/physics/src/CharacterRayResult.cpp b/libraries/physics/src/CharacterRayResult.cpp new file mode 100644 index 0000000000..45a84a8274 --- /dev/null +++ b/libraries/physics/src/CharacterRayResult.cpp @@ -0,0 +1,29 @@ +// +// CharaterRayResult.cpp +// libraries/physcis/src +// +// Created by Andrew Meadows 2016.09.05 +// Copyright 2016 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#include "CharacterRayResult.h" + +#include "CharacterGhostObject.h" + +CharacterRayResult::CharacterRayResult (const CharacterGhostObject* character) : + btCollisionWorld::ClosestRayResultCallback(btVector3(0.0f, 0.0f, 0.0f), btVector3(0.0f, 0.0f, 0.0f)), + _character(character) +{ + assert(_character); + _character->getCollisionGroupAndMask(m_collisionFilterGroup, m_collisionFilterMask); +} + +btScalar CharacterRayResult::addSingleResult(btCollisionWorld::LocalRayResult& rayResult, bool normalInWorldSpace) { + if (rayResult.m_collisionObject == _character) { + return 1.0f; + } + return ClosestRayResultCallback::addSingleResult (rayResult, normalInWorldSpace); +} diff --git a/libraries/physics/src/CharacterRayResult.h b/libraries/physics/src/CharacterRayResult.h new file mode 100644 index 0000000000..b74959aa34 --- /dev/null +++ b/libraries/physics/src/CharacterRayResult.h @@ -0,0 +1,44 @@ +// +// CharaterRayResult.h +// libraries/physcis/src +// +// Created by Andrew Meadows 2016.09.05 +// Copyright 2016 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#ifndef hifi_CharacterRayResult_h +#define hifi_CharacterRayResult_h + +#include +#include + +class CharacterGhostObject; + +class CharacterRayResult : public btCollisionWorld::ClosestRayResultCallback { +public: + CharacterRayResult (const CharacterGhostObject* character); + + virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult, bool normalInWorldSpace) override; + +protected: + const CharacterGhostObject* _character; + + // Note: Public data members inherited from ClosestRayResultCallback + // + // btVector3 m_rayFromWorld;//used to calculate hitPointWorld from hitFraction + // btVector3 m_rayToWorld; + // btVector3 m_hitNormalWorld; + // btVector3 m_hitPointWorld; + // + // Note: Public data members inherited from RayResultCallback + // + // btScalar m_closestHitFraction; + // const btCollisionObject* m_collisionObject; + // short int m_collisionFilterGroup; + // short int m_collisionFilterMask; +}; + +#endif // hifi_CharacterRayResult_h diff --git a/libraries/physics/src/CharacterSweepResult.cpp b/libraries/physics/src/CharacterSweepResult.cpp new file mode 100644 index 0000000000..86c9871e87 --- /dev/null +++ b/libraries/physics/src/CharacterSweepResult.cpp @@ -0,0 +1,40 @@ +// +// CharaterSweepResult.cpp +// libraries/physcis/src +// +// Created by Andrew Meadows 2016.09.01 +// Copyright 2016 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#include "CharacterSweepResult.h" + +#include "CharacterGhostObject.h" + +CharacterSweepResult::CharacterSweepResult(const CharacterGhostObject* character) + : btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0f, 0.0f, 0.0f), btVector3(0.0f, 0.0f, 0.0f)), + _character(character) +{ + // set collision group and mask to match _character + assert(_character); + _character->getCollisionGroupAndMask(m_collisionFilterGroup, m_collisionFilterMask); +} + +btScalar CharacterSweepResult::addSingleResult(btCollisionWorld::LocalConvexResult& convexResult, bool useWorldFrame) { + // skip objects that we shouldn't collide with + if (!convexResult.m_hitCollisionObject->hasContactResponse()) { + return btScalar(1.0); + } + if (convexResult.m_hitCollisionObject == _character) { + return btScalar(1.0); + } + + return ClosestConvexResultCallback::addSingleResult(convexResult, useWorldFrame); +} + +void CharacterSweepResult::resetHitHistory() { + m_hitCollisionObject = nullptr; + m_closestHitFraction = btScalar(1.0f); +} diff --git a/libraries/physics/src/CharacterSweepResult.h b/libraries/physics/src/CharacterSweepResult.h new file mode 100644 index 0000000000..f80bd0bb47 --- /dev/null +++ b/libraries/physics/src/CharacterSweepResult.h @@ -0,0 +1,45 @@ +// +// CharaterSweepResult.h +// libraries/physcis/src +// +// Created by Andrew Meadows 2016.09.01 +// Copyright 2016 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#ifndef hifi_CharacterSweepResult_h +#define hifi_CharacterSweepResult_h + +#include +#include + + +class CharacterGhostObject; + +class CharacterSweepResult : public btCollisionWorld::ClosestConvexResultCallback { +public: + CharacterSweepResult(const CharacterGhostObject* character); + virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult, bool useWorldFrame) override; + void resetHitHistory(); +protected: + const CharacterGhostObject* _character; + + // NOTE: Public data members inherited from ClosestConvexResultCallback: + // + // btVector3 m_convexFromWorld; // unused except by btClosestNotMeConvexResultCallback + // btVector3 m_convexToWorld; // unused except by btClosestNotMeConvexResultCallback + // btVector3 m_hitNormalWorld; + // btVector3 m_hitPointWorld; + // const btCollisionObject* m_hitCollisionObject; + // + // NOTE: Public data members inherited from ConvexResultCallback: + // + // btScalar m_closestHitFraction; + // short int m_collisionFilterGroup; + // short int m_collisionFilterMask; + +}; + +#endif // hifi_CharacterSweepResult_h From 418271e06bba44654e048a303688dfdee19a1018 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 14 Sep 2016 11:49:00 -0700 Subject: [PATCH 021/101] ghost tracks the collision group --- libraries/physics/src/CharacterController.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 81772c59b3..7cab21e7e2 100644 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -124,9 +124,7 @@ void CharacterController::setDynamicsWorld(btDynamicsWorld* world) { _ghost.setCollisionShape(_rigidBody->getCollisionShape()); // KINEMATIC_CONTROLLER_HACK } // KINEMATIC_CONTROLLER_HACK - int16_t group = BULLET_COLLISION_GROUP_MY_AVATAR; - int16_t mask = BULLET_COLLISION_MASK_MY_AVATAR & (~ group); - _ghost.setCollisionGroupAndMask(group, mask); + _ghost.setCollisionGroupAndMask(_collisionGroup, BULLET_COLLISION_MASK_MY_AVATAR & (~ _collisionGroup)); _ghost.setCollisionWorld(_dynamicsWorld); _ghost.setDistanceToFeet(_radius + _halfHeight); _ghost.setMaxStepHeight(0.75f * (_radius + _halfHeight)); // HACK @@ -355,6 +353,7 @@ void CharacterController::setCollisionGroup(int16_t group) { if (_collisionGroup != group) { _collisionGroup = group; _pendingFlags |= PENDING_FLAG_UPDATE_COLLISION_GROUP; + _ghost.setCollisionGroupAndMask(_collisionGroup, BULLET_COLLISION_MASK_MY_AVATAR & (~ _collisionGroup)); } } From d444603a54003e8720c7a85c880b24e1a07928fd Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 14 Sep 2016 14:57:47 -0700 Subject: [PATCH 022/101] simplified Aabb management for GhostObject --- libraries/physics/src/CharacterController.cpp | 6 ++- .../physics/src/CharacterGhostObject.cpp | 39 +++++++------------ libraries/physics/src/CharacterGhostObject.h | 7 +++- libraries/physics/src/CharacterGhostShape.cpp | 20 ++++++++++ libraries/physics/src/CharacterGhostShape.h | 26 +++++++++++++ 5 files changed, 69 insertions(+), 29 deletions(-) create mode 100644 libraries/physics/src/CharacterGhostShape.cpp create mode 100644 libraries/physics/src/CharacterGhostShape.h diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 7cab21e7e2..16f447f790 100644 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -11,6 +11,8 @@ #include "CharacterController.h" +//#include + #include #include "ObjectMotionState.h" @@ -121,7 +123,9 @@ void CharacterController::setDynamicsWorld(btDynamicsWorld* world) { _dynamicsWorld->addAction(this); // restore gravity settings _rigidBody->setGravity(oldGravity); - _ghost.setCollisionShape(_rigidBody->getCollisionShape()); // KINEMATIC_CONTROLLER_HACK + btCollisionShape* shape = _rigidBody->getCollisionShape(); + assert(shape && shape->getShapeType() == CAPSULE_SHAPE_PROXYTYPE); + _ghost.setCharacterShape(static_cast(shape)); // KINEMATIC_CONTROLLER_HACK } // KINEMATIC_CONTROLLER_HACK _ghost.setCollisionGroupAndMask(_collisionGroup, BULLET_COLLISION_MASK_MY_AVATAR & (~ _collisionGroup)); diff --git a/libraries/physics/src/CharacterGhostObject.cpp b/libraries/physics/src/CharacterGhostObject.cpp index f3e1da3585..250ff39e98 100644 --- a/libraries/physics/src/CharacterGhostObject.cpp +++ b/libraries/physics/src/CharacterGhostObject.cpp @@ -13,6 +13,7 @@ #include +#include "CharacterGhostShape.h" #include "CharacterRayResult.h" const btScalar DEFAULT_STEP_UP_HEIGHT = 0.5f; @@ -20,7 +21,11 @@ const btScalar DEFAULT_STEP_UP_HEIGHT = 0.5f; CharacterGhostObject::~CharacterGhostObject() { removeFromWorld(); - setCollisionShape(nullptr); + if (_ghostShape) { + delete _ghostShape; + _ghostShape = nullptr; + setCollisionShape(nullptr); + } } void CharacterGhostObject::setCollisionGroupAndMask(int16_t group, int16_t mask) { @@ -50,17 +55,14 @@ void CharacterGhostObject::setMotorVelocity(const btVector3& velocity) { } // override of btCollisionObject::setCollisionShape() -void CharacterGhostObject::setCollisionShape(btCollisionShape* shape) { - assert(!shape || shape->isConvex()); // if shape is valid then please make it convex - if (shape != getCollisionShape()) { - bool wasInWorld = _inWorld; - removeFromWorld(); - btCollisionObject::setCollisionShape(shape); - if (wasInWorld) { - assert(shape); // please remove from world before setting null shape - addToWorld(); - } +void CharacterGhostObject::setCharacterShape(btCapsuleShape* capsule) { + assert(capsule); + // we create our own CharacterGhostShape which has a larger Aabb for more reliable sweep tests + if (_ghostShape) { + delete _ghostShape; } + _ghostShape = new CharacterGhostShape(capsule->getRadius(), 2.0f * capsule->getHalfHeight()); + setCollisionShape(_ghostShape); } void CharacterGhostObject::setCollisionWorld(btCollisionWorld* world) { @@ -125,20 +127,6 @@ void CharacterGhostObject::move(btScalar dt, btScalar overshoot) { btScalar longSweepDistance = stepDistance + overshoot; forwardSweep *= longSweepDistance / stepDistance; - // expand this object's Aabb in the broadphase and - // update the pairCache for the sweepTests we intend to do - btVector3 minAabb, maxAabb; - getCollisionShape()->getAabb(getWorldTransform(), minAabb, maxAabb); - minAabb.setMin(minAabb - btVector3(margin, margin, margin)); - maxAabb.setMax(maxAabb + btVector3(margin, margin, margin)); - minAabb.setMin(minAabb + forwardSweep); - maxAabb.setMax(maxAabb + forwardSweep); - minAabb.setMin(minAabb + _maxStepHeight * _upDirection); - maxAabb.setMax(maxAabb + _maxStepHeight * _upDirection); - - // this updates both pairCaches: world broadphase and ghostobject - _world->getBroadphase()->setAabb(getBroadphaseHandle(), minAabb, maxAabb, _world->getDispatcher()); - // step forward CharacterSweepResult result(this); btTransform startTransform = getWorldTransform(); @@ -338,7 +326,6 @@ void CharacterGhostObject::refreshOverlappingPairCache() { assert(_world && _inWorld); btVector3 minAabb, maxAabb; getCollisionShape()->getAabb(getWorldTransform(), minAabb, maxAabb); - // this updates both pairCaches: world broadphase and ghostobject _world->getBroadphase()->setAabb(getBroadphaseHandle(), minAabb, maxAabb, _world->getDispatcher()); } diff --git a/libraries/physics/src/CharacterGhostObject.h b/libraries/physics/src/CharacterGhostObject.h index df569181b9..be8a68594e 100644 --- a/libraries/physics/src/CharacterGhostObject.h +++ b/libraries/physics/src/CharacterGhostObject.h @@ -18,6 +18,7 @@ #include "CharacterSweepResult.h" #include "CharacterRayResult.h" +class CharacterGhostShape; class CharacterGhostObject : public btPairCachingGhostObject { public: @@ -36,7 +37,7 @@ public: const btVector3& getLinearVelocity() const { return _linearVelocity; } - void setCollisionShape(btCollisionShape* shape) override; + void setCharacterShape(btCapsuleShape* capsule); void setCollisionWorld(btCollisionWorld* world); @@ -72,10 +73,12 @@ protected: btScalar _gravity { 0.0f }; // input, amplitude of gravity along _upDirection (should be negative) btScalar _maxWallNormalUpComponent { 0.0f }; // input: max vertical component of wall normal btScalar _maxStepHeight { 0.0f }; // input, max step height the character can climb + btCapsuleShape* _characterShape { nullptr }; // input, shape of character + CharacterGhostShape* _ghostShape{ nullptr }; // internal, shape whose Aabb is used for overlap cache int16_t _collisionFilterGroup { 0 }; int16_t _collisionFilterMask { 0 }; bool _inWorld { false }; // internal, was added to world - bool _hovering { false }; // internal, + bool _hovering { false }; // internal, bool _onFloor { false }; // output, is actually standing on floor bool _hasFloor { false }; // output, has floor underneath to fall on }; diff --git a/libraries/physics/src/CharacterGhostShape.cpp b/libraries/physics/src/CharacterGhostShape.cpp new file mode 100644 index 0000000000..bf031ec569 --- /dev/null +++ b/libraries/physics/src/CharacterGhostShape.cpp @@ -0,0 +1,20 @@ +// +// CharacterGhostShape.cpp +// libraries/physcis/src +// +// Created by Andrew Meadows 2016.09.14 +// Copyright 2016 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#include "CharacterGhostShape.h" + +void CharacterGhostShape::getAabb (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { + btCapsuleShape::getAabb(t, aabbMin, aabbMax); + // double the size of the Aabb by expanding both corners by half the extent + btVector3 expansion = 0.5f * (aabbMax - aabbMin); + aabbMin -= expansion; + aabbMax += expansion; +} diff --git a/libraries/physics/src/CharacterGhostShape.h b/libraries/physics/src/CharacterGhostShape.h new file mode 100644 index 0000000000..9c9efded34 --- /dev/null +++ b/libraries/physics/src/CharacterGhostShape.h @@ -0,0 +1,26 @@ +// +// CharacterGhostShape.h +// libraries/physcis/src +// +// Created by Andrew Meadows 2016.09.14 +// Copyright 2016 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#ifndef hifi_CharacterGhostShape_h +#define hifi_CharacterGhostShape_h + +#include + +class CharacterGhostShape : public btCapsuleShape { + // Same as btCapsuleShape but reports an expanded Aabb for larger ghost overlap cache +public: + CharacterGhostShape(btScalar radius, btScalar height) : btCapsuleShape(radius, height) { + } + + virtual void getAabb (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const override; +}; + +#endif // hifi_CharacterGhostShape_h From 821570fa76184ec61b157aa03a9074e4b3cfa35b Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 15 Sep 2016 09:32:03 -0700 Subject: [PATCH 023/101] fix Windows build and minor format/namechange --- interface/src/avatar/MyAvatar.cpp | 2 ++ libraries/physics/src/CharacterController.cpp | 2 +- libraries/physics/src/CharacterGhostObject.cpp | 5 +++-- libraries/physics/src/CharacterGhostObject.h | 4 +++- libraries/physics/src/CharacterRayResult.cpp | 2 ++ libraries/physics/src/CharacterSweepResult.cpp | 2 ++ 6 files changed, 13 insertions(+), 4 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 8d5ec30424..1822e5e9aa 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -1328,6 +1328,8 @@ void MyAvatar::prepareForPhysicsSimulation() { } void MyAvatar::harvestResultsFromPhysicsSimulation(float deltaTime) { + // ANDREW TODO -- measure maxHipOffsetRadius here and transmit that to Rig + glm::vec3 position = getPosition(); glm::quat orientation = getOrientation(); if (_characterController.isEnabledAndReady()) { diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 16f447f790..d3a38ed533 100644 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -125,7 +125,7 @@ void CharacterController::setDynamicsWorld(btDynamicsWorld* world) { _rigidBody->setGravity(oldGravity); btCollisionShape* shape = _rigidBody->getCollisionShape(); assert(shape && shape->getShapeType() == CAPSULE_SHAPE_PROXYTYPE); - _ghost.setCharacterShape(static_cast(shape)); // KINEMATIC_CONTROLLER_HACK + _ghost.setCharacterCapsule(static_cast(shape)); // KINEMATIC_CONTROLLER_HACK } // KINEMATIC_CONTROLLER_HACK _ghost.setCollisionGroupAndMask(_collisionGroup, BULLET_COLLISION_MASK_MY_AVATAR & (~ _collisionGroup)); diff --git a/libraries/physics/src/CharacterGhostObject.cpp b/libraries/physics/src/CharacterGhostObject.cpp index 250ff39e98..c503f8a927 100644 --- a/libraries/physics/src/CharacterGhostObject.cpp +++ b/libraries/physics/src/CharacterGhostObject.cpp @@ -11,6 +11,7 @@ #include "CharacterGhostObject.h" +#include #include #include "CharacterGhostShape.h" @@ -55,7 +56,7 @@ void CharacterGhostObject::setMotorVelocity(const btVector3& velocity) { } // override of btCollisionObject::setCollisionShape() -void CharacterGhostObject::setCharacterShape(btCapsuleShape* capsule) { +void CharacterGhostObject::setCharacterCapsule(btCapsuleShape* capsule) { assert(capsule); // we create our own CharacterGhostShape which has a larger Aabb for more reliable sweep tests if (_ghostShape) { @@ -240,7 +241,7 @@ bool CharacterGhostObject::rayTest(const btVector3& start, CharacterRayResult& result) const { if (_world && _inWorld) { _world->rayTest(start, end, result); - } + } return result.hasHit(); } diff --git a/libraries/physics/src/CharacterGhostObject.h b/libraries/physics/src/CharacterGhostObject.h index be8a68594e..8faf429542 100644 --- a/libraries/physics/src/CharacterGhostObject.h +++ b/libraries/physics/src/CharacterGhostObject.h @@ -12,6 +12,8 @@ #ifndef hifi_CharacterGhostObject_h #define hifi_CharacterGhostObject_h +#include + #include #include @@ -37,7 +39,7 @@ public: const btVector3& getLinearVelocity() const { return _linearVelocity; } - void setCharacterShape(btCapsuleShape* capsule); + void setCharacterCapsule(btCapsuleShape* capsule); void setCollisionWorld(btCollisionWorld* world); diff --git a/libraries/physics/src/CharacterRayResult.cpp b/libraries/physics/src/CharacterRayResult.cpp index 45a84a8274..e0d5212e52 100644 --- a/libraries/physics/src/CharacterRayResult.cpp +++ b/libraries/physics/src/CharacterRayResult.cpp @@ -11,6 +11,8 @@ #include "CharacterRayResult.h" +#include + #include "CharacterGhostObject.h" CharacterRayResult::CharacterRayResult (const CharacterGhostObject* character) : diff --git a/libraries/physics/src/CharacterSweepResult.cpp b/libraries/physics/src/CharacterSweepResult.cpp index 86c9871e87..13937354ae 100644 --- a/libraries/physics/src/CharacterSweepResult.cpp +++ b/libraries/physics/src/CharacterSweepResult.cpp @@ -11,6 +11,8 @@ #include "CharacterSweepResult.h" +#include + #include "CharacterGhostObject.h" CharacterSweepResult::CharacterSweepResult(const CharacterGhostObject* character) From 5c3da41067a292f87ecdde17fb71c01babee2e3a Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 15 Sep 2016 14:51:05 -0700 Subject: [PATCH 024/101] fix windows build --- libraries/physics/src/CharacterRayResult.cpp | 0 libraries/physics/src/CharacterSweepResult.cpp | 0 2 files changed, 0 insertions(+), 0 deletions(-) mode change 100644 => 100755 libraries/physics/src/CharacterRayResult.cpp mode change 100644 => 100755 libraries/physics/src/CharacterSweepResult.cpp diff --git a/libraries/physics/src/CharacterRayResult.cpp b/libraries/physics/src/CharacterRayResult.cpp old mode 100644 new mode 100755 diff --git a/libraries/physics/src/CharacterSweepResult.cpp b/libraries/physics/src/CharacterSweepResult.cpp old mode 100644 new mode 100755 From 83157b573a9002b31d82bb0f7b15bda60531918d Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 15 Sep 2016 14:51:21 -0700 Subject: [PATCH 025/101] measure max hips offset and clamp it in IK --- interface/src/avatar/MyAvatar.cpp | 10 ++++- .../animation/src/AnimInverseKinematics.cpp | 10 +++++ .../animation/src/AnimInverseKinematics.h | 3 ++ libraries/animation/src/Rig.cpp | 13 +++++++ libraries/animation/src/Rig.h | 1 + libraries/physics/src/CharacterController.cpp | 31 ++++++++++++++- libraries/physics/src/CharacterController.h | 3 +- .../physics/src/CharacterGhostObject.cpp | 38 ++++++++++--------- libraries/physics/src/CharacterGhostObject.h | 9 +++-- 9 files changed, 92 insertions(+), 26 deletions(-) mode change 100644 => 100755 interface/src/avatar/MyAvatar.cpp mode change 100644 => 100755 libraries/physics/src/CharacterController.cpp mode change 100644 => 100755 libraries/physics/src/CharacterGhostObject.cpp mode change 100644 => 100755 libraries/physics/src/CharacterGhostObject.h diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp old mode 100644 new mode 100755 index 9978d915fd..1ace282023 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -1354,7 +1354,15 @@ void MyAvatar::prepareForPhysicsSimulation() { } void MyAvatar::harvestResultsFromPhysicsSimulation(float deltaTime) { - // ANDREW TODO -- measure maxHipOffsetRadius here and transmit that to Rig + // figoure out how far the hips can move before they hit something + int hipsJoint = getJointIndex("Hips"); + glm::vec3 hipsPosition; // rig-frame + // OUTOFBODY_HACK -- hardcoded maxHipsOffsetRadius (ultimately must exceed FollowHelper lateral/forward/back walk thresholds) + float maxHipsOffsetRadius = 3.0f * _characterController.getCapsuleRadius(); + if (_rig->getJointPosition(hipsJoint, hipsPosition)) { + maxHipsOffsetRadius = _characterController.measureMaxHipsOffsetRadius(hipsPosition, maxHipsOffsetRadius); + } + _rig->setMaxHipsOffsetLength(maxHipsOffsetRadius); glm::vec3 position = getPosition(); glm::quat orientation = getOrientation(); diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 27300699c4..48e3fdb978 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -535,6 +535,11 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars // smooth transitions by relaxing _hipsOffset toward the new value const float HIPS_OFFSET_SLAVE_TIMESCALE = 0.15f; float tau = dt < HIPS_OFFSET_SLAVE_TIMESCALE ? dt / HIPS_OFFSET_SLAVE_TIMESCALE : 1.0f; + float newOffsetLength = glm::length(newHipsOffset); + if (newOffsetLength > _maxHipsOffsetLength) { + // clamp the hips offset + newHipsOffset *= _maxHipsOffsetLength / newOffsetLength; + } _hipsOffset += (newHipsOffset - _hipsOffset) * tau; } } @@ -548,6 +553,11 @@ void AnimInverseKinematics::clearIKJointLimitHistory() { } } +void AnimInverseKinematics::setMaxHipsOffsetLength(float maxLength) { + assert(maxLength > 0.0f); + _maxHipsOffsetLength = maxLength; +} + RotationConstraint* AnimInverseKinematics::getConstraint(int index) { RotationConstraint* constraint = nullptr; std::map::iterator constraintItr = _constraints.find(index); diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h index c9560c7383..7e4a7e5473 100644 --- a/libraries/animation/src/AnimInverseKinematics.h +++ b/libraries/animation/src/AnimInverseKinematics.h @@ -39,6 +39,8 @@ public: void clearIKJointLimitHistory(); + void setMaxHipsOffsetLength(float maxLength); + protected: void computeTargets(const AnimVariantMap& animVars, std::vector& targets, const AnimPoseVec& underPoses); void solveWithCyclicCoordinateDescent(const std::vector& targets); @@ -83,6 +85,7 @@ protected: // experimental data for moving hips during IK glm::vec3 _hipsOffset { Vectors::ZERO }; + float _maxHipsOffsetLength { 1.0f }; int _headIndex { -1 }; int _hipsIndex { -1 }; int _hipsParentIndex { -1 }; diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 2d11a1e17f..240190ae2c 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -310,6 +310,19 @@ void Rig::clearIKJointLimitHistory() { } } +void Rig::setMaxHipsOffsetLength(float maxLength) { + if (_animNode) { + _animNode->traverse([&](AnimNode::Pointer node) { + // only report clip nodes as valid roles. + auto ikNode = std::dynamic_pointer_cast(node); + if (ikNode) { + ikNode->setMaxHipsOffsetLength(maxLength); + } + return true; + }); + } +} + void Rig::setJointTranslation(int index, bool valid, const glm::vec3& translation, float priority) { if (isIndexValid(index)) { if (valid) { diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index 7eb0316889..f0cc68a828 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -104,6 +104,7 @@ public: void clearJointAnimationPriority(int index); void clearIKJointLimitHistory(); + void setMaxHipsOffsetLength(float maxLength); // geometry space void setJointState(int index, bool valid, const glm::quat& rotation, const glm::vec3& translation, float priority); diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp old mode 100644 new mode 100755 index d3a38ed533..256b64421b --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -323,7 +323,7 @@ void CharacterController::setState(State desiredState) { } } -void CharacterController::setLocalBoundingBox(const glm::vec3& corner, const glm::vec3& scale) { +void CharacterController::setLocalBoundingBox(const glm::vec3& minCorner, const glm::vec3& scale) { _boxScale = scale; float x = _boxScale.x; @@ -350,7 +350,7 @@ void CharacterController::setLocalBoundingBox(const glm::vec3& corner, const glm } // it's ok to change offset immediately -- there are no thread safety issues here - _shapeLocalOffset = corner + 0.5f * _boxScale; + _shapeLocalOffset = minCorner + 0.5f * _boxScale; } void CharacterController::setCollisionGroup(int16_t group) { @@ -724,6 +724,33 @@ void CharacterController::setFlyingAllowed(bool value) { } } +float CharacterController::measureMaxHipsOffsetRadius(const glm::vec3& currentHipsOffset, float maxSweepDistance) { + btVector3 hipsOffset = glmToBullet(currentHipsOffset); // rig-frame + btScalar hipsOffsetLength = hipsOffset.length(); + if (hipsOffsetLength > FLT_EPSILON) { + const btTransform& transform = _rigidBody->getWorldTransform(); + + // rotate into world-frame + btTransform rotation = transform; + rotation.setOrigin(btVector3(0.0f, 0.0f, 0.0f)); + btVector3 startPos = transform.getOrigin() - rotation * glmToBullet(_shapeLocalOffset); + btVector3 endPos = startPos + rotation * ((maxSweepDistance / hipsOffsetLength) * hipsOffset); + + // sweep test a sphere + btSphereShape sphere(_radius); + CharacterSweepResult result(&_ghost); + btTransform endTransform = transform; + endTransform.setOrigin(endPos); + _ghost.sweepTest(&sphere, transform, endTransform, result); + + // measure sweep success + if (result.hasHit()) { + maxSweepDistance *= result.m_closestHitFraction; + } + } + return maxSweepDistance; +} + void CharacterController::setMoveKinematically(bool kinematic) { if (kinematic != _moveKinematically) { _moveKinematically = kinematic; diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h index 3b1ec6e945..2a3a81b416 100644 --- a/libraries/physics/src/CharacterController.h +++ b/libraries/physics/src/CharacterController.h @@ -110,7 +110,7 @@ public: State getState() const { return _state; } - void setLocalBoundingBox(const glm::vec3& corner, const glm::vec3& scale); + void setLocalBoundingBox(const glm::vec3& minCorner, const glm::vec3& scale); bool isEnabledAndReady() const { return _dynamicsWorld; } @@ -122,6 +122,7 @@ public: void setFlyingAllowed(bool value); + float measureMaxHipsOffsetRadius(const glm::vec3& currentHipsOffset, float maxSweepDistance); void setMoveKinematically(bool kinematic); // KINEMATIC_CONTROLLER_HACK protected: diff --git a/libraries/physics/src/CharacterGhostObject.cpp b/libraries/physics/src/CharacterGhostObject.cpp old mode 100644 new mode 100755 index c503f8a927..903d84c646 --- a/libraries/physics/src/CharacterGhostObject.cpp +++ b/libraries/physics/src/CharacterGhostObject.cpp @@ -29,6 +29,8 @@ CharacterGhostObject::~CharacterGhostObject() { } } +const int16_t wtf = 9; // adebug wtf? + void CharacterGhostObject::setCollisionGroupAndMask(int16_t group, int16_t mask) { _collisionFilterGroup = group; _collisionFilterMask = mask; @@ -199,6 +201,24 @@ void CharacterGhostObject::move(btScalar dt, btScalar overshoot) { updateTraction(); } +bool CharacterGhostObject::sweepTest( + const btConvexShape* shape, + const btTransform& start, + const btTransform& end, + CharacterSweepResult& result) const { + if (_world && _inWorld) { + assert(shape); + + btScalar allowedPenetration = _world->getDispatchInfo().m_allowedCcdPenetration; + convexSweepTest(shape, start, end, result, allowedPenetration); + + if (result.hasHit()) { + return true; + } + } + return false; +} + void CharacterGhostObject::removeFromWorld() { if (_world && _inWorld) { _world->removeCollisionObject(this); @@ -218,24 +238,6 @@ void CharacterGhostObject::addToWorld() { } } -bool CharacterGhostObject::sweepTest( - const btConvexShape* shape, - const btTransform& start, - const btTransform& end, - CharacterSweepResult& result) const { - if (_world && _inWorld) { - assert(shape); - - btScalar allowedPenetration = _world->getDispatchInfo().m_allowedCcdPenetration; - convexSweepTest(shape, start, end, result, allowedPenetration); - - if (result.hasHit()) { - return true; - } - } - return false; -} - bool CharacterGhostObject::rayTest(const btVector3& start, const btVector3& end, CharacterRayResult& result) const { diff --git a/libraries/physics/src/CharacterGhostObject.h b/libraries/physics/src/CharacterGhostObject.h old mode 100644 new mode 100755 index 8faf429542..dd2f694a59 --- a/libraries/physics/src/CharacterGhostObject.h +++ b/libraries/physics/src/CharacterGhostObject.h @@ -16,6 +16,7 @@ #include #include +#include #include "CharacterSweepResult.h" #include "CharacterRayResult.h" @@ -45,14 +46,14 @@ public: void move(btScalar dt, btScalar overshoot); -protected: - void removeFromWorld(); - void addToWorld(); - bool sweepTest(const btConvexShape* shape, const btTransform& start, const btTransform& end, CharacterSweepResult& result) const; +protected: + void removeFromWorld(); + void addToWorld(); + bool rayTest(const btVector3& start, const btVector3& end, CharacterRayResult& result) const; From 7f381ac4c4045b113bde0fce913cbe2becb1e986 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 15 Sep 2016 15:33:04 -0700 Subject: [PATCH 026/101] remove unused variables --- libraries/physics/src/CharacterGhostObject.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/libraries/physics/src/CharacterGhostObject.cpp b/libraries/physics/src/CharacterGhostObject.cpp index 903d84c646..bdd147fb74 100755 --- a/libraries/physics/src/CharacterGhostObject.cpp +++ b/libraries/physics/src/CharacterGhostObject.cpp @@ -17,8 +17,6 @@ #include "CharacterGhostShape.h" #include "CharacterRayResult.h" -const btScalar DEFAULT_STEP_UP_HEIGHT = 0.5f; - CharacterGhostObject::~CharacterGhostObject() { removeFromWorld(); @@ -29,8 +27,6 @@ CharacterGhostObject::~CharacterGhostObject() { } } -const int16_t wtf = 9; // adebug wtf? - void CharacterGhostObject::setCollisionGroupAndMask(int16_t group, int16_t mask) { _collisionFilterGroup = group; _collisionFilterMask = mask; From c7caf9fe0667dbea92a5d017fc72ef14d3b4068c Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Thu, 15 Sep 2016 17:08:36 -0700 Subject: [PATCH 027/101] Turn and drive the HMD with linear velocity --- interface/src/avatar/MyAvatar.cpp | 173 ++++++++++++------ libraries/physics/src/CharacterController.cpp | 5 +- 2 files changed, 118 insertions(+), 60 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 9978d915fd..0b0da82452 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -1300,12 +1300,18 @@ void MyAvatar::updateMotors() { } const float DEFAULT_MOTOR_TIMESCALE = 0.2f; const float INVALID_MOTOR_TIMESCALE = 1.0e6f; - if (_isPushing || _isBraking || !_isBeingPushed) { - _characterController.addMotor(_actionMotorVelocity, motorRotation, DEFAULT_MOTOR_TIMESCALE, INVALID_MOTOR_TIMESCALE); + + if (!qApp->isHMDMode()) { + // OUTOFBODY_HACK: add default zero velocity motor to the characterController + _characterController.addMotor(glm::vec3(), motorRotation, DEFAULT_MOTOR_TIMESCALE, INVALID_MOTOR_TIMESCALE); } else { - // _isBeingPushed must be true --> disable action motor by giving it a long timescale, - // otherwise it's attempt to "stand in in place" could defeat scripted motor/thrusts - _characterController.addMotor(_actionMotorVelocity, motorRotation, INVALID_MOTOR_TIMESCALE); + if (_isPushing || _isBraking || !_isBeingPushed) { + _characterController.addMotor(_actionMotorVelocity, motorRotation, DEFAULT_MOTOR_TIMESCALE, INVALID_MOTOR_TIMESCALE); + } else { + // _isBeingPushed must be true --> disable action motor by giving it a long timescale, + // otherwise it's attempt to "stand in in place" could defeat scripted motor/thrusts + _characterController.addMotor(_actionMotorVelocity, motorRotation, INVALID_MOTOR_TIMESCALE); + } } } if (_motionBehaviors & AVATAR_MOTION_SCRIPTED_MOTOR_ENABLED) { @@ -1692,11 +1698,20 @@ void MyAvatar::updateOrientation(float deltaTime) { // update body orientation by movement inputs - setOrientation(getOrientation() * glm::quat(glm::radians(glm::vec3(0.0f, totalBodyYaw, 0.0f)))); + glm::quat deltaRotation = glm::quat(glm::radians(glm::vec3(0.0f, totalBodyYaw, 0.0f))); + setOrientation(getOrientation() * deltaRotation); getHead()->setBasePitch(getHead()->getBasePitch() + _driveKeys[PITCH] * _pitchSpeed * deltaTime); if (qApp->isHMDMode()) { + + // rotate the sensorToWorldMatrix about the HMD! + glm::vec3 hmdOffset = extractTranslation(getHMDSensorMatrix()); + _sensorToWorldMatrix = (_sensorToWorldMatrix * + createMatFromQuatAndPos(glm::quat(), hmdOffset) * + createMatFromQuatAndPos(deltaRotation, glm::vec3()) * + createMatFromQuatAndPos(glm::quat(), -hmdOffset)); + glm::quat orientation = glm::quat_cast(getSensorToWorldMatrix()) * getHMDSensorOrientation(); glm::quat bodyOrientation = getWorldBodyOrientation(); glm::quat localOrientation = glm::inverse(bodyOrientation) * orientation; @@ -1713,65 +1728,93 @@ void MyAvatar::updateOrientation(float deltaTime) { } void MyAvatar::updateActionMotor(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 motor 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(_actionMotorVelocity); - const float MIN_ACTION_BRAKE_SPEED = 0.1f; - _isBraking = _wasPushing || (_isBraking && speed > MIN_ACTION_BRAKE_SPEED); - } - // compute action input - glm::vec3 front = (_driveKeys[TRANSLATE_Z]) * IDENTITY_FRONT; - glm::vec3 right = (_driveKeys[TRANSLATE_X]) * IDENTITY_RIGHT; + if (qApp->isHMDMode()) { + // actions are constant velocity, for your comfort + // OUTOFBODY_HACK TODO: what about flying?!?! - 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; - } + // compute action input + glm::vec3 front = (_driveKeys[TRANSLATE_Z]) * IDENTITY_FRONT; + glm::vec3 right = (_driveKeys[TRANSLATE_X]) * IDENTITY_RIGHT; + glm::vec3 direction = front + right; - _wasPushing = _isPushing; - float directionLength = glm::length(direction); - _isPushing = directionLength > EPSILON; + _isPushing = false; + _isBeingPushed = false; - // normalize direction - if (_isPushing) { - direction /= directionLength; - } else { - 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(_actionMotorVelocity); - float finalMaxMotorSpeed = getUniformScale() * MAX_ACTION_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; + _wasPushing = _isPushing; + float directionLength = glm::length(direction); + _isPushing = directionLength > EPSILON; + // normalize direction if (_isPushing) { - if (motorSpeed < maxBoostSpeed) { - // an active action motor should never be slower than this - float boostCoefficient = (maxBoostSpeed - motorSpeed) / maxBoostSpeed; - motorSpeed += MIN_AVATAR_SPEED * boostCoefficient; - } else if (motorSpeed > finalMaxMotorSpeed) { - motorSpeed = finalMaxMotorSpeed; - } + direction /= directionLength; + } else { + direction = Vectors::ZERO; } - _actionMotorVelocity = motorSpeed * direction; - } else { - // we're interacting with a floor --> simple horizontal speed and exponential decay + _actionMotorVelocity = MAX_WALKING_SPEED * direction; + + } else { + 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 motor 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(_actionMotorVelocity); + const float MIN_ACTION_BRAKE_SPEED = 0.1f; + _isBraking = _wasPushing || (_isBraking && speed > MIN_ACTION_BRAKE_SPEED); + } + + // compute action 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; + } + + _wasPushing = _isPushing; + float directionLength = glm::length(direction); + _isPushing = directionLength > EPSILON; + + // normalize direction + if (_isPushing) { + direction /= directionLength; + } else { + 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(_actionMotorVelocity); + float finalMaxMotorSpeed = getUniformScale() * MAX_ACTION_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 action motor should never be slower than this + float boostCoefficient = (maxBoostSpeed - motorSpeed) / maxBoostSpeed; + motorSpeed += MIN_AVATAR_SPEED * boostCoefficient; + } else if (motorSpeed > finalMaxMotorSpeed) { + motorSpeed = finalMaxMotorSpeed; + } + } + _actionMotorVelocity = motorSpeed * direction; + } else { + // we're interacting with a floor --> simple horizontal speed and exponential decay + _actionMotorVelocity = MAX_WALKING_SPEED * direction; + } } float boomChange = _driveKeys[ZOOM]; @@ -1799,9 +1842,23 @@ void MyAvatar::updatePosition(float deltaTime) { measureMotionDerivatives(deltaTime); _moving = speed2 > MOVING_SPEED_THRESHOLD_SQUARED; } else { - // physics physics simulation updated elsewhere float speed2 = glm::length2(velocity); _moving = speed2 > MOVING_SPEED_THRESHOLD_SQUARED; + + // OUTOFBODY_HACK, apply _actionMotorVelocity directly to the sensorToWorld matrix! + glm::quat motorRotation; + glm::quat liftRotation; + swingTwistDecomposition(glmExtractRotation(_sensorToWorldMatrix * getHMDSensorMatrix()), _worldUpDirection, liftRotation, motorRotation); + + if (qApp->isHMDMode()) { + float speed2 = glm::length2(_actionMotorVelocity); + if (speed2 > MIN_AVATAR_SPEED_SQUARED) { + glm::vec3 worldVelocity = motorRotation * _actionMotorVelocity; + // update sensor to world position ourselves + glm::vec3 position = extractTranslation(_sensorToWorldMatrix) + deltaTime * worldVelocity; + _sensorToWorldMatrix[3] = glm::vec4(position, 1); + } + } } // capture the head rotation, in sensor space, when the user first indicates they would like to move/fly. diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index d3a38ed533..b66b80baeb 100644 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -222,9 +222,10 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) { _rigidBody->setLinearVelocity(velocity + _parentVelocity); if (_following) { // OUTOFBODY_HACK -- these consts were copied from elsewhere, and then tuned - const float NORMAL_WALKING_SPEED = 0.5f; + const float NORMAL_WALKING_SPEED = 1.5f; // actual walk speed is 2.5 m/sec const float FOLLOW_TIME = 0.8f; const float FOLLOW_ROTATION_THRESHOLD = cosf(PI / 6.0f); + const float FOLLOW_FACTOR = 0.5f; const float MAX_ANGULAR_SPEED = FOLLOW_ROTATION_THRESHOLD / FOLLOW_TIME; @@ -232,7 +233,7 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) { btVector3 startPos = bodyTransform.getOrigin(); btVector3 deltaPos = _followDesiredBodyTransform.getOrigin() - startPos; - btVector3 vel = deltaPos * (0.5f / dt); + btVector3 vel = deltaPos * (FOLLOW_FACTOR / dt); btScalar speed = vel.length(); if (speed > NORMAL_WALKING_SPEED) { vel *= NORMAL_WALKING_SPEED / speed; From 20418d5f58c49a5565ec599232342445408a74d4 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 15 Sep 2016 18:39:57 -0700 Subject: [PATCH 028/101] fix hips sweep test --- interface/src/avatar/MyAvatar.cpp | 5 +++- .../animation/src/AnimInverseKinematics.cpp | 25 ++++++++++++++++--- libraries/physics/src/CharacterController.cpp | 6 +++-- 3 files changed, 30 insertions(+), 6 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 1ace282023..d3acc15be6 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -1354,12 +1354,15 @@ void MyAvatar::prepareForPhysicsSimulation() { } void MyAvatar::harvestResultsFromPhysicsSimulation(float deltaTime) { - // figoure out how far the hips can move before they hit something + // figure out how far the hips can move before they hit something int hipsJoint = getJointIndex("Hips"); glm::vec3 hipsPosition; // rig-frame // OUTOFBODY_HACK -- hardcoded maxHipsOffsetRadius (ultimately must exceed FollowHelper lateral/forward/back walk thresholds) float maxHipsOffsetRadius = 3.0f * _characterController.getCapsuleRadius(); if (_rig->getJointPosition(hipsJoint, hipsPosition)) { + // OUTOFBODY_HACK -- flip PI about yAxis + hipsPosition.x *= -1.0f; + hipsPosition.z *= -1.0f; maxHipsOffsetRadius = _characterController.measureMaxHipsOffsetRadius(hipsPosition, maxHipsOffsetRadius); } _rig->setMaxHipsOffsetLength(maxHipsOffsetRadius); diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 48e3fdb978..68fc0b45dc 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -505,6 +505,12 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars // measure new _hipsOffset for next frame // by looking for discrepancies between where a targeted endEffector is // and where it wants to be (after IK solutions are done) + + // OUTOFBODY_HACK:use weighted average between HMD and other targets + // ANDREW TODO: change how HMD IK target is handled to allow torso to lean over + float HMD_WEIGHT = 10.0f; + float OTHER_WEIGHT = 1.0f; + float totalWeight = 0.0f; glm::vec3 newHipsOffset = Vectors::ZERO; for (auto& target: targets) { int targetIndex = target.getIndex(); @@ -516,21 +522,32 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars glm::vec3 under = _skeleton->getAbsolutePose(_headIndex, underPoses).trans; glm::vec3 actual = _skeleton->getAbsolutePose(_headIndex, _relativePoses).trans; const float HEAD_OFFSET_SLAVE_FACTOR = 0.65f; - newHipsOffset += HEAD_OFFSET_SLAVE_FACTOR * (actual - under); + newHipsOffset += (OTHER_WEIGHT * HEAD_OFFSET_SLAVE_FACTOR) * (actual - under); + totalWeight += OTHER_WEIGHT; } else if (target.getType() == IKTarget::Type::HmdHead) { + /* OUTOFBODY_HACK: keep this old code to remind us of what changed // we want to shift the hips to bring the head to its designated position glm::vec3 actual = _skeleton->getAbsolutePose(_headIndex, _relativePoses).trans; _hipsOffset += target.getTranslation() - actual; // and ignore all other targets newHipsOffset = _hipsOffset; break; + */ + glm::vec3 actual = _skeleton->getAbsolutePose(_headIndex, _relativePoses).trans; + newHipsOffset += HMD_WEIGHT * (target.getTranslation() - actual); + totalWeight += HMD_WEIGHT; } } else if (target.getType() == IKTarget::Type::RotationAndPosition) { glm::vec3 actualPosition = _skeleton->getAbsolutePose(targetIndex, _relativePoses).trans; glm::vec3 targetPosition = target.getTranslation(); - newHipsOffset += targetPosition - actualPosition; + newHipsOffset += OTHER_WEIGHT * (targetPosition - actualPosition); + totalWeight += OTHER_WEIGHT; } } + if (totalWeight == 0.0f) { + totalWeight = 1.0f; + } + newHipsOffset /= totalWeight; // smooth transitions by relaxing _hipsOffset toward the new value const float HIPS_OFFSET_SLAVE_TIMESCALE = 0.15f; @@ -555,7 +572,9 @@ void AnimInverseKinematics::clearIKJointLimitHistory() { void AnimInverseKinematics::setMaxHipsOffsetLength(float maxLength) { assert(maxLength > 0.0f); - _maxHipsOffsetLength = maxLength; + // OUTOFBODY_HACK: manually adjust scale here + const float METERS_TO_CENTIMETERS = 100.0f; + _maxHipsOffsetLength = METERS_TO_CENTIMETERS * maxLength; } RotationConstraint* AnimInverseKinematics::getConstraint(int index) { diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 256b64421b..9f5a88847f 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -734,14 +734,16 @@ float CharacterController::measureMaxHipsOffsetRadius(const glm::vec3& currentHi btTransform rotation = transform; rotation.setOrigin(btVector3(0.0f, 0.0f, 0.0f)); btVector3 startPos = transform.getOrigin() - rotation * glmToBullet(_shapeLocalOffset); + btTransform startTransform = transform; + startTransform.setOrigin(startPos); btVector3 endPos = startPos + rotation * ((maxSweepDistance / hipsOffsetLength) * hipsOffset); // sweep test a sphere btSphereShape sphere(_radius); CharacterSweepResult result(&_ghost); - btTransform endTransform = transform; + btTransform endTransform = startTransform; endTransform.setOrigin(endPos); - _ghost.sweepTest(&sphere, transform, endTransform, result); + _ghost.sweepTest(&sphere, startTransform, endTransform, result); // measure sweep success if (result.hasHit()) { From 61a05eb4d103e7d318f60e9b3f36d892b7d01198 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 16 Sep 2016 10:16:18 -0700 Subject: [PATCH 029/101] failed experiment for getting hip lean over table --- .../animation/src/AnimInverseKinematics.cpp | 41 ++++++++++++------- .../animation/src/AnimInverseKinematics.h | 1 + 2 files changed, 28 insertions(+), 14 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 68fc0b45dc..059f192f3c 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -244,6 +244,11 @@ int AnimInverseKinematics::solveTargetWithCCD(const IKTarget& target, AnimPoseVe // the tip's parent-relative as we proceed up the chain glm::quat tipParentOrientation = absolutePoses[pivotIndex].rot; + /* OUTOFBODY_HACK -- experimental override target type when HmdHead pushes outside hipsOffset limit + if (targetType == IKTarget::Type::HmdHead && _hipsAreOver) { + targetType = IKTarget::Type::RotationAndPosition; + } + */ if (targetType == IKTarget::Type::HmdHead) { // rotate tip directly to target orientation tipOrientation = target.getRotation(); @@ -525,17 +530,19 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars newHipsOffset += (OTHER_WEIGHT * HEAD_OFFSET_SLAVE_FACTOR) * (actual - under); totalWeight += OTHER_WEIGHT; } else if (target.getType() == IKTarget::Type::HmdHead) { - /* OUTOFBODY_HACK: keep this old code to remind us of what changed - // we want to shift the hips to bring the head to its designated position glm::vec3 actual = _skeleton->getAbsolutePose(_headIndex, _relativePoses).trans; - _hipsOffset += target.getTranslation() - actual; - // and ignore all other targets - newHipsOffset = _hipsOffset; - break; - */ - glm::vec3 actual = _skeleton->getAbsolutePose(_headIndex, _relativePoses).trans; - newHipsOffset += HMD_WEIGHT * (target.getTranslation() - actual); - totalWeight += HMD_WEIGHT; + glm::vec3 thisOffset = target.getTranslation() - actual; + glm::vec3 futureHipsOffset = _hipsOffset + thisOffset; + if (glm::length(futureHipsOffset) < _maxHipsOffsetLength) { + // it is imperative to shift the hips and bring the head to its designated position + // so we slam newHipsOffset here and ignore all other targets + newHipsOffset = futureHipsOffset; + totalWeight = 0.0f; + break; + } else { + newHipsOffset += HMD_WEIGHT * (target.getTranslation() - actual); + totalWeight += HMD_WEIGHT; + } } } else if (target.getType() == IKTarget::Type::RotationAndPosition) { glm::vec3 actualPosition = _skeleton->getAbsolutePose(targetIndex, _relativePoses).trans; @@ -544,13 +551,12 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars totalWeight += OTHER_WEIGHT; } } - if (totalWeight == 0.0f) { - totalWeight = 1.0f; + if (totalWeight > 1.0f) { + newHipsOffset /= totalWeight; } - newHipsOffset /= totalWeight; // smooth transitions by relaxing _hipsOffset toward the new value - const float HIPS_OFFSET_SLAVE_TIMESCALE = 0.15f; + const float HIPS_OFFSET_SLAVE_TIMESCALE = 0.10f; float tau = dt < HIPS_OFFSET_SLAVE_TIMESCALE ? dt / HIPS_OFFSET_SLAVE_TIMESCALE : 1.0f; float newOffsetLength = glm::length(newHipsOffset); if (newOffsetLength > _maxHipsOffsetLength) { @@ -558,6 +564,13 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars newHipsOffset *= _maxHipsOffsetLength / newOffsetLength; } _hipsOffset += (newHipsOffset - _hipsOffset) * tau; + /* OUTOFBODY_HACK: experimental code for disabling HmdHead IK behavior when hips over limit + if (_hipsAreOver) { + _hipsAreOver = glm::length(newHipsOffset) - _maxHipsOffsetLength > -1.0f; + } else { + _hipsAreOver = glm::length(newHipsOffset) - _maxHipsOffsetLength > 1.0f; + } + */ } } } diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h index 7e4a7e5473..5b83b9734e 100644 --- a/libraries/animation/src/AnimInverseKinematics.h +++ b/libraries/animation/src/AnimInverseKinematics.h @@ -86,6 +86,7 @@ protected: // experimental data for moving hips during IK glm::vec3 _hipsOffset { Vectors::ZERO }; float _maxHipsOffsetLength { 1.0f }; + bool _hipsAreOver { false }; // OUTOFBODY_HACK: experimental int _headIndex { -1 }; int _hipsIndex { -1 }; int _hipsParentIndex { -1 }; From 49224d9698284b96d6496a08a5b565b35a56069f Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 16 Sep 2016 14:50:08 -0700 Subject: [PATCH 030/101] avoid unecessary branch --- libraries/physics/src/CharacterGhostObject.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/libraries/physics/src/CharacterGhostObject.cpp b/libraries/physics/src/CharacterGhostObject.cpp index bdd147fb74..6529f2c944 100755 --- a/libraries/physics/src/CharacterGhostObject.cpp +++ b/libraries/physics/src/CharacterGhostObject.cpp @@ -204,13 +204,9 @@ bool CharacterGhostObject::sweepTest( CharacterSweepResult& result) const { if (_world && _inWorld) { assert(shape); - btScalar allowedPenetration = _world->getDispatchInfo().m_allowedCcdPenetration; convexSweepTest(shape, start, end, result, allowedPenetration); - - if (result.hasHit()) { - return true; - } + return result.hasHit(); } return false; } From a58773823d31311a99603d8df4ff6e8f7187b2ac Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 16 Sep 2016 14:50:29 -0700 Subject: [PATCH 031/101] cleanup unused cruft --- libraries/animation/src/AnimInverseKinematics.cpp | 15 +-------------- libraries/animation/src/AnimInverseKinematics.h | 1 - 2 files changed, 1 insertion(+), 15 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 059f192f3c..15774141cc 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -244,11 +244,6 @@ int AnimInverseKinematics::solveTargetWithCCD(const IKTarget& target, AnimPoseVe // the tip's parent-relative as we proceed up the chain glm::quat tipParentOrientation = absolutePoses[pivotIndex].rot; - /* OUTOFBODY_HACK -- experimental override target type when HmdHead pushes outside hipsOffset limit - if (targetType == IKTarget::Type::HmdHead && _hipsAreOver) { - targetType = IKTarget::Type::RotationAndPosition; - } - */ if (targetType == IKTarget::Type::HmdHead) { // rotate tip directly to target orientation tipOrientation = target.getRotation(); @@ -512,10 +507,10 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars // and where it wants to be (after IK solutions are done) // OUTOFBODY_HACK:use weighted average between HMD and other targets - // ANDREW TODO: change how HMD IK target is handled to allow torso to lean over float HMD_WEIGHT = 10.0f; float OTHER_WEIGHT = 1.0f; float totalWeight = 0.0f; + glm::vec3 newHipsOffset = Vectors::ZERO; for (auto& target: targets) { int targetIndex = target.getIndex(); @@ -564,13 +559,6 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars newHipsOffset *= _maxHipsOffsetLength / newOffsetLength; } _hipsOffset += (newHipsOffset - _hipsOffset) * tau; - /* OUTOFBODY_HACK: experimental code for disabling HmdHead IK behavior when hips over limit - if (_hipsAreOver) { - _hipsAreOver = glm::length(newHipsOffset) - _maxHipsOffsetLength > -1.0f; - } else { - _hipsAreOver = glm::length(newHipsOffset) - _maxHipsOffsetLength > 1.0f; - } - */ } } } @@ -584,7 +572,6 @@ void AnimInverseKinematics::clearIKJointLimitHistory() { } void AnimInverseKinematics::setMaxHipsOffsetLength(float maxLength) { - assert(maxLength > 0.0f); // OUTOFBODY_HACK: manually adjust scale here const float METERS_TO_CENTIMETERS = 100.0f; _maxHipsOffsetLength = METERS_TO_CENTIMETERS * maxLength; diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h index 5b83b9734e..7e4a7e5473 100644 --- a/libraries/animation/src/AnimInverseKinematics.h +++ b/libraries/animation/src/AnimInverseKinematics.h @@ -86,7 +86,6 @@ protected: // experimental data for moving hips during IK glm::vec3 _hipsOffset { Vectors::ZERO }; float _maxHipsOffsetLength { 1.0f }; - bool _hipsAreOver { false }; // OUTOFBODY_HACK: experimental int _headIndex { -1 }; int _hipsIndex { -1 }; int _hipsParentIndex { -1 }; From 91ff9722ec25a9b7188497b0d027f0d16e4aa204 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 16 Sep 2016 15:23:41 -0700 Subject: [PATCH 032/101] remove crufty comments --- libraries/animation/src/Rig.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 240190ae2c..2d44a660a6 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -300,7 +300,6 @@ void Rig::clearJointAnimationPriority(int index) { void Rig::clearIKJointLimitHistory() { if (_animNode) { _animNode->traverse([&](AnimNode::Pointer node) { - // only report clip nodes as valid roles. auto ikNode = std::dynamic_pointer_cast(node); if (ikNode) { ikNode->clearIKJointLimitHistory(); @@ -313,7 +312,6 @@ void Rig::clearIKJointLimitHistory() { void Rig::setMaxHipsOffsetLength(float maxLength) { if (_animNode) { _animNode->traverse([&](AnimNode::Pointer node) { - // only report clip nodes as valid roles. auto ikNode = std::dynamic_pointer_cast(node); if (ikNode) { ikNode->setMaxHipsOffsetLength(maxLength); From a028d3ba58b08c3216f6f6b2f06b8cda21c9d864 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 19 Sep 2016 14:58:55 -0700 Subject: [PATCH 033/101] Remove hacks for debug rendering of IK targets A AnimContext class was introduced. This context is passed into every node during evaluation/overlay. It holds non-animVar "global" data passed from the application. --- interface/src/avatar/MyAvatar.cpp | 10 +++---- libraries/animation/src/AnimBlendLinear.cpp | 14 ++++----- libraries/animation/src/AnimBlendLinear.h | 4 +-- .../animation/src/AnimBlendLinearMove.cpp | 14 ++++----- libraries/animation/src/AnimBlendLinearMove.h | 4 +-- libraries/animation/src/AnimClip.cpp | 2 +- libraries/animation/src/AnimClip.h | 2 +- libraries/animation/src/AnimContext.cpp | 16 ++++++++++ libraries/animation/src/AnimContext.h | 30 +++++++++++++++++++ .../animation/src/AnimInverseKinematics.cpp | 21 ++++++++----- .../animation/src/AnimInverseKinematics.h | 6 ++-- libraries/animation/src/AnimManipulator.cpp | 6 ++-- libraries/animation/src/AnimManipulator.h | 4 +-- libraries/animation/src/AnimNode.h | 8 +++-- libraries/animation/src/AnimOverlay.cpp | 6 ++-- libraries/animation/src/AnimOverlay.h | 2 +- libraries/animation/src/AnimStateMachine.cpp | 14 ++++----- libraries/animation/src/AnimStateMachine.h | 4 +-- libraries/animation/src/Rig.cpp | 13 ++++---- libraries/animation/src/Rig.h | 3 ++ 20 files changed, 120 insertions(+), 63 deletions(-) create mode 100644 libraries/animation/src/AnimContext.cpp create mode 100644 libraries/animation/src/AnimContext.h diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index b6d38e01fd..28590e0de6 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -82,9 +82,6 @@ const float MyAvatar::ZOOM_MIN = 0.5f; const float MyAvatar::ZOOM_MAX = 25.0f; const float MyAvatar::ZOOM_DEFAULT = 1.5f; -// OUTOFBODY_HACK defined in Rig.cpp -extern bool OUTOFBODY_HACK_ENABLE_DEBUG_DRAW_IK_TARGETS; - // OUTOFBODY_HACK defined in SkeletonModel.cpp extern glm::vec3 TRUNCATE_IK_CAPSULE_POSITION; extern float TRUNCATE_IK_CAPSULE_LENGTH; @@ -429,6 +426,11 @@ void MyAvatar::simulate(float deltaTime) { { PerformanceTimer perfTimer("skeleton"); + + if (_rig) { + _rig->setEnableDebugDrawIKTargets(_enableDebugDrawIKTargets); + } + _skeletonModel->simulate(deltaTime); } @@ -859,8 +861,6 @@ void MyAvatar::setEnableDebugDrawSensorToWorldMatrix(bool isEnabled) { void MyAvatar::setEnableDebugDrawIKTargets(bool isEnabled) { _enableDebugDrawIKTargets = isEnabled; - - OUTOFBODY_HACK_ENABLE_DEBUG_DRAW_IK_TARGETS = isEnabled; } diff --git a/libraries/animation/src/AnimBlendLinear.cpp b/libraries/animation/src/AnimBlendLinear.cpp index 52c440a14e..936126bf52 100644 --- a/libraries/animation/src/AnimBlendLinear.cpp +++ b/libraries/animation/src/AnimBlendLinear.cpp @@ -24,7 +24,7 @@ AnimBlendLinear::~AnimBlendLinear() { } -const AnimPoseVec& AnimBlendLinear::evaluate(const AnimVariantMap& animVars, float dt, Triggers& triggersOut) { +const AnimPoseVec& AnimBlendLinear::evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut) { _alpha = animVars.lookup(_alphaVar, _alpha); @@ -33,7 +33,7 @@ const AnimPoseVec& AnimBlendLinear::evaluate(const AnimVariantMap& animVars, flo pose = AnimPose::identity; } } else if (_children.size() == 1) { - _poses = _children[0]->evaluate(animVars, dt, triggersOut); + _poses = _children[0]->evaluate(animVars, context, dt, triggersOut); } else { float clampedAlpha = glm::clamp(_alpha, 0.0f, (float)(_children.size() - 1)); @@ -41,7 +41,7 @@ const AnimPoseVec& AnimBlendLinear::evaluate(const AnimVariantMap& animVars, flo size_t nextPoseIndex = glm::ceil(clampedAlpha); float alpha = glm::fract(clampedAlpha); - evaluateAndBlendChildren(animVars, triggersOut, alpha, prevPoseIndex, nextPoseIndex, dt); + evaluateAndBlendChildren(animVars, context, triggersOut, alpha, prevPoseIndex, nextPoseIndex, dt); } return _poses; } @@ -51,15 +51,15 @@ const AnimPoseVec& AnimBlendLinear::getPosesInternal() const { return _poses; } -void AnimBlendLinear::evaluateAndBlendChildren(const AnimVariantMap& animVars, Triggers& triggersOut, float alpha, +void AnimBlendLinear::evaluateAndBlendChildren(const AnimVariantMap& animVars, const AnimContext& context, Triggers& triggersOut, float alpha, size_t prevPoseIndex, size_t nextPoseIndex, float dt) { if (prevPoseIndex == nextPoseIndex) { // this can happen if alpha is on an integer boundary - _poses = _children[prevPoseIndex]->evaluate(animVars, dt, triggersOut); + _poses = _children[prevPoseIndex]->evaluate(animVars, context, dt, triggersOut); } else { // need to eval and blend between two children. - auto prevPoses = _children[prevPoseIndex]->evaluate(animVars, dt, triggersOut); - auto nextPoses = _children[nextPoseIndex]->evaluate(animVars, dt, triggersOut); + auto prevPoses = _children[prevPoseIndex]->evaluate(animVars, context, dt, triggersOut); + auto nextPoses = _children[nextPoseIndex]->evaluate(animVars, context, dt, triggersOut); if (prevPoses.size() > 0 && prevPoses.size() == nextPoses.size()) { _poses.resize(prevPoses.size()); diff --git a/libraries/animation/src/AnimBlendLinear.h b/libraries/animation/src/AnimBlendLinear.h index 2478f9b473..0dae6aabdb 100644 --- a/libraries/animation/src/AnimBlendLinear.h +++ b/libraries/animation/src/AnimBlendLinear.h @@ -30,7 +30,7 @@ public: AnimBlendLinear(const QString& id, float alpha); virtual ~AnimBlendLinear() override; - virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, float dt, Triggers& triggersOut) override; + virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut) override; void setAlphaVar(const QString& alphaVar) { _alphaVar = alphaVar; } @@ -38,7 +38,7 @@ protected: // for AnimDebugDraw rendering virtual const AnimPoseVec& getPosesInternal() const override; - void evaluateAndBlendChildren(const AnimVariantMap& animVars, Triggers& triggersOut, float alpha, + void evaluateAndBlendChildren(const AnimVariantMap& animVars, const AnimContext& context, Triggers& triggersOut, float alpha, size_t prevPoseIndex, size_t nextPoseIndex, float dt); AnimPoseVec _poses; diff --git a/libraries/animation/src/AnimBlendLinearMove.cpp b/libraries/animation/src/AnimBlendLinearMove.cpp index 609b464512..40fbb5a6f7 100644 --- a/libraries/animation/src/AnimBlendLinearMove.cpp +++ b/libraries/animation/src/AnimBlendLinearMove.cpp @@ -26,7 +26,7 @@ AnimBlendLinearMove::~AnimBlendLinearMove() { } -const AnimPoseVec& AnimBlendLinearMove::evaluate(const AnimVariantMap& animVars, float dt, Triggers& triggersOut) { +const AnimPoseVec& AnimBlendLinearMove::evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut) { assert(_children.size() == _characteristicSpeeds.size()); @@ -43,7 +43,7 @@ const AnimPoseVec& AnimBlendLinearMove::evaluate(const AnimVariantMap& animVars, const int nextPoseIndex = 0; float prevDeltaTime, nextDeltaTime; setFrameAndPhase(dt, alpha, prevPoseIndex, nextPoseIndex, &prevDeltaTime, &nextDeltaTime, triggersOut); - evaluateAndBlendChildren(animVars, triggersOut, alpha, prevPoseIndex, nextPoseIndex, prevDeltaTime, nextDeltaTime); + evaluateAndBlendChildren(animVars, context, triggersOut, alpha, prevPoseIndex, nextPoseIndex, prevDeltaTime, nextDeltaTime); } else { auto clampedAlpha = glm::clamp(_alpha, 0.0f, (float)(_children.size() - 1)); @@ -52,7 +52,7 @@ const AnimPoseVec& AnimBlendLinearMove::evaluate(const AnimVariantMap& animVars, auto alpha = glm::fract(clampedAlpha); float prevDeltaTime, nextDeltaTime; setFrameAndPhase(dt, alpha, prevPoseIndex, nextPoseIndex, &prevDeltaTime, &nextDeltaTime, triggersOut); - evaluateAndBlendChildren(animVars, triggersOut, alpha, prevPoseIndex, nextPoseIndex, prevDeltaTime, nextDeltaTime); + evaluateAndBlendChildren(animVars, context, triggersOut, alpha, prevPoseIndex, nextPoseIndex, prevDeltaTime, nextDeltaTime); } return _poses; } @@ -62,16 +62,16 @@ const AnimPoseVec& AnimBlendLinearMove::getPosesInternal() const { return _poses; } -void AnimBlendLinearMove::evaluateAndBlendChildren(const AnimVariantMap& animVars, Triggers& triggersOut, float alpha, +void AnimBlendLinearMove::evaluateAndBlendChildren(const AnimVariantMap& animVars, const AnimContext& context, Triggers& triggersOut, float alpha, size_t prevPoseIndex, size_t nextPoseIndex, float prevDeltaTime, float nextDeltaTime) { if (prevPoseIndex == nextPoseIndex) { // this can happen if alpha is on an integer boundary - _poses = _children[prevPoseIndex]->evaluate(animVars, prevDeltaTime, triggersOut); + _poses = _children[prevPoseIndex]->evaluate(animVars, context, prevDeltaTime, triggersOut); } else { // need to eval and blend between two children. - auto prevPoses = _children[prevPoseIndex]->evaluate(animVars, prevDeltaTime, triggersOut); - auto nextPoses = _children[nextPoseIndex]->evaluate(animVars, nextDeltaTime, triggersOut); + auto prevPoses = _children[prevPoseIndex]->evaluate(animVars, context, prevDeltaTime, triggersOut); + auto nextPoses = _children[nextPoseIndex]->evaluate(animVars, context, nextDeltaTime, triggersOut); if (prevPoses.size() > 0 && prevPoses.size() == nextPoses.size()) { _poses.resize(prevPoses.size()); diff --git a/libraries/animation/src/AnimBlendLinearMove.h b/libraries/animation/src/AnimBlendLinearMove.h index 4e04ce29cb..083858f873 100644 --- a/libraries/animation/src/AnimBlendLinearMove.h +++ b/libraries/animation/src/AnimBlendLinearMove.h @@ -39,7 +39,7 @@ public: AnimBlendLinearMove(const QString& id, float alpha, float desiredSpeed, const std::vector& characteristicSpeeds); virtual ~AnimBlendLinearMove() override; - virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, float dt, Triggers& triggersOut) override; + virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut) override; void setAlphaVar(const QString& alphaVar) { _alphaVar = alphaVar; } void setDesiredSpeedVar(const QString& desiredSpeedVar) { _desiredSpeedVar = desiredSpeedVar; } @@ -48,7 +48,7 @@ protected: // for AnimDebugDraw rendering virtual const AnimPoseVec& getPosesInternal() const override; - void evaluateAndBlendChildren(const AnimVariantMap& animVars, Triggers& triggersOut, float alpha, + void evaluateAndBlendChildren(const AnimVariantMap& animVars, const AnimContext& context, Triggers& triggersOut, float alpha, size_t prevPoseIndex, size_t nextPoseIndex, float prevDeltaTime, float nextDeltaTime); diff --git a/libraries/animation/src/AnimClip.cpp b/libraries/animation/src/AnimClip.cpp index a5747e4f96..273b83743d 100644 --- a/libraries/animation/src/AnimClip.cpp +++ b/libraries/animation/src/AnimClip.cpp @@ -31,7 +31,7 @@ AnimClip::~AnimClip() { } -const AnimPoseVec& AnimClip::evaluate(const AnimVariantMap& animVars, float dt, Triggers& triggersOut) { +const AnimPoseVec& AnimClip::evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut) { // lookup parameters from animVars, using current instance variables as defaults. _startFrame = animVars.lookup(_startFrameVar, _startFrame); diff --git a/libraries/animation/src/AnimClip.h b/libraries/animation/src/AnimClip.h index 7989f6d172..c7e7ebf3ee 100644 --- a/libraries/animation/src/AnimClip.h +++ b/libraries/animation/src/AnimClip.h @@ -30,7 +30,7 @@ public: AnimClip(const QString& id, const QString& url, float startFrame, float endFrame, float timeScale, bool loopFlag, bool mirrorFlag); virtual ~AnimClip() override; - virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, float dt, Triggers& triggersOut) override; + virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut) override; void setStartFrameVar(const QString& startFrameVar) { _startFrameVar = startFrameVar; } void setEndFrameVar(const QString& endFrameVar) { _endFrameVar = endFrameVar; } diff --git a/libraries/animation/src/AnimContext.cpp b/libraries/animation/src/AnimContext.cpp new file mode 100644 index 0000000000..c8d3e7bcda --- /dev/null +++ b/libraries/animation/src/AnimContext.cpp @@ -0,0 +1,16 @@ +// +// AnimContext.cpp +// +// Created by Anthony J. Thibault on 9/19/16. +// Copyright (c) 2016 High Fidelity, Inc. All rights reserved. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#include "AnimContext.h" + +AnimContext::AnimContext(bool enableDebugDrawIKTargets, const glm::mat4& geometryToRigMatrix) : + _enableDebugDrawIKTargets(enableDebugDrawIKTargets), + _geometryToRigMatrix(geometryToRigMatrix) { +} diff --git a/libraries/animation/src/AnimContext.h b/libraries/animation/src/AnimContext.h new file mode 100644 index 0000000000..3170911e14 --- /dev/null +++ b/libraries/animation/src/AnimContext.h @@ -0,0 +1,30 @@ +// +// AnimContext.h +// +// Created by Anthony J. Thibault on 9/19/16. +// Copyright (c) 2016 High Fidelity, Inc. All rights reserved. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#ifndef hifi_AnimContext_h +#define hifi_AnimContext_h + +#include +#include + +class AnimContext { +public: + AnimContext(bool enableDebugDrawIKTargets, const glm::mat4& geometryToRigMatrix); + + bool getEnableDebugDrawIKTargets() const { return _enableDebugDrawIKTargets; } + const glm::mat4& getGeometryToRigMatrix() const { return _geometryToRigMatrix; } + +protected: + + bool _enableDebugDrawIKTargets { false }; + glm::mat4 _geometryToRigMatrix; +}; + +#endif // hifi_AnimContext_h diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 15774141cc..b58813a214 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -21,9 +21,6 @@ #include "SwingTwistConstraint.h" #include "AnimationLogging.h" -bool OUTOFBODY_HACK_ENABLE_DEBUG_DRAW_IK_TARGETS = false; -Rig* OUTOFBODY_HACK_RIG_POINTER = nullptr; - AnimInverseKinematics::AnimInverseKinematics(const QString& id) : AnimNode(AnimNode::Type::InverseKinematics, id) { } @@ -381,14 +378,14 @@ int AnimInverseKinematics::solveTargetWithCCD(const IKTarget& target, AnimPoseVe } //virtual -const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVars, float dt, AnimNode::Triggers& triggersOut) { +const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimNode::Triggers& triggersOut) { // don't call this function, call overlay() instead assert(false); return _relativePoses; } //virtual -const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) { +const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) { const float MAX_OVERLAY_DT = 1.0f / 30.0f; // what to clamp delta-time to in AnimInverseKinematics::overlay if (dt > MAX_OVERLAY_DT) { @@ -442,20 +439,28 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars computeTargets(animVars, targets, underPoses); } - if (OUTOFBODY_HACK_ENABLE_DEBUG_DRAW_IK_TARGETS && OUTOFBODY_HACK_RIG_POINTER) { + // debug render ik targets + if (context.getEnableDebugDrawIKTargets()) { const vec4 WHITE(1.0f); - glm::mat4 geomToRigMat = OUTOFBODY_HACK_RIG_POINTER->getGeometryToRigTransform(); glm::mat4 rigToAvatarMat = createMatFromQuatAndPos(Quaternions::Y_180, glm::vec3()); for (auto& target : targets) { glm::mat4 geomTargetMat = createMatFromQuatAndPos(target.getRotation(), target.getTranslation()); - glm::mat4 avatarTargetMat = rigToAvatarMat * geomToRigMat * geomTargetMat; + glm::mat4 avatarTargetMat = rigToAvatarMat * context.getGeometryToRigMatrix() * geomTargetMat; std::string name = "ikTarget" + std::to_string(target.getIndex()); DebugDraw::getInstance().addMyAvatarMarker(name, glmExtractRotation(avatarTargetMat), extractTranslation(avatarTargetMat), WHITE); } + } else if (context.getEnableDebugDrawIKTargets() != _previousEnableDebugIKTargets) { + // remove markers if they were added last frame. + for (auto& target : targets) { + std::string name = "ikTarget" + std::to_string(target.getIndex()); + DebugDraw::getInstance().removeMyAvatarMarker(name); + } } + _previousEnableDebugIKTargets = context.getEnableDebugDrawIKTargets(); + if (targets.empty()) { // no IK targets but still need to enforce constraints std::map::iterator constraintItr = _constraints.begin(); diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h index 7e4a7e5473..b61343a97b 100644 --- a/libraries/animation/src/AnimInverseKinematics.h +++ b/libraries/animation/src/AnimInverseKinematics.h @@ -34,8 +34,8 @@ public: void setTargetVars(const QString& jointName, const QString& positionVar, const QString& rotationVar, const QString& typeVar); - virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, float dt, AnimNode::Triggers& triggersOut) override; - virtual const AnimPoseVec& overlay(const AnimVariantMap& animVars, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) override; + virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimNode::Triggers& triggersOut) override; + virtual const AnimPoseVec& overlay(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) override; void clearIKJointLimitHistory(); @@ -93,6 +93,8 @@ protected: // _maxTargetIndex is tracked to help optimize the recalculation of absolute poses // during the the cyclic coordinate descent algorithm int _maxTargetIndex { 0 }; + + bool _previousEnableDebugIKTargets { false }; }; #endif // hifi_AnimInverseKinematics_h diff --git a/libraries/animation/src/AnimManipulator.cpp b/libraries/animation/src/AnimManipulator.cpp index 3eedec5dbd..37e239f3e1 100644 --- a/libraries/animation/src/AnimManipulator.cpp +++ b/libraries/animation/src/AnimManipulator.cpp @@ -22,11 +22,11 @@ AnimManipulator::~AnimManipulator() { } -const AnimPoseVec& AnimManipulator::evaluate(const AnimVariantMap& animVars, float dt, Triggers& triggersOut) { - return overlay(animVars, dt, triggersOut, _skeleton->getRelativeBindPoses()); +const AnimPoseVec& AnimManipulator::evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut) { + return overlay(animVars, context, dt, triggersOut, _skeleton->getRelativeBindPoses()); } -const AnimPoseVec& AnimManipulator::overlay(const AnimVariantMap& animVars, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) { +const AnimPoseVec& AnimManipulator::overlay(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) { _alpha = animVars.lookup(_alphaVar, _alpha); _poses = underPoses; diff --git a/libraries/animation/src/AnimManipulator.h b/libraries/animation/src/AnimManipulator.h index 8534b9c269..26f50a7dd9 100644 --- a/libraries/animation/src/AnimManipulator.h +++ b/libraries/animation/src/AnimManipulator.h @@ -22,8 +22,8 @@ public: AnimManipulator(const QString& id, float alpha); virtual ~AnimManipulator() override; - virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, float dt, Triggers& triggersOut) override; - virtual const AnimPoseVec& overlay(const AnimVariantMap& animVars, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) override; + virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut) override; + virtual const AnimPoseVec& overlay(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) override; void setAlphaVar(const QString& alphaVar) { _alphaVar = alphaVar; } diff --git a/libraries/animation/src/AnimNode.h b/libraries/animation/src/AnimNode.h index 23f2e1c7b3..10db38f42e 100644 --- a/libraries/animation/src/AnimNode.h +++ b/libraries/animation/src/AnimNode.h @@ -20,6 +20,7 @@ #include "AnimSkeleton.h" #include "AnimVariant.h" +#include "AnimContext.h" class QJsonObject; @@ -72,9 +73,10 @@ public: AnimSkeleton::ConstPointer getSkeleton() const { return _skeleton; } - virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, float dt, Triggers& triggersOut) = 0; - virtual const AnimPoseVec& overlay(const AnimVariantMap& animVars, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) { - return evaluate(animVars, dt, triggersOut); + virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut) = 0; + virtual const AnimPoseVec& overlay(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut, + const AnimPoseVec& underPoses) { + return evaluate(animVars, context, dt, triggersOut); } void setCurrentFrame(float frame); diff --git a/libraries/animation/src/AnimOverlay.cpp b/libraries/animation/src/AnimOverlay.cpp index 8f60b972ce..dbc635af66 100644 --- a/libraries/animation/src/AnimOverlay.cpp +++ b/libraries/animation/src/AnimOverlay.cpp @@ -39,7 +39,7 @@ void AnimOverlay::buildBoneSet(BoneSet boneSet) { } } -const AnimPoseVec& AnimOverlay::evaluate(const AnimVariantMap& animVars, float dt, Triggers& triggersOut) { +const AnimPoseVec& AnimOverlay::evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut) { // lookup parameters from animVars, using current instance variables as defaults. // NOTE: switching bonesets can be an expensive operation, let's try to avoid it. @@ -51,8 +51,8 @@ const AnimPoseVec& AnimOverlay::evaluate(const AnimVariantMap& animVars, float d _alpha = animVars.lookup(_alphaVar, _alpha); if (_children.size() >= 2) { - auto& underPoses = _children[1]->evaluate(animVars, dt, triggersOut); - auto& overPoses = _children[0]->overlay(animVars, dt, triggersOut, underPoses); + auto& underPoses = _children[1]->evaluate(animVars, context, dt, triggersOut); + auto& overPoses = _children[0]->overlay(animVars, context, dt, triggersOut, underPoses); if (underPoses.size() > 0 && underPoses.size() == overPoses.size()) { _poses.resize(underPoses.size()); diff --git a/libraries/animation/src/AnimOverlay.h b/libraries/animation/src/AnimOverlay.h index eda8847d40..2f34c07309 100644 --- a/libraries/animation/src/AnimOverlay.h +++ b/libraries/animation/src/AnimOverlay.h @@ -43,7 +43,7 @@ public: AnimOverlay(const QString& id, BoneSet boneSet, float alpha); virtual ~AnimOverlay() override; - virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, float dt, Triggers& triggersOut) override; + virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut) override; void setBoneSetVar(const QString& boneSetVar) { _boneSetVar = boneSetVar; } void setAlphaVar(const QString& alphaVar) { _alphaVar = alphaVar; } diff --git a/libraries/animation/src/AnimStateMachine.cpp b/libraries/animation/src/AnimStateMachine.cpp index 41d8a94b0a..4e86b92c0b 100644 --- a/libraries/animation/src/AnimStateMachine.cpp +++ b/libraries/animation/src/AnimStateMachine.cpp @@ -21,7 +21,7 @@ AnimStateMachine::~AnimStateMachine() { } -const AnimPoseVec& AnimStateMachine::evaluate(const AnimVariantMap& animVars, float dt, Triggers& triggersOut) { +const AnimPoseVec& AnimStateMachine::evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut) { QString desiredStateID = animVars.lookup(_currentStateVar, _currentState->getID()); if (_currentState->getID() != desiredStateID) { @@ -29,7 +29,7 @@ const AnimPoseVec& AnimStateMachine::evaluate(const AnimVariantMap& animVars, fl bool foundState = false; for (auto& state : _states) { if (state->getID() == desiredStateID) { - switchState(animVars, state); + switchState(animVars, context, state); foundState = true; break; } @@ -42,7 +42,7 @@ const AnimPoseVec& AnimStateMachine::evaluate(const AnimVariantMap& animVars, fl // evaluate currentState transitions auto desiredState = evaluateTransitions(animVars); if (desiredState != _currentState) { - switchState(animVars, desiredState); + switchState(animVars, context, desiredState); } assert(_currentState); @@ -62,7 +62,7 @@ const AnimPoseVec& AnimStateMachine::evaluate(const AnimVariantMap& animVars, fl } else if (_interpType == InterpType::SnapshotPrev) { // interp between the prev snapshot and evaluated next target. // this is useful for interping into a blend - localNextPoses = currentStateNode->evaluate(animVars, dt, triggersOut); + localNextPoses = currentStateNode->evaluate(animVars, context, dt, triggersOut); prevPoses = &_prevPoses; nextPoses = &localNextPoses; } else { @@ -79,7 +79,7 @@ const AnimPoseVec& AnimStateMachine::evaluate(const AnimVariantMap& animVars, fl } } if (!_duringInterp) { - _poses = currentStateNode->evaluate(animVars, dt, triggersOut); + _poses = currentStateNode->evaluate(animVars, context, dt, triggersOut); } return _poses; } @@ -92,7 +92,7 @@ void AnimStateMachine::addState(State::Pointer state) { _states.push_back(state); } -void AnimStateMachine::switchState(const AnimVariantMap& animVars, State::Pointer desiredState) { +void AnimStateMachine::switchState(const AnimVariantMap& animVars, const AnimContext& context, State::Pointer desiredState) { const float FRAMES_PER_SECOND = 30.0f; @@ -114,7 +114,7 @@ void AnimStateMachine::switchState(const AnimVariantMap& animVars, State::Pointe _prevPoses = _poses; // snapshot next pose at the target frame. nextStateNode->setCurrentFrame(desiredState->_interpTarget); - _nextPoses = nextStateNode->evaluate(animVars, dt, triggers); + _nextPoses = nextStateNode->evaluate(animVars, context, dt, triggers); } else if (_interpType == InterpType::SnapshotPrev) { // snapshot previoius pose _prevPoses = _poses; diff --git a/libraries/animation/src/AnimStateMachine.h b/libraries/animation/src/AnimStateMachine.h index d92b94d1b5..711326a9ae 100644 --- a/libraries/animation/src/AnimStateMachine.h +++ b/libraries/animation/src/AnimStateMachine.h @@ -113,7 +113,7 @@ public: explicit AnimStateMachine(const QString& id); virtual ~AnimStateMachine() override; - virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, float dt, Triggers& triggersOut) override; + virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut) override; void setCurrentStateVar(QString& currentStateVar) { _currentStateVar = currentStateVar; } @@ -123,7 +123,7 @@ protected: void addState(State::Pointer state); - void switchState(const AnimVariantMap& animVars, State::Pointer desiredState); + void switchState(const AnimVariantMap& animVars, const AnimContext& context, State::Pointer desiredState); State::Pointer evaluateTransitions(const AnimVariantMap& animVars) const; // for AnimDebugDraw rendering diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 2d44a660a6..a7a2e750bb 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -47,8 +47,6 @@ const glm::vec3 DEFAULT_LEFT_EYE_POS(0.3f, 0.9f, 0.0f); const glm::vec3 DEFAULT_HEAD_POS(0.0f, 0.75f, 0.0f); const glm::vec3 DEFAULT_NECK_POS(0.0f, 0.70f, 0.0f); -extern Rig* OUTOFBODY_HACK_RIG_POINTER; - void Rig::overrideAnimation(const QString& url, float fps, bool loop, float firstFrame, float lastFrame) { UserAnimState::ClipNodeEnum clipNodeEnum; @@ -887,10 +885,11 @@ void Rig::updateAnimations(float deltaTime, glm::mat4 rootTransform) { updateAnimationStateHandlers(); _animVars.setRigToGeometryTransform(_rigToGeometryTransform); + AnimContext context(_enableDebugDrawIKTargets, getGeometryToRigTransform()); + // evaluate the animation - OUTOFBODY_HACK_RIG_POINTER = this; AnimNode::Triggers triggersOut; - _internalPoseSet._relativePoses = _animNode->evaluate(_animVars, deltaTime, triggersOut); + _internalPoseSet._relativePoses = _animNode->evaluate(_animVars, context, deltaTime, triggersOut); if ((int)_internalPoseSet._relativePoses.size() != _animSkeleton->getNumJoints()) { // animations haven't fully loaded yet. _internalPoseSet._relativePoses = _animSkeleton->getRelativeDefaultPoses(); @@ -899,7 +898,6 @@ void Rig::updateAnimations(float deltaTime, glm::mat4 rootTransform) { for (auto& trigger : triggersOut) { _animVars.setTrigger(trigger); } - OUTOFBODY_HACK_RIG_POINTER = nullptr; } applyOverridePoses(); @@ -1365,9 +1363,10 @@ void Rig::computeAvatarBoundingCapsule( // call overlay twice: once to verify AnimPoseVec joints and again to do the IK AnimNode::Triggers triggersOut; + AnimContext context(false, glm::mat4()); float dt = 1.0f; // the value of this does not matter - ikNode.overlay(animVars, dt, triggersOut, _animSkeleton->getRelativeBindPoses()); - AnimPoseVec finalPoses = ikNode.overlay(animVars, dt, triggersOut, _animSkeleton->getRelativeBindPoses()); + ikNode.overlay(animVars, context, dt, triggersOut, _animSkeleton->getRelativeBindPoses()); + AnimPoseVec finalPoses = ikNode.overlay(animVars, context, dt, triggersOut, _animSkeleton->getRelativeBindPoses()); // convert relative poses to absolute _animSkeleton->convertRelativePosesToAbsolute(finalPoses); diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index f0cc68a828..4491397af0 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -211,6 +211,8 @@ public: const glm::mat4& getGeometryToRigTransform() const { return _geometryToRigTransform; } + void setEnableDebugDrawIKTargets(bool enableDebugDrawIKTargets) { _enableDebugDrawIKTargets = enableDebugDrawIKTargets; } + signals: void onLoadComplete(); @@ -315,6 +317,7 @@ protected: mutable uint32_t _jointNameWarningCount { 0 }; glm::vec3 _desiredRigHeadPosition; bool _truncateIKTargets { false }; + bool _enableDebugDrawIKTargets { false }; private: QMap _stateHandlers; From 6f0acb382695e9b016987e935638858dec2ab905 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 19 Sep 2016 16:56:36 -0700 Subject: [PATCH 034/101] Fixed motor velocity in HMD mode --- interface/src/avatar/MyAvatar.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index b6d38e01fd..cb939b4e00 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -1301,7 +1301,7 @@ void MyAvatar::updateMotors() { const float DEFAULT_MOTOR_TIMESCALE = 0.2f; const float INVALID_MOTOR_TIMESCALE = 1.0e6f; - if (!qApp->isHMDMode()) { + if (qApp->isHMDMode()) { // OUTOFBODY_HACK: add default zero velocity motor to the characterController _characterController.addMotor(glm::vec3(), motorRotation, DEFAULT_MOTOR_TIMESCALE, INVALID_MOTOR_TIMESCALE); } else { From 80b970f2d985da7c872640f284ace633ef6737c0 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 19 Sep 2016 08:58:19 -0700 Subject: [PATCH 035/101] adding MyAvatar::_canonicalScale, but not used yet --- interface/src/avatar/MyAvatar.cpp | 2 ++ interface/src/avatar/MyAvatar.h | 1 + 2 files changed, 3 insertions(+) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index e5ea9fe1af..245eba0098 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -1229,6 +1229,8 @@ void MyAvatar::rebuildCollisionShape() { float scale = getUniformScale(); float radius = scale * _skeletonModel->getBoundingCapsuleRadius(); float height = scale * _skeletonModel->getBoundingCapsuleHeight() + 2.0f * radius; + const float CANONICAL_AVATAR_HEIGHT = 2.0f; + _canonicalScale = height / CANONICAL_AVATAR_HEIGHT; glm::vec3 corner(-radius, -0.5f * height, -radius); corner += scale * _skeletonModel->getBoundingCapsuleOffset(); glm::vec3 diagonal(2.0f * radius, height, 2.0f * radius); diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 87daba0267..6e00894cb3 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -500,6 +500,7 @@ private: bool _hmdLeanRecenterEnabled = true; bool _moveKinematically { false }; // KINEMATIC_CONTROLLER_HACK + float _canonicalScale { 1.0f }; float AVATAR_MOVEMENT_ENERGY_CONSTANT { 0.001f }; float AUDIO_ENERGY_CONSTANT { 0.000001f }; From 1db1295556638a60f84e0ba72fe0425d0ec195af Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 19 Sep 2016 08:58:51 -0700 Subject: [PATCH 036/101] cleanup around CharacterController::_targetVelocity --- libraries/physics/src/CharacterController.cpp | 14 +++++++++----- libraries/physics/src/CharacterController.h | 1 - 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 97d505577e..7bb8d98ebe 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -135,6 +135,7 @@ void CharacterController::setDynamicsWorld(btDynamicsWorld* world) { _ghost.setMinWallAngle(PI / 4.0f); // HACK _ghost.setUpDirection(_currentUp); _ghost.setGravity(DEFAULT_CHARACTER_GRAVITY); + _ghost.setWorldTransform(_rigidBody->getWorldTransform()); } if (_dynamicsWorld) { if (_pendingFlags & PENDING_FLAG_UPDATE_SHAPE) { @@ -209,7 +210,7 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) { btTransform transform = _rigidBody->getWorldTransform(); transform.setOrigin(_ghost.getWorldTransform().getOrigin()); _ghost.setWorldTransform(transform); - _ghost.setMotorVelocity(_simpleMotorVelocity); + _ghost.setMotorVelocity(_targetVelocity); float overshoot = 1.0f * _radius; _ghost.move(dt, overshoot); _rigidBody->setWorldTransform(_ghost.getWorldTransform()); @@ -525,7 +526,7 @@ void CharacterController::applyMotor(int index, btScalar dt, btVector3& worldVel // add components back together and rotate into world-frame velocity = (hVelocity + vVelocity).rotate(axis, angle); - _simpleMotorVelocity += maxTau * (hTargetVelocity + vTargetVelocity).rotate(axis, angle); + _targetVelocity += maxTau * (hTargetVelocity + vTargetVelocity).rotate(axis, angle); // store velocity and weights velocities.push_back(velocity); @@ -543,7 +544,7 @@ void CharacterController::computeNewVelocity(btScalar dt, btVector3& velocity) { velocities.reserve(_motors.size()); std::vector weights; weights.reserve(_motors.size()); - _simpleMotorVelocity = btVector3(0.0f, 0.0f, 0.0f); + _targetVelocity = btVector3(0.0f, 0.0f, 0.0f); for (int i = 0; i < (int)_motors.size(); ++i) { applyMotor(i, dt, velocity, velocities, weights); } @@ -559,15 +560,18 @@ void CharacterController::computeNewVelocity(btScalar dt, btVector3& velocity) { for (size_t i = 0; i < velocities.size(); ++i) { velocity += (weights[i] / totalWeight) * velocities[i]; } - _simpleMotorVelocity /= totalWeight; + _targetVelocity /= totalWeight; } if (velocity.length2() < MIN_TARGET_SPEED_SQUARED) { velocity = btVector3(0.0f, 0.0f, 0.0f); } // 'thrust' is applied at the very end + _targetVelocity += dt * _linearAcceleration; velocity += dt * _linearAcceleration; - _targetVelocity = velocity; + // Note the differences between these two variables: + // _targetVelocity = ideal final velocity according to input + // velocity = real final velocity after motors are applied to current velocity } void CharacterController::computeNewVelocity(btScalar dt, glm::vec3& velocity) { diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h index 2a3a81b416..85a02cd56a 100644 --- a/libraries/physics/src/CharacterController.h +++ b/libraries/physics/src/CharacterController.h @@ -152,7 +152,6 @@ protected: btVector3 _parentVelocity; btVector3 _preSimulationVelocity; btVector3 _velocityChange; - btVector3 _simpleMotorVelocity; // KINEMATIC_CONTROLLER_HACK btTransform _followDesiredBodyTransform; btTransform _characterBodyTransform; From 8dd5c9b92b66c0e0917b72dce2daf679dc60dbcd Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 19 Sep 2016 12:40:16 -0700 Subject: [PATCH 037/101] fix kinematic motion for ground and hover --- interface/src/avatar/MyAvatar.cpp | 3 +- libraries/physics/src/CharacterController.cpp | 26 +++-- libraries/physics/src/CharacterController.h | 4 +- .../physics/src/CharacterGhostObject.cpp | 105 +++++++++++------- libraries/physics/src/CharacterGhostObject.h | 14 ++- 5 files changed, 93 insertions(+), 59 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 245eba0098..cd4ee4784b 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -1378,7 +1378,8 @@ void MyAvatar::harvestResultsFromPhysicsSimulation(float deltaTime) { glm::vec3 position = getPosition(); glm::quat orientation = getOrientation(); if (_characterController.isEnabledAndReady()) { - _characterController.getPositionAndOrientation(position, orientation); + glm::quat bogusOrientation; + _characterController.getPositionAndOrientation(position, bogusOrientation); } nextAttitude(position, orientation); diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 7bb8d98ebe..acd82d73ea 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -130,7 +130,7 @@ void CharacterController::setDynamicsWorld(btDynamicsWorld* world) { // KINEMATIC_CONTROLLER_HACK _ghost.setCollisionGroupAndMask(_collisionGroup, BULLET_COLLISION_MASK_MY_AVATAR & (~ _collisionGroup)); _ghost.setCollisionWorld(_dynamicsWorld); - _ghost.setDistanceToFeet(_radius + _halfHeight); + _ghost.setRadiusAndHalfHeight(_radius, _halfHeight); _ghost.setMaxStepHeight(0.75f * (_radius + _halfHeight)); // HACK _ghost.setMinWallAngle(PI / 4.0f); // HACK _ghost.setUpDirection(_currentUp); @@ -177,10 +177,10 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) cons void CharacterController::preStep(btCollisionWorld* collisionWorld) { // trace a ray straight down to see if we're standing on the ground - const btTransform& xform = _rigidBody->getWorldTransform(); + const btTransform& transform = _rigidBody->getWorldTransform(); // rayStart is at center of bottom sphere - btVector3 rayStart = xform.getOrigin() - _halfHeight * _currentUp; + btVector3 rayStart = transform.getOrigin() - _halfHeight * _currentUp; // rayEnd is some short distance outside bottom sphere const btScalar FLOOR_PROXIMITY_THRESHOLD = 0.3f * _radius; @@ -213,7 +213,8 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) { _ghost.setMotorVelocity(_targetVelocity); float overshoot = 1.0f * _radius; _ghost.move(dt, overshoot); - _rigidBody->setWorldTransform(_ghost.getWorldTransform()); + transform.setOrigin(_ghost.getWorldTransform().getOrigin()); + _rigidBody->setWorldTransform(transform); _rigidBody->setLinearVelocity(_ghost.getLinearVelocity()); } else { // Dynamicaly compute a follow velocity to move this body toward the _followDesiredBodyTransform. @@ -400,9 +401,8 @@ void CharacterController::setPositionAndOrientation( // TODO: update gravity if up has changed updateUpAxis(orientation); - btQuaternion bodyOrientation = glmToBullet(orientation); - btVector3 bodyPosition = glmToBullet(position + orientation * _shapeLocalOffset); - _characterBodyTransform = btTransform(bodyOrientation, bodyPosition); + _rotation = glmToBullet(orientation); + _position = glmToBullet(position + orientation * _shapeLocalOffset); } void CharacterController::getPositionAndOrientation(glm::vec3& position, glm::quat& rotation) const { @@ -485,10 +485,11 @@ void CharacterController::applyMotor(int index, btScalar dt, btVector3& worldVel if (tau > 1.0f) { tau = 1.0f; } - velocity += (motor.velocity - velocity) * tau; + velocity += tau * (motor.velocity - velocity); // rotate back into world-frame velocity = velocity.rotate(axis, angle); + _targetVelocity += (tau * motor.velocity).rotate(axis, angle); // store the velocity and weight velocities.push_back(velocity); @@ -584,14 +585,14 @@ void CharacterController::preSimulation() { if (_dynamicsWorld) { quint64 now = usecTimestampNow(); - // slam body to where it is supposed to be - _rigidBody->setWorldTransform(_characterBodyTransform); + // slam body transform + _rigidBody->setWorldTransform(btTransform(btTransform(_rotation, _position))); btVector3 velocity = _rigidBody->getLinearVelocity(); _preSimulationVelocity = velocity; // scan for distant floor // rayStart is at center of bottom sphere - btVector3 rayStart = _characterBodyTransform.getOrigin(); + btVector3 rayStart = _position; // rayEnd is straight down MAX_FALL_HEIGHT btScalar rayLength = _radius + MAX_FALL_HEIGHT; @@ -679,6 +680,9 @@ void CharacterController::preSimulation() { } break; } + if (_moveKinematically && _ghost.isHovering()) { + SET_STATE(State::Hover, "kinematic motion"); // HACK + } } else { // OUTOFBODY_HACK -- in collisionless state switch between Ground and Hover states if (rayHasHit) { diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h index 85a02cd56a..74649d1572 100644 --- a/libraries/physics/src/CharacterController.h +++ b/libraries/physics/src/CharacterController.h @@ -153,7 +153,9 @@ protected: btVector3 _preSimulationVelocity; btVector3 _velocityChange; btTransform _followDesiredBodyTransform; - btTransform _characterBodyTransform; + btVector3 _position; + btQuaternion _rotation; + //btTransform _characterBodyTransform; glm::vec3 _shapeLocalOffset; diff --git a/libraries/physics/src/CharacterGhostObject.cpp b/libraries/physics/src/CharacterGhostObject.cpp index 6529f2c944..a4b6e90266 100755 --- a/libraries/physics/src/CharacterGhostObject.cpp +++ b/libraries/physics/src/CharacterGhostObject.cpp @@ -14,6 +14,8 @@ #include #include +#include + #include "CharacterGhostShape.h" #include "CharacterRayResult.h" @@ -38,6 +40,10 @@ void CharacterGhostObject::getCollisionGroupAndMask(int16_t& group, int16_t& mas mask = _collisionFilterMask; } +void CharacterGhostObject::setRadiusAndHalfHeight(btScalar radius, btScalar halfHeight) { + _radius = radius; + _halfHeight = halfHeight; +} void CharacterGhostObject::setUpDirection(const btVector3& up) { btScalar length = up.length(); @@ -99,10 +105,12 @@ void CharacterGhostObject::move(btScalar dt, btScalar overshoot) { // TODO: figure out how to untrap character } + btTransform startTransform = getWorldTransform(); + btVector3 startPosition = startTransform.getOrigin(); if (_onFloor) { - // a floor was identified during resolvePenetration() - _hovering = false; - updateTraction(); + // resolvePenetration() pushed the avatar out of a floor so + // we must updateTraction() before using _linearVelocity + updateTraction(startPosition); } btVector3 forwardSweep = dt * _linearVelocity; @@ -110,7 +118,7 @@ void CharacterGhostObject::move(btScalar dt, btScalar overshoot) { btScalar MIN_SWEEP_DISTANCE = 0.0001f; if (stepDistance < MIN_SWEEP_DISTANCE) { // not moving, no need to sweep - updateHoverState(getWorldTransform()); + updateTraction(startPosition); return; } @@ -128,22 +136,19 @@ void CharacterGhostObject::move(btScalar dt, btScalar overshoot) { // step forward CharacterSweepResult result(this); - btTransform startTransform = getWorldTransform(); - btTransform transform = startTransform; - btTransform nextTransform = transform; - nextTransform.setOrigin(transform.getOrigin() + forwardSweep); - sweepTest(convexShape, transform, nextTransform, result); // forward + btTransform nextTransform = startTransform; + nextTransform.setOrigin(startPosition + forwardSweep); + sweepTest(convexShape, startTransform, nextTransform, result); // forward if (!result.hasHit()) { - nextTransform.setOrigin(transform.getOrigin() + (stepDistance / longSweepDistance) * forwardSweep); + nextTransform.setOrigin(startPosition + (stepDistance / longSweepDistance) * forwardSweep); setWorldTransform(nextTransform); - updateHoverState(nextTransform); - updateTraction(); + updateTraction(nextTransform.getOrigin()); return; } // check if this hit is obviously unsteppable - btVector3 hitFromBase = result.m_hitPointWorld - (transform.getOrigin() - (_distanceToFeet * _upDirection)); + btVector3 hitFromBase = result.m_hitPointWorld - (startPosition - ((_radius + _halfHeight) * _upDirection)); btScalar hitHeight = hitFromBase.dot(_upDirection); if (hitHeight > _maxStepHeight) { // capsule can't step over the obstacle so move forward as much as possible before we bail @@ -152,8 +157,8 @@ void CharacterGhostObject::move(btScalar dt, btScalar overshoot) { if (forwardDistance > stepDistance) { forwardTranslation *= stepDistance / forwardDistance; } - transform.setOrigin(transform.getOrigin() + forwardTranslation); - setWorldTransform(transform); + nextTransform.setOrigin(startPosition + forwardTranslation); + setWorldTransform(nextTransform); return; } // if we get here then we hit something that might be steppable @@ -166,35 +171,37 @@ void CharacterGhostObject::move(btScalar dt, btScalar overshoot) { // raise by availableStepHeight before sweeping forward result.resetHitHistory(); - transform.setOrigin(startTransform.getOrigin() + availableStepHeight * _upDirection); - nextTransform.setOrigin(transform.getOrigin() + forwardSweep); - sweepTest(convexShape, transform, nextTransform, result); + startTransform.setOrigin(startPosition + availableStepHeight * _upDirection); + nextTransform.setOrigin(startTransform.getOrigin() + forwardSweep); + sweepTest(convexShape, startTransform, nextTransform, result); if (result.hasHit()) { - transform.setOrigin(transform.getOrigin() + result.m_closestHitFraction * forwardSweep); + startTransform.setOrigin(startTransform.getOrigin() + result.m_closestHitFraction * forwardSweep); } else { - transform = nextTransform; + startTransform = nextTransform; } // sweep down in search of future landing spot result.resetHitHistory(); - btVector3 downSweep = (dt * _linearVelocity.dot(_upDirection) - availableStepHeight) * _upDirection; - nextTransform.setOrigin(transform.getOrigin() + downSweep); - sweepTest(convexShape, transform, nextTransform, result); + btVector3 downSweep = (- availableStepHeight) * _upDirection; + nextTransform.setOrigin(startTransform.getOrigin() + downSweep); + sweepTest(convexShape, startTransform, nextTransform, result); if (result.hasHit() && result.m_hitNormalWorld.dot(_upDirection) > _maxWallNormalUpComponent) { // can stand on future landing spot, so we interpolate toward it _floorNormal = result.m_hitNormalWorld; + _floorContact = result.m_hitPointWorld; _onFloor = true; _hovering = false; - nextTransform.setOrigin(transform.getOrigin() + result.m_closestHitFraction * downSweep); - btVector3 totalStep = nextTransform.getOrigin() - startTransform.getOrigin(); - transform.setOrigin(startTransform.getOrigin() + (stepDistance / totalStep.length()) * totalStep); + nextTransform.setOrigin(startTransform.getOrigin() + result.m_closestHitFraction * downSweep); + btVector3 totalStep = nextTransform.getOrigin() - startPosition; + nextTransform.setOrigin(startPosition + (stepDistance / totalStep.length()) * totalStep); + updateTraction(nextTransform.getOrigin()); } else { // either there is no future landing spot, or there is but we can't stand on it // in any case: we go forward as much as possible - transform.setOrigin(startTransform.getOrigin() + forwardSweepHitFraction * (stepDistance / longSweepDistance) * forwardSweep); + nextTransform.setOrigin(startPosition + forwardSweepHitFraction * (stepDistance / longSweepDistance) * forwardSweep); + updateTraction(nextTransform.getOrigin()); } - setWorldTransform(transform); - updateTraction(); + setWorldTransform(nextTransform); } bool CharacterGhostObject::sweepTest( @@ -297,6 +304,11 @@ bool CharacterGhostObject::resolvePenetration(int numTries) { if (normalDotUp > _maxWallNormalUpComponent) { mostFloorPenetration = penetrationDepth; _floorNormal = normal; + if (directionSign > 0.0f) { + _floorContact = pt.m_positionWorldOnA; + } else { + _floorContact = pt.m_positionWorldOnB; + } _onFloor = true; } } @@ -327,17 +339,36 @@ void CharacterGhostObject::refreshOverlappingPairCache() { void CharacterGhostObject::updateVelocity(btScalar dt) { if (_hovering) { - _linearVelocity *= 0.99f; // HACK damping + _linearVelocity *= 0.999f; // HACK damping } else { _linearVelocity += (dt * _gravity) * _upDirection; } } -void CharacterGhostObject::updateTraction() { +void CharacterGhostObject::updateHoverState(const btVector3& position) { + if (_onFloor) { + _hovering = false; + } else { + // cast a ray down looking for floor support + CharacterRayResult rayResult(this); + btScalar distanceToFeet = _radius + _halfHeight; + btScalar slop = 2.0f * getCollisionShape()->getMargin(); // slop to help ray start OUTSIDE the floor object + btVector3 startPos = position - ((distanceToFeet - slop) * _upDirection); + btVector3 endPos = startPos - (2.0f * distanceToFeet) * _upDirection; + rayTest(startPos, endPos, rayResult); + // we're hovering if the ray didn't hit anything or hit unstandable slope + _hovering = !rayResult.hasHit() || rayResult.m_hitNormalWorld.dot(_upDirection) < _maxWallNormalUpComponent; + } +} + +void CharacterGhostObject::updateTraction(const btVector3& position) { + updateHoverState(position); if (_hovering) { _linearVelocity = _motorVelocity; } else if (_onFloor) { - btVector3 pathDirection = _floorNormal.cross(_motorVelocity).cross(_floorNormal); + // compute a velocity that swings the capsule around the _floorContact + btVector3 leverArm = _floorContact - position; + btVector3 pathDirection = leverArm.cross(_motorVelocity.cross(leverArm)); btScalar pathLength = pathDirection.length(); if (pathLength > FLT_EPSILON) { _linearVelocity = (_motorSpeed / pathLength) * pathDirection; @@ -360,13 +391,3 @@ btScalar CharacterGhostObject::measureAvailableStepHeight() const { return result.m_closestHitFraction * _maxStepHeight; } -void CharacterGhostObject::updateHoverState(const btTransform& transform) { - // cast a ray down looking for floor support - CharacterRayResult rayResult(this); - btVector3 startPos = transform.getOrigin() - ((_distanceToFeet - 0.1f) * _upDirection); // 0.1 HACK to make ray hit - btVector3 endPos = startPos - (2.0f * _distanceToFeet) * _upDirection; - rayTest(startPos, endPos, rayResult); - // we're hovering if the ray didn't hit an object we can stand on - _hovering = !(rayResult.hasHit() && rayResult.m_hitNormalWorld.dot(_upDirection) > _maxWallNormalUpComponent); -} - diff --git a/libraries/physics/src/CharacterGhostObject.h b/libraries/physics/src/CharacterGhostObject.h index dd2f694a59..274057a907 100755 --- a/libraries/physics/src/CharacterGhostObject.h +++ b/libraries/physics/src/CharacterGhostObject.h @@ -31,7 +31,7 @@ public: void setCollisionGroupAndMask(int16_t group, int16_t mask); void getCollisionGroupAndMask(int16_t& group, int16_t& mask) const; - void setDistanceToFeet(btScalar distance) { _distanceToFeet = distance; } + void setRadiusAndHalfHeight(btScalar radius, btScalar halfHeight); void setUpDirection(const btVector3& up); void setMotorVelocity(const btVector3& velocity); void setGravity(btScalar gravity) { _gravity = gravity; } // NOTE: we expect _gravity to be negative (in _upDirection) @@ -50,6 +50,9 @@ public: const btTransform& start, const btTransform& end, CharacterSweepResult& result) const; + + bool isHovering() const { return _hovering; } + protected: void removeFromWorld(); void addToWorld(); @@ -61,17 +64,20 @@ protected: bool resolvePenetration(int numTries); void refreshOverlappingPairCache(); void updateVelocity(btScalar dt); - void updateTraction(); + void updateTraction(const btVector3& position); btScalar measureAvailableStepHeight() const; - void updateHoverState(const btTransform& transform); + void updateHoverState(const btVector3& position); protected: btVector3 _upDirection { 0.0f, 1.0f, 0.0f }; // input, up in world-frame btVector3 _motorVelocity { 0.0f, 0.0f, 0.0f }; // input, velocity character is trying to achieve btVector3 _linearVelocity { 0.0f, 0.0f, 0.0f }; // internal, actual character velocity btVector3 _floorNormal { 0.0f, 0.0f, 0.0f }; // internal, probable floor normal + btVector3 _floorContact { 0.0f, 0.0f, 0.0f }; // internal, last floor contact point btCollisionWorld* _world { nullptr }; // input, pointer to world - btScalar _distanceToFeet { 0.0f }; // input, distance from object center to lowest point on shape + //btScalar _distanceToFeet { 0.0f }; // input, distance from object center to lowest point on shape + btScalar _halfHeight { 0.0f }; + btScalar _radius { 0.0f }; btScalar _motorSpeed { 0.0f }; // internal, cached for speed btScalar _gravity { 0.0f }; // input, amplitude of gravity along _upDirection (should be negative) btScalar _maxWallNormalUpComponent { 0.0f }; // input: max vertical component of wall normal From a8af8d6027e7fc9c1145ecddaeb317cf27c717b7 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 19 Sep 2016 15:03:03 -0700 Subject: [PATCH 038/101] move code into CharacterController::updateState() --- libraries/physics/src/CharacterController.cpp | 197 +++++++++--------- libraries/physics/src/CharacterController.h | 1 + 2 files changed, 100 insertions(+), 98 deletions(-) diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index acd82d73ea..1372a6b304 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -581,116 +581,121 @@ void CharacterController::computeNewVelocity(btScalar dt, glm::vec3& velocity) { velocity = bulletToGLM(btVelocity); } -void CharacterController::preSimulation() { - if (_dynamicsWorld) { - quint64 now = usecTimestampNow(); +void CharacterController::updateState() { + const btScalar FLY_TO_GROUND_THRESHOLD = 0.1f * _radius; + const btScalar GROUND_TO_FLY_THRESHOLD = 0.8f * _radius + _halfHeight; + const quint64 TAKE_OFF_TO_IN_AIR_PERIOD = 250 * MSECS_PER_SECOND; + const btScalar MIN_HOVER_HEIGHT = 2.5f; + const quint64 JUMP_TO_HOVER_PERIOD = 1100 * MSECS_PER_SECOND; - // slam body transform - _rigidBody->setWorldTransform(btTransform(btTransform(_rotation, _position))); - btVector3 velocity = _rigidBody->getLinearVelocity(); - _preSimulationVelocity = velocity; + // scan for distant floor + // rayStart is at center of bottom sphere + btVector3 rayStart = _position; - // scan for distant floor - // rayStart is at center of bottom sphere - btVector3 rayStart = _position; + // rayEnd is straight down MAX_FALL_HEIGHT + btScalar rayLength = _radius + MAX_FALL_HEIGHT; + btVector3 rayEnd = rayStart - rayLength * _currentUp; - // rayEnd is straight down MAX_FALL_HEIGHT - btScalar rayLength = _radius + MAX_FALL_HEIGHT; - btVector3 rayEnd = rayStart - rayLength * _currentUp; - - const btScalar FLY_TO_GROUND_THRESHOLD = 0.1f * _radius; - const btScalar GROUND_TO_FLY_THRESHOLD = 0.8f * _radius + _halfHeight; - const quint64 TAKE_OFF_TO_IN_AIR_PERIOD = 250 * MSECS_PER_SECOND; - const btScalar MIN_HOVER_HEIGHT = 2.5f; - const quint64 JUMP_TO_HOVER_PERIOD = 1100 * MSECS_PER_SECOND; - const btScalar MAX_WALKING_SPEED = 2.5f; + ClosestNotMe rayCallback(_rigidBody); + rayCallback.m_closestHitFraction = 1.0f; + _dynamicsWorld->rayTest(rayStart, rayEnd, rayCallback); + bool rayHasHit = rayCallback.hasHit(); + quint64 now = usecTimestampNow(); + if (rayHasHit) { + _rayHitStartTime = now; + _floorDistance = rayLength * rayCallback.m_closestHitFraction - (_radius + _halfHeight); + } else { const quint64 RAY_HIT_START_PERIOD = 500 * MSECS_PER_SECOND; - - ClosestNotMe rayCallback(_rigidBody); - rayCallback.m_closestHitFraction = 1.0f; - _dynamicsWorld->rayTest(rayStart, rayEnd, rayCallback); - bool rayHasHit = rayCallback.hasHit(); - if (rayHasHit) { - _rayHitStartTime = now; - _floorDistance = rayLength * rayCallback.m_closestHitFraction - (_radius + _halfHeight); - } else if ((now - _rayHitStartTime) < RAY_HIT_START_PERIOD) { + if ((now - _rayHitStartTime) < RAY_HIT_START_PERIOD) { rayHasHit = true; } else { _floorDistance = FLT_MAX; } + } - // record a time stamp when the jump button was first pressed. - if ((_previousFlags & PENDING_FLAG_JUMP) != (_pendingFlags & PENDING_FLAG_JUMP)) { - if (_pendingFlags & PENDING_FLAG_JUMP) { - _jumpButtonDownStartTime = now; - _jumpButtonDownCount++; - } + // record a time stamp when the jump button was first pressed. + bool jumpButtonHeld = _pendingFlags & PENDING_FLAG_JUMP; + if ((_previousFlags & PENDING_FLAG_JUMP) != (_pendingFlags & PENDING_FLAG_JUMP)) { + if (_pendingFlags & PENDING_FLAG_JUMP) { + _jumpButtonDownStartTime = now; + _jumpButtonDownCount++; } + } - bool jumpButtonHeld = _pendingFlags & PENDING_FLAG_JUMP; + btVector3 velocity = _preSimulationVelocity; - btVector3 actualHorizVelocity = velocity - velocity.dot(_currentUp) * _currentUp; - bool flyingFast = _state == State::Hover && actualHorizVelocity.length() > (MAX_WALKING_SPEED * 0.75f); - - // OUTOFBODY_HACK -- disable normal state transitions while collisionless - if (_collisionGroup == BULLET_COLLISION_GROUP_MY_AVATAR) { - switch (_state) { - case State::Ground: - if (!rayHasHit && !_hasSupport) { - SET_STATE(State::Hover, "no ground detected"); - } else if (_pendingFlags & PENDING_FLAG_JUMP && _jumpButtonDownCount != _takeoffJumpButtonID) { - _takeoffJumpButtonID = _jumpButtonDownCount; - _takeoffToInAirStartTime = now; - SET_STATE(State::Takeoff, "jump pressed"); - } else if (rayHasHit && !_hasSupport && _floorDistance > GROUND_TO_FLY_THRESHOLD) { - SET_STATE(State::InAir, "falling"); - } - break; - case State::Takeoff: - if (!rayHasHit && !_hasSupport) { - SET_STATE(State::Hover, "no ground"); - } else if ((now - _takeoffToInAirStartTime) > TAKE_OFF_TO_IN_AIR_PERIOD) { - SET_STATE(State::InAir, "takeoff done"); - velocity += _jumpSpeed * _currentUp; - _rigidBody->setLinearVelocity(velocity); - } - break; - case State::InAir: { - if ((velocity.dot(_currentUp) <= (JUMP_SPEED / 2.0f)) && ((_floorDistance < FLY_TO_GROUND_THRESHOLD) || _hasSupport)) { - SET_STATE(State::Ground, "hit ground"); - } else { - btVector3 desiredVelocity = _targetVelocity; - if (desiredVelocity.length2() < MIN_TARGET_SPEED_SQUARED) { - desiredVelocity = btVector3(0.0f, 0.0f, 0.0f); - } - bool vertTargetSpeedIsNonZero = desiredVelocity.dot(_currentUp) > MIN_TARGET_SPEED; - if ((jumpButtonHeld || vertTargetSpeedIsNonZero) && (_takeoffJumpButtonID != _jumpButtonDownCount)) { - SET_STATE(State::Hover, "double jump button"); - } else if ((jumpButtonHeld || vertTargetSpeedIsNonZero) && (now - _jumpButtonDownStartTime) > JUMP_TO_HOVER_PERIOD) { - SET_STATE(State::Hover, "jump button held"); - } - } - break; + // OUTOFBODY_HACK -- disable normal state transitions while collisionless + if (_collisionGroup == BULLET_COLLISION_GROUP_MY_AVATAR) { + switch (_state) { + case State::Ground: + if (!rayHasHit && !_hasSupport) { + SET_STATE(State::Hover, "no ground detected"); + } else if (_pendingFlags & PENDING_FLAG_JUMP && _jumpButtonDownCount != _takeoffJumpButtonID) { + _takeoffJumpButtonID = _jumpButtonDownCount; + _takeoffToInAirStartTime = now; + SET_STATE(State::Takeoff, "jump pressed"); + } else if (rayHasHit && !_hasSupport && _floorDistance > GROUND_TO_FLY_THRESHOLD) { + SET_STATE(State::InAir, "falling"); } - case State::Hover: - if ((_floorDistance < MIN_HOVER_HEIGHT) && !jumpButtonHeld && !flyingFast) { - SET_STATE(State::InAir, "near ground"); - } else if (((_floorDistance < FLY_TO_GROUND_THRESHOLD) || _hasSupport) && !flyingFast) { - SET_STATE(State::Ground, "touching ground"); - } - break; + break; + case State::Takeoff: + if (!rayHasHit && !_hasSupport) { + SET_STATE(State::Hover, "no ground"); + } else if ((now - _takeoffToInAirStartTime) > TAKE_OFF_TO_IN_AIR_PERIOD) { + SET_STATE(State::InAir, "takeoff done"); + velocity += _jumpSpeed * _currentUp; + _rigidBody->setLinearVelocity(velocity); } - if (_moveKinematically && _ghost.isHovering()) { - SET_STATE(State::Hover, "kinematic motion"); // HACK - } - } else { - // OUTOFBODY_HACK -- in collisionless state switch between Ground and Hover states - if (rayHasHit) { - SET_STATE(State::Ground, "collisionless above ground"); + break; + case State::InAir: { + if ((velocity.dot(_currentUp) <= (JUMP_SPEED / 2.0f)) && ((_floorDistance < FLY_TO_GROUND_THRESHOLD) || _hasSupport)) { + SET_STATE(State::Ground, "hit ground"); } else { - SET_STATE(State::Hover, "collisionless in air"); + btVector3 desiredVelocity = _targetVelocity; + if (desiredVelocity.length2() < MIN_TARGET_SPEED_SQUARED) { + desiredVelocity = btVector3(0.0f, 0.0f, 0.0f); + } + bool vertTargetSpeedIsNonZero = desiredVelocity.dot(_currentUp) > MIN_TARGET_SPEED; + if ((jumpButtonHeld || vertTargetSpeedIsNonZero) && (_takeoffJumpButtonID != _jumpButtonDownCount)) { + SET_STATE(State::Hover, "double jump button"); + } else if ((jumpButtonHeld || vertTargetSpeedIsNonZero) && (now - _jumpButtonDownStartTime) > JUMP_TO_HOVER_PERIOD) { + SET_STATE(State::Hover, "jump button held"); + } } + break; } + case State::Hover: + btVector3 actualHorizVelocity = velocity - velocity.dot(_currentUp) * _currentUp; + const btScalar MAX_WALKING_SPEED = 2.5f; + bool flyingFast = _state == State::Hover && actualHorizVelocity.length() > (MAX_WALKING_SPEED * 0.75f); + + if ((_floorDistance < MIN_HOVER_HEIGHT) && !jumpButtonHeld && !flyingFast) { + SET_STATE(State::InAir, "near ground"); + } else if (((_floorDistance < FLY_TO_GROUND_THRESHOLD) || _hasSupport) && !flyingFast) { + SET_STATE(State::Ground, "touching ground"); + } + break; + } + if (_moveKinematically && _ghost.isHovering()) { + SET_STATE(State::Hover, "kinematic motion"); // HACK + } + } else { + // OUTOFBODY_HACK -- in collisionless state switch only between Ground and Hover states + if (rayHasHit) { + SET_STATE(State::Ground, "collisionless above ground"); + } else { + SET_STATE(State::Hover, "collisionless in air"); + } + } +} + +void CharacterController::preSimulation() { + if (_dynamicsWorld) { + // slam body transform and remember velocity + _rigidBody->setWorldTransform(btTransform(btTransform(_rotation, _position))); + _preSimulationVelocity = _rigidBody->getLinearVelocity(); + + updateState(); } _previousFlags = _pendingFlags; @@ -702,13 +707,9 @@ void CharacterController::preSimulation() { } void CharacterController::postSimulation() { - // postSimulation() exists for symmetry and just in case we need to do something here later - - btVector3 velocity = _rigidBody->getLinearVelocity(); - _velocityChange = velocity - _preSimulationVelocity; + _velocityChange = _rigidBody->getLinearVelocity() - _preSimulationVelocity; } - bool CharacterController::getRigidBodyLocation(glm::vec3& avatarRigidBodyPosition, glm::quat& avatarRigidBodyRotation) { if (!_rigidBody) { return false; diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h index 74649d1572..7f112ca385 100644 --- a/libraries/physics/src/CharacterController.h +++ b/libraries/physics/src/CharacterController.h @@ -109,6 +109,7 @@ public: }; State getState() const { return _state; } + void updateState(); void setLocalBoundingBox(const glm::vec3& minCorner, const glm::vec3& scale); From 59e6ca8f8d0ae0dc15e155e35e0b918fde970d20 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 20 Sep 2016 08:29:02 -0700 Subject: [PATCH 039/101] avatar follows HMD using velocity motor --- interface/src/avatar/MyAvatar.cpp | 4 +- libraries/physics/src/CharacterController.cpp | 104 +++++++----------- libraries/physics/src/CharacterController.h | 7 -- 3 files changed, 38 insertions(+), 77 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index cd4ee4784b..c8b88bea32 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -1387,9 +1387,7 @@ void MyAvatar::harvestResultsFromPhysicsSimulation(float deltaTime) { //_bodySensorMatrix = deriveBodyFromHMDSensor(); if (_characterController.isEnabledAndReady()) { - setVelocity(_characterController.getLinearVelocity() + _characterController.getFollowVelocity()); - } else { - setVelocity(getVelocity() + _characterController.getFollowVelocity()); + setVelocity(_characterController.getLinearVelocity()); } _follow.postPhysicsUpdate(*this); diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 1372a6b304..ccd985c1ad 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -75,9 +75,6 @@ CharacterController::CharacterController() { _takeoffToInAirStartTime = 0; _jumpButtonDownStartTime = 0; _jumpButtonDownCount = 0; - _followTime = 0.0f; - _followLinearDisplacement = btVector3(0, 0, 0); - _followAngularDisplacement = btQuaternion::getIdentity(); _hasSupport = false; _pendingFlags = PENDING_FLAG_UPDATE_SHAPE; @@ -203,6 +200,43 @@ const btScalar MIN_TARGET_SPEED_SQUARED = MIN_TARGET_SPEED * MIN_TARGET_SPEED; void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) { btVector3 velocity = _rigidBody->getLinearVelocity() - _parentVelocity; + if (_following) { + // OUTOFBODY_HACK -- these consts were copied from elsewhere, and then tuned + const float NORMAL_WALKING_SPEED = 2.5f; // actual walk speed is 2.5 m/sec + const float FOLLOW_TIMESCALE = 0.8f; + const float FOLLOW_ROTATION_THRESHOLD = PI / 6.0f; + const float FOLLOW_FACTOR = 0.5f; + const float MAX_ANGULAR_SPEED = FOLLOW_ROTATION_THRESHOLD / FOLLOW_TIMESCALE; + + // linear part uses a motor + btTransform bodyTransform = _rigidBody->getWorldTransform(); + btVector3 startPos = bodyTransform.getOrigin(); + btVector3 deltaPos = _followDesiredBodyTransform.getOrigin() - startPos; + btVector3 vel = deltaPos * (FOLLOW_FACTOR / dt); + btScalar speed = vel.length(); + if (speed > NORMAL_WALKING_SPEED) { + vel *= NORMAL_WALKING_SPEED / speed; + } + const float HORIZONTAL_FOLLOW_TIMESCALE = 0.01f; // a very small timescale here is OK + const float VERTICAL_FOLLOW_TIMESCALE = (_state == State::Hover) ? HORIZONTAL_FOLLOW_TIMESCALE : 20.0f; + glm::quat motorRotation; + addMotor(bulletToGLM(vel), motorRotation, HORIZONTAL_FOLLOW_TIMESCALE, VERTICAL_FOLLOW_TIMESCALE); + + // angular part uses incremental teleports + btQuaternion startRot = bodyTransform.getRotation(); + glm::vec2 currentFacing = getFacingDir2D(bulletToGLM(startRot)); + glm::vec2 currentRight(currentFacing.y, - currentFacing.x); + glm::vec2 desiredFacing = getFacingDir2D(bulletToGLM(_followDesiredBodyTransform.getRotation())); + float deltaAngle = acosf(glm::clamp(glm::dot(currentFacing, desiredFacing), -1.0f, 1.0f)); + float angularSpeed = 0.5f * deltaAngle / dt; + if (angularSpeed > MAX_ANGULAR_SPEED) { + angularSpeed *= MAX_ANGULAR_SPEED / angularSpeed; + } + float sign = copysignf(1.0f, glm::dot(desiredFacing, currentRight)); + btQuaternion angularDisplacement = btQuaternion(btVector3(0.0f, 1.0f, 0.0f), sign * angularSpeed * dt); + btQuaternion endRot = angularDisplacement * startRot; + _rigidBody->setWorldTransform(btTransform(endRot, startPos)); + } computeNewVelocity(dt, velocity); if (_moveKinematically) { @@ -222,50 +256,6 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) { // This mirrors the computation done in MyAvatar::FollowHelper::postPhysicsUpdate(). _rigidBody->setLinearVelocity(velocity + _parentVelocity); - if (_following) { - // OUTOFBODY_HACK -- these consts were copied from elsewhere, and then tuned - const float NORMAL_WALKING_SPEED = 1.5f; // actual walk speed is 2.5 m/sec - const float FOLLOW_TIME = 0.8f; - const float FOLLOW_ROTATION_THRESHOLD = cosf(PI / 6.0f); - const float FOLLOW_FACTOR = 0.5f; - - const float MAX_ANGULAR_SPEED = FOLLOW_ROTATION_THRESHOLD / FOLLOW_TIME; - - btTransform bodyTransform = _rigidBody->getWorldTransform(); - - btVector3 startPos = bodyTransform.getOrigin(); - btVector3 deltaPos = _followDesiredBodyTransform.getOrigin() - startPos; - btVector3 vel = deltaPos * (FOLLOW_FACTOR / dt); - btScalar speed = vel.length(); - if (speed > NORMAL_WALKING_SPEED) { - vel *= NORMAL_WALKING_SPEED / speed; - } - btVector3 linearDisplacement = vel * dt; - btVector3 endPos = startPos + linearDisplacement; - - btQuaternion startRot = bodyTransform.getRotation(); - glm::vec2 currentFacing = getFacingDir2D(bulletToGLM(startRot)); - glm::vec2 currentRight(currentFacing.y, -currentFacing.x); - glm::vec2 desiredFacing = getFacingDir2D(bulletToGLM(_followDesiredBodyTransform.getRotation())); - float deltaAngle = acosf(glm::clamp(glm::dot(currentFacing, desiredFacing), -1.0f, 1.0f)); - float angularSpeed = 0.5f * deltaAngle / dt; - if (angularSpeed > MAX_ANGULAR_SPEED) { - angularSpeed *= MAX_ANGULAR_SPEED / angularSpeed; - } - float sign = copysignf(1.0f, glm::dot(desiredFacing, currentRight)); - btQuaternion angularDisplacement = btQuaternion(btVector3(0.0f, 1.0f, 0.0f), sign * angularSpeed * dt); - btQuaternion endRot = angularDisplacement * startRot; - - // in order to accumulate displacement of avatar position, we need to take _shapeLocalOffset into account. - btVector3 shapeLocalOffset = glmToBullet(_shapeLocalOffset); - btVector3 swingDisplacement = rotateVector(endRot, -shapeLocalOffset) - rotateVector(startRot, -shapeLocalOffset); - - _followLinearDisplacement = linearDisplacement + swingDisplacement + _followLinearDisplacement; - _followAngularDisplacement = angularDisplacement * _followAngularDisplacement; - - _rigidBody->setWorldTransform(btTransform(endRot, endPos)); - } - _followTime += dt; _ghost.setWorldTransform(_rigidBody->getWorldTransform()); } } @@ -422,22 +412,6 @@ void CharacterController::setFollowParameters(const glm::mat4& desiredWorldBodyM _following = true; } -glm::vec3 CharacterController::getFollowLinearDisplacement() const { - return bulletToGLM(_followLinearDisplacement); -} - -glm::quat CharacterController::getFollowAngularDisplacement() const { - return bulletToGLM(_followAngularDisplacement); -} - -glm::vec3 CharacterController::getFollowVelocity() const { - if (_followTime > 0.0f) { - return bulletToGLM(_followLinearDisplacement) / _followTime; - } else { - return glm::vec3(); - } -} - glm::vec3 CharacterController::getLinearVelocity() const { glm::vec3 velocity(0.0f); if (_rigidBody) { @@ -700,10 +674,6 @@ void CharacterController::preSimulation() { _previousFlags = _pendingFlags; _pendingFlags &= ~PENDING_FLAG_JUMP; - - _followTime = 0.0f; - _followLinearDisplacement = btVector3(0.0f, 0.0f, 0.0f); - _followAngularDisplacement = btQuaternion::getIdentity(); } void CharacterController::postSimulation() { diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h index 7f112ca385..a9ce30c4d9 100644 --- a/libraries/physics/src/CharacterController.h +++ b/libraries/physics/src/CharacterController.h @@ -88,10 +88,6 @@ public: void setParentVelocity(const glm::vec3& parentVelocity); void setFollowParameters(const glm::mat4& desiredWorldBodyMatrix); void disableFollow() { _following = false; } - float getFollowTime() const { return _followTime; } - glm::vec3 getFollowLinearDisplacement() const; - glm::quat getFollowAngularDisplacement() const; - glm::vec3 getFollowVelocity() const; glm::vec3 getLinearVelocity() const; glm::vec3 getVelocityChange() const; @@ -177,9 +173,6 @@ protected: btScalar _gravity; btScalar _jumpSpeed; - btScalar _followTime; - btVector3 _followLinearDisplacement; - btQuaternion _followAngularDisplacement; btVector3 _linearAcceleration; bool _following { false }; From 7258835f3f481da9f698a03e2fbcf114adee8e3b Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 20 Sep 2016 09:14:11 -0700 Subject: [PATCH 040/101] restore orientation from physics to Avatar --- interface/src/avatar/MyAvatar.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index c8b88bea32..a1625bd42e 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -1378,8 +1378,7 @@ void MyAvatar::harvestResultsFromPhysicsSimulation(float deltaTime) { glm::vec3 position = getPosition(); glm::quat orientation = getOrientation(); if (_characterController.isEnabledAndReady()) { - glm::quat bogusOrientation; - _characterController.getPositionAndOrientation(position, bogusOrientation); + _characterController.getPositionAndOrientation(position, orientation); } nextAttitude(position, orientation); From 0ef8ef473404ecc9ac99d094cc651564570b83b2 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 20 Sep 2016 09:37:51 -0700 Subject: [PATCH 041/101] tune follow speeds --- libraries/physics/src/CharacterController.cpp | 26 ++++++++++++------- 1 file changed, 16 insertions(+), 10 deletions(-) diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index ccd985c1ad..11917e2f70 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -202,25 +202,31 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) { btVector3 velocity = _rigidBody->getLinearVelocity() - _parentVelocity; if (_following) { // OUTOFBODY_HACK -- these consts were copied from elsewhere, and then tuned - const float NORMAL_WALKING_SPEED = 2.5f; // actual walk speed is 2.5 m/sec + const float NORMAL_WALKING_SPEED = 2.0f; // actual walk speed is 2.5 m/sec const float FOLLOW_TIMESCALE = 0.8f; + const float ONE_STEP_AT_NORMAL_WALKING_SPEED = FOLLOW_TIMESCALE * NORMAL_WALKING_SPEED; const float FOLLOW_ROTATION_THRESHOLD = PI / 6.0f; - const float FOLLOW_FACTOR = 0.5f; + const float FOLLOW_FACTOR = 0.25f; const float MAX_ANGULAR_SPEED = FOLLOW_ROTATION_THRESHOLD / FOLLOW_TIMESCALE; + const float MIN_DELTA_DISTANCE = 0.01f; // linear part uses a motor btTransform bodyTransform = _rigidBody->getWorldTransform(); btVector3 startPos = bodyTransform.getOrigin(); btVector3 deltaPos = _followDesiredBodyTransform.getOrigin() - startPos; - btVector3 vel = deltaPos * (FOLLOW_FACTOR / dt); - btScalar speed = vel.length(); - if (speed > NORMAL_WALKING_SPEED) { - vel *= NORMAL_WALKING_SPEED / speed; + btScalar deltaDistance = deltaPos.length(); + if (deltaDistance > MIN_DELTA_DISTANCE) { + btVector3 vel = deltaPos; + if (deltaDistance > ONE_STEP_AT_NORMAL_WALKING_SPEED) { + vel *= NORMAL_WALKING_SPEED / deltaDistance; + } else { + vel *= NORMAL_WALKING_SPEED * (deltaDistance / ONE_STEP_AT_NORMAL_WALKING_SPEED); + } + const float HORIZONTAL_FOLLOW_TIMESCALE = 0.01f; // a very small timescale here is OK + const float VERTICAL_FOLLOW_TIMESCALE = (_state == State::Hover) ? HORIZONTAL_FOLLOW_TIMESCALE : 20.0f; + glm::quat worldFrameRotation; // identity + addMotor(bulletToGLM(vel), worldFrameRotation, HORIZONTAL_FOLLOW_TIMESCALE, VERTICAL_FOLLOW_TIMESCALE); } - const float HORIZONTAL_FOLLOW_TIMESCALE = 0.01f; // a very small timescale here is OK - const float VERTICAL_FOLLOW_TIMESCALE = (_state == State::Hover) ? HORIZONTAL_FOLLOW_TIMESCALE : 20.0f; - glm::quat motorRotation; - addMotor(bulletToGLM(vel), motorRotation, HORIZONTAL_FOLLOW_TIMESCALE, VERTICAL_FOLLOW_TIMESCALE); // angular part uses incremental teleports btQuaternion startRot = bodyTransform.getRotation(); From 55e1d058876cf4bbfc9ab60f307b6ceb1d5fa8b4 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 20 Sep 2016 10:11:25 -0700 Subject: [PATCH 042/101] more follow velocity tuning --- libraries/physics/src/CharacterController.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 11917e2f70..0906b43156 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -206,7 +206,6 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) { const float FOLLOW_TIMESCALE = 0.8f; const float ONE_STEP_AT_NORMAL_WALKING_SPEED = FOLLOW_TIMESCALE * NORMAL_WALKING_SPEED; const float FOLLOW_ROTATION_THRESHOLD = PI / 6.0f; - const float FOLLOW_FACTOR = 0.25f; const float MAX_ANGULAR_SPEED = FOLLOW_ROTATION_THRESHOLD / FOLLOW_TIMESCALE; const float MIN_DELTA_DISTANCE = 0.01f; @@ -222,7 +221,7 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) { } else { vel *= NORMAL_WALKING_SPEED * (deltaDistance / ONE_STEP_AT_NORMAL_WALKING_SPEED); } - const float HORIZONTAL_FOLLOW_TIMESCALE = 0.01f; // a very small timescale here is OK + const float HORIZONTAL_FOLLOW_TIMESCALE = 0.25f; const float VERTICAL_FOLLOW_TIMESCALE = (_state == State::Hover) ? HORIZONTAL_FOLLOW_TIMESCALE : 20.0f; glm::quat worldFrameRotation; // identity addMotor(bulletToGLM(vel), worldFrameRotation, HORIZONTAL_FOLLOW_TIMESCALE, VERTICAL_FOLLOW_TIMESCALE); From 3e02bac412d3d1ebe3b2bc3ccee3fbe9fc3da37f Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 20 Sep 2016 10:45:13 -0700 Subject: [PATCH 043/101] more velocity tuning --- libraries/physics/src/CharacterController.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 0906b43156..ff868d5485 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -202,7 +202,7 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) { btVector3 velocity = _rigidBody->getLinearVelocity() - _parentVelocity; if (_following) { // OUTOFBODY_HACK -- these consts were copied from elsewhere, and then tuned - const float NORMAL_WALKING_SPEED = 2.0f; // actual walk speed is 2.5 m/sec + const float NORMAL_WALKING_SPEED = 2.5f; // actual walk speed is 2.5 m/sec const float FOLLOW_TIMESCALE = 0.8f; const float ONE_STEP_AT_NORMAL_WALKING_SPEED = FOLLOW_TIMESCALE * NORMAL_WALKING_SPEED; const float FOLLOW_ROTATION_THRESHOLD = PI / 6.0f; @@ -219,9 +219,9 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) { if (deltaDistance > ONE_STEP_AT_NORMAL_WALKING_SPEED) { vel *= NORMAL_WALKING_SPEED / deltaDistance; } else { - vel *= NORMAL_WALKING_SPEED * (deltaDistance / ONE_STEP_AT_NORMAL_WALKING_SPEED); + vel /= FOLLOW_TIMESCALE; } - const float HORIZONTAL_FOLLOW_TIMESCALE = 0.25f; + const float HORIZONTAL_FOLLOW_TIMESCALE = 0.2f; const float VERTICAL_FOLLOW_TIMESCALE = (_state == State::Hover) ? HORIZONTAL_FOLLOW_TIMESCALE : 20.0f; glm::quat worldFrameRotation; // identity addMotor(bulletToGLM(vel), worldFrameRotation, HORIZONTAL_FOLLOW_TIMESCALE, VERTICAL_FOLLOW_TIMESCALE); From 3c5e13b34b48daf97b60291cbce8eece348d0784 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 20 Sep 2016 13:45:31 -0700 Subject: [PATCH 044/101] final tuning of follow speeds --- libraries/physics/src/CharacterController.cpp | 36 ++++++++++++------- 1 file changed, 23 insertions(+), 13 deletions(-) diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index ff868d5485..aa119689f7 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -201,39 +201,49 @@ const btScalar MIN_TARGET_SPEED_SQUARED = MIN_TARGET_SPEED * MIN_TARGET_SPEED; void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) { btVector3 velocity = _rigidBody->getLinearVelocity() - _parentVelocity; if (_following) { - // OUTOFBODY_HACK -- these consts were copied from elsewhere, and then tuned - const float NORMAL_WALKING_SPEED = 2.5f; // actual walk speed is 2.5 m/sec - const float FOLLOW_TIMESCALE = 0.8f; - const float ONE_STEP_AT_NORMAL_WALKING_SPEED = FOLLOW_TIMESCALE * NORMAL_WALKING_SPEED; - const float FOLLOW_ROTATION_THRESHOLD = PI / 6.0f; - const float MAX_ANGULAR_SPEED = FOLLOW_ROTATION_THRESHOLD / FOLLOW_TIMESCALE; - const float MIN_DELTA_DISTANCE = 0.01f; - // linear part uses a motor + const float MAX_WALKING_SPEED = 2.5f; // TODO: scale this stuff with avatar size + const float MAX_WALKING_SPEED_DISTANCE = 1.0f; + const float NORMAL_WALKING_SPEED = 0.5f * MAX_WALKING_SPEED; + const float NORMAL_WALKING_SPEED_DISTANCE = 0.5f * MAX_WALKING_SPEED_DISTANCE; + const float FEW_SUBSTEPS = 4.0f * dt; btTransform bodyTransform = _rigidBody->getWorldTransform(); btVector3 startPos = bodyTransform.getOrigin(); btVector3 deltaPos = _followDesiredBodyTransform.getOrigin() - startPos; btScalar deltaDistance = deltaPos.length(); + const float MIN_DELTA_DISTANCE = 0.01f; // TODO: scale by avatar size but cap at (NORMAL_WALKING_SPEED * FEW_SUBSTEPS) if (deltaDistance > MIN_DELTA_DISTANCE) { btVector3 vel = deltaPos; - if (deltaDistance > ONE_STEP_AT_NORMAL_WALKING_SPEED) { - vel *= NORMAL_WALKING_SPEED / deltaDistance; + if (deltaDistance > MAX_WALKING_SPEED_DISTANCE) { + // cap max speed + vel *= MAX_WALKING_SPEED / deltaDistance; + } else if (deltaDistance > NORMAL_WALKING_SPEED_DISTANCE) { + // linearly interpolate to NORMAL_WALKING_SPEED + btScalar alpha = (deltaDistance - NORMAL_WALKING_SPEED_DISTANCE) / (MAX_WALKING_SPEED_DISTANCE - NORMAL_WALKING_SPEED_DISTANCE); + vel *= NORMAL_WALKING_SPEED * (1.0f - alpha) + MAX_WALKING_SPEED * alpha; } else { - vel /= FOLLOW_TIMESCALE; + // use exponential decay but cap at NORMAL_WALKING_SPEED + vel /= FEW_SUBSTEPS; + btScalar speed = vel.length(); + if (speed > NORMAL_WALKING_SPEED) { + vel *= NORMAL_WALKING_SPEED / speed; + } } - const float HORIZONTAL_FOLLOW_TIMESCALE = 0.2f; + const float HORIZONTAL_FOLLOW_TIMESCALE = 0.1f; const float VERTICAL_FOLLOW_TIMESCALE = (_state == State::Hover) ? HORIZONTAL_FOLLOW_TIMESCALE : 20.0f; glm::quat worldFrameRotation; // identity addMotor(bulletToGLM(vel), worldFrameRotation, HORIZONTAL_FOLLOW_TIMESCALE, VERTICAL_FOLLOW_TIMESCALE); } // angular part uses incremental teleports + const float ANGULAR_FOLLOW_TIMESCALE = 0.8f; + const float MAX_ANGULAR_SPEED = (PI / 2.0f) / ANGULAR_FOLLOW_TIMESCALE; btQuaternion startRot = bodyTransform.getRotation(); glm::vec2 currentFacing = getFacingDir2D(bulletToGLM(startRot)); glm::vec2 currentRight(currentFacing.y, - currentFacing.x); glm::vec2 desiredFacing = getFacingDir2D(bulletToGLM(_followDesiredBodyTransform.getRotation())); float deltaAngle = acosf(glm::clamp(glm::dot(currentFacing, desiredFacing), -1.0f, 1.0f)); - float angularSpeed = 0.5f * deltaAngle / dt; + float angularSpeed = deltaAngle / FEW_SUBSTEPS; if (angularSpeed > MAX_ANGULAR_SPEED) { angularSpeed *= MAX_ANGULAR_SPEED / angularSpeed; } From dfe828982b0963ac4260c2ee84cb7e86d680f3e6 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 20 Sep 2016 14:04:22 -0700 Subject: [PATCH 045/101] remove cruft --- libraries/physics/src/CharacterController.h | 1 - 1 file changed, 1 deletion(-) diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h index a9ce30c4d9..7d739bfae3 100644 --- a/libraries/physics/src/CharacterController.h +++ b/libraries/physics/src/CharacterController.h @@ -152,7 +152,6 @@ protected: btTransform _followDesiredBodyTransform; btVector3 _position; btQuaternion _rotation; - //btTransform _characterBodyTransform; glm::vec3 _shapeLocalOffset; From 4afd60f22a004b80db3ebae01aa4c59ac96330df Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Tue, 20 Sep 2016 16:00:49 -0700 Subject: [PATCH 046/101] Flying behavior now works again --- interface/src/avatar/MyAvatar.cpp | 186 +++++++++++++++--------------- interface/src/avatar/MyAvatar.h | 1 + 2 files changed, 93 insertions(+), 94 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index a1625bd42e..659223b893 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -1288,6 +1288,9 @@ controller::Pose MyAvatar::getRightHandControllerPoseInAvatarFrame() const { } void MyAvatar::updateMotors() { + const float DEFAULT_MOTOR_TIMESCALE = 0.2f; + const float INVALID_MOTOR_TIMESCALE = 1.0e6f; + _characterController.clearMotors(); glm::quat motorRotation; if (_motionBehaviors & AVATAR_MOTION_ACTION_MOTOR_ENABLED) { @@ -1300,12 +1303,10 @@ void MyAvatar::updateMotors() { glm::quat liftRotation; swingTwistDecomposition(getHead()->getCameraOrientation(), _worldUpDirection, liftRotation, motorRotation); } - const float DEFAULT_MOTOR_TIMESCALE = 0.2f; - const float INVALID_MOTOR_TIMESCALE = 1.0e6f; if (qApp->isHMDMode()) { - // OUTOFBODY_HACK: add default zero velocity motor to the characterController - _characterController.addMotor(glm::vec3(), motorRotation, DEFAULT_MOTOR_TIMESCALE, INVALID_MOTOR_TIMESCALE); + // OUTOFBODY_HACK: only apply vertical component of _actionMotorVelocity to the characterController + _characterController.addMotor(glm::vec3(0, _actionMotorVelocity.y, 0), motorRotation, DEFAULT_MOTOR_TIMESCALE, INVALID_MOTOR_TIMESCALE); } else { if (_isPushing || _isBraking || !_isBeingPushed) { _characterController.addMotor(_actionMotorVelocity, motorRotation, DEFAULT_MOTOR_TIMESCALE, INVALID_MOTOR_TIMESCALE); @@ -1325,7 +1326,12 @@ void MyAvatar::updateMotors() { // world-frame motorRotation = glm::quat(); } - _characterController.addMotor(_scriptedMotorVelocity, motorRotation, _scriptedMotorTimescale); + if (qApp->isHMDMode()) { + // OUTOFBODY_HACK: only apply vertical component of _scriptedMotorVelocity to the characterController + _characterController.addMotor(glm::vec3(0, _scriptedMotorVelocity.y, 0), motorRotation, DEFAULT_MOTOR_TIMESCALE, INVALID_MOTOR_TIMESCALE); + } else { + _characterController.addMotor(_scriptedMotorVelocity, motorRotation, _scriptedMotorTimescale); + } } // legacy support for 'MyAvatar::applyThrust()', which has always been implemented as a @@ -1740,92 +1746,65 @@ void MyAvatar::updateOrientation(float deltaTime) { void MyAvatar::updateActionMotor(float deltaTime) { - if (qApp->isHMDMode()) { - // actions are constant velocity, for your comfort - // OUTOFBODY_HACK TODO: what about flying?!?! - - // compute action input - glm::vec3 front = (_driveKeys[TRANSLATE_Z]) * IDENTITY_FRONT; - glm::vec3 right = (_driveKeys[TRANSLATE_X]) * IDENTITY_RIGHT; - glm::vec3 direction = front + right; - - _isPushing = false; - _isBeingPushed = false; - - _wasPushing = _isPushing; - float directionLength = glm::length(direction); - _isPushing = directionLength > EPSILON; - - // normalize direction - if (_isPushing) { - direction /= directionLength; - } else { - direction = Vectors::ZERO; - } - - _actionMotorVelocity = MAX_WALKING_SPEED * direction; - + 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 motor to brake if a script is pushing the avatar around + // (we assume the avatar is driving itself via script) + _isBraking = false; } else { - 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 motor 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(_actionMotorVelocity); - const float MIN_ACTION_BRAKE_SPEED = 0.1f; - _isBraking = _wasPushing || (_isBraking && speed > MIN_ACTION_BRAKE_SPEED); - } + float speed = glm::length(_actionMotorVelocity); + const float MIN_ACTION_BRAKE_SPEED = 0.1f; + _isBraking = _wasPushing || (_isBraking && speed > MIN_ACTION_BRAKE_SPEED); + } - // compute action input - glm::vec3 front = (_driveKeys[TRANSLATE_Z]) * IDENTITY_FRONT; - glm::vec3 right = (_driveKeys[TRANSLATE_X]) * IDENTITY_RIGHT; + // compute action 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; + 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; + } - _wasPushing = _isPushing; - float directionLength = glm::length(direction); - _isPushing = directionLength > EPSILON; + _wasPushing = _isPushing; + float directionLength = glm::length(direction); + _isPushing = directionLength > EPSILON; + + // normalize direction + if (_isPushing) { + direction /= directionLength; + } else { + 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(glm::vec3(_actionMotorVelocity.x, _actionMotorVelocity.y, _actionMotorVelocity.z)); + float finalMaxMotorSpeed = getUniformScale() * MAX_ACTION_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; - // normalize direction if (_isPushing) { - direction /= directionLength; - } else { - 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(_actionMotorVelocity); - float finalMaxMotorSpeed = getUniformScale() * MAX_ACTION_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 action motor should never be slower than this - float boostCoefficient = (maxBoostSpeed - motorSpeed) / maxBoostSpeed; - motorSpeed += MIN_AVATAR_SPEED * boostCoefficient; - } else if (motorSpeed > finalMaxMotorSpeed) { - motorSpeed = finalMaxMotorSpeed; - } + if (motorSpeed < maxBoostSpeed) { + // an active action motor should never be slower than this + float boostCoefficient = (maxBoostSpeed - motorSpeed) / maxBoostSpeed; + motorSpeed += MIN_AVATAR_SPEED * boostCoefficient; + } else if (motorSpeed > finalMaxMotorSpeed) { + motorSpeed = finalMaxMotorSpeed; } - _actionMotorVelocity = motorSpeed * direction; - } else { - // we're interacting with a floor --> simple horizontal speed and exponential decay - _actionMotorVelocity = MAX_WALKING_SPEED * direction; } + _actionMotorVelocity = motorSpeed * direction; + } else { + // we're interacting with a floor --> simple horizontal speed and exponential decay + _actionMotorVelocity = MAX_WALKING_SPEED * direction; } float boomChange = _driveKeys[ZOOM]; @@ -1833,6 +1812,16 @@ void MyAvatar::updateActionMotor(float deltaTime) { _boomLength = glm::clamp(_boomLength, ZOOM_MIN, ZOOM_MAX); } +void MyAvatar::applyVelocityToSensorToWorldMatrix(const glm::vec3& velocity, float deltaTime) { + glm::vec3 horizontalVelocity(velocity.x, 0.0f, velocity.z); + float speed2 = glm::length2(horizontalVelocity); + if (speed2 > MIN_AVATAR_SPEED_SQUARED) { + glm::vec3 position = extractTranslation(_sensorToWorldMatrix) + deltaTime * horizontalVelocity; + // update the position column of matrix + _sensorToWorldMatrix[3] = glm::vec4(position, 1); + } +} + void MyAvatar::updatePosition(float deltaTime) { if (_motionBehaviors & AVATAR_MOTION_ACTION_MOTOR_ENABLED) { updateActionMotor(deltaTime); @@ -1856,18 +1845,27 @@ void MyAvatar::updatePosition(float deltaTime) { float speed2 = glm::length2(velocity); _moving = speed2 > MOVING_SPEED_THRESHOLD_SQUARED; - // OUTOFBODY_HACK, apply _actionMotorVelocity directly to the sensorToWorld matrix! - glm::quat motorRotation; - glm::quat liftRotation; - swingTwistDecomposition(glmExtractRotation(_sensorToWorldMatrix * getHMDSensorMatrix()), _worldUpDirection, liftRotation, motorRotation); - if (qApp->isHMDMode()) { - float speed2 = glm::length2(_actionMotorVelocity); - if (speed2 > MIN_AVATAR_SPEED_SQUARED) { - glm::vec3 worldVelocity = motorRotation * _actionMotorVelocity; - // update sensor to world position ourselves - glm::vec3 position = extractTranslation(_sensorToWorldMatrix) + deltaTime * worldVelocity; - _sensorToWorldMatrix[3] = glm::vec4(position, 1); + + // Apply _actionMotorVelocity directly to the sensorToWorld matrix. + glm::quat motorRotation; + glm::quat liftRotation; + swingTwistDecomposition(glmExtractRotation(_sensorToWorldMatrix * getHMDSensorMatrix()), _worldUpDirection, liftRotation, motorRotation); + glm::vec3 worldVelocity = motorRotation * _actionMotorVelocity; + applyVelocityToSensorToWorldMatrix(worldVelocity, deltaTime); + + // Apply _scriptedMotorVelocity to the sensorToWorld matrix. + 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(); + } + worldVelocity = motorRotation * _scriptedMotorVelocity; + applyVelocityToSensorToWorldMatrix(worldVelocity, deltaTime); } } } diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 6e00894cb3..2ce9973ce0 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -375,6 +375,7 @@ private: virtual void updatePalms() override {} void lateUpdatePalms(); + void applyVelocityToSensorToWorldMatrix(const glm::vec3& velocity, float deltaTime); float _driveKeys[MAX_DRIVE_KEYS]; bool _wasPushing; From 4c99848f292d97f364dbe0afb7c42ae3cd6d3a5b Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Tue, 20 Sep 2016 17:05:01 -0700 Subject: [PATCH 047/101] disable vertical re-centering behavior --- interface/src/avatar/MyAvatar.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 659223b893..99e84d5348 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -2236,6 +2236,9 @@ void MyAvatar::FollowHelper::updateHorizontalActivation(const MyAvatar& myAvatar } void MyAvatar::FollowHelper::updateVerticalActivation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) { + + // OUTOFBODY_HACK: disable vertical follow behavior + /* const float CYLINDER_TOP = 0.1f; const float CYLINDER_BOTTOM = -1.5f; const float MIN_VERTICAL_OFFSET = 0.02f; @@ -2248,6 +2251,7 @@ void MyAvatar::FollowHelper::updateVerticalActivation(const MyAvatar& myAvatar, } else if (offset.y > CYLINDER_TOP || offset.y < CYLINDER_BOTTOM) { activate(Vertical); } + */ } void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) { From 1f7dc25f12967a6a4a6d42caba89dfe55229a8fd Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Wed, 21 Sep 2016 11:48:54 -0700 Subject: [PATCH 048/101] MyAvatar.addThrust() works again. --- interface/src/avatar/MyAvatar.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 99e84d5348..dc0262e9f0 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -1336,8 +1336,7 @@ void MyAvatar::updateMotors() { // legacy support for 'MyAvatar::applyThrust()', which has always been implemented as a // short-lived linearAcceleration - _characterController.setLinearAcceleration(_thrust); - _thrust = Vectors::ZERO; + _characterController.setLinearAcceleration(glm::vec3(0.0f, _thrust.y, 0.0f)); } void MyAvatar::prepareForPhysicsSimulation() { @@ -1867,9 +1866,15 @@ void MyAvatar::updatePosition(float deltaTime) { worldVelocity = motorRotation * _scriptedMotorVelocity; applyVelocityToSensorToWorldMatrix(worldVelocity, deltaTime); } + + // OUTOFBODY_HACK: apply scaling factor to _thrust, to get the same behavior as an periodically applied motor. + const float THRUST_DAMPING_FACTOR = 0.25f; + applyVelocityToSensorToWorldMatrix(THRUST_DAMPING_FACTOR * _thrust, deltaTime); } } + _thrust = Vectors::ZERO; + // 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)) { _hoverReferenceCameraFacingIsCaptured = true; From 9fa8fc11c965469c209e6c81264b68fe9fe6845e Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Wed, 21 Sep 2016 18:20:35 -0700 Subject: [PATCH 049/101] More stability in hips offset calculation. --- .../animation/src/AnimInverseKinematics.cpp | 32 +++++++++++-------- .../animation/src/AnimInverseKinematics.h | 1 + libraries/physics/src/CharacterController.cpp | 3 ++ 3 files changed, 23 insertions(+), 13 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index b58813a214..a16506a2ff 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -392,6 +392,10 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars dt = MAX_OVERLAY_DT; } + // OUTOFBODY_HACK: smoothly update update _hipsOffsetLength, otherwise we risk introducing oscillation in the hips offset. + float tau = dt / 0.5f; + _maxHipsOffsetLength = (1.0f - tau) * _maxHipsOffsetLength + tau * _desiredMaxHipsOffsetLength; + if (_relativePoses.size() != underPoses.size()) { loadPoses(underPoses); } else { @@ -516,7 +520,7 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars float OTHER_WEIGHT = 1.0f; float totalWeight = 0.0f; - glm::vec3 newHipsOffset = Vectors::ZERO; + glm::vec3 additionalHipsOffset = Vectors::ZERO; for (auto& target: targets) { int targetIndex = target.getIndex(); if (targetIndex == _headIndex && _headIndex != -1) { @@ -527,43 +531,45 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars glm::vec3 under = _skeleton->getAbsolutePose(_headIndex, underPoses).trans; glm::vec3 actual = _skeleton->getAbsolutePose(_headIndex, _relativePoses).trans; const float HEAD_OFFSET_SLAVE_FACTOR = 0.65f; - newHipsOffset += (OTHER_WEIGHT * HEAD_OFFSET_SLAVE_FACTOR) * (actual - under); + additionalHipsOffset += (OTHER_WEIGHT * HEAD_OFFSET_SLAVE_FACTOR) * (actual - under); totalWeight += OTHER_WEIGHT; } else if (target.getType() == IKTarget::Type::HmdHead) { glm::vec3 actual = _skeleton->getAbsolutePose(_headIndex, _relativePoses).trans; glm::vec3 thisOffset = target.getTranslation() - actual; glm::vec3 futureHipsOffset = _hipsOffset + thisOffset; - if (glm::length(futureHipsOffset) < _maxHipsOffsetLength) { + if (glm::length(glm::vec2(futureHipsOffset.x, futureHipsOffset.z)) < _maxHipsOffsetLength) { // it is imperative to shift the hips and bring the head to its designated position // so we slam newHipsOffset here and ignore all other targets - newHipsOffset = futureHipsOffset; + additionalHipsOffset = futureHipsOffset - _hipsOffset; totalWeight = 0.0f; break; } else { - newHipsOffset += HMD_WEIGHT * (target.getTranslation() - actual); + additionalHipsOffset += HMD_WEIGHT * (target.getTranslation() - actual); totalWeight += HMD_WEIGHT; } } } else if (target.getType() == IKTarget::Type::RotationAndPosition) { glm::vec3 actualPosition = _skeleton->getAbsolutePose(targetIndex, _relativePoses).trans; glm::vec3 targetPosition = target.getTranslation(); - newHipsOffset += OTHER_WEIGHT * (targetPosition - actualPosition); + additionalHipsOffset += OTHER_WEIGHT * (targetPosition - actualPosition); totalWeight += OTHER_WEIGHT; } } if (totalWeight > 1.0f) { - newHipsOffset /= totalWeight; + additionalHipsOffset /= totalWeight; } // smooth transitions by relaxing _hipsOffset toward the new value const float HIPS_OFFSET_SLAVE_TIMESCALE = 0.10f; float tau = dt < HIPS_OFFSET_SLAVE_TIMESCALE ? dt / HIPS_OFFSET_SLAVE_TIMESCALE : 1.0f; - float newOffsetLength = glm::length(newHipsOffset); - if (newOffsetLength > _maxHipsOffsetLength) { - // clamp the hips offset - newHipsOffset *= _maxHipsOffsetLength / newOffsetLength; + _hipsOffset += additionalHipsOffset * tau; + + // clamp the horizontal component of the hips offset + float hipsOffsetLength2D = glm::length(glm::vec2(_hipsOffset.x, _hipsOffset.z)); + if (hipsOffsetLength2D > _maxHipsOffsetLength) { + _hipsOffset.x *= _maxHipsOffsetLength / hipsOffsetLength2D; + _hipsOffset.z *= _maxHipsOffsetLength / hipsOffsetLength2D; } - _hipsOffset += (newHipsOffset - _hipsOffset) * tau; } } } @@ -579,7 +585,7 @@ void AnimInverseKinematics::clearIKJointLimitHistory() { void AnimInverseKinematics::setMaxHipsOffsetLength(float maxLength) { // OUTOFBODY_HACK: manually adjust scale here const float METERS_TO_CENTIMETERS = 100.0f; - _maxHipsOffsetLength = METERS_TO_CENTIMETERS * maxLength; + _desiredMaxHipsOffsetLength = METERS_TO_CENTIMETERS * maxLength; } RotationConstraint* AnimInverseKinematics::getConstraint(int index) { diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h index b61343a97b..4c8d29c7ed 100644 --- a/libraries/animation/src/AnimInverseKinematics.h +++ b/libraries/animation/src/AnimInverseKinematics.h @@ -85,6 +85,7 @@ protected: // experimental data for moving hips during IK glm::vec3 _hipsOffset { Vectors::ZERO }; + float _desiredMaxHipsOffsetLength { 1.0f }; float _maxHipsOffsetLength { 1.0f }; int _headIndex { -1 }; int _hipsIndex { -1 }; diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index aa119689f7..514e8a4f8f 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -733,6 +733,9 @@ float CharacterController::measureMaxHipsOffsetRadius(const glm::vec3& currentHi startTransform.setOrigin(startPos); btVector3 endPos = startPos + rotation * ((maxSweepDistance / hipsOffsetLength) * hipsOffset); + // ensure sweep is horizontal. + startPos.setY(endPos.getY()); + // sweep test a sphere btSphereShape sphere(_radius); CharacterSweepResult result(&_ghost); From 06ff984f905413cfa380e8b3d8fbef83a750d3bb Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 21 Sep 2016 15:21:56 -0700 Subject: [PATCH 050/101] sky-hook for walking up steps --- .../src/avatar/MyCharacterController.cpp | 11 --- libraries/physics/src/CharacterController.cpp | 97 ++++++++++++------- libraries/physics/src/CharacterController.h | 14 ++- 3 files changed, 71 insertions(+), 51 deletions(-) diff --git a/interface/src/avatar/MyCharacterController.cpp b/interface/src/avatar/MyCharacterController.cpp index ef0c2d1cac..ad7879d2cc 100644 --- a/interface/src/avatar/MyCharacterController.cpp +++ b/interface/src/avatar/MyCharacterController.cpp @@ -34,17 +34,6 @@ void MyCharacterController::updateShapeIfNecessary() { if (_pendingFlags & PENDING_FLAG_UPDATE_SHAPE) { _pendingFlags &= ~PENDING_FLAG_UPDATE_SHAPE; - // compute new dimensions from avatar's bounding box - float x = _boxScale.x; - float z = _boxScale.z; - setCapsuleRadius(0.5f * sqrtf(0.5f * (x * x + z * z))); - _halfHeight = 0.5f * _boxScale.y - _radius; - float MIN_HALF_HEIGHT = 0.1f; - if (_halfHeight < MIN_HALF_HEIGHT) { - _halfHeight = MIN_HALF_HEIGHT; - } - // NOTE: _shapeLocalOffset is already computed - if (_radius > 0.0f) { // create RigidBody if it doesn't exist if (!_rigidBody) { diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 514e8a4f8f..b1047c8e1e 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -63,7 +63,6 @@ CharacterController::CharacterMotor::CharacterMotor(const glm::vec3& vel, const } CharacterController::CharacterController() { - _halfHeight = 1.0f; _floorDistance = MAX_FALL_HEIGHT; _targetVelocity.setValue(0.0f, 0.0f, 0.0f); @@ -146,30 +145,40 @@ void CharacterController::setDynamicsWorld(btDynamicsWorld* world) { } } -static const float COS_PI_OVER_THREE = cosf(PI / 3.0f); +bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) { + _stepHeight = _minStepHeight; // clears last step obstacle + btDispatcher* dispatcher = collisionWorld->getDispatcher(); + int numManifolds = dispatcher->getNumManifolds(); + bool hasFloor = false; + const float COS_PI_OVER_THREE = cosf(PI / 3.0f); + + btTransform transform = _rigidBody->getWorldTransform(); + transform.setOrigin(btVector3(0.0f, 0.0f, 0.0f)); -bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) const { - int numManifolds = collisionWorld->getDispatcher()->getNumManifolds(); for (int i = 0; i < numManifolds; i++) { - btPersistentManifold* contactManifold = collisionWorld->getDispatcher()->getManifoldByIndexInternal(i); - const btCollisionObject* obA = static_cast(contactManifold->getBody0()); - const btCollisionObject* obB = static_cast(contactManifold->getBody1()); - if (obA == _rigidBody || obB == _rigidBody) { + btPersistentManifold* contactManifold = dispatcher->getManifoldByIndexInternal(i); + if (_rigidBody == contactManifold->getBody1() || _rigidBody == contactManifold->getBody0()) { + bool characterIsFirst = _rigidBody == contactManifold->getBody0(); int numContacts = contactManifold->getNumContacts(); for (int j = 0; j < numContacts; j++) { - btManifoldPoint& pt = contactManifold->getContactPoint(j); - - // check to see if contact point is touching the bottom sphere of the capsule. - // and the contact normal is not slanted too much. - float contactPointY = (obA == _rigidBody) ? pt.m_localPointA.getY() : pt.m_localPointB.getY(); - btVector3 normal = (obA == _rigidBody) ? pt.m_normalWorldOnB : -pt.m_normalWorldOnB; - if (contactPointY < -_halfHeight && normal.dot(_currentUp) > COS_PI_OVER_THREE) { - return true; + // check for "floor" + btManifoldPoint& contact = contactManifold->getContactPoint(j); + btVector3 pointOnCharacter = characterIsFirst ? contact.m_localPointA : contact.m_localPointB; // object-local-frame + btVector3 normal = characterIsFirst ? contact.m_normalWorldOnB : -contact.m_normalWorldOnB; // points toward character + btScalar hitHeight = _halfHeight + _radius + pointOnCharacter.dot(_currentUp); + if (hitHeight < _radius && normal.dot(_currentUp) > COS_PI_OVER_THREE) { + hasFloor = true; + } + // remember highest step obstacle + if (hitHeight > _stepHeight && hitHeight < _maxStepHeight && normal.dot(_targetVelocity) < 0.0f ) { + _stepHeight = hitHeight; + _stepPoint = transform * pointOnCharacter; // rotate into world-frame + _stepNormal = normal; } } } } - return false; + return hasFloor; } void CharacterController::preStep(btCollisionWorld* collisionWorld) { @@ -332,23 +341,22 @@ void CharacterController::setState(State desiredState) { } void CharacterController::setLocalBoundingBox(const glm::vec3& minCorner, const glm::vec3& scale) { - _boxScale = scale; - - float x = _boxScale.x; - float z = _boxScale.z; + float x = scale.x; + float z = scale.z; float radius = 0.5f * sqrtf(0.5f * (x * x + z * z)); - float halfHeight = 0.5f * _boxScale.y - radius; + float halfHeight = 0.5f * scale.y - radius; float MIN_HALF_HEIGHT = 0.1f; if (halfHeight < MIN_HALF_HEIGHT) { halfHeight = MIN_HALF_HEIGHT; } // compare dimensions - float radiusDelta = glm::abs(radius - _radius); - float heightDelta = glm::abs(halfHeight - _halfHeight); - if (radiusDelta < FLT_EPSILON && heightDelta < FLT_EPSILON) { - // shape hasn't changed --> nothing to do - } else { + if (glm::abs(radius - _radius) > FLT_EPSILON || glm::abs(halfHeight - _halfHeight) > FLT_EPSILON) { + _radius = radius; + _halfHeight = halfHeight; + _minStepHeight = 0.02f; // HACK: hardcoded now but should be shape margin + _maxStepHeight = 0.75f * (_halfHeight + _radius); + if (_dynamicsWorld) { // must REMOVE from world prior to shape update _pendingFlags |= PENDING_FLAG_REMOVE_FROM_SIMULATION; @@ -358,7 +366,7 @@ void CharacterController::setLocalBoundingBox(const glm::vec3& minCorner, const } // it's ok to change offset immediately -- there are no thread safety issues here - _shapeLocalOffset = minCorner + 0.5f * _boxScale; + _shapeLocalOffset = minCorner + 0.5f * scale; } void CharacterController::setCollisionGroup(int16_t group) { @@ -435,10 +443,6 @@ glm::vec3 CharacterController::getLinearVelocity() const { return velocity; } -void CharacterController::setCapsuleRadius(float radius) { - _radius = radius; -} - glm::vec3 CharacterController::getVelocityChange() const { if (_rigidBody) { return bulletToGLM(_velocityChange); @@ -487,11 +491,32 @@ void CharacterController::applyMotor(int index, btScalar dt, btVector3& worldVel // compute local UP btVector3 up = _currentUp.rotate(axis, -angle); + // add sky hook when encountering a step obstacle + btVector3 motorVelocity = motor.velocity; + btScalar vTimescale = motor.vTimescale; + if (_stepHeight > _minStepHeight) { + // there is a step + btVector3 motorVelocityWF = motorVelocity.rotate(axis, angle); + if (motorVelocityWF.dot(_stepNormal) < 0.0f) { + // the motor pushes against step + btVector3 leverArm = _stepPoint; + motorVelocityWF = _stepNormal.cross(leverArm.cross(motorVelocityWF)); + btScalar distortedLength = motorVelocityWF.length(); + if (distortedLength > FLT_EPSILON) { + // scale the motor in the correct direction and rotate back to motor-frame + motorVelocityWF *= (motorVelocity.length() / distortedLength); + motorVelocity += motorVelocityWF.rotate(axis, -angle); + // make vTimescale as small as possible + vTimescale = glm::min(vTimescale, motor.hTimescale); + } + } + } + // 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; + btVector3 vTargetVelocity = motorVelocity.dot(up) * up; + btVector3 hTargetVelocity = motorVelocity - vTargetVelocity; // modify each component separately btScalar maxTau = 0.0f; @@ -503,8 +528,8 @@ void CharacterController::applyMotor(int index, btScalar dt, btVector3& worldVel maxTau = tau; hVelocity += (hTargetVelocity - hVelocity) * tau; } - if (motor.vTimescale < MAX_CHARACTER_MOTOR_TIMESCALE) { - btScalar tau = dt / motor.vTimescale; + if (vTimescale < MAX_CHARACTER_MOTOR_TIMESCALE) { + btScalar tau = dt / vTimescale; if (tau > 1.0f) { tau = 1.0f; } diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h index 7d739bfae3..9591579d63 100644 --- a/libraries/physics/src/CharacterController.h +++ b/libraries/physics/src/CharacterController.h @@ -92,7 +92,6 @@ public: glm::vec3 getLinearVelocity() const; glm::vec3 getVelocityChange() const; - virtual void setCapsuleRadius(float radius); float getCapsuleRadius() const { return _radius; } float getCapsuleHalfHeight() const { return _halfHeight; } glm::vec3 getCapsuleLocalOffset() const { return _shapeLocalOffset; } @@ -130,7 +129,7 @@ protected: #endif void updateUpAxis(const glm::quat& rotation); - bool checkForSupport(btCollisionWorld* collisionWorld) const; + bool checkForSupport(btCollisionWorld* collisionWorld); protected: struct CharacterMotor { @@ -163,8 +162,15 @@ protected: quint32 _jumpButtonDownCount; quint32 _takeoffJumpButtonID; - btScalar _halfHeight; - btScalar _radius; + // data for walking up steps + btVector3 _stepPoint; + btVector3 _stepNormal; + btScalar _stepHeight { 0.0f }; + btScalar _minStepHeight { 0.0f }; + btScalar _maxStepHeight { 0.0f }; + + btScalar _halfHeight { 0.0f }; + btScalar _radius { 0.0f }; btScalar _floorDistance; bool _hasSupport; From 819e1dc694deb3f7d9be2689343a49ac84e776fe Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 22 Sep 2016 16:12:35 -0700 Subject: [PATCH 051/101] prevent avatars from walking up vertical walls --- libraries/physics/src/CharacterController.cpp | 29 +++++++++++++------ 1 file changed, 20 insertions(+), 9 deletions(-) diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index b1047c8e1e..e11efa8aeb 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -152,14 +152,16 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) { bool hasFloor = false; const float COS_PI_OVER_THREE = cosf(PI / 3.0f); - btTransform transform = _rigidBody->getWorldTransform(); - transform.setOrigin(btVector3(0.0f, 0.0f, 0.0f)); + btTransform rotation = _rigidBody->getWorldTransform(); + rotation.setOrigin(btVector3(0.0f, 0.0f, 0.0f)); // clear translation part for (int i = 0; i < numManifolds; i++) { btPersistentManifold* contactManifold = dispatcher->getManifoldByIndexInternal(i); if (_rigidBody == contactManifold->getBody1() || _rigidBody == contactManifold->getBody0()) { bool characterIsFirst = _rigidBody == contactManifold->getBody0(); int numContacts = contactManifold->getNumContacts(); + int stepContactIndex = -1; + float highestStep = _minStepHeight; for (int j = 0; j < numContacts; j++) { // check for "floor" btManifoldPoint& contact = contactManifold->getContactPoint(j); @@ -170,12 +172,24 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) { hasFloor = true; } // remember highest step obstacle - if (hitHeight > _stepHeight && hitHeight < _maxStepHeight && normal.dot(_targetVelocity) < 0.0f ) { - _stepHeight = hitHeight; - _stepPoint = transform * pointOnCharacter; // rotate into world-frame - _stepNormal = normal; + if (hitHeight > _maxStepHeight) { + // this manifold is invalidated by point that is too high + stepContactIndex = -1; + break; + } else if (hitHeight > highestStep && normal.dot(_targetVelocity) < 0.0f ) { + highestStep = hitHeight; + stepContactIndex = j; } } + if (stepContactIndex > -1 && highestStep > _stepHeight) { + // remember step info for later + btManifoldPoint& contact = contactManifold->getContactPoint(stepContactIndex); + btVector3 pointOnCharacter = characterIsFirst ? contact.m_localPointA : contact.m_localPointB; // object-local-frame + btVector3 normal = characterIsFirst ? contact.m_normalWorldOnB : -contact.m_normalWorldOnB; // points toward character + _stepHeight = highestStep; + _stepPoint = rotation * pointOnCharacter; // rotate into world-frame + _stepNormal = normal; + } } } return hasFloor; @@ -758,9 +772,6 @@ float CharacterController::measureMaxHipsOffsetRadius(const glm::vec3& currentHi startTransform.setOrigin(startPos); btVector3 endPos = startPos + rotation * ((maxSweepDistance / hipsOffsetLength) * hipsOffset); - // ensure sweep is horizontal. - startPos.setY(endPos.getY()); - // sweep test a sphere btSphereShape sphere(_radius); CharacterSweepResult result(&_ghost); From 61be99b87ed7b5f163297de36a5c0cc3d003e74f Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 22 Sep 2016 16:14:19 -0700 Subject: [PATCH 052/101] fix IK bug for RotationOnly head target --- libraries/animation/src/AnimInverseKinematics.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index a16506a2ff..72483d21b1 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -531,7 +531,7 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars glm::vec3 under = _skeleton->getAbsolutePose(_headIndex, underPoses).trans; glm::vec3 actual = _skeleton->getAbsolutePose(_headIndex, _relativePoses).trans; const float HEAD_OFFSET_SLAVE_FACTOR = 0.65f; - additionalHipsOffset += (OTHER_WEIGHT * HEAD_OFFSET_SLAVE_FACTOR) * (actual - under); + additionalHipsOffset += (OTHER_WEIGHT * HEAD_OFFSET_SLAVE_FACTOR) * (under- actual); totalWeight += OTHER_WEIGHT; } else if (target.getType() == IKTarget::Type::HmdHead) { glm::vec3 actual = _skeleton->getAbsolutePose(_headIndex, _relativePoses).trans; From b68dbab994aa4fdc5bd592eba3f9dc1d65a893aa Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Fri, 23 Sep 2016 11:44:36 -0700 Subject: [PATCH 053/101] Fix for incorrect hand offset when backing into collision. We know properly account for the offset of the head due to clamping from a small maxHipsOffset. This means the hands should look more natural when you are out-of-body and are moving your hand controllers. --- interface/src/avatar/MyAvatar.cpp | 2 +- interface/src/avatar/SkeletonModel.cpp | 11 +++++++++++ .../animation/src/AnimInverseKinematics.cpp | 6 +----- libraries/animation/src/AnimInverseKinematics.h | 1 - libraries/animation/src/Rig.cpp | 16 ++++++++++++++-- libraries/animation/src/Rig.h | 6 +++++- 6 files changed, 32 insertions(+), 10 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index dc0262e9f0..33523fc907 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -1378,7 +1378,7 @@ void MyAvatar::harvestResultsFromPhysicsSimulation(float deltaTime) { hipsPosition.z *= -1.0f; maxHipsOffsetRadius = _characterController.measureMaxHipsOffsetRadius(hipsPosition, maxHipsOffsetRadius); } - _rig->setMaxHipsOffsetLength(maxHipsOffsetRadius); + _rig->updateMaxHipsOffsetLength(maxHipsOffsetRadius, deltaTime); glm::vec3 position = getPosition(); glm::quat orientation = getOrientation(); diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index 36f74b8784..b2bf5f64fc 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -149,6 +149,17 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { _rig->updateFromHeadParameters(headParams, deltaTime); + // OUTOFBODY_HACK: clamp horizontal component of head by maxHipsOffset. + // This is to prevent the hands from being incorrect relative to the head because + // the hips are being constrained by a small maxHipsOffset due to collision. + if (myAvatar->isOutOfBody()) { + float headOffsetLength2D = glm::length(glm::vec2(truncatedHMDPositionInRigSpace.x, truncatedHMDPositionInRigSpace.z)); + if (headOffsetLength2D > _rig->getMaxHipsOffsetLength()) { + truncatedHMDPositionInRigSpace.x *= _rig->getMaxHipsOffsetLength() / headOffsetLength2D; + truncatedHMDPositionInRigSpace.z *= _rig->getMaxHipsOffsetLength() / headOffsetLength2D; + } + } + Rig::HandParameters handParams; auto leftPose = myAvatar->getLeftHandControllerPoseInAvatarFrame(); diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 72483d21b1..4dca0aa6a8 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -392,10 +392,6 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars dt = MAX_OVERLAY_DT; } - // OUTOFBODY_HACK: smoothly update update _hipsOffsetLength, otherwise we risk introducing oscillation in the hips offset. - float tau = dt / 0.5f; - _maxHipsOffsetLength = (1.0f - tau) * _maxHipsOffsetLength + tau * _desiredMaxHipsOffsetLength; - if (_relativePoses.size() != underPoses.size()) { loadPoses(underPoses); } else { @@ -585,7 +581,7 @@ void AnimInverseKinematics::clearIKJointLimitHistory() { void AnimInverseKinematics::setMaxHipsOffsetLength(float maxLength) { // OUTOFBODY_HACK: manually adjust scale here const float METERS_TO_CENTIMETERS = 100.0f; - _desiredMaxHipsOffsetLength = METERS_TO_CENTIMETERS * maxLength; + _maxHipsOffsetLength = METERS_TO_CENTIMETERS * maxLength; } RotationConstraint* AnimInverseKinematics::getConstraint(int index) { diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h index 4c8d29c7ed..b61343a97b 100644 --- a/libraries/animation/src/AnimInverseKinematics.h +++ b/libraries/animation/src/AnimInverseKinematics.h @@ -85,7 +85,6 @@ protected: // experimental data for moving hips during IK glm::vec3 _hipsOffset { Vectors::ZERO }; - float _desiredMaxHipsOffsetLength { 1.0f }; float _maxHipsOffsetLength { 1.0f }; int _headIndex { -1 }; int _hipsIndex { -1 }; diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 9f77ce4772..05e589918a 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -307,18 +307,30 @@ void Rig::clearIKJointLimitHistory() { } } -void Rig::setMaxHipsOffsetLength(float maxLength) { +void Rig::updateMaxHipsOffsetLength(float maxLength, float deltaTime) { + + _desiredMaxHipsOffsetLength = maxLength; + + // OUTOFBODY_HACK: smoothly update update _hipsOffsetLength, otherwise we risk introducing oscillation in the hips offset. + const float MAX_HIPS_OFFSET_TIMESCALE = 0.33f; + float tau = deltaTime / MAX_HIPS_OFFSET_TIMESCALE; + _maxHipsOffsetLength = (1.0f - tau) * _maxHipsOffsetLength + tau * _desiredMaxHipsOffsetLength; + if (_animNode) { _animNode->traverse([&](AnimNode::Pointer node) { auto ikNode = std::dynamic_pointer_cast(node); if (ikNode) { - ikNode->setMaxHipsOffsetLength(maxLength); + ikNode->setMaxHipsOffsetLength(_maxHipsOffsetLength); } return true; }); } } +float Rig::getMaxHipsOffsetLength() const { + return _maxHipsOffsetLength; +} + void Rig::setJointTranslation(int index, bool valid, const glm::vec3& translation, float priority) { if (isIndexValid(index)) { if (valid) { diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index 4491397af0..aa1a2ad969 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -104,7 +104,8 @@ public: void clearJointAnimationPriority(int index); void clearIKJointLimitHistory(); - void setMaxHipsOffsetLength(float maxLength); + void updateMaxHipsOffsetLength(float maxLength, float deltaTime); + float getMaxHipsOffsetLength() const; // geometry space void setJointState(int index, bool valid, const glm::quat& rotation, const glm::vec3& translation, float priority); @@ -319,6 +320,9 @@ protected: bool _truncateIKTargets { false }; bool _enableDebugDrawIKTargets { false }; + float _maxHipsOffsetLength { 1.0f }; + float _desiredMaxHipsOffsetLength { 1.0f }; + private: QMap _stateHandlers; int _nextStateHandlerId { 0 }; From 9567ec11aff3c01f4c2e8f24be96248214e203c3 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Fri, 23 Sep 2016 15:22:52 -0700 Subject: [PATCH 054/101] Prevent character from walking when pinned against collision. This is fixed by using the pre-action velocity from CharacterController, which does not include any motors or follow velocity. This pre-action velocity reflects the actual rigid body velocity after collision constraints are resolved. This should prevent the character f --- interface/src/avatar/MyAvatar.cpp | 4 ++++ interface/src/avatar/MyAvatar.h | 2 ++ interface/src/avatar/SkeletonModel.cpp | 2 +- libraries/physics/src/CharacterController.cpp | 11 +++++++++++ libraries/physics/src/CharacterController.h | 7 +++---- 5 files changed, 21 insertions(+), 5 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 33523fc907..d07835e44a 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -1927,6 +1927,10 @@ bool findAvatarAvatarPenetration(const glm::vec3 positionA, float radiusA, float return false; } +glm::vec3 MyAvatar::getPreActionVelocity() const { + return _characterController.getPreActionLinearVelocity(); +} + void MyAvatar::increaseSize() { if ((1.0f + SCALING_RATIO) * _targetScale < MAX_AVATAR_SCALE) { _targetScale *= (1.0f + SCALING_RATIO); diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 2ce9973ce0..d265acee93 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -284,6 +284,8 @@ public: virtual glm::quat getAbsoluteJointRotationInObjectFrame(int index) const override; virtual glm::vec3 getAbsoluteJointTranslationInObjectFrame(int index) const override; + glm::vec3 getPreActionVelocity() const; + public slots: void increaseSize(); void decreaseSize(); diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index b2bf5f64fc..47eb18ce74 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -200,7 +200,7 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { Rig::CharacterControllerState ccState = convertCharacterControllerState(myAvatar->getCharacterController()->getState()); - auto velocity = myAvatar->getLocalVelocity(); + auto velocity = myAvatar->getPreActionVelocity(); auto position = myAvatar->getLocalPosition(); auto orientation = myAvatar->getLocalOrientation(); _rig->computeMotionAnimationState(deltaTime, position, velocity, orientation, ccState); diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index e11efa8aeb..4c62a4072f 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -195,6 +195,13 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) { return hasFloor; } +void CharacterController::updateAction(btCollisionWorld* collisionWorld, btScalar deltaTime) +{ + _preActionVelocity = getLinearVelocity(); + preStep(collisionWorld); + playerStep(collisionWorld, deltaTime); +} + void CharacterController::preStep(btCollisionWorld* collisionWorld) { // trace a ray straight down to see if we're standing on the ground const btTransform& transform = _rigidBody->getWorldTransform(); @@ -457,6 +464,10 @@ glm::vec3 CharacterController::getLinearVelocity() const { return velocity; } +glm::vec3 CharacterController::getPreActionLinearVelocity() const { + return _preActionVelocity; +} + glm::vec3 CharacterController::getVelocityChange() const { if (_rigidBody) { return bulletToGLM(_velocityChange); diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h index 9591579d63..e111c4cefe 100644 --- a/libraries/physics/src/CharacterController.h +++ b/libraries/physics/src/CharacterController.h @@ -60,10 +60,7 @@ public: virtual void warp(const btVector3& origin) override { } virtual void debugDraw(btIDebugDraw* debugDrawer) override { } virtual void setUpInterpolate(bool value) override { } - virtual void updateAction(btCollisionWorld* collisionWorld, btScalar deltaTime) override { - preStep(collisionWorld); - playerStep(collisionWorld, deltaTime); - } + virtual void updateAction(btCollisionWorld* collisionWorld, btScalar deltaTime) override; virtual void preStep(btCollisionWorld *collisionWorld) override; virtual void playerStep(btCollisionWorld *collisionWorld, btScalar dt) override; virtual bool canJump() const override { assert(false); return false; } // never call this @@ -90,6 +87,7 @@ public: void disableFollow() { _following = false; } glm::vec3 getLinearVelocity() const; + glm::vec3 getPreActionLinearVelocity() const; glm::vec3 getVelocityChange() const; float getCapsuleRadius() const { return _radius; } @@ -147,6 +145,7 @@ protected: btVector3 _targetVelocity; btVector3 _parentVelocity; btVector3 _preSimulationVelocity; + glm::vec3 _preActionVelocity; btVector3 _velocityChange; btTransform _followDesiredBodyTransform; btVector3 _position; From f4c4b3474bfecde8e0ad5a401bcbe6fd2f7fdcf5 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Fri, 23 Sep 2016 16:55:38 -0700 Subject: [PATCH 055/101] capsule projection and distance tests now work for non-vertical capsules --- interface/src/avatar/MyAvatar.cpp | 13 +++---- interface/src/avatar/SkeletonModel.cpp | 6 ++-- libraries/shared/src/GeometryUtil.cpp | 50 ++++++++++++++------------ libraries/shared/src/GeometryUtil.h | 7 ++-- 4 files changed, 39 insertions(+), 37 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index d07835e44a..1a184a6d12 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -2300,16 +2300,11 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat } void MyAvatar::FollowHelper::postPhysicsUpdate(MyAvatar& myAvatar) { - - // get HMD position from sensor space into world space, and back into rig space glm::mat4 worldHMDMat = myAvatar.getSensorToWorldMatrix() * myAvatar.getHMDSensorMatrix(); - glm::mat4 rigToWorld = createMatFromQuatAndPos(myAvatar.getRotation() * Quaternions::Y_180, myAvatar.getPosition()); - glm::mat4 worldToRig = glm::inverse(rigToWorld); - glm::mat4 rigHMDMat = worldToRig * worldHMDMat; - glm::vec3 rigHMDPosition = extractTranslation(rigHMDMat); - - // detect if the rig head position is too far from the avatar's position. - _isOutOfBody = !pointIsInsideCapsule(rigHMDPosition, TRUNCATE_IK_CAPSULE_POSITION, TRUNCATE_IK_CAPSULE_LENGTH, TRUNCATE_IK_CAPSULE_RADIUS); + glm::vec3 worldHMDPosition = extractTranslation(worldHMDMat); + glm::vec3 capsuleStart = myAvatar.getPosition() + Vectors::UNIT_Y * (TRUNCATE_IK_CAPSULE_LENGTH / 2.0f); + glm::vec3 capsuleEnd = myAvatar.getPosition() - Vectors::UNIT_Y * (TRUNCATE_IK_CAPSULE_LENGTH / 2.0f); + _isOutOfBody = !pointIsInsideCapsule(worldHMDPosition, capsuleStart, capsuleEnd, TRUNCATE_IK_CAPSULE_RADIUS); } float MyAvatar::getAccelerationEnergy() { diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index 47eb18ce74..5aab7813eb 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -124,10 +124,12 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { hmdPositionInRigSpace = extractTranslation(rigHMDMat); + glm::vec3 capsuleStart = Vectors::UNIT_Y * (TRUNCATE_IK_CAPSULE_LENGTH / 2.0f); + glm::vec3 capsuleEnd = -Vectors::UNIT_Y * (TRUNCATE_IK_CAPSULE_LENGTH / 2.0f); + // truncate head IK target if it's out of body if (myAvatar->isOutOfBody()) { - truncatedHMDPositionInRigSpace = projectPointOntoCapsule(hmdPositionInRigSpace, TRUNCATE_IK_CAPSULE_POSITION, - TRUNCATE_IK_CAPSULE_LENGTH, TRUNCATE_IK_CAPSULE_RADIUS); + truncatedHMDPositionInRigSpace = projectPointOntoCapsule(hmdPositionInRigSpace, capsuleStart, capsuleEnd, TRUNCATE_IK_CAPSULE_RADIUS); } else { truncatedHMDPositionInRigSpace = hmdPositionInRigSpace; } diff --git a/libraries/shared/src/GeometryUtil.cpp b/libraries/shared/src/GeometryUtil.cpp index bc4dc7cb9a..9cf668f733 100644 --- a/libraries/shared/src/GeometryUtil.cpp +++ b/libraries/shared/src/GeometryUtil.cpp @@ -579,33 +579,37 @@ float coneSphereAngle(const glm::vec3& coneCenter, const glm::vec3& coneDirectio return glm::max(0.0f, theta - phi); } -bool pointIsInsideCapsule(const glm::vec3& point, const glm::vec3& capsulePosition, float capsuleLength, float capsuleRadius) { - glm::vec3 top = capsulePosition.y + glm::vec3(0.0f, capsuleLength / 2.0f, 0.0f); - glm::vec3 bottom = capsulePosition.y - glm::vec3(0.0f, capsuleLength / 2.0f, 0.0f); - if (point.y > top.y + capsuleRadius) { - return false; - } else if (point.y > top.y) { - return glm::length(point - top) < capsuleRadius; - } else if (point.y < bottom.y - capsuleRadius) { - return false; - } else if (point.y < bottom.y) { - return glm::length(point - bottom) < capsuleRadius; +glm::vec3 nearestPointOnLineSegment(const glm::vec3& point, const glm::vec3& segmentStart, const glm::vec3& segmentEnd) { + glm::vec3 n = segmentEnd - segmentStart; + float len = glm::length(n); + if (len < EPSILON) { + return segmentStart; } else { - return glm::length(glm::vec2(point.x, point.z) - glm::vec2(capsulePosition.x, capsulePosition.z)) < capsuleRadius; + n /= len; + float projection = glm::dot(point - segmentStart, n); + projection = glm::clamp(projection, 0.0f, len); + return segmentStart + n * projection; } } -glm::vec3 projectPointOntoCapsule(const glm::vec3& point, const glm::vec3& capsulePosition, float capsuleLength, float capsuleRadius) { - glm::vec3 top = capsulePosition.y + glm::vec3(0.0f, capsuleLength / 2.0f, 0.0f); - glm::vec3 bottom = capsulePosition.y - glm::vec3(0.0f, capsuleLength / 2.0f, 0.0f); - if (point.y > top.y) { - return capsuleRadius * glm::normalize(point - top) + top; - } else if (point.y < bottom.y) { - return capsuleRadius * glm::normalize(point - bottom) + bottom; +float distanceFromLineSegment(const glm::vec3& point, const glm::vec3& segmentStart, const glm::vec3& segmentEnd) { + return glm::length(point - nearestPointOnLineSegment(point, segmentStart, segmentEnd)); +} + +bool pointIsInsideCapsule(const glm::vec3& point, const glm::vec3& capsuleStart, const glm::vec3& capsuleEnd, float capsuleRadius) { + return distanceFromLineSegment(point, capsuleStart, capsuleEnd) < capsuleRadius; +} + +glm::vec3 projectPointOntoCapsule(const glm::vec3& point, const glm::vec3& capsuleStart, const glm::vec3& capsuleEnd, float capsuleRadius) { + glm::vec3 nearestPoint = nearestPointOnLineSegment(point, capsuleStart, capsuleEnd); + glm::vec3 d = point - nearestPoint; + float dLen = glm::length(d); + if (dLen > EPSILON) { + return nearestPoint; // TODO: maybe we should pick a point actually on the surface... } else { - glm::vec2 capsulePosition2D(capsulePosition.x, capsulePosition.z); - glm::vec2 point2D(point.x, point.z); - glm::vec2 projectedPoint2D = capsuleRadius * glm::normalize(point2D - capsulePosition2D) + capsulePosition2D; - return glm::vec3(projectedPoint2D.x, point.y, projectedPoint2D.y); + return nearestPoint + d * (capsuleRadius / dLen); } } + + + diff --git a/libraries/shared/src/GeometryUtil.h b/libraries/shared/src/GeometryUtil.h index 01459e51a9..05ca415cac 100644 --- a/libraries/shared/src/GeometryUtil.h +++ b/libraries/shared/src/GeometryUtil.h @@ -160,8 +160,9 @@ private: static void copyCleanArray(int& lengthA, glm::vec2* vertexArrayA, int& lengthB, glm::vec2* vertexArrayB); }; -// vertical capsule -bool pointIsInsideCapsule(const glm::vec3& point, const glm::vec3& capsulePosition, float capsuleLength, float capsuleRadius); -glm::vec3 projectPointOntoCapsule(const glm::vec3& point, const glm::vec3& capsulePosition, float capsuleLength, float capsuleRadius); +glm::vec3 nearestPointOnLineSegment(const glm::vec3& point, const glm::vec3& segmentStart, const glm::vec3& segmentEnd); +float distanceFromLineSegment(const glm::vec3& point, const glm::vec3& segmentStart, const glm::vec3& segmentEnd); +bool pointIsInsideCapsule(const glm::vec3& point, const glm::vec3& capsuleStart, const glm::vec3& capsuleEnd, float capsuleRadius); +glm::vec3 projectPointOntoCapsule(const glm::vec3& point, const glm::vec3& capsuleStart, const glm::vec3& capsuleEnd, float capsuleRadius); #endif // hifi_GeometryUtil_h From 27e6c0f872e2665ec277ae80d8645e75b52ff3ff Mon Sep 17 00:00:00 2001 From: Anthony Thibault Date: Sat, 24 Sep 2016 12:55:05 -0700 Subject: [PATCH 056/101] Added debug draw path trace for left and right feet --- interface/src/Menu.cpp | 4 ++++ interface/src/Menu.h | 2 ++ interface/src/avatar/MyAvatar.cpp | 27 +++++++++++++++++++++++++++ interface/src/avatar/MyAvatar.h | 15 +++++++++++++++ 4 files changed, 48 insertions(+) diff --git a/interface/src/Menu.cpp b/interface/src/Menu.cpp index 378838f0e0..7fb8096c35 100644 --- a/interface/src/Menu.cpp +++ b/interface/src/Menu.cpp @@ -485,6 +485,10 @@ Menu::Menu() { addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::RenderMyLookAtVectors, 0, false); addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::RenderOtherLookAtVectors, 0, false); addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::FixGaze, 0, false); + addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::DisplayLeftFootTrace, 0, false, + avatar, SLOT(setEnableDebugDrawLeftFootTrace(bool))); + addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::DisplayRightFootTrace, 0, false, + avatar, SLOT(setEnableDebugDrawRightFootTrace(bool))); addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::AnimDebugDrawDefaultPose, 0, false, avatar, SLOT(setEnableDebugDrawDefaultPose(bool))); addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::AnimDebugDrawAnimPose, 0, false, diff --git a/interface/src/Menu.h b/interface/src/Menu.h index 003b8b8121..a24966973f 100644 --- a/interface/src/Menu.h +++ b/interface/src/Menu.h @@ -88,6 +88,8 @@ namespace MenuOption { const QString DiskCacheEditor = "Disk Cache Editor"; const QString DisplayCrashOptions = "Display Crash Options"; const QString DisplayHandTargets = "Show Hand Targets"; + const QString DisplayLeftFootTrace = "Show Left Foot Trace"; + const QString DisplayRightFootTrace = "Show Right Foot Trace"; const QString DisplayModelBounds = "Display Model Bounds"; const QString DisplayModelTriangles = "Display Model Triangles"; const QString DisplayModelElementChildProxies = "Display Model Element Children"; diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 1a184a6d12..9f079288f4 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -817,6 +817,14 @@ float loadSetting(Settings& settings, const QString& name, float defaultValue) { return value; } +void MyAvatar::setEnableDebugDrawLeftFootTrace(bool isEnabled) { + _enableDebugDrawLeftFootTrace = isEnabled; +} + +void MyAvatar::setEnableDebugDrawRightFootTrace(bool isEnabled) { + _enableDebugDrawRightFootTrace = isEnabled; +} + void MyAvatar::setEnableDebugDrawDefaultPose(bool isEnabled) { _enableDebugDrawDefaultPose = isEnabled; @@ -1619,6 +1627,25 @@ void MyAvatar::postUpdate(float deltaTime) { DebugDraw::getInstance().updateMyAvatarPos(getPosition()); DebugDraw::getInstance().updateMyAvatarRot(getOrientation()); + + if (_enableDebugDrawLeftFootTrace || _enableDebugDrawRightFootTrace) { + int boneIndex = _enableDebugDrawLeftFootTrace ? getJointIndex("LeftFoot") : getJointIndex("RightFoot"); + const glm::vec4 RED(1.0f, 0.0f, 0.0f, 1.0f); + const glm::vec4 WHITE(1.0f, 1.0f, 1.0f, 1.0f); + const glm::vec4 TRANS(1.0f, 1.0f, 1.0f, 0.0f); + static bool colorBit = true; + colorBit = !colorBit; + glm::vec4 color = colorBit ? RED : WHITE; + + _debugLineLoop[_debugLineLoopIndex] = DebugDrawVertex(getJointPosition(boneIndex), color); + _debugLineLoopIndex = (_debugLineLoopIndex + 1) % DEBUG_LINE_LOOP_SIZE; + _debugLineLoop[_debugLineLoopIndex] = DebugDrawVertex(getJointPosition(boneIndex), TRANS); + for (size_t prev = DEBUG_LINE_LOOP_SIZE - 1, next = 0; next < DEBUG_LINE_LOOP_SIZE; prev = next, next++) { + if (_debugLineLoop[prev].color.w > 0.0f) { + DebugDraw::getInstance().drawRay(_debugLineLoop[prev].pos, _debugLineLoop[next].pos, _debugLineLoop[prev].color); + } + } + } } diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index d265acee93..b6b6f1373b 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -55,6 +55,8 @@ enum AudioListenerMode { }; Q_DECLARE_METATYPE(AudioListenerMode); +const size_t DEBUG_LINE_LOOP_SIZE = 1000; + class MyAvatar : public Avatar { Q_OBJECT Q_PROPERTY(bool shouldRenderLocally READ getShouldRenderLocally WRITE setShouldRenderLocally) @@ -303,6 +305,8 @@ public slots: Q_INVOKABLE void updateMotionBehaviorFromMenu(); + void setEnableDebugDrawLeftFootTrace(bool isEnabled); + void setEnableDebugDrawRightFootTrace(bool isEnabled); void setEnableDebugDrawDefaultPose(bool isEnabled); void setEnableDebugDrawAnimPose(bool isEnabled); void setEnableDebugDrawPosition(bool isEnabled); @@ -481,6 +485,8 @@ private: RigPointer _rig; bool _prevShouldDrawHead; + bool _enableDebugDrawLeftFootTrace { false }; + bool _enableDebugDrawRightFootTrace { false }; bool _enableDebugDrawDefaultPose { false }; bool _enableDebugDrawAnimPose { false }; bool _enableDebugDrawHandControllers { false }; @@ -517,6 +523,15 @@ private: float getEnergy(); void setEnergy(float value); bool didTeleport(); + + struct DebugDrawVertex { + DebugDrawVertex() : pos(), color() {} + DebugDrawVertex(const glm::vec3& posIn, const glm::vec4& colorIn) : pos(posIn), color(colorIn) {} + glm::vec3 pos; + glm::vec4 color; + }; + DebugDrawVertex _debugLineLoop[DEBUG_LINE_LOOP_SIZE]; + size_t _debugLineLoopIndex { 0 }; }; QScriptValue audioListenModeToScriptValue(QScriptEngine* engine, const AudioListenerMode& audioListenerMode); From 10c5da572812572dba9e4e5d77ba04488206648e Mon Sep 17 00:00:00 2001 From: Anthony Thibault Date: Sat, 24 Sep 2016 14:28:56 -0700 Subject: [PATCH 057/101] Smooth transition of hands from in-body to out-of-body modes. --- interface/src/avatar/SkeletonModel.cpp | 31 +++++++++++++++++++++----- libraries/shared/src/GeometryUtil.cpp | 6 ++++- libraries/shared/src/GeometryUtil.h | 1 + 3 files changed, 31 insertions(+), 7 deletions(-) diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index 5aab7813eb..02ffb903a1 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -164,16 +164,30 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { Rig::HandParameters handParams; + // compute interp factor between in body and out of body hand positions. + const float MIN_OUT_OF_BODY_DISTANCE = TRUNCATE_IK_CAPSULE_RADIUS - 0.1f; + const float MAX_OUT_OF_BODY_DISTANCE = TRUNCATE_IK_CAPSULE_RADIUS + 0.1f; + glm::vec3 capsuleStart = Vectors::UNIT_Y * (TRUNCATE_IK_CAPSULE_LENGTH / 2.0f); + glm::vec3 capsuleEnd = -Vectors::UNIT_Y * (TRUNCATE_IK_CAPSULE_LENGTH / 2.0f); + float outOfBodyAlpha = distanceFromCapsule(hmdPositionInRigSpace, capsuleStart, capsuleEnd, TRUNCATE_IK_CAPSULE_RADIUS); + outOfBodyAlpha = (glm::clamp(outOfBodyAlpha, MIN_OUT_OF_BODY_DISTANCE, MAX_OUT_OF_BODY_DISTANCE) - MIN_OUT_OF_BODY_DISTANCE) / + (MAX_OUT_OF_BODY_DISTANCE - MIN_OUT_OF_BODY_DISTANCE); + auto leftPose = myAvatar->getLeftHandControllerPoseInAvatarFrame(); if (leftPose.isValid()) { handParams.isLeftEnabled = true; handParams.leftPosition = Quaternions::Y_180 * leftPose.getTranslation(); handParams.leftOrientation = Quaternions::Y_180 * leftPose.getRotation(); - // truncate hand target - if (myAvatar->isOutOfBody() && qApp->isHMDMode()) { + // adjust hand position if head is out of body. + if (qApp->isHMDMode()) { + + // compute the out of body hand position. glm::vec3 offset = handParams.leftPosition - hmdPositionInRigSpace; - handParams.leftPosition = truncatedHMDPositionInRigSpace + offset; + glm::vec3 outOfBodyLeftPosition = truncatedHMDPositionInRigSpace + offset; + + // interpolate between in body and out of body hand position. + handParams.leftPosition = lerp(handParams.leftPosition, outOfBodyLeftPosition, outOfBodyAlpha); } } else { handParams.isLeftEnabled = false; @@ -185,10 +199,15 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { handParams.rightPosition = Quaternions::Y_180 * rightPose.getTranslation(); handParams.rightOrientation = Quaternions::Y_180 * rightPose.getRotation(); - // truncate hand target - if (myAvatar->isOutOfBody() && qApp->isHMDMode()) { + // adjust hand position if head is out of body. + if (qApp->isHMDMode()) { + + // compute the out of body hand position. glm::vec3 offset = handParams.rightPosition - hmdPositionInRigSpace; - handParams.rightPosition = truncatedHMDPositionInRigSpace + offset; + glm::vec3 outOfBodyRightPosition = truncatedHMDPositionInRigSpace + offset; + + // interpolate between in body and out of body hand position. + handParams.rightPosition = lerp(handParams.rightPosition, outOfBodyRightPosition, outOfBodyAlpha); } } else { handParams.isRightEnabled = false; diff --git a/libraries/shared/src/GeometryUtil.cpp b/libraries/shared/src/GeometryUtil.cpp index 9cf668f733..91623f1e83 100644 --- a/libraries/shared/src/GeometryUtil.cpp +++ b/libraries/shared/src/GeometryUtil.cpp @@ -596,6 +596,10 @@ float distanceFromLineSegment(const glm::vec3& point, const glm::vec3& segmentSt return glm::length(point - nearestPointOnLineSegment(point, segmentStart, segmentEnd)); } +float distanceFromCapsule(const glm::vec3& point, const glm::vec3& segmentStart, const glm::vec3& segmentEnd, float capsuleRadius) { + return distanceFromLineSegment(point, segmentStart, segmentEnd) - capsuleRadius; +} + bool pointIsInsideCapsule(const glm::vec3& point, const glm::vec3& capsuleStart, const glm::vec3& capsuleEnd, float capsuleRadius) { return distanceFromLineSegment(point, capsuleStart, capsuleEnd) < capsuleRadius; } @@ -604,7 +608,7 @@ glm::vec3 projectPointOntoCapsule(const glm::vec3& point, const glm::vec3& capsu glm::vec3 nearestPoint = nearestPointOnLineSegment(point, capsuleStart, capsuleEnd); glm::vec3 d = point - nearestPoint; float dLen = glm::length(d); - if (dLen > EPSILON) { + if (dLen < EPSILON) { return nearestPoint; // TODO: maybe we should pick a point actually on the surface... } else { return nearestPoint + d * (capsuleRadius / dLen); diff --git a/libraries/shared/src/GeometryUtil.h b/libraries/shared/src/GeometryUtil.h index 05ca415cac..47f069931d 100644 --- a/libraries/shared/src/GeometryUtil.h +++ b/libraries/shared/src/GeometryUtil.h @@ -162,6 +162,7 @@ private: glm::vec3 nearestPointOnLineSegment(const glm::vec3& point, const glm::vec3& segmentStart, const glm::vec3& segmentEnd); float distanceFromLineSegment(const glm::vec3& point, const glm::vec3& segmentStart, const glm::vec3& segmentEnd); +float distanceFromCapsule(const glm::vec3& point, const glm::vec3& segmentStart, const glm::vec3& segmentEnd, float capsuleRadius); bool pointIsInsideCapsule(const glm::vec3& point, const glm::vec3& capsuleStart, const glm::vec3& capsuleEnd, float capsuleRadius); glm::vec3 projectPointOntoCapsule(const glm::vec3& point, const glm::vec3& capsuleStart, const glm::vec3& capsuleEnd, float capsuleRadius); From 203ff958492f1562c63f2d5e6edc9484131840a2 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 26 Sep 2016 09:59:50 -0700 Subject: [PATCH 058/101] Animation tuning to reduce foot sliding. * Navigation walk speed has been reduced * Tuned IdleToWalk timescale and interp time to reduce foot sliding * Tuned fwd, back and lateral characteristicSpeeds to better match the animations. This reduces foot sliding when moving forward and backward. * Reduced rig state machine hysteresis to 1/60th of a second. --- interface/resources/avatar/avatar-animation.json | 14 +++++++------- interface/src/avatar/MyAvatar.cpp | 2 +- libraries/animation/src/Rig.cpp | 8 ++++---- 3 files changed, 12 insertions(+), 12 deletions(-) diff --git a/interface/resources/avatar/avatar-animation.json b/interface/resources/avatar/avatar-animation.json index 834a3fc277..e1fd396501 100644 --- a/interface/resources/avatar/avatar-animation.json +++ b/interface/resources/avatar/avatar-animation.json @@ -255,8 +255,8 @@ }, { "id": "idleToWalkFwd", - "interpTarget": 3, - "interpDuration": 3, + "interpTarget": 5, + "interpDuration": 3.5, "transitions": [ { "var": "idleToWalkFwdOnDone", "state": "walkFwd" }, { "var": "isNotMoving", "state": "idle" }, @@ -523,7 +523,7 @@ "data": { "alpha": 0.0, "desiredSpeed": 1.4, - "characteristicSpeeds": [0.5, 1.4, 4.5], + "characteristicSpeeds": [0.5, 1.3, 4.5], "alphaVar": "moveForwardAlpha", "desiredSpeedVar": "moveForwardSpeed" }, @@ -573,7 +573,7 @@ "url": "animations/idle_to_walk.fbx", "startFrame": 1.0, "endFrame": 13.0, - "timeScale": 1.0, + "timeScale": 0.9, "loopFlag": false }, "children": [] @@ -584,7 +584,7 @@ "data": { "alpha": 0.0, "desiredSpeed": 1.4, - "characteristicSpeeds": [0.6, 1.45], + "characteristicSpeeds": [0.6, 1.05], "alphaVar": "moveBackwardAlpha", "desiredSpeedVar": "moveBackwardSpeed" }, @@ -646,7 +646,7 @@ "data": { "alpha": 0.0, "desiredSpeed": 1.4, - "characteristicSpeeds": [0.2, 0.65], + "characteristicSpeeds": [0.2, 0.5], "alphaVar": "moveLateralAlpha", "desiredSpeedVar": "moveLateralSpeed" }, @@ -683,7 +683,7 @@ "data": { "alpha": 0.0, "desiredSpeed": 1.4, - "characteristicSpeeds": [0.2, 0.65], + "characteristicSpeeds": [0.2, 0.5], "alphaVar": "moveLateralAlpha", "desiredSpeedVar": "moveLateralSpeed" }, diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 9f079288f4..54e09ea64e 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -59,7 +59,7 @@ 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.6f; // human walking speed +const float MAX_WALKING_SPEED = 2.0f; // human walking speed const float MAX_BOOST_SPEED = 0.5f * MAX_WALKING_SPEED; // action motor gets additive boost below this speed 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 diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 05e589918a..07287e8aaf 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -501,9 +501,9 @@ bool Rig::getRelativeDefaultJointTranslation(int index, glm::vec3& translationOu } // animation reference speeds. -static const std::vector FORWARD_SPEEDS = { 0.4f, 1.4f, 4.5f }; // m/s -static const std::vector BACKWARD_SPEEDS = { 0.6f, 1.45f }; // m/s -static const std::vector LATERAL_SPEEDS = { 0.2f, 0.65f }; // m/s +static const std::vector FORWARD_SPEEDS = { 0.4f, 1.3f, 4.5f }; // m/s +static const std::vector BACKWARD_SPEEDS = { 0.6f, 1.05f }; // m/s +static const std::vector LATERAL_SPEEDS = { 0.2f, 0.5f }; // m/s void Rig::computeMotionAnimationState(float deltaTime, const glm::vec3& worldPosition, const glm::vec3& worldVelocity, const glm::quat& worldRotation, CharacterControllerState ccState) { @@ -598,7 +598,7 @@ void Rig::computeMotionAnimationState(float deltaTime, const glm::vec3& worldPos } } - const float STATE_CHANGE_HYSTERESIS_TIMER = 0.1f; + const float STATE_CHANGE_HYSTERESIS_TIMER = 1.0f / 60.0f; // Skip hystersis timer for jump transitions. if (_desiredState == RigRole::Takeoff) { From 0a72805874b1294c7fe595975fd7c6724b5a4ebd Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 26 Sep 2016 10:41:16 -0700 Subject: [PATCH 059/101] Tuned transition from idle to back to reduce foot sliding --- interface/resources/avatar/avatar-animation.json | 4 ++-- interface/src/avatar/MyAvatar.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/interface/resources/avatar/avatar-animation.json b/interface/resources/avatar/avatar-animation.json index e1fd396501..93665fefe6 100644 --- a/interface/resources/avatar/avatar-animation.json +++ b/interface/resources/avatar/avatar-animation.json @@ -292,8 +292,8 @@ }, { "id": "walkBwd", - "interpTarget": 6, - "interpDuration": 6, + "interpTarget": 4, + "interpDuration": 4, "transitions": [ { "var": "isNotMoving", "state": "idle" }, { "var": "isMovingForward", "state": "walkFwd" }, diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index b6b6f1373b..a8d2b6a4e2 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -55,7 +55,7 @@ enum AudioListenerMode { }; Q_DECLARE_METATYPE(AudioListenerMode); -const size_t DEBUG_LINE_LOOP_SIZE = 1000; +const size_t DEBUG_LINE_LOOP_SIZE = 500; class MyAvatar : public Avatar { Q_OBJECT From effadd38a6d34705f5497c0cb9d54b167f904dbb Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 26 Sep 2016 13:37:56 -0700 Subject: [PATCH 060/101] Reduce foot sliding for tall and short avatars. Scale the desiredVelocity of the animation blends to account for limb length. i.e. a short avatar should have a faster walk cycle to move at the same speed as a tall avatar --- libraries/animation/src/Rig.cpp | 25 ++++++++++++++++++------- 1 file changed, 18 insertions(+), 7 deletions(-) diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 07287e8aaf..cba1e4400d 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -504,12 +504,19 @@ bool Rig::getRelativeDefaultJointTranslation(int index, glm::vec3& translationOu static const std::vector FORWARD_SPEEDS = { 0.4f, 1.3f, 4.5f }; // m/s static const std::vector BACKWARD_SPEEDS = { 0.6f, 1.05f }; // m/s static const std::vector LATERAL_SPEEDS = { 0.2f, 0.5f }; // m/s +static const float DEFAULT_AVATAR_EYE_HEIGHT = 1.65f; // movement speeds are for characters of this eye-height. ~170 cm tall. void Rig::computeMotionAnimationState(float deltaTime, const glm::vec3& worldPosition, const glm::vec3& worldVelocity, const glm::quat& worldRotation, CharacterControllerState ccState) { glm::vec3 front = worldRotation * IDENTITY_FRONT; glm::vec3 workingVelocity = worldVelocity; + // TODO: account for avatar scaling + int eyeJoint = indexOfJoint("LeftEye"); + int toeJoint = indexOfJoint("LeftToeBase"); + const float AVATAR_EYE_HEIGHT = (eyeJoint >= 0 && toeJoint >= 0) ? getAbsoluteDefaultPose(eyeJoint).trans.y - getAbsoluteDefaultPose(toeJoint).trans.y : DEFAULT_AVATAR_EYE_HEIGHT; + const float AVATAR_HEIGHT_RATIO = DEFAULT_AVATAR_EYE_HEIGHT / AVATAR_EYE_HEIGHT; + { glm::vec3 localVel = glm::inverse(worldRotation) * workingVelocity; @@ -529,18 +536,22 @@ void Rig::computeMotionAnimationState(float deltaTime, const glm::vec3& worldPos float moveBackwardAlpha = 0.0f; float moveLateralAlpha = 0.0f; - // calcuate the animation alpha and timeScale values based on current speeds and animation reference speeds. - calcAnimAlpha(_averageForwardSpeed.getAverage(), FORWARD_SPEEDS, &moveForwardAlpha); - calcAnimAlpha(-_averageForwardSpeed.getAverage(), BACKWARD_SPEEDS, &moveBackwardAlpha); - calcAnimAlpha(fabsf(_averageLateralSpeed.getAverage()), LATERAL_SPEEDS, &moveLateralAlpha); + float averageForwardSpeed = AVATAR_HEIGHT_RATIO * _averageForwardSpeed.getAverage(); + float averageBackwardSpeed = -averageForwardSpeed; + float averageLateralSpeed = AVATAR_HEIGHT_RATIO * fabsf(_averageLateralSpeed.getAverage()); - _animVars.set("moveForwardSpeed", _averageForwardSpeed.getAverage()); + // calcuate the animation alpha and timeScale values based on current speeds and animation reference speeds. + calcAnimAlpha(averageForwardSpeed, FORWARD_SPEEDS, &moveForwardAlpha); + calcAnimAlpha(averageBackwardSpeed, BACKWARD_SPEEDS, &moveBackwardAlpha); + calcAnimAlpha(averageLateralSpeed, LATERAL_SPEEDS, &moveLateralAlpha); + + _animVars.set("moveForwardSpeed", averageForwardSpeed); _animVars.set("moveForwardAlpha", moveForwardAlpha); - _animVars.set("moveBackwardSpeed", -_averageForwardSpeed.getAverage()); + _animVars.set("moveBackwardSpeed", averageBackwardSpeed); _animVars.set("moveBackwardAlpha", moveBackwardAlpha); - _animVars.set("moveLateralSpeed", fabsf(_averageLateralSpeed.getAverage())); + _animVars.set("moveLateralSpeed", averageLateralSpeed); _animVars.set("moveLateralAlpha", moveLateralAlpha); const float MOVE_ENTER_SPEED_THRESHOLD = 0.2f; // m/sec From f9a1a21b2ccfaac9157bfa16ea6a684f507f8224 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 26 Sep 2016 14:24:01 -0700 Subject: [PATCH 061/101] apply linearMoveBlend to idleToWalk transition to reduce foot sliding --- .../resources/avatar/avatar-animation.json | 33 +++++++++++++------ 1 file changed, 23 insertions(+), 10 deletions(-) diff --git a/interface/resources/avatar/avatar-animation.json b/interface/resources/avatar/avatar-animation.json index 93665fefe6..ba4c66f045 100644 --- a/interface/resources/avatar/avatar-animation.json +++ b/interface/resources/avatar/avatar-animation.json @@ -255,10 +255,10 @@ }, { "id": "idleToWalkFwd", - "interpTarget": 5, - "interpDuration": 3.5, + "interpTarget": 4, + "interpDuration": 3.0, "transitions": [ - { "var": "idleToWalkFwdOnDone", "state": "walkFwd" }, + { "var": "idleToWalkFwdClipOnDone", "state": "walkFwd" }, { "var": "isNotMoving", "state": "idle" }, { "var": "isMovingBackward", "state": "walkBwd" }, { "var": "isMovingRight", "state": "strafeRight" }, @@ -568,15 +568,28 @@ }, { "id": "idleToWalkFwd", - "type": "clip", + "type": "blendLinearMove", "data": { - "url": "animations/idle_to_walk.fbx", - "startFrame": 1.0, - "endFrame": 13.0, - "timeScale": 0.9, - "loopFlag": false + "alpha": 0.0, + "desiredSpeed": 1.2, + "characteristicSpeeds": [1.2], + "alphaVar": 0.0, + "desiredSpeedVar": "moveForwardSpeed" }, - "children": [] + "children": [ + { + "id": "idleToWalkFwdClip", + "type": "clip", + "data": { + "url": "animations/idle_to_walk.fbx", + "startFrame": 1.0, + "endFrame": 13.0, + "timeScale": 0.9, + "loopFlag": false + }, + "children": [] + } + ] }, { "id": "walkBwd", From c327c5c6cef31293a6068de7e5abcc2c863d4237 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Tue, 27 Sep 2016 09:47:44 -0700 Subject: [PATCH 062/101] coding convention fix --- libraries/physics/src/CharacterController.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 4c62a4072f..457304bebd 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -195,8 +195,7 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) { return hasFloor; } -void CharacterController::updateAction(btCollisionWorld* collisionWorld, btScalar deltaTime) -{ +void CharacterController::updateAction(btCollisionWorld* collisionWorld, btScalar deltaTime) { _preActionVelocity = getLinearVelocity(); preStep(collisionWorld); playerStep(collisionWorld, deltaTime); From 961d262df185173986277d52ca9dea6c64ccc6ed Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Tue, 27 Sep 2016 16:38:48 -0700 Subject: [PATCH 063/101] Improved body follow behavior * follow helper lean re-centering / reconciliation now modifies bodySensorMatrix, NOT the character controller. * The character controller now always follows the bodySensorMatrix (in world space). This decouples the lean re-centering velocity from the velocity used to move the character controller. We can now independently tune these things separately. --- interface/src/Application.cpp | 2 +- interface/src/avatar/MyAvatar.cpp | 60 ++++++++++--------- interface/src/avatar/MyAvatar.h | 6 +- libraries/physics/src/CharacterController.cpp | 2 +- 4 files changed, 36 insertions(+), 34 deletions(-) diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index 8ef9c228c4..928e372d78 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -3811,7 +3811,7 @@ void Application::update(float deltaTime) { avatarManager->getObjectsToChange(motionStates); _physicsEngine->changeObjects(motionStates); - myAvatar->prepareForPhysicsSimulation(); + myAvatar->prepareForPhysicsSimulation(deltaTime); _physicsEngine->forEachAction([&](EntityActionPointer action) { action->prepareForPhysicsSimulation(); }); diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 54e09ea64e..3ac22222f6 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -36,6 +36,7 @@ #include #include #include +#include #include #include #include @@ -1347,7 +1348,7 @@ void MyAvatar::updateMotors() { _characterController.setLinearAcceleration(glm::vec3(0.0f, _thrust.y, 0.0f)); } -void MyAvatar::prepareForPhysicsSimulation() { +void MyAvatar::prepareForPhysicsSimulation(float deltaTime) { relayDriveKeysToCharacterController(); updateMotors(); @@ -1365,9 +1366,14 @@ void MyAvatar::prepareForPhysicsSimulation() { _characterController.setPositionAndOrientation(position, orientation); if (qApp->isHMDMode()) { - glm::mat4 bodyToWorldMatrix = createMatFromQuatAndPos(orientation, position); - glm::mat4 currentBodySensorMatrix = glm::inverse(_sensorToWorldMatrix) * bodyToWorldMatrix; - _follow.prePhysicsUpdate(*this, deriveBodyFromHMDSensor(), currentBodySensorMatrix); + // update the _bodySensorMatrix based on leaning behavior of the avatar. + _bodySensorMatrix = _follow.prePhysicsUpdate(*this, deriveBodyFromHMDSensor(), _bodySensorMatrix, hasDriveInput(), deltaTime); + + // The avatar physics body always follows the _bodySensorMatrix. + glm::mat4 worldBodyMatrix = _sensorToWorldMatrix * _bodySensorMatrix; + getCharacterController()->setFollowParameters(worldBodyMatrix); + + DebugDraw::getInstance().addMarker("bodySensorMatrix", glmExtractRotation(worldBodyMatrix), extractTranslation(worldBodyMatrix), glm::vec4(1.0f)); } else { _follow.deactivate(); getCharacterController()->disableFollow(); @@ -2230,10 +2236,10 @@ void MyAvatar::FollowHelper::updateRotationActivation(const MyAvatar& myAvatar, deactivate(Rotation); } else { const float FOLLOW_ROTATION_THRESHOLD = cosf(PI / 6.0f); // 30 degrees - const float STOP_FOLLOW_ROTATION_THRESHOLD = 0.99f; + const float STOP_FOLLOW_ROTATION_THRESHOLD = cosf(PI / 180.0f); // 1 degree glm::vec2 bodyFacing = getFacingDir2D(currentBodyMatrix); if (isActive(Rotation)) { - if (glm::dot(myAvatar.getHMDSensorFacingMovingAverage(), bodyFacing) > STOP_FOLLOW_ROTATION_THRESHOLD) { + if (glm::dot(myAvatar.getHMDSensorFacing(), bodyFacing) > STOP_FOLLOW_ROTATION_THRESHOLD) { deactivate(Rotation); } } else if (glm::dot(myAvatar.getHMDSensorFacingMovingAverage(), bodyFacing) < FOLLOW_ROTATION_THRESHOLD) { @@ -2272,9 +2278,6 @@ void MyAvatar::FollowHelper::updateHorizontalActivation(const MyAvatar& myAvatar } void MyAvatar::FollowHelper::updateVerticalActivation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) { - - // OUTOFBODY_HACK: disable vertical follow behavior - /* const float CYLINDER_TOP = 0.1f; const float CYLINDER_BOTTOM = -1.5f; const float MIN_VERTICAL_OFFSET = 0.02f; @@ -2287,42 +2290,41 @@ void MyAvatar::FollowHelper::updateVerticalActivation(const MyAvatar& myAvatar, } else if (offset.y > CYLINDER_TOP || offset.y < CYLINDER_BOTTOM) { activate(Vertical); } - */ } -void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) { - _desiredBodyMatrix = desiredBodyMatrix; - +glm::mat4 MyAvatar::FollowHelper::prePhysicsUpdate(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix, bool hasDriveInput, float deltaTime) { if (myAvatar.getHMDLeanRecenterEnabled()) { updateRotationActivation(myAvatar, desiredBodyMatrix, currentBodyMatrix); updateHorizontalActivation(myAvatar, desiredBodyMatrix, currentBodyMatrix); updateVerticalActivation(myAvatar, desiredBodyMatrix, currentBodyMatrix); - glm::mat4 desiredWorldMatrix = myAvatar.getSensorToWorldMatrix() * _desiredBodyMatrix; - glm::mat4 currentWorldMatrix = createMatFromQuatAndPos(myAvatar.getOrientation(), myAvatar.getPosition()); + AnimPose currentBodyPose(currentBodyMatrix); + AnimPose desiredBodyPose(desiredBodyMatrix); + AnimPose followBodyPose(currentBodyMatrix); - AnimPose followWorldPose(currentWorldMatrix); - if (isActive(Rotation)) { - followWorldPose.rot = glmExtractRotation(desiredWorldMatrix); + if (isActive(Rotation) || hasDriveInput) { + followBodyPose.rot = desiredBodyPose.rot; } - if (isActive(Horizontal)) { - glm::vec3 desiredTranslation = extractTranslation(desiredWorldMatrix); - followWorldPose.trans.x = desiredTranslation.x; - followWorldPose.trans.z = desiredTranslation.z; + if (isActive(Horizontal) || hasDriveInput) { + followBodyPose.trans.x = desiredBodyPose.trans.x; + followBodyPose.trans.z = desiredBodyPose.trans.z; } - if (isActive(Vertical)) { - glm::vec3 desiredTranslation = extractTranslation(desiredWorldMatrix); - followWorldPose.trans.y = desiredTranslation.y; + if (isActive(Vertical) || hasDriveInput) { + followBodyPose.trans.y = desiredBodyPose.trans.y; } - if (isActive()) { - myAvatar.getCharacterController()->setFollowParameters(followWorldPose); + if (isActive() || hasDriveInput) { + const float TIMESCALE = 0.2f; + const float tau = deltaTime / TIMESCALE; + AnimPose newBodyPose; + blend(1, ¤tBodyPose, &followBodyPose, tau, &newBodyPose); + return (glm::mat4)newBodyPose; } else { - myAvatar.getCharacterController()->disableFollow(); + return currentBodyMatrix; } } else { deactivate(); - myAvatar.getCharacterController()->disableFollow(); + return currentBodyMatrix; } } diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index a8d2b6a4e2..554de24034 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -109,6 +109,7 @@ public: const glm::mat4& getHMDSensorMatrix() const { return _hmdSensorMatrix; } const glm::vec3& getHMDSensorPosition() const { return _hmdSensorPosition; } const glm::quat& getHMDSensorOrientation() const { return _hmdSensorOrientation; } + const glm::vec2& getHMDSensorFacing() const { return _hmdSensorFacing; } const glm::vec2& getHMDSensorFacingMovingAverage() const { return _hmdSensorFacingMovingAverage; } Q_INVOKABLE void setOrientationVar(const QVariant& newOrientationVar); @@ -235,7 +236,7 @@ public: const MyCharacterController* getCharacterController() const { return &_characterController; } void updateMotors(); - void prepareForPhysicsSimulation(); + void prepareForPhysicsSimulation(float deltaTime); void harvestResultsFromPhysicsSimulation(float deltaTime); const QString& getCollisionSoundURL() { return _collisionSoundURL; } @@ -459,7 +460,6 @@ private: Vertical, NumFollowTypes }; - glm::mat4 _desiredBodyMatrix; uint8_t _activeBits { 0 }; bool _isOutOfBody { false }; @@ -472,7 +472,7 @@ private: void updateRotationActivation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix); void updateHorizontalActivation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix); void updateVerticalActivation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix); - void prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat4& bodySensorMatrix, const glm::mat4& currentBodyMatrix); + glm::mat4 prePhysicsUpdate(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix, bool hasDriveInput, float deltaTime); void postPhysicsUpdate(MyAvatar& myAvatar); }; FollowHelper _follow; diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 457304bebd..8405bda208 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -231,7 +231,7 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) { btVector3 velocity = _rigidBody->getLinearVelocity() - _parentVelocity; if (_following) { // linear part uses a motor - const float MAX_WALKING_SPEED = 2.5f; // TODO: scale this stuff with avatar size + const float MAX_WALKING_SPEED = 30.5f; // TODO: scale this stuff with avatar size const float MAX_WALKING_SPEED_DISTANCE = 1.0f; const float NORMAL_WALKING_SPEED = 0.5f * MAX_WALKING_SPEED; const float NORMAL_WALKING_SPEED_DISTANCE = 0.5f * MAX_WALKING_SPEED_DISTANCE; From 62dc25023786794feb2f96f36d0fe0518eeb2912 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Tue, 27 Sep 2016 16:48:15 -0700 Subject: [PATCH 064/101] removed DebugDraw calls --- interface/src/avatar/MyAvatar.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 3ac22222f6..7e3900e340 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -1372,8 +1372,6 @@ void MyAvatar::prepareForPhysicsSimulation(float deltaTime) { // The avatar physics body always follows the _bodySensorMatrix. glm::mat4 worldBodyMatrix = _sensorToWorldMatrix * _bodySensorMatrix; getCharacterController()->setFollowParameters(worldBodyMatrix); - - DebugDraw::getInstance().addMarker("bodySensorMatrix", glmExtractRotation(worldBodyMatrix), extractTranslation(worldBodyMatrix), glm::vec4(1.0f)); } else { _follow.deactivate(); getCharacterController()->disableFollow(); From 9a804e19fc68cd3747a0f2fc95655d0a259c83f2 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Tue, 27 Sep 2016 18:44:54 -0700 Subject: [PATCH 065/101] Fix for null gravity when falling off ledges --- libraries/physics/src/CharacterController.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 8405bda208..0465362dce 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -261,6 +261,7 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) { const float HORIZONTAL_FOLLOW_TIMESCALE = 0.1f; const float VERTICAL_FOLLOW_TIMESCALE = (_state == State::Hover) ? HORIZONTAL_FOLLOW_TIMESCALE : 20.0f; glm::quat worldFrameRotation; // identity + vel.setY(0.0f); // don't allow any vertical component of the follow velocity to enter the _targetVelocity. addMotor(bulletToGLM(vel), worldFrameRotation, HORIZONTAL_FOLLOW_TIMESCALE, VERTICAL_FOLLOW_TIMESCALE); } From a10ae20bf7f516a505defbd49f4ca34d70882579 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 27 Sep 2016 16:59:50 -0700 Subject: [PATCH 066/101] measure avatar velocity while driving and add to followVelocity for tighter tracking at high speeds --- libraries/physics/src/CharacterController.cpp | 55 ++++++++++++++----- libraries/physics/src/CharacterController.h | 3 + 2 files changed, 44 insertions(+), 14 deletions(-) diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 0465362dce..0c706e8642 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -230,6 +230,7 @@ const btScalar MIN_TARGET_SPEED_SQUARED = MIN_TARGET_SPEED * MIN_TARGET_SPEED; void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) { btVector3 velocity = _rigidBody->getLinearVelocity() - _parentVelocity; if (_following) { + _followTimeAccumulator += dt; // linear part uses a motor const float MAX_WALKING_SPEED = 30.5f; // TODO: scale this stuff with avatar size const float MAX_WALKING_SPEED_DISTANCE = 1.0f; @@ -243,22 +244,28 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) { const float MIN_DELTA_DISTANCE = 0.01f; // TODO: scale by avatar size but cap at (NORMAL_WALKING_SPEED * FEW_SUBSTEPS) if (deltaDistance > MIN_DELTA_DISTANCE) { btVector3 vel = deltaPos; - if (deltaDistance > MAX_WALKING_SPEED_DISTANCE) { - // cap max speed - vel *= MAX_WALKING_SPEED / deltaDistance; - } else if (deltaDistance > NORMAL_WALKING_SPEED_DISTANCE) { - // linearly interpolate to NORMAL_WALKING_SPEED - btScalar alpha = (deltaDistance - NORMAL_WALKING_SPEED_DISTANCE) / (MAX_WALKING_SPEED_DISTANCE - NORMAL_WALKING_SPEED_DISTANCE); - vel *= NORMAL_WALKING_SPEED * (1.0f - alpha) + MAX_WALKING_SPEED * alpha; + if (_state == State::Hover) { + btScalar HOVER_FOLLOW_VELOCITY_TIMESCALE = 0.1f; + vel /= HOVER_FOLLOW_VELOCITY_TIMESCALE; } else { - // use exponential decay but cap at NORMAL_WALKING_SPEED - vel /= FEW_SUBSTEPS; - btScalar speed = vel.length(); - if (speed > NORMAL_WALKING_SPEED) { - vel *= NORMAL_WALKING_SPEED / speed; + if (deltaDistance > MAX_WALKING_SPEED_DISTANCE) { + // cap max speed + vel *= MAX_WALKING_SPEED / deltaDistance; + } else if (deltaDistance > NORMAL_WALKING_SPEED_DISTANCE) { + // linearly interpolate to NORMAL_WALKING_SPEED + btScalar alpha = (deltaDistance - NORMAL_WALKING_SPEED_DISTANCE) / (MAX_WALKING_SPEED_DISTANCE - NORMAL_WALKING_SPEED_DISTANCE); + vel *= NORMAL_WALKING_SPEED * (1.0f - alpha) + MAX_WALKING_SPEED * alpha; + } else { + // use exponential decay but cap at NORMAL_WALKING_SPEED + vel /= FEW_SUBSTEPS; + btScalar speed = vel.length(); + if (speed > NORMAL_WALKING_SPEED) { + vel *= NORMAL_WALKING_SPEED / speed; + } } } - const float HORIZONTAL_FOLLOW_TIMESCALE = 0.1f; + vel += _followVelocity; + const float HORIZONTAL_FOLLOW_TIMESCALE = 0.05f; const float VERTICAL_FOLLOW_TIMESCALE = (_state == State::Hover) ? HORIZONTAL_FOLLOW_TIMESCALE : 20.0f; glm::quat worldFrameRotation; // identity vel.setY(0.0f); // don't allow any vertical component of the follow velocity to enter the _targetVelocity. @@ -453,7 +460,27 @@ void CharacterController::setParentVelocity(const glm::vec3& velocity) { void CharacterController::setFollowParameters(const glm::mat4& desiredWorldBodyMatrix) { _followDesiredBodyTransform = glmToBullet(desiredWorldBodyMatrix) * btTransform(btQuaternion::getIdentity(), glmToBullet(_shapeLocalOffset)); - _following = true; + if (!_following) { + _following = true; + _followVelocity = btVector3(0.0f, 0.0f, 0.0f); + } else if (_followTimeAccumulator > 0.0f) { + btVector3 newFollowVelocity = (_followDesiredBodyTransform.getOrigin() - _previousFollowPosition) / _followTimeAccumulator; + const float dontDivideByZero = 0.001f; + float newSpeed = newFollowVelocity.length() + dontDivideByZero; + float oldSpeed = _followVelocity.length(); + const float VERY_SLOW_HOVER_SPEED = 0.25f; + if (oldSpeed / newSpeed > 100.0f && newSpeed < VERY_SLOW_HOVER_SPEED) { + // avatar is stopping quickly + // HACK: slam _followVelocity and _rigidBody velocity immediately + _followVelocity = newFollowVelocity; + _rigidBody->setLinearVelocity(_followVelocity); + } else { + const float blend = 0.2f; + _followVelocity = (1.0f - blend) * _followVelocity + blend * newFollowVelocity; + } + } + _previousFollowPosition = _followDesiredBodyTransform.getOrigin(); + _followTimeAccumulator = 0.0f; } glm::vec3 CharacterController::getLinearVelocity() const { diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h index e111c4cefe..0ee42f43a9 100644 --- a/libraries/physics/src/CharacterController.h +++ b/libraries/physics/src/CharacterController.h @@ -148,6 +148,8 @@ protected: glm::vec3 _preActionVelocity; btVector3 _velocityChange; btTransform _followDesiredBodyTransform; + btVector3 _followVelocity { 0.0f, 0.0f, 0.0f }; + btVector3 _previousFollowPosition { 0.0f, 0.0f, 0.0f }; btVector3 _position; btQuaternion _rotation; @@ -175,6 +177,7 @@ protected: bool _hasSupport; btScalar _gravity; + btScalar _followTimeAccumulator { 0.0f }; btScalar _jumpSpeed; btVector3 _linearAcceleration; From 4e1c1aec0af7ce19a60120d7826ae04bd721f2f2 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 28 Sep 2016 18:06:00 -0700 Subject: [PATCH 067/101] less magic --- libraries/physics/src/CharacterController.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 0c706e8642..c004654dd0 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -469,7 +469,8 @@ void CharacterController::setFollowParameters(const glm::mat4& desiredWorldBodyM float newSpeed = newFollowVelocity.length() + dontDivideByZero; float oldSpeed = _followVelocity.length(); const float VERY_SLOW_HOVER_SPEED = 0.25f; - if (oldSpeed / newSpeed > 100.0f && newSpeed < VERY_SLOW_HOVER_SPEED) { + const FAST_CHANGE_SPEED_RATIO = 100.0f; + if (oldSpeed / newSpeed > FAST_CHANGE_SPEED_RATIO && newSpeed < VERY_SLOW_HOVER_SPEED) { // avatar is stopping quickly // HACK: slam _followVelocity and _rigidBody velocity immediately _followVelocity = newFollowVelocity; From a443cd65f4b185a63bab6876afb9ba693cedbffa Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 28 Sep 2016 18:08:06 -0700 Subject: [PATCH 068/101] add comment for simple noise filter --- libraries/physics/src/CharacterController.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index c004654dd0..7b2e9ebd30 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -476,6 +476,7 @@ void CharacterController::setFollowParameters(const glm::mat4& desiredWorldBodyM _followVelocity = newFollowVelocity; _rigidBody->setLinearVelocity(_followVelocity); } else { + // use simple blending to filter noise from the velocity measurement const float blend = 0.2f; _followVelocity = (1.0f - blend) * _followVelocity + blend * newFollowVelocity; } From 9fa97841351da46986957e917f153699e3364366 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 29 Sep 2016 11:09:09 -0700 Subject: [PATCH 069/101] fix typo that broke build --- libraries/physics/src/CharacterController.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 7b2e9ebd30..b3f30cb843 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -469,7 +469,7 @@ void CharacterController::setFollowParameters(const glm::mat4& desiredWorldBodyM float newSpeed = newFollowVelocity.length() + dontDivideByZero; float oldSpeed = _followVelocity.length(); const float VERY_SLOW_HOVER_SPEED = 0.25f; - const FAST_CHANGE_SPEED_RATIO = 100.0f; + const float FAST_CHANGE_SPEED_RATIO = 100.0f; if (oldSpeed / newSpeed > FAST_CHANGE_SPEED_RATIO && newSpeed < VERY_SLOW_HOVER_SPEED) { // avatar is stopping quickly // HACK: slam _followVelocity and _rigidBody velocity immediately From d29386c43fe481673969c8fd3c3eb24ad0d9fafa Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 28 Sep 2016 10:13:14 -0700 Subject: [PATCH 070/101] better tracking of avatar gravity setting --- .../src/avatar/MyCharacterController.cpp | 2 +- libraries/physics/src/CharacterController.cpp | 44 +++++++------------ libraries/physics/src/CharacterController.h | 4 +- .../physics/src/CharacterGhostObject.cpp | 36 ++++++++++----- libraries/physics/src/CharacterGhostObject.h | 15 ++++--- 5 files changed, 53 insertions(+), 48 deletions(-) diff --git a/interface/src/avatar/MyCharacterController.cpp b/interface/src/avatar/MyCharacterController.cpp index ad7879d2cc..d98c66c8c6 100644 --- a/interface/src/avatar/MyCharacterController.cpp +++ b/interface/src/avatar/MyCharacterController.cpp @@ -61,7 +61,7 @@ void MyCharacterController::updateShapeIfNecessary() { if (_state == State::Hover) { _rigidBody->setGravity(btVector3(0.0f, 0.0f, 0.0f)); } else { - _rigidBody->setGravity(DEFAULT_CHARACTER_GRAVITY * _currentUp); + _rigidBody->setGravity(_gravity * _currentUp); } // KINEMATIC_CONTROLLER_HACK if (_moveKinematically) { diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index b3f30cb843..5058a1aa9c 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -16,6 +16,7 @@ #include #include "ObjectMotionState.h" +#include "PhysicsHelpers.h" #include "PhysicsLogging.h" const btVector3 LOCAL_UP_AXIS(0.0f, 1.0f, 0.0f); @@ -112,13 +113,10 @@ void CharacterController::setDynamicsWorld(btDynamicsWorld* world) { // add to new world _dynamicsWorld = world; _pendingFlags &= ~PENDING_FLAG_JUMP; - // Before adding the RigidBody to the world we must save its oldGravity to the side - // because adding an object to the world will overwrite it with the default gravity. - btVector3 oldGravity = _rigidBody->getGravity(); _dynamicsWorld->addRigidBody(_rigidBody, _collisionGroup, BULLET_COLLISION_MASK_MY_AVATAR); _dynamicsWorld->addAction(this); - // restore gravity settings - _rigidBody->setGravity(oldGravity); + // restore gravity settings because adding an object to the world overwrites its gravity setting + _rigidBody->setGravity(_gravity * _currentUp); btCollisionShape* shape = _rigidBody->getCollisionShape(); assert(shape && shape->getShapeType() == CAPSULE_SHAPE_PROXYTYPE); _ghost.setCharacterCapsule(static_cast(shape)); // KINEMATIC_CONTROLLER_HACK @@ -130,7 +128,6 @@ void CharacterController::setDynamicsWorld(btDynamicsWorld* world) { _ghost.setMaxStepHeight(0.75f * (_radius + _halfHeight)); // HACK _ghost.setMinWallAngle(PI / 4.0f); // HACK _ghost.setUpDirection(_currentUp); - _ghost.setGravity(DEFAULT_CHARACTER_GRAVITY); _ghost.setWorldTransform(_rigidBody->getWorldTransform()); } if (_dynamicsWorld) { @@ -145,7 +142,7 @@ void CharacterController::setDynamicsWorld(btDynamicsWorld* world) { } } -bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) { +bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld, btScalar dt) { _stepHeight = _minStepHeight; // clears last step obstacle btDispatcher* dispatcher = collisionWorld->getDispatcher(); int numManifolds = dispatcher->getNumManifolds(); @@ -220,14 +217,12 @@ void CharacterController::preStep(btCollisionWorld* collisionWorld) { if (rayCallback.hasHit()) { _floorDistance = rayLength * rayCallback.m_closestHitFraction - _radius; } - - _hasSupport = checkForSupport(collisionWorld); } 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) { +void CharacterController::playerStep(btCollisionWorld* collisionWorld, btScalar dt) { btVector3 velocity = _rigidBody->getLinearVelocity() - _parentVelocity; if (_following) { _followTimeAccumulator += dt; @@ -289,6 +284,8 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) { btQuaternion endRot = angularDisplacement * startRot; _rigidBody->setWorldTransform(btTransform(endRot, startPos)); } + + _hasSupport = checkForSupport(collisionWorld, dt); computeNewVelocity(dt, velocity); if (_moveKinematically) { @@ -298,7 +295,7 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) { _ghost.setWorldTransform(transform); _ghost.setMotorVelocity(_targetVelocity); float overshoot = 1.0f * _radius; - _ghost.move(dt, overshoot); + _ghost.move(dt, overshoot, _gravity); transform.setOrigin(_ghost.getWorldTransform().getOrigin()); _rigidBody->setWorldTransform(transform); _rigidBody->setLinearVelocity(_ghost.getLinearVelocity()); @@ -360,7 +357,7 @@ void CharacterController::setState(State desiredState) { if (_collisionGroup == BULLET_COLLISION_GROUP_COLLISIONLESS) { _rigidBody->setGravity(btVector3(0.0f, 0.0f, 0.0f)); } else { - _rigidBody->setGravity(DEFAULT_CHARACTER_GRAVITY * _currentUp); + _rigidBody->setGravity(_gravity * _currentUp); } } } @@ -382,7 +379,7 @@ void CharacterController::setLocalBoundingBox(const glm::vec3& minCorner, const if (glm::abs(radius - _radius) > FLT_EPSILON || glm::abs(halfHeight - _halfHeight) > FLT_EPSILON) { _radius = radius; _halfHeight = halfHeight; - _minStepHeight = 0.02f; // HACK: hardcoded now but should be shape margin + _minStepHeight = 0.041f; // HACK: hardcoded now but should be shape margin _maxStepHeight = 0.75f * (_halfHeight + _radius); if (_dynamicsWorld) { @@ -415,11 +412,8 @@ void CharacterController::handleChangedCollisionGroup() { _pendingFlags &= ~PENDING_FLAG_UPDATE_COLLISION_GROUP; if (_state != State::Hover && _rigidBody) { - if (_collisionGroup == BULLET_COLLISION_GROUP_COLLISIONLESS) { - _rigidBody->setGravity(btVector3(0.0f, 0.0f, 0.0f)); - } else { - _rigidBody->setGravity(DEFAULT_CHARACTER_GRAVITY * _currentUp); - } + _gravity = _collisionGroup == BULLET_COLLISION_GROUP_COLLISIONLESS ? 0.0f : DEFAULT_CHARACTER_GRAVITY; + _rigidBody->setGravity(_gravity * _currentUp); } } } @@ -428,20 +422,14 @@ void CharacterController::updateUpAxis(const glm::quat& rotation) { _currentUp = quatRotate(glmToBullet(rotation), LOCAL_UP_AXIS); _ghost.setUpDirection(_currentUp); if (_state != State::Hover && _rigidBody) { - if (_collisionGroup == BULLET_COLLISION_GROUP_COLLISIONLESS) { - _rigidBody->setGravity(btVector3(0.0f, 0.0f, 0.0f)); - } else { - _rigidBody->setGravity(DEFAULT_CHARACTER_GRAVITY * _currentUp); - } + _rigidBody->setGravity(_gravity * _currentUp); } } void CharacterController::setPositionAndOrientation( const glm::vec3& position, const glm::quat& orientation) { - // TODO: update gravity if up has changed updateUpAxis(orientation); - _rotation = glmToBullet(orientation); _position = glmToBullet(position + orientation * _shapeLocalOffset); } @@ -555,10 +543,10 @@ void CharacterController::applyMotor(int index, btScalar dt, btVector3& worldVel // the motor pushes against step btVector3 leverArm = _stepPoint; motorVelocityWF = _stepNormal.cross(leverArm.cross(motorVelocityWF)); - btScalar distortedLength = motorVelocityWF.length(); - if (distortedLength > FLT_EPSILON) { + btScalar doubleCrossLength = motorVelocityWF.length(); + if (doubleCrossLength > FLT_EPSILON) { // scale the motor in the correct direction and rotate back to motor-frame - motorVelocityWF *= (motorVelocity.length() / distortedLength); + motorVelocityWF *= (motorVelocity.length() / doubleCrossLength); motorVelocity += motorVelocityWF.rotate(axis, -angle); // make vTimescale as small as possible vTimescale = glm::min(vTimescale, motor.hTimescale); diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h index 0ee42f43a9..69168699eb 100644 --- a/libraries/physics/src/CharacterController.h +++ b/libraries/physics/src/CharacterController.h @@ -127,7 +127,7 @@ protected: #endif void updateUpAxis(const glm::quat& rotation); - bool checkForSupport(btCollisionWorld* collisionWorld); + bool checkForSupport(btCollisionWorld* collisionWorld, btScalar dt); protected: struct CharacterMotor { @@ -176,7 +176,7 @@ protected: btScalar _floorDistance; bool _hasSupport; - btScalar _gravity; + btScalar _gravity { DEFAULT_CHARACTER_GRAVITY }; btScalar _followTimeAccumulator { 0.0f }; btScalar _jumpSpeed; diff --git a/libraries/physics/src/CharacterGhostObject.cpp b/libraries/physics/src/CharacterGhostObject.cpp index a4b6e90266..f39b9c3995 100755 --- a/libraries/physics/src/CharacterGhostObject.cpp +++ b/libraries/physics/src/CharacterGhostObject.cpp @@ -54,11 +54,6 @@ void CharacterGhostObject::setUpDirection(const btVector3& up) { } } -void CharacterGhostObject::setMotorVelocity(const btVector3& velocity) { - _motorVelocity = velocity; - _motorSpeed = _motorVelocity.length(); -} - // override of btCollisionObject::setCollisionShape() void CharacterGhostObject::setCharacterCapsule(btCapsuleShape* capsule) { assert(capsule); @@ -78,10 +73,11 @@ void CharacterGhostObject::setCollisionWorld(btCollisionWorld* world) { } } -void CharacterGhostObject::move(btScalar dt, btScalar overshoot) { +void CharacterGhostObject::move(btScalar dt, btScalar overshoot, btScalar gravity) { _onFloor = false; + _steppingUp = false; assert(_world && _inWorld); - updateVelocity(dt); + updateVelocity(dt, gravity); // resolve any penetrations before sweeping int32_t MAX_LOOPS = 4; @@ -113,8 +109,9 @@ void CharacterGhostObject::move(btScalar dt, btScalar overshoot) { updateTraction(startPosition); } + btScalar speed = _linearVelocity.length(); btVector3 forwardSweep = dt * _linearVelocity; - btScalar stepDistance = forwardSweep.length(); + btScalar stepDistance = dt * speed; btScalar MIN_SWEEP_DISTANCE = 0.0001f; if (stepDistance < MIN_SWEEP_DISTANCE) { // not moving, no need to sweep @@ -146,6 +143,22 @@ void CharacterGhostObject::move(btScalar dt, btScalar overshoot) { updateTraction(nextTransform.getOrigin()); return; } + bool verticalOnly = btFabs(btFabs(_linearVelocity.dot(_upDirection)) - speed) < margin; + if (verticalOnly) { + // no need to step + nextTransform.setOrigin(startPosition + (result.m_closestHitFraction * stepDistance / longSweepDistance) * forwardSweep); + setWorldTransform(nextTransform); + + if (result.m_hitNormalWorld.dot(_upDirection) > _maxWallNormalUpComponent) { + _floorNormal = result.m_hitNormalWorld; + _floorContact = result.m_hitPointWorld; + _steppingUp = false; + _onFloor = true; + _hovering = false; + } + updateTraction(nextTransform.getOrigin()); + return; + } // check if this hit is obviously unsteppable btVector3 hitFromBase = result.m_hitPointWorld - (startPosition - ((_radius + _halfHeight) * _upDirection)); @@ -189,6 +202,7 @@ void CharacterGhostObject::move(btScalar dt, btScalar overshoot) { // can stand on future landing spot, so we interpolate toward it _floorNormal = result.m_hitNormalWorld; _floorContact = result.m_hitPointWorld; + _steppingUp = true; _onFloor = true; _hovering = false; nextTransform.setOrigin(startTransform.getOrigin() + result.m_closestHitFraction * downSweep); @@ -337,11 +351,11 @@ void CharacterGhostObject::refreshOverlappingPairCache() { _world->getBroadphase()->setAabb(getBroadphaseHandle(), minAabb, maxAabb, _world->getDispatcher()); } -void CharacterGhostObject::updateVelocity(btScalar dt) { +void CharacterGhostObject::updateVelocity(btScalar dt, btScalar gravity) { if (_hovering) { _linearVelocity *= 0.999f; // HACK damping } else { - _linearVelocity += (dt * _gravity) * _upDirection; + _linearVelocity += (dt * gravity) * _upDirection; } } @@ -371,7 +385,7 @@ void CharacterGhostObject::updateTraction(const btVector3& position) { btVector3 pathDirection = leverArm.cross(_motorVelocity.cross(leverArm)); btScalar pathLength = pathDirection.length(); if (pathLength > FLT_EPSILON) { - _linearVelocity = (_motorSpeed / pathLength) * pathDirection; + _linearVelocity = (_motorVelocity.length() / pathLength) * pathDirection; } else { _linearVelocity = btVector3(0.0f, 0.0f, 0.0f); } diff --git a/libraries/physics/src/CharacterGhostObject.h b/libraries/physics/src/CharacterGhostObject.h index 274057a907..4b943833b3 100755 --- a/libraries/physics/src/CharacterGhostObject.h +++ b/libraries/physics/src/CharacterGhostObject.h @@ -33,18 +33,18 @@ public: void setRadiusAndHalfHeight(btScalar radius, btScalar halfHeight); void setUpDirection(const btVector3& up); - void setMotorVelocity(const btVector3& velocity); - void setGravity(btScalar gravity) { _gravity = gravity; } // NOTE: we expect _gravity to be negative (in _upDirection) + void setMotorVelocity(const btVector3& velocity) { _motorVelocity = velocity; } void setMinWallAngle(btScalar angle) { _maxWallNormalUpComponent = cosf(angle); } void setMaxStepHeight(btScalar height) { _maxStepHeight = height; } + void setLinearVelocity(const btVector3& velocity) { _linearVelocity = velocity; } const btVector3& getLinearVelocity() const { return _linearVelocity; } void setCharacterCapsule(btCapsuleShape* capsule); void setCollisionWorld(btCollisionWorld* world); - void move(btScalar dt, btScalar overshoot); + void move(btScalar dt, btScalar overshoot, btScalar gravity); bool sweepTest(const btConvexShape* shape, const btTransform& start, @@ -52,6 +52,10 @@ public: CharacterSweepResult& result) const; bool isHovering() const { return _hovering; } + void setHovering(bool hovering) { _hovering = hovering; } + + bool hasSupport() const { return _onFloor; } + bool isSteppingUp() const { return _steppingUp; } protected: void removeFromWorld(); @@ -63,7 +67,7 @@ protected: bool resolvePenetration(int numTries); void refreshOverlappingPairCache(); - void updateVelocity(btScalar dt); + void updateVelocity(btScalar dt, btScalar gravity); void updateTraction(const btVector3& position); btScalar measureAvailableStepHeight() const; void updateHoverState(const btVector3& position); @@ -78,8 +82,6 @@ protected: //btScalar _distanceToFeet { 0.0f }; // input, distance from object center to lowest point on shape btScalar _halfHeight { 0.0f }; btScalar _radius { 0.0f }; - btScalar _motorSpeed { 0.0f }; // internal, cached for speed - btScalar _gravity { 0.0f }; // input, amplitude of gravity along _upDirection (should be negative) btScalar _maxWallNormalUpComponent { 0.0f }; // input: max vertical component of wall normal btScalar _maxStepHeight { 0.0f }; // input, max step height the character can climb btCapsuleShape* _characterShape { nullptr }; // input, shape of character @@ -89,6 +91,7 @@ protected: bool _inWorld { false }; // internal, was added to world bool _hovering { false }; // internal, bool _onFloor { false }; // output, is actually standing on floor + bool _steppingUp { false }; // output, future sweep hit a steppable ledge bool _hasFloor { false }; // output, has floor underneath to fall on }; From 4fb6d5023b17bbef2121b81df3be5dbe2a1d867b Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 28 Sep 2016 16:56:51 -0700 Subject: [PATCH 071/101] prevent avatars from walking up walls --- libraries/physics/src/CharacterController.cpp | 66 ++++++++++++++----- .../physics/src/CharacterGhostObject.cpp | 22 +++++-- libraries/physics/src/CharacterGhostObject.h | 5 +- 3 files changed, 72 insertions(+), 21 deletions(-) diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 5058a1aa9c..0858c8a856 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -128,6 +128,7 @@ void CharacterController::setDynamicsWorld(btDynamicsWorld* world) { _ghost.setMaxStepHeight(0.75f * (_radius + _halfHeight)); // HACK _ghost.setMinWallAngle(PI / 4.0f); // HACK _ghost.setUpDirection(_currentUp); + _ghost.setMotorOnly(!_moveKinematically); _ghost.setWorldTransform(_rigidBody->getWorldTransform()); } if (_dynamicsWorld) { @@ -143,7 +144,24 @@ void CharacterController::setDynamicsWorld(btDynamicsWorld* world) { } bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld, btScalar dt) { + if (_moveKinematically) { + // kinematic motion will move() the _ghost later + return _ghost.hasSupport(); + } _stepHeight = _minStepHeight; // clears last step obstacle + + btScalar targetSpeed = _targetVelocity.length(); + if (targetSpeed > FLT_EPSILON) { + // move the _ghost forward to test for step + btTransform transform = _rigidBody->getWorldTransform(); + transform.setOrigin(transform.getOrigin()); + _ghost.setWorldTransform(transform); + _ghost.setMotorVelocity(_targetVelocity); + float overshoot = _radius; + _ghost.setHovering(_state == State::Hover); + _ghost.move(dt, overshoot, _gravity); + } + btDispatcher* dispatcher = collisionWorld->getDispatcher(); int numManifolds = dispatcher->getNumManifolds(); bool hasFloor = false; @@ -165,17 +183,26 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld, btSc btVector3 pointOnCharacter = characterIsFirst ? contact.m_localPointA : contact.m_localPointB; // object-local-frame btVector3 normal = characterIsFirst ? contact.m_normalWorldOnB : -contact.m_normalWorldOnB; // points toward character btScalar hitHeight = _halfHeight + _radius + pointOnCharacter.dot(_currentUp); - if (hitHeight < _radius && normal.dot(_currentUp) > COS_PI_OVER_THREE) { + if (hitHeight < _maxStepHeight && normal.dot(_currentUp) > COS_PI_OVER_THREE) { hasFloor = true; + if (!_ghost.isSteppingUp()) { + // early exit since all we need to know is that we're on a floor + break; + } } - // remember highest step obstacle - if (hitHeight > _maxStepHeight) { - // this manifold is invalidated by point that is too high - stepContactIndex = -1; - break; - } else if (hitHeight > highestStep && normal.dot(_targetVelocity) < 0.0f ) { - highestStep = hitHeight; - stepContactIndex = j; + // analysis of the step info using manifold data is unreliable, so we only proceed + // when the _ghost has detected a steppable obstacle + if (_ghost.isSteppingUp()) { + // remember highest step obstacle + if (hitHeight > _maxStepHeight) { + // this manifold is invalidated by point that is too high + stepContactIndex = -1; + break; + } else if (hitHeight > highestStep && normal.dot(_targetVelocity) < 0.0f ) { + highestStep = hitHeight; + stepContactIndex = j; + hasFloor = true; + } } } if (stepContactIndex > -1 && highestStep > _stepHeight) { @@ -187,6 +214,10 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld, btSc _stepPoint = rotation * pointOnCharacter; // rotate into world-frame _stepNormal = normal; } + if (hasFloor && !_ghost.isSteppingUp()) { + // early exit since all we need to know is that we're on a floor + break; + } } } return hasFloor; @@ -532,9 +563,13 @@ void CharacterController::applyMotor(int index, btScalar dt, btVector3& worldVel } else { // compute local UP btVector3 up = _currentUp.rotate(axis, -angle); - - // add sky hook when encountering a step obstacle btVector3 motorVelocity = motor.velocity; + + // save these non-adjusted components for later + btVector3 vTargetVelocity = motorVelocity.dot(up) * up; + btVector3 hTargetVelocity = motorVelocity - vTargetVelocity; + + // adjust motorVelocity uphill when encountering a step obstacle btScalar vTimescale = motor.vTimescale; if (_stepHeight > _minStepHeight) { // there is a step @@ -557,8 +592,8 @@ void CharacterController::applyMotor(int index, btScalar dt, btVector3& worldVel // split velocity into horizontal and vertical components btVector3 vVelocity = velocity.dot(up) * up; btVector3 hVelocity = velocity - vVelocity; - btVector3 vTargetVelocity = motorVelocity.dot(up) * up; - btVector3 hTargetVelocity = motorVelocity - vTargetVelocity; + btVector3 vMotorVelocity = motorVelocity.dot(up) * up; + btVector3 hMotorVelocity = motorVelocity - vMotorVelocity; // modify each component separately btScalar maxTau = 0.0f; @@ -568,7 +603,7 @@ void CharacterController::applyMotor(int index, btScalar dt, btVector3& worldVel tau = 1.0f; } maxTau = tau; - hVelocity += (hTargetVelocity - hVelocity) * tau; + hVelocity += (hMotorVelocity - hVelocity) * tau; } if (vTimescale < MAX_CHARACTER_MOTOR_TIMESCALE) { btScalar tau = dt / vTimescale; @@ -578,7 +613,7 @@ void CharacterController::applyMotor(int index, btScalar dt, btVector3& worldVel if (tau > maxTau) { maxTau = tau; } - vVelocity += (vTargetVelocity - vVelocity) * tau; + vVelocity += (vMotorVelocity - vVelocity) * tau; } // add components back together and rotate into world-frame @@ -819,5 +854,6 @@ void CharacterController::setMoveKinematically(bool kinematic) { if (kinematic != _moveKinematically) { _moveKinematically = kinematic; _pendingFlags |= PENDING_FLAG_UPDATE_SHAPE; + _ghost.setMotorOnly(!_moveKinematically); } } diff --git a/libraries/physics/src/CharacterGhostObject.cpp b/libraries/physics/src/CharacterGhostObject.cpp index f39b9c3995..52689a2328 100755 --- a/libraries/physics/src/CharacterGhostObject.cpp +++ b/libraries/physics/src/CharacterGhostObject.cpp @@ -54,6 +54,13 @@ void CharacterGhostObject::setUpDirection(const btVector3& up) { } } +void CharacterGhostObject::setMotorVelocity(const btVector3& velocity) { + _motorVelocity = velocity; + if (_motorOnly) { + _linearVelocity = _motorVelocity; + } +} + // override of btCollisionObject::setCollisionShape() void CharacterGhostObject::setCharacterCapsule(btCapsuleShape* capsule) { assert(capsule); @@ -74,6 +81,7 @@ void CharacterGhostObject::setCollisionWorld(btCollisionWorld* world) { } void CharacterGhostObject::move(btScalar dt, btScalar overshoot, btScalar gravity) { + bool oldOnFloor = _onFloor; _onFloor = false; _steppingUp = false; assert(_world && _inWorld); @@ -172,6 +180,7 @@ void CharacterGhostObject::move(btScalar dt, btScalar overshoot, btScalar gravit } nextTransform.setOrigin(startPosition + forwardTranslation); setWorldTransform(nextTransform); + _onFloor = _onFloor || oldOnFloor; return; } // if we get here then we hit something that might be steppable @@ -213,6 +222,7 @@ void CharacterGhostObject::move(btScalar dt, btScalar overshoot, btScalar gravit // either there is no future landing spot, or there is but we can't stand on it // in any case: we go forward as much as possible nextTransform.setOrigin(startPosition + forwardSweepHitFraction * (stepDistance / longSweepDistance) * forwardSweep); + _onFloor = _onFloor || oldOnFloor; updateTraction(nextTransform.getOrigin()); } setWorldTransform(nextTransform); @@ -352,10 +362,12 @@ void CharacterGhostObject::refreshOverlappingPairCache() { } void CharacterGhostObject::updateVelocity(btScalar dt, btScalar gravity) { - if (_hovering) { - _linearVelocity *= 0.999f; // HACK damping - } else { - _linearVelocity += (dt * gravity) * _upDirection; + if (!_motorOnly) { + if (_hovering) { + _linearVelocity *= 0.999f; // HACK damping + } else { + _linearVelocity += (dt * gravity) * _upDirection; + } } } @@ -377,7 +389,7 @@ void CharacterGhostObject::updateHoverState(const btVector3& position) { void CharacterGhostObject::updateTraction(const btVector3& position) { updateHoverState(position); - if (_hovering) { + if (_hovering || _motorOnly) { _linearVelocity = _motorVelocity; } else if (_onFloor) { // compute a velocity that swings the capsule around the _floorContact diff --git a/libraries/physics/src/CharacterGhostObject.h b/libraries/physics/src/CharacterGhostObject.h index 4b943833b3..6d6de640d5 100755 --- a/libraries/physics/src/CharacterGhostObject.h +++ b/libraries/physics/src/CharacterGhostObject.h @@ -33,7 +33,7 @@ public: void setRadiusAndHalfHeight(btScalar radius, btScalar halfHeight); void setUpDirection(const btVector3& up); - void setMotorVelocity(const btVector3& velocity) { _motorVelocity = velocity; } + void setMotorVelocity(const btVector3& velocity); void setMinWallAngle(btScalar angle) { _maxWallNormalUpComponent = cosf(angle); } void setMaxStepHeight(btScalar height) { _maxStepHeight = height; } @@ -53,9 +53,11 @@ public: bool isHovering() const { return _hovering; } void setHovering(bool hovering) { _hovering = hovering; } + void setMotorOnly(bool motorOnly) { _motorOnly = motorOnly; } bool hasSupport() const { return _onFloor; } bool isSteppingUp() const { return _steppingUp; } + const btVector3& getFloorNormal() const { return _floorNormal; } protected: void removeFromWorld(); @@ -93,6 +95,7 @@ protected: bool _onFloor { false }; // output, is actually standing on floor bool _steppingUp { false }; // output, future sweep hit a steppable ledge bool _hasFloor { false }; // output, has floor underneath to fall on + bool _motorOnly { false }; // input, _linearVelocity slaves to _motorVelocity }; #endif // hifi_CharacterGhostObject_h From 681bdf4c2385cda288191c3774b5b002cd3abe18 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 29 Sep 2016 17:39:59 -0700 Subject: [PATCH 072/101] can scan for penetration at arbitrary positions --- libraries/physics/src/CharacterController.cpp | 10 +++ libraries/physics/src/CharacterController.h | 2 + .../physics/src/CharacterGhostObject.cpp | 87 ++++++++++--------- libraries/physics/src/CharacterGhostObject.h | 3 + 4 files changed, 63 insertions(+), 39 deletions(-) diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 0858c8a856..a57d2b17a8 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -857,3 +857,13 @@ void CharacterController::setMoveKinematically(bool kinematic) { _ghost.setMotorOnly(!_moveKinematically); } } + +bool CharacterController::queryPenetration(const btTransform& transform) { + btVector3 minBox; + btVector3 maxBox; + _ghost.queryPenetration(transform, minBox, maxBox); + btVector3 penetration = maxBox; + penetration.setMax(minBox.absolute()); + const btScalar MIN_PENETRATION_SQUARED = 0.0016f; // 0.04^2 + return penetration.length2() > MIN_PENETRATION_SQUARED; +} diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h index 69168699eb..8c690766e6 100644 --- a/libraries/physics/src/CharacterController.h +++ b/libraries/physics/src/CharacterController.h @@ -119,6 +119,8 @@ public: float measureMaxHipsOffsetRadius(const glm::vec3& currentHipsOffset, float maxSweepDistance); void setMoveKinematically(bool kinematic); // KINEMATIC_CONTROLLER_HACK + bool queryPenetration(const btTransform& transform); + protected: #ifdef DEBUG_STATE_CHANGE void setState(State state, const char* reason); diff --git a/libraries/physics/src/CharacterGhostObject.cpp b/libraries/physics/src/CharacterGhostObject.cpp index 52689a2328..07dc986c9e 100755 --- a/libraries/physics/src/CharacterGhostObject.cpp +++ b/libraries/physics/src/CharacterGhostObject.cpp @@ -242,51 +242,30 @@ bool CharacterGhostObject::sweepTest( return false; } -void CharacterGhostObject::removeFromWorld() { - if (_world && _inWorld) { - _world->removeCollisionObject(this); - _inWorld = false; - } +void CharacterGhostObject::queryPenetration(const btTransform& transform, btVector3& minBoxOut, btVector3& maxBoxOut) { + // place in world and refresh overlapping pairs + setWorldTransform(transform); + measurePenetration(minBoxOut, maxBoxOut); } -void CharacterGhostObject::addToWorld() { - if (_world && !_inWorld) { - assert(getCollisionShape()); - setCollisionFlags(getCollisionFlags() | btCollisionObject::CF_NO_CONTACT_RESPONSE); - //assert(getBroadphaseHandle()); - //int16_t group = getBroadphaseHandle()->m_collisionFilterGroup; - //int16_t mask = getBroadphaseHandle()->m_collisionFilterMask; - _world->addCollisionObject(this, _collisionFilterGroup, _collisionFilterMask); - _inWorld = true; - } -} +void CharacterGhostObject::measurePenetration(btVector3& minBoxOut, btVector3& maxBoxOut) { + // minBoxOut and maxBoxOut will be updated with penetration envelope. + // If one of the points is at <0,0,0> then the penetration may be resolvable in a single step, + // but if the space spanned by those two corners extends in both directions along at least one + // component then this object is sandwiched between two opposing objects. -bool CharacterGhostObject::rayTest(const btVector3& start, - const btVector3& end, - CharacterRayResult& result) const { - if (_world && _inWorld) { - _world->rayTest(start, end, result); - } - return result.hasHit(); -} - -bool CharacterGhostObject::resolvePenetration(int numTries) { - assert(_world); - // We refresh the overlapping pairCache because any previous movement may have pushed us - // into an overlap that was not in the cache. + // We assume this object has just been moved to its current location, or else objects have been + // moved around it since the last step so we must update the overlapping pairs. refreshOverlappingPairCache(); // compute collision details btHashedOverlappingPairCache* pairCache = getOverlappingPairCache(); _world->getDispatcher()->dispatchAllCollisionPairs(pairCache, _world->getDispatchInfo(), _world->getDispatcher()); - // loop over contact manifolds - btTransform transform = getWorldTransform(); - btVector3 position = transform.getOrigin(); - btVector3 minBox =btVector3(0.0f, 0.0f, 0.0f); - btVector3 maxBox = btVector3(0.0f, 0.0f, 0.0f); + // loop over contact manifolds to compute the penetration box + minBoxOut = btVector3(0.0f, 0.0f, 0.0f); + maxBoxOut = btVector3(0.0f, 0.0f, 0.0f); btManifoldArray manifoldArray; - const btScalar PENETRATION_RESOLUTION_FUDGE_FACTOR = 0.0001f; // speeds up resolvation int numPairs = pairCache->getNumOverlappingPairs(); for (int i = 0; i < numPairs; i++) { @@ -337,16 +316,46 @@ bool CharacterGhostObject::resolvePenetration(int numTries) { } } - btVector3 penetration = (-penetrationDepth + PENETRATION_RESOLUTION_FUDGE_FACTOR) * normal; - minBox.setMin(penetration); - maxBox.setMax(penetration); + btVector3 penetration = (-penetrationDepth) * normal; + minBoxOut.setMin(penetration); + maxBoxOut.setMax(penetration); } } } +} +void CharacterGhostObject::removeFromWorld() { + if (_world && _inWorld) { + _world->removeCollisionObject(this); + _inWorld = false; + } +} + +void CharacterGhostObject::addToWorld() { + if (_world && !_inWorld) { + assert(getCollisionShape()); + setCollisionFlags(getCollisionFlags() | btCollisionObject::CF_NO_CONTACT_RESPONSE); + _world->addCollisionObject(this, _collisionFilterGroup, _collisionFilterMask); + _inWorld = true; + } +} + +bool CharacterGhostObject::rayTest(const btVector3& start, + const btVector3& end, + CharacterRayResult& result) const { + if (_world && _inWorld) { + _world->rayTest(start, end, result); + } + return result.hasHit(); +} + +bool CharacterGhostObject::resolvePenetration(int numTries) { + btVector3 minBox, maxBox; + measurePenetration(minBox, maxBox); btVector3 restore = maxBox + minBox; if (restore.length2() > 0.0f) { - transform.setOrigin(position + restore); + btTransform transform = getWorldTransform(); + transform.setOrigin(transform.getOrigin() + restore); setWorldTransform(transform); return false; } diff --git a/libraries/physics/src/CharacterGhostObject.h b/libraries/physics/src/CharacterGhostObject.h index 6d6de640d5..9e3c5bb433 100755 --- a/libraries/physics/src/CharacterGhostObject.h +++ b/libraries/physics/src/CharacterGhostObject.h @@ -59,6 +59,9 @@ public: bool isSteppingUp() const { return _steppingUp; } const btVector3& getFloorNormal() const { return _floorNormal; } + void queryPenetration(const btTransform& transform, btVector3& minBoxOut, btVector3& maxBoxOut); + void measurePenetration(btVector3& minBoxOut, btVector3& maxBoxOut); + protected: void removeFromWorld(); void addToWorld(); From cd61f55c4ae605f5d9f89aa3520cf37002feb6c3 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 30 Sep 2016 16:52:20 -0700 Subject: [PATCH 073/101] avatar jumps to HMD position at large offset if the avatar has a large offset to HMD position it will teleport to its target position when that jump does not put it into a state of penetration --- libraries/physics/src/CharacterController.cpp | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index a57d2b17a8..31cfa5b597 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -11,8 +11,6 @@ #include "CharacterController.h" -//#include - #include #include "ObjectMotionState.h" @@ -487,10 +485,21 @@ void CharacterController::setFollowParameters(const glm::mat4& desiredWorldBodyM const float dontDivideByZero = 0.001f; float newSpeed = newFollowVelocity.length() + dontDivideByZero; float oldSpeed = _followVelocity.length(); + + bool successfulSnap = false; + btVector3 offset = _followDesiredBodyTransform.getOrigin() - _position; + const btScalar MAX_FOLLOW_OFFSET = 10.0f * _radius; + if (offset.length() > MAX_FOLLOW_OFFSET) { + successfulSnap = queryPenetration(_followDesiredBodyTransform); + if (successfulSnap) { + _position = _followDesiredBodyTransform.getOrigin(); + } + } + const float VERY_SLOW_HOVER_SPEED = 0.25f; const float FAST_CHANGE_SPEED_RATIO = 100.0f; - if (oldSpeed / newSpeed > FAST_CHANGE_SPEED_RATIO && newSpeed < VERY_SLOW_HOVER_SPEED) { - // avatar is stopping quickly + if (successfulSnap || (oldSpeed / newSpeed > FAST_CHANGE_SPEED_RATIO && newSpeed < VERY_SLOW_HOVER_SPEED)) { + // character is snapping to avatar position or avatar is stopping quickly // HACK: slam _followVelocity and _rigidBody velocity immediately _followVelocity = newFollowVelocity; _rigidBody->setLinearVelocity(_followVelocity); @@ -865,5 +874,5 @@ bool CharacterController::queryPenetration(const btTransform& transform) { btVector3 penetration = maxBox; penetration.setMax(minBox.absolute()); const btScalar MIN_PENETRATION_SQUARED = 0.0016f; // 0.04^2 - return penetration.length2() > MIN_PENETRATION_SQUARED; + return penetration.length2() < MIN_PENETRATION_SQUARED; } From 9544c749ea6d30ee63ec0e902fcfc1bd40d4bc7d Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 4 Oct 2016 15:01:03 -0700 Subject: [PATCH 074/101] cleanup --- libraries/physics/src/CharacterController.cpp | 7 ++++--- libraries/physics/src/CharacterGhostObject.cpp | 12 +++--------- libraries/physics/src/CharacterGhostObject.h | 1 - 3 files changed, 7 insertions(+), 13 deletions(-) diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 31cfa5b597..420435b245 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -870,9 +870,10 @@ void CharacterController::setMoveKinematically(bool kinematic) { bool CharacterController::queryPenetration(const btTransform& transform) { btVector3 minBox; btVector3 maxBox; - _ghost.queryPenetration(transform, minBox, maxBox); - btVector3 penetration = maxBox; - penetration.setMax(minBox.absolute()); + _ghost.setWorldTransform(transform); + _ghost.measurePenetration(minBox, maxBox); + btVector3 penetration = minBox; + penetration.setMax(maxBox.absolute()); const btScalar MIN_PENETRATION_SQUARED = 0.0016f; // 0.04^2 return penetration.length2() < MIN_PENETRATION_SQUARED; } diff --git a/libraries/physics/src/CharacterGhostObject.cpp b/libraries/physics/src/CharacterGhostObject.cpp index 07dc986c9e..328d869f01 100755 --- a/libraries/physics/src/CharacterGhostObject.cpp +++ b/libraries/physics/src/CharacterGhostObject.cpp @@ -242,17 +242,11 @@ bool CharacterGhostObject::sweepTest( return false; } -void CharacterGhostObject::queryPenetration(const btTransform& transform, btVector3& minBoxOut, btVector3& maxBoxOut) { - // place in world and refresh overlapping pairs - setWorldTransform(transform); - measurePenetration(minBoxOut, maxBoxOut); -} - void CharacterGhostObject::measurePenetration(btVector3& minBoxOut, btVector3& maxBoxOut) { // minBoxOut and maxBoxOut will be updated with penetration envelope. - // If one of the points is at <0,0,0> then the penetration may be resolvable in a single step, - // but if the space spanned by those two corners extends in both directions along at least one - // component then this object is sandwiched between two opposing objects. + // If one of the corner points is <0,0,0> then the penetration is resolvable in a single step, + // but if the space spanned by the two corners extends in both directions along at least one + // component then we the object is sandwiched between two opposing objects. // We assume this object has just been moved to its current location, or else objects have been // moved around it since the last step so we must update the overlapping pairs. diff --git a/libraries/physics/src/CharacterGhostObject.h b/libraries/physics/src/CharacterGhostObject.h index 9e3c5bb433..87310714c8 100755 --- a/libraries/physics/src/CharacterGhostObject.h +++ b/libraries/physics/src/CharacterGhostObject.h @@ -59,7 +59,6 @@ public: bool isSteppingUp() const { return _steppingUp; } const btVector3& getFloorNormal() const { return _floorNormal; } - void queryPenetration(const btTransform& transform, btVector3& minBoxOut, btVector3& maxBoxOut); void measurePenetration(btVector3& minBoxOut, btVector3& maxBoxOut); protected: From 084a1cf8ae69dad7c2f27e340434c553ccfdfe23 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 5 Oct 2016 10:59:30 -0700 Subject: [PATCH 075/101] fix avatar step-up behavior -- eliminate hop --- libraries/physics/src/CharacterController.cpp | 50 +++++++------------ libraries/physics/src/CharacterController.h | 7 --- 2 files changed, 18 insertions(+), 39 deletions(-) diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 420435b245..5a6ccac5ca 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -146,7 +146,10 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld, btSc // kinematic motion will move() the _ghost later return _ghost.hasSupport(); } - _stepHeight = _minStepHeight; // clears last step obstacle + btScalar minStepHeight = 0.041f; // HACK: hardcoded now but should be shape margin + btScalar maxStepHeight = 0.75f * (_halfHeight + _radius); + btScalar stepHeight = minStepHeight; + btVector3 stepNormal = btVector3(0.0f, 0.0f, 0.0f); btScalar targetSpeed = _targetVelocity.length(); if (targetSpeed > FLT_EPSILON) { @@ -174,14 +177,14 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld, btSc bool characterIsFirst = _rigidBody == contactManifold->getBody0(); int numContacts = contactManifold->getNumContacts(); int stepContactIndex = -1; - float highestStep = _minStepHeight; + float highestStep = minStepHeight; for (int j = 0; j < numContacts; j++) { // check for "floor" btManifoldPoint& contact = contactManifold->getContactPoint(j); btVector3 pointOnCharacter = characterIsFirst ? contact.m_localPointA : contact.m_localPointB; // object-local-frame btVector3 normal = characterIsFirst ? contact.m_normalWorldOnB : -contact.m_normalWorldOnB; // points toward character btScalar hitHeight = _halfHeight + _radius + pointOnCharacter.dot(_currentUp); - if (hitHeight < _maxStepHeight && normal.dot(_currentUp) > COS_PI_OVER_THREE) { + if (hitHeight < maxStepHeight && normal.dot(_currentUp) > COS_PI_OVER_THREE) { hasFloor = true; if (!_ghost.isSteppingUp()) { // early exit since all we need to know is that we're on a floor @@ -192,7 +195,7 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld, btSc // when the _ghost has detected a steppable obstacle if (_ghost.isSteppingUp()) { // remember highest step obstacle - if (hitHeight > _maxStepHeight) { + if (hitHeight > maxStepHeight) { // this manifold is invalidated by point that is too high stepContactIndex = -1; break; @@ -203,14 +206,13 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld, btSc } } } - if (stepContactIndex > -1 && highestStep > _stepHeight) { + if (stepContactIndex > -1 && highestStep > stepHeight) { // remember step info for later btManifoldPoint& contact = contactManifold->getContactPoint(stepContactIndex); btVector3 pointOnCharacter = characterIsFirst ? contact.m_localPointA : contact.m_localPointB; // object-local-frame btVector3 normal = characterIsFirst ? contact.m_normalWorldOnB : -contact.m_normalWorldOnB; // points toward character - _stepHeight = highestStep; - _stepPoint = rotation * pointOnCharacter; // rotate into world-frame - _stepNormal = normal; + stepHeight = highestStep; + stepNormal = normal; } if (hasFloor && !_ghost.isSteppingUp()) { // early exit since all we need to know is that we're on a floor @@ -218,6 +220,12 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld, btSc } } } + if (_ghost.isSteppingUp() && stepHeight > minStepHeight && _targetVelocity.dot(stepNormal) < 0.0f) { + // move avatar up according to kinematic character logic + btTransform transform = _rigidBody->getWorldTransform(); + transform.setOrigin(_ghost.getWorldTransform().getOrigin()); + _rigidBody->setWorldTransform(transform); + } return hasFloor; } @@ -408,8 +416,6 @@ void CharacterController::setLocalBoundingBox(const glm::vec3& minCorner, const if (glm::abs(radius - _radius) > FLT_EPSILON || glm::abs(halfHeight - _halfHeight) > FLT_EPSILON) { _radius = radius; _halfHeight = halfHeight; - _minStepHeight = 0.041f; // HACK: hardcoded now but should be shape margin - _maxStepHeight = 0.75f * (_halfHeight + _radius); if (_dynamicsWorld) { // must REMOVE from world prior to shape update @@ -578,26 +584,6 @@ void CharacterController::applyMotor(int index, btScalar dt, btVector3& worldVel btVector3 vTargetVelocity = motorVelocity.dot(up) * up; btVector3 hTargetVelocity = motorVelocity - vTargetVelocity; - // adjust motorVelocity uphill when encountering a step obstacle - btScalar vTimescale = motor.vTimescale; - if (_stepHeight > _minStepHeight) { - // there is a step - btVector3 motorVelocityWF = motorVelocity.rotate(axis, angle); - if (motorVelocityWF.dot(_stepNormal) < 0.0f) { - // the motor pushes against step - btVector3 leverArm = _stepPoint; - motorVelocityWF = _stepNormal.cross(leverArm.cross(motorVelocityWF)); - btScalar doubleCrossLength = motorVelocityWF.length(); - if (doubleCrossLength > FLT_EPSILON) { - // scale the motor in the correct direction and rotate back to motor-frame - motorVelocityWF *= (motorVelocity.length() / doubleCrossLength); - motorVelocity += motorVelocityWF.rotate(axis, -angle); - // make vTimescale as small as possible - vTimescale = glm::min(vTimescale, motor.hTimescale); - } - } - } - // split velocity into horizontal and vertical components btVector3 vVelocity = velocity.dot(up) * up; btVector3 hVelocity = velocity - vVelocity; @@ -614,8 +600,8 @@ void CharacterController::applyMotor(int index, btScalar dt, btVector3& worldVel maxTau = tau; hVelocity += (hMotorVelocity - hVelocity) * tau; } - if (vTimescale < MAX_CHARACTER_MOTOR_TIMESCALE) { - btScalar tau = dt / vTimescale; + if (motor.vTimescale < MAX_CHARACTER_MOTOR_TIMESCALE) { + btScalar tau = dt / motor.vTimescale; if (tau > 1.0f) { tau = 1.0f; } diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h index 8c690766e6..27ffa055f0 100644 --- a/libraries/physics/src/CharacterController.h +++ b/libraries/physics/src/CharacterController.h @@ -165,13 +165,6 @@ protected: quint32 _jumpButtonDownCount; quint32 _takeoffJumpButtonID; - // data for walking up steps - btVector3 _stepPoint; - btVector3 _stepNormal; - btScalar _stepHeight { 0.0f }; - btScalar _minStepHeight { 0.0f }; - btScalar _maxStepHeight { 0.0f }; - btScalar _halfHeight { 0.0f }; btScalar _radius { 0.0f }; From 5154e98a2e41903f8aff3f8e8b85d846720fd47f Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 5 Oct 2016 13:43:22 -0700 Subject: [PATCH 076/101] remove warning about unused variable --- libraries/physics/src/CharacterController.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 5a6ccac5ca..0f6e62d02d 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -209,7 +209,6 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld, btSc if (stepContactIndex > -1 && highestStep > stepHeight) { // remember step info for later btManifoldPoint& contact = contactManifold->getContactPoint(stepContactIndex); - btVector3 pointOnCharacter = characterIsFirst ? contact.m_localPointA : contact.m_localPointB; // object-local-frame btVector3 normal = characterIsFirst ? contact.m_normalWorldOnB : -contact.m_normalWorldOnB; // points toward character stepHeight = highestStep; stepNormal = normal; From 2fdd5e43680ca1184ee005a19a03083d89501528 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Wed, 5 Oct 2016 17:57:48 -0700 Subject: [PATCH 077/101] Build fixes for change in getMyAvatar() from DepenencyManager It now returns a smart_ptr, not a raw pointer. --- interface/src/Menu.cpp | 8 ++++---- interface/src/scripting/HMDScriptingInterface.cpp | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/interface/src/Menu.cpp b/interface/src/Menu.cpp index fabe8b570c..9bdb7a0792 100644 --- a/interface/src/Menu.cpp +++ b/interface/src/Menu.cpp @@ -517,9 +517,9 @@ Menu::Menu() { addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::RenderOtherLookAtVectors, 0, false); addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::FixGaze, 0, false); addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::DisplayLeftFootTrace, 0, false, - avatar, SLOT(setEnableDebugDrawLeftFootTrace(bool))); + avatar.get(), SLOT(setEnableDebugDrawLeftFootTrace(bool))); addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::DisplayRightFootTrace, 0, false, - avatar, SLOT(setEnableDebugDrawRightFootTrace(bool))); + avatar.get(), SLOT(setEnableDebugDrawRightFootTrace(bool))); addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::AnimDebugDrawDefaultPose, 0, false, avatar.get(), SLOT(setEnableDebugDrawDefaultPose(bool))); addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::AnimDebugDrawAnimPose, 0, false, @@ -553,11 +553,11 @@ Menu::Menu() { // KINEMATIC_CONTROLLER_HACK addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::MoveKinematically, 0, false, - avatar, SLOT(updateMotionBehaviorFromMenu()), + avatar.get(), SLOT(updateMotionBehaviorFromMenu()), UNSPECIFIED_POSITION, "Developer"); addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::EnableVerticalComfortMode, 0, false, - avatar, SLOT(setEnableVerticalComfortMode(bool))); + avatar.get(), SLOT(setEnableVerticalComfortMode(bool))); // Developer > Hands >>> MenuWrapper* handOptionsMenu = developerMenu->addMenu("Hands"); diff --git a/interface/src/scripting/HMDScriptingInterface.cpp b/interface/src/scripting/HMDScriptingInterface.cpp index 98dbf3ae07..54f8ed089d 100644 --- a/interface/src/scripting/HMDScriptingInterface.cpp +++ b/interface/src/scripting/HMDScriptingInterface.cpp @@ -95,7 +95,7 @@ void HMDScriptingInterface::setPosition(const glm::vec3& position) { QMetaObject::invokeMethod(this, "setPosition", Qt::QueuedConnection, Q_ARG(const glm::vec3&, position)); return; } else { - MyAvatar* myAvatar = DependencyManager::get()->getMyAvatar(); + auto myAvatar = DependencyManager::get()->getMyAvatar(); glm::mat4 hmdToSensor = myAvatar->getHMDSensorMatrix(); glm::mat4 sensorToWorld = myAvatar->getSensorToWorldMatrix(); glm::mat4 hmdToWorld = sensorToWorld * hmdToSensor; @@ -156,6 +156,6 @@ void HMDScriptingInterface::centerUI() { } void HMDScriptingInterface::snapToAvatar() { - MyAvatar* myAvatar = DependencyManager::get()->getMyAvatar(); + auto myAvatar = DependencyManager::get()->getMyAvatar(); myAvatar->updateSensorToWorldMatrix(); } From b0c7564b604bba17a0bf2191fc471c8e9b108c10 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Wed, 5 Oct 2016 18:06:49 -0700 Subject: [PATCH 078/101] Fix for debug assert in glm::lerp() --- interface/src/avatar/MyAvatar.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 4587ef065f..4ee20088fa 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -2318,7 +2318,7 @@ glm::mat4 MyAvatar::FollowHelper::prePhysicsUpdate(const MyAvatar& myAvatar, con if (isActive() || hasDriveInput) { const float TIMESCALE = 0.2f; - const float tau = deltaTime / TIMESCALE; + const float tau = glm::clamp(deltaTime / TIMESCALE, 0.0f, 1.0f); AnimPose newBodyPose; blend(1, ¤tBodyPose, &followBodyPose, tau, &newBodyPose); return (glm::mat4)newBodyPose; From ef465d44438cbb05a48822db817f12f672443883 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 11 Oct 2016 16:37:29 -0700 Subject: [PATCH 079/101] prevent avatar from recentering below ground --- interface/src/avatar/MyAvatar.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 4ee20088fa..48613049f3 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -251,6 +251,15 @@ void MyAvatar::centerBody() { auto worldBodyPos = extractTranslation(worldBodyMatrix); auto worldBodyRot = glm::normalize(glm::quat_cast(worldBodyMatrix)); + if (_characterController.getState() == CharacterController::State::Ground) { + // the avatar's physical aspect thinks it is standing on something + // therefore need to be careful to not "center" the body below the floor + float downStep = glm::dot(worldBodyPos - getPosition(), _worldUpDirection); + if (downStep < -0.5f * _characterController.getCapsuleHalfHeight() + _characterController.getCapsuleRadius()) { + worldBodyPos -= downStep * _worldUpDirection; + } + } + // this will become our new position. setPosition(worldBodyPos); setOrientation(worldBodyRot); From 35aebd21e661559ca4fd5ea49be5c73004bacfc4 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Thu, 13 Oct 2016 18:10:53 -0700 Subject: [PATCH 080/101] Fix for upward jerk, when switching between domains. --- libraries/physics/src/CharacterController.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 0f6e62d02d..54f6aaf0b8 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -507,7 +507,8 @@ void CharacterController::setFollowParameters(const glm::mat4& desiredWorldBodyM // character is snapping to avatar position or avatar is stopping quickly // HACK: slam _followVelocity and _rigidBody velocity immediately _followVelocity = newFollowVelocity; - _rigidBody->setLinearVelocity(_followVelocity); + newFollowVelocity.setY(0.0f); + _rigidBody->setLinearVelocity(newFollowVelocity); } else { // use simple blending to filter noise from the velocity measurement const float blend = 0.2f; From 6125357104dbf2f9170b13802f1902fbc92c83ae Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 14 Oct 2016 22:35:37 -0700 Subject: [PATCH 081/101] restore avatar steps up without sweeps --- libraries/physics/src/CharacterController.cpp | 91 +++++++++++-------- libraries/physics/src/CharacterController.h | 8 ++ 2 files changed, 62 insertions(+), 37 deletions(-) diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 54f6aaf0b8..07442b4f45 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -146,22 +146,8 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld, btSc // kinematic motion will move() the _ghost later return _ghost.hasSupport(); } - btScalar minStepHeight = 0.041f; // HACK: hardcoded now but should be shape margin - btScalar maxStepHeight = 0.75f * (_halfHeight + _radius); - btScalar stepHeight = minStepHeight; - btVector3 stepNormal = btVector3(0.0f, 0.0f, 0.0f); - btScalar targetSpeed = _targetVelocity.length(); - if (targetSpeed > FLT_EPSILON) { - // move the _ghost forward to test for step - btTransform transform = _rigidBody->getWorldTransform(); - transform.setOrigin(transform.getOrigin()); - _ghost.setWorldTransform(transform); - _ghost.setMotorVelocity(_targetVelocity); - float overshoot = _radius; - _ghost.setHovering(_state == State::Hover); - _ghost.move(dt, overshoot, _gravity); - } + bool pushing = _targetVelocity.length2() > FLT_EPSILON; btDispatcher* dispatcher = collisionWorld->getDispatcher(); int numManifolds = dispatcher->getNumManifolds(); @@ -177,25 +163,25 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld, btSc bool characterIsFirst = _rigidBody == contactManifold->getBody0(); int numContacts = contactManifold->getNumContacts(); int stepContactIndex = -1; - float highestStep = minStepHeight; + float highestStep = _minStepHeight; for (int j = 0; j < numContacts; j++) { // check for "floor" btManifoldPoint& contact = contactManifold->getContactPoint(j); btVector3 pointOnCharacter = characterIsFirst ? contact.m_localPointA : contact.m_localPointB; // object-local-frame btVector3 normal = characterIsFirst ? contact.m_normalWorldOnB : -contact.m_normalWorldOnB; // points toward character btScalar hitHeight = _halfHeight + _radius + pointOnCharacter.dot(_currentUp); - if (hitHeight < maxStepHeight && normal.dot(_currentUp) > COS_PI_OVER_THREE) { + if (hitHeight < _maxStepHeight && normal.dot(_currentUp) > COS_PI_OVER_THREE) { + //std::cout << "adebug manifoldIndex = " << i << " contactIndex = " << j << " hitOnCharacter*up = " << pointOnCharacter.dot(_currentUp) << std::endl; // adebug hasFloor = true; - if (!_ghost.isSteppingUp()) { - // early exit since all we need to know is that we're on a floor + if (!pushing) { + // we're not pushing against anything so we can early exit + // (all we need to know is that there is a floor) break; } } - // analysis of the step info using manifold data is unreliable, so we only proceed - // when the _ghost has detected a steppable obstacle - if (_ghost.isSteppingUp()) { + if (pushing && _targetVelocity.dot(normal) < 0.0f) { // remember highest step obstacle - if (hitHeight > maxStepHeight) { + if (hitHeight > _maxStepHeight) { // this manifold is invalidated by point that is too high stepContactIndex = -1; break; @@ -206,25 +192,20 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld, btSc } } } - if (stepContactIndex > -1 && highestStep > stepHeight) { + if (stepContactIndex > -1 && highestStep > _stepHeight) { // remember step info for later btManifoldPoint& contact = contactManifold->getContactPoint(stepContactIndex); - btVector3 normal = characterIsFirst ? contact.m_normalWorldOnB : -contact.m_normalWorldOnB; // points toward character - stepHeight = highestStep; - stepNormal = normal; + btVector3 pointOnCharacter = characterIsFirst ? contact.m_localPointA : contact.m_localPointB; // object-local-frame + _stepNormal = characterIsFirst ? contact.m_normalWorldOnB : -contact.m_normalWorldOnB; // points toward character + _stepHeight = highestStep; + _stepPoint = rotation * pointOnCharacter; // rotate into world-frame } - if (hasFloor && !_ghost.isSteppingUp()) { + if (hasFloor && !pushing) { // early exit since all we need to know is that we're on a floor break; } } } - if (_ghost.isSteppingUp() && stepHeight > minStepHeight && _targetVelocity.dot(stepNormal) < 0.0f) { - // move avatar up according to kinematic character logic - btTransform transform = _rigidBody->getWorldTransform(); - transform.setOrigin(_ghost.getWorldTransform().getOrigin()); - _rigidBody->setWorldTransform(transform); - } return hasFloor; } @@ -259,6 +240,7 @@ const btScalar MIN_TARGET_SPEED = 0.001f; const btScalar MIN_TARGET_SPEED_SQUARED = MIN_TARGET_SPEED * MIN_TARGET_SPEED; void CharacterController::playerStep(btCollisionWorld* collisionWorld, btScalar dt) { + _stepHeight = _minStepHeight; // clears memory of last step obstacle btVector3 velocity = _rigidBody->getLinearVelocity() - _parentVelocity; if (_following) { _followTimeAccumulator += dt; @@ -336,10 +318,25 @@ void CharacterController::playerStep(btCollisionWorld* collisionWorld, btScalar _rigidBody->setWorldTransform(transform); _rigidBody->setLinearVelocity(_ghost.getLinearVelocity()); } else { - // Dynamicaly compute a follow velocity to move this body toward the _followDesiredBodyTransform. - // 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(). + float stepUpSpeed2 = _stepUpVelocity.length2(); + if (stepUpSpeed2 > FLT_EPSILON) { + // we step up with teleports rather than applying velocity + // use a speed that would ballistically reach _stepHeight under gravity + _stepUpVelocity /= sqrtf(stepUpSpeed2); + btScalar minStepUpSpeed = sqrtf(fabsf(2.0f * _gravity * _stepHeight)); + btTransform transform = _rigidBody->getWorldTransform(); + transform.setOrigin(transform.getOrigin() + (dt * minStepUpSpeed) * _stepUpVelocity); + _rigidBody->setWorldTransform(transform); + + // make sure the upward velocity is large enough to clear the very top of the step + const btScalar MAGIC_STEP_OVERSHOOT_SPEED_COEFFICIENT = 0.5f; + minStepUpSpeed = MAGIC_STEP_OVERSHOOT_SPEED_COEFFICIENT * sqrtf(fabsf(2.0f * _gravity * _minStepHeight)); + btScalar vDotUp = velocity.dot(_currentUp); + if (vDotUp < minStepUpSpeed) { + velocity += (minStepUpSpeed - vDotUp) * _stepUpVelocity; + } + } _rigidBody->setLinearVelocity(velocity + _parentVelocity); _ghost.setWorldTransform(_rigidBody->getWorldTransform()); } @@ -415,6 +412,10 @@ void CharacterController::setLocalBoundingBox(const glm::vec3& minCorner, const if (glm::abs(radius - _radius) > FLT_EPSILON || glm::abs(halfHeight - _halfHeight) > FLT_EPSILON) { _radius = radius; _halfHeight = halfHeight; + const btScalar DEFAULT_MIN_STEP_HEIGHT = 0.041f; // HACK: hardcoded now but should just larger than shape margin + const btScalar MAX_STEP_FRACTION_OF_HALF_HEIGHT = 0.56f; + _minStepHeight = DEFAULT_MIN_STEP_HEIGHT; + _maxStepHeight = MAX_STEP_FRACTION_OF_HALF_HEIGHT * (_halfHeight + _radius); if (_dynamicsWorld) { // must REMOVE from world prior to shape update @@ -584,6 +585,21 @@ void CharacterController::applyMotor(int index, btScalar dt, btVector3& worldVel btVector3 vTargetVelocity = motorVelocity.dot(up) * up; btVector3 hTargetVelocity = motorVelocity - vTargetVelocity; + if (_stepHeight > _minStepHeight) { + // there is a step --> compute velocity direction to go over step + btVector3 motorVelocityWF = motorVelocity.rotate(axis, angle); + if (motorVelocityWF.dot(_stepNormal) < 0.0f) { + // the motor pushes against step + motorVelocityWF = _stepNormal.cross(_stepPoint.cross(motorVelocityWF)); + btScalar doubleCrossLength2 = motorVelocityWF.length2(); + if (doubleCrossLength2 > FLT_EPSILON) { + // scale the motor in the correct direction and rotate back to motor-frame + motorVelocityWF *= (motorVelocity.length() / sqrtf(doubleCrossLength2)); + _stepUpVelocity += motorVelocityWF.rotate(axis, -angle); + } + } + } + // split velocity into horizontal and vertical components btVector3 vVelocity = velocity.dot(up) * up; btVector3 hVelocity = velocity - vVelocity; @@ -632,6 +648,7 @@ void CharacterController::computeNewVelocity(btScalar dt, btVector3& velocity) { std::vector weights; weights.reserve(_motors.size()); _targetVelocity = btVector3(0.0f, 0.0f, 0.0f); + _stepUpVelocity = btVector3(0.0f, 0.0f, 0.0f); for (int i = 0; i < (int)_motors.size(); ++i) { applyMotor(i, dt, velocity, velocities, weights); } diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h index 27ffa055f0..feb01482f8 100644 --- a/libraries/physics/src/CharacterController.h +++ b/libraries/physics/src/CharacterController.h @@ -165,6 +165,14 @@ protected: quint32 _jumpButtonDownCount; quint32 _takeoffJumpButtonID; + // data for walking up steps + btVector3 _stepPoint; + btVector3 _stepNormal { 0.0f, 0.0f, 0.0f }; + btVector3 _stepUpVelocity { 0.0f, 0.0f, 0.0f }; + btScalar _stepHeight { 0.0f }; + btScalar _minStepHeight { 0.0f }; + btScalar _maxStepHeight { 0.0f }; + btScalar _halfHeight { 0.0f }; btScalar _radius { 0.0f }; From 72ad974b3e5c6520a229911b5606768b9068a8ed Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Sat, 15 Oct 2016 16:00:02 -0700 Subject: [PATCH 082/101] improved shape prevents avatar walking up walls --- .../src/avatar/MyCharacterController.cpp | 22 ++++++++++- libraries/physics/src/CharacterController.cpp | 5 +-- .../physics/src/CharacterGhostObject.cpp | 39 ++++++++----------- libraries/physics/src/CharacterGhostObject.h | 6 +-- libraries/physics/src/CharacterGhostShape.cpp | 9 ++++- libraries/physics/src/CharacterGhostShape.h | 9 ++--- 6 files changed, 54 insertions(+), 36 deletions(-) diff --git a/interface/src/avatar/MyCharacterController.cpp b/interface/src/avatar/MyCharacterController.cpp index d98c66c8c6..9b46aae84d 100644 --- a/interface/src/avatar/MyCharacterController.cpp +++ b/interface/src/avatar/MyCharacterController.cpp @@ -37,12 +37,32 @@ void MyCharacterController::updateShapeIfNecessary() { if (_radius > 0.0f) { // create RigidBody if it doesn't exist if (!_rigidBody) { + // HACK: the avatar collides using convex hull with a collision margin equal to + // the old capsule radius. Two points define a capsule and additional points are + // spread out at chest level to produce a slight taper toward the feet. This + // makes the avatar more likely to collide with vertical walls at a higher point + // and thus less likely to produce a single-point collision manifold below the + // _maxStepHeight when walking into against vertical surfaces --> fixes a bug + // where the "walk up steps" feature would allow the avatar to walk up vertical + // walls. + const int32_t NUM_POINTS = 6; + btVector3 points[NUM_POINTS]; + btVector3 xAxis = btVector3(1.0f, 0.0f, 0.0f); + btVector3 yAxis = btVector3(0.0f, 1.0f, 0.0f); + btVector3 zAxis = btVector3(0.0f, 0.0f, 1.0f); + points[0] = _halfHeight * yAxis; + points[1] = -_halfHeight * yAxis; + points[2] = (0.75f * _halfHeight) * yAxis - (0.1f * _radius) * zAxis; + points[3] = (0.75f * _halfHeight) * yAxis + (0.1f * _radius) * zAxis; + points[4] = (0.75f * _halfHeight) * yAxis - (0.1f * _radius) * xAxis; + points[5] = (0.75f * _halfHeight) * yAxis + (0.1f * _radius) * xAxis; + btCollisionShape* shape = new btConvexHullShape(reinterpret_cast(points), NUM_POINTS); + shape->setMargin(_radius); // HACK: use some simple mass property defaults for now const float DEFAULT_AVATAR_MASS = 100.0f; const btVector3 DEFAULT_AVATAR_INERTIA_TENSOR(30.0f, 8.0f, 30.0f); - btCollisionShape* shape = new btCapsuleShape(_radius, 2.0f * _halfHeight); _rigidBody = new btRigidBody(DEFAULT_AVATAR_MASS, nullptr, shape, DEFAULT_AVATAR_INERTIA_TENSOR); } else { btCollisionShape* shape = _rigidBody->getCollisionShape(); diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 07442b4f45..64025449dd 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -116,8 +116,8 @@ void CharacterController::setDynamicsWorld(btDynamicsWorld* world) { // restore gravity settings because adding an object to the world overwrites its gravity setting _rigidBody->setGravity(_gravity * _currentUp); btCollisionShape* shape = _rigidBody->getCollisionShape(); - assert(shape && shape->getShapeType() == CAPSULE_SHAPE_PROXYTYPE); - _ghost.setCharacterCapsule(static_cast(shape)); // KINEMATIC_CONTROLLER_HACK + assert(shape && shape->getShapeType() == CONVEX_HULL_SHAPE_PROXYTYPE); + _ghost.setCharacterShape(static_cast(shape)); } // KINEMATIC_CONTROLLER_HACK _ghost.setCollisionGroupAndMask(_collisionGroup, BULLET_COLLISION_MASK_MY_AVATAR & (~ _collisionGroup)); @@ -171,7 +171,6 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld, btSc btVector3 normal = characterIsFirst ? contact.m_normalWorldOnB : -contact.m_normalWorldOnB; // points toward character btScalar hitHeight = _halfHeight + _radius + pointOnCharacter.dot(_currentUp); if (hitHeight < _maxStepHeight && normal.dot(_currentUp) > COS_PI_OVER_THREE) { - //std::cout << "adebug manifoldIndex = " << i << " contactIndex = " << j << " hitOnCharacter*up = " << pointOnCharacter.dot(_currentUp) << std::endl; // adebug hasFloor = true; if (!pushing) { // we're not pushing against anything so we can early exit diff --git a/libraries/physics/src/CharacterGhostObject.cpp b/libraries/physics/src/CharacterGhostObject.cpp index 328d869f01..f897711622 100755 --- a/libraries/physics/src/CharacterGhostObject.cpp +++ b/libraries/physics/src/CharacterGhostObject.cpp @@ -16,8 +16,8 @@ #include -#include "CharacterGhostShape.h" #include "CharacterRayResult.h" +#include "CharacterGhostShape.h" CharacterGhostObject::~CharacterGhostObject() { @@ -62,13 +62,14 @@ void CharacterGhostObject::setMotorVelocity(const btVector3& velocity) { } // override of btCollisionObject::setCollisionShape() -void CharacterGhostObject::setCharacterCapsule(btCapsuleShape* capsule) { - assert(capsule); - // we create our own CharacterGhostShape which has a larger Aabb for more reliable sweep tests +void CharacterGhostObject::setCharacterShape(btConvexHullShape* shape) { + assert(shape); + // we create our own shape with an expanded Aabb for more reliable sweep tests if (_ghostShape) { delete _ghostShape; } - _ghostShape = new CharacterGhostShape(capsule->getRadius(), 2.0f * capsule->getHalfHeight()); + + _ghostShape = new CharacterGhostShape(static_cast(shape)); setCollisionShape(_ghostShape); } @@ -127,14 +128,10 @@ void CharacterGhostObject::move(btScalar dt, btScalar overshoot, btScalar gravit return; } - const btCollisionShape* shape = getCollisionShape(); - assert(shape->isConvex()); - const btConvexShape* convexShape= static_cast(shape); - // augment forwardSweep to help slow moving sweeps get over steppable ledges - btScalar margin = shape->getMargin(); - if (overshoot < margin) { - overshoot = margin; + const btScalar MIN_OVERSHOOT = 0.04f; // default margin + if (overshoot < MIN_OVERSHOOT) { + overshoot = MIN_OVERSHOOT; } btScalar longSweepDistance = stepDistance + overshoot; forwardSweep *= longSweepDistance / stepDistance; @@ -143,7 +140,7 @@ void CharacterGhostObject::move(btScalar dt, btScalar overshoot, btScalar gravit CharacterSweepResult result(this); btTransform nextTransform = startTransform; nextTransform.setOrigin(startPosition + forwardSweep); - sweepTest(convexShape, startTransform, nextTransform, result); // forward + sweepTest(_characterShape, startTransform, nextTransform, result); // forward if (!result.hasHit()) { nextTransform.setOrigin(startPosition + (stepDistance / longSweepDistance) * forwardSweep); @@ -151,7 +148,7 @@ void CharacterGhostObject::move(btScalar dt, btScalar overshoot, btScalar gravit updateTraction(nextTransform.getOrigin()); return; } - bool verticalOnly = btFabs(btFabs(_linearVelocity.dot(_upDirection)) - speed) < margin; + bool verticalOnly = btFabs(btFabs(_linearVelocity.dot(_upDirection)) - speed) < MIN_OVERSHOOT; if (verticalOnly) { // no need to step nextTransform.setOrigin(startPosition + (result.m_closestHitFraction * stepDistance / longSweepDistance) * forwardSweep); @@ -172,7 +169,7 @@ void CharacterGhostObject::move(btScalar dt, btScalar overshoot, btScalar gravit btVector3 hitFromBase = result.m_hitPointWorld - (startPosition - ((_radius + _halfHeight) * _upDirection)); btScalar hitHeight = hitFromBase.dot(_upDirection); if (hitHeight > _maxStepHeight) { - // capsule can't step over the obstacle so move forward as much as possible before we bail + // shape can't step over the obstacle so move forward as much as possible before we bail btVector3 forwardTranslation = result.m_closestHitFraction * forwardSweep; btScalar forwardDistance = forwardTranslation.length(); if (forwardDistance > stepDistance) { @@ -195,7 +192,7 @@ void CharacterGhostObject::move(btScalar dt, btScalar overshoot, btScalar gravit result.resetHitHistory(); startTransform.setOrigin(startPosition + availableStepHeight * _upDirection); nextTransform.setOrigin(startTransform.getOrigin() + forwardSweep); - sweepTest(convexShape, startTransform, nextTransform, result); + sweepTest(_characterShape, startTransform, nextTransform, result); if (result.hasHit()) { startTransform.setOrigin(startTransform.getOrigin() + result.m_closestHitFraction * forwardSweep); } else { @@ -206,7 +203,7 @@ void CharacterGhostObject::move(btScalar dt, btScalar overshoot, btScalar gravit result.resetHitHistory(); btVector3 downSweep = (- availableStepHeight) * _upDirection; nextTransform.setOrigin(startTransform.getOrigin() + downSweep); - sweepTest(convexShape, startTransform, nextTransform, result); + sweepTest(_characterShape, startTransform, nextTransform, result); if (result.hasHit() && result.m_hitNormalWorld.dot(_upDirection) > _maxWallNormalUpComponent) { // can stand on future landing spot, so we interpolate toward it _floorNormal = result.m_hitNormalWorld; @@ -395,7 +392,7 @@ void CharacterGhostObject::updateTraction(const btVector3& position) { if (_hovering || _motorOnly) { _linearVelocity = _motorVelocity; } else if (_onFloor) { - // compute a velocity that swings the capsule around the _floorContact + // compute a velocity that swings the shape around the _floorContact btVector3 leverArm = _floorContact - position; btVector3 pathDirection = leverArm.cross(_motorVelocity.cross(leverArm)); btScalar pathLength = pathDirection.length(); @@ -408,15 +405,11 @@ void CharacterGhostObject::updateTraction(const btVector3& position) { } btScalar CharacterGhostObject::measureAvailableStepHeight() const { - const btCollisionShape* shape = getCollisionShape(); - assert(shape->isConvex()); - const btConvexShape* convexShape= static_cast(shape); - CharacterSweepResult result(this); btTransform transform = getWorldTransform(); btTransform nextTransform = transform; nextTransform.setOrigin(transform.getOrigin() + _maxStepHeight * _upDirection); - sweepTest(convexShape, transform, nextTransform, result); + sweepTest(_characterShape, transform, nextTransform, result); return result.m_closestHitFraction * _maxStepHeight; } diff --git a/libraries/physics/src/CharacterGhostObject.h b/libraries/physics/src/CharacterGhostObject.h index 87310714c8..2567cae204 100755 --- a/libraries/physics/src/CharacterGhostObject.h +++ b/libraries/physics/src/CharacterGhostObject.h @@ -40,7 +40,7 @@ public: void setLinearVelocity(const btVector3& velocity) { _linearVelocity = velocity; } const btVector3& getLinearVelocity() const { return _linearVelocity; } - void setCharacterCapsule(btCapsuleShape* capsule); + void setCharacterShape(btConvexHullShape* shape); void setCollisionWorld(btCollisionWorld* world); @@ -88,8 +88,8 @@ protected: btScalar _radius { 0.0f }; btScalar _maxWallNormalUpComponent { 0.0f }; // input: max vertical component of wall normal btScalar _maxStepHeight { 0.0f }; // input, max step height the character can climb - btCapsuleShape* _characterShape { nullptr }; // input, shape of character - CharacterGhostShape* _ghostShape{ nullptr }; // internal, shape whose Aabb is used for overlap cache + btConvexHullShape* _characterShape { nullptr }; // input, shape of character + CharacterGhostShape* _ghostShape { nullptr }; // internal, shape whose Aabb is used for overlap cache int16_t _collisionFilterGroup { 0 }; int16_t _collisionFilterMask { 0 }; bool _inWorld { false }; // internal, was added to world diff --git a/libraries/physics/src/CharacterGhostShape.cpp b/libraries/physics/src/CharacterGhostShape.cpp index bf031ec569..156d18a22b 100644 --- a/libraries/physics/src/CharacterGhostShape.cpp +++ b/libraries/physics/src/CharacterGhostShape.cpp @@ -10,9 +10,16 @@ // #include "CharacterGhostShape.h" +CharacterGhostShape::CharacterGhostShape(const btConvexHullShape* shape) : + btConvexHullShape(reinterpret_cast(shape->getUnscaledPoints()), shape->getNumPoints(), sizeof(btVector3)) { + assert(shape); + assert(shape->getUnscaledPoints()); + assert(shape->getNumPoints() > 0); + setMargin(shape->getMargin()); +} void CharacterGhostShape::getAabb (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { - btCapsuleShape::getAabb(t, aabbMin, aabbMax); + btConvexHullShape::getAabb(t, aabbMin, aabbMax); // double the size of the Aabb by expanding both corners by half the extent btVector3 expansion = 0.5f * (aabbMax - aabbMin); aabbMin -= expansion; diff --git a/libraries/physics/src/CharacterGhostShape.h b/libraries/physics/src/CharacterGhostShape.h index 9c9efded34..614bc168af 100644 --- a/libraries/physics/src/CharacterGhostShape.h +++ b/libraries/physics/src/CharacterGhostShape.h @@ -12,13 +12,12 @@ #ifndef hifi_CharacterGhostShape_h #define hifi_CharacterGhostShape_h -#include +#include -class CharacterGhostShape : public btCapsuleShape { - // Same as btCapsuleShape but reports an expanded Aabb for larger ghost overlap cache +class CharacterGhostShape : public btConvexHullShape { + // Same as btConvexHullShape but reports an expanded Aabb for larger ghost overlap cache public: - CharacterGhostShape(btScalar radius, btScalar height) : btCapsuleShape(radius, height) { - } + CharacterGhostShape(const btConvexHullShape* shape); virtual void getAabb (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const override; }; From 515b8ef3956c5a370c9f8974b10ecd136a6194ea Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Sun, 16 Oct 2016 06:27:13 -0700 Subject: [PATCH 083/101] fix build error on windows --- libraries/physics/src/CharacterGhostShape.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/libraries/physics/src/CharacterGhostShape.cpp b/libraries/physics/src/CharacterGhostShape.cpp index 156d18a22b..e5a5cb59de 100644 --- a/libraries/physics/src/CharacterGhostShape.cpp +++ b/libraries/physics/src/CharacterGhostShape.cpp @@ -10,6 +10,10 @@ // #include "CharacterGhostShape.h" + +#include + + CharacterGhostShape::CharacterGhostShape(const btConvexHullShape* shape) : btConvexHullShape(reinterpret_cast(shape->getUnscaledPoints()), shape->getNumPoints(), sizeof(btVector3)) { assert(shape); From 6f5f6d2493223fb0e2b8303486933397efdcf870 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 17 Oct 2016 17:58:52 -0700 Subject: [PATCH 084/101] adding ray shotgun scan for avatar motion using the shotgun scan results to deterimine if slope is walkable --- interface/src/avatar/MyAvatar.cpp | 12 +- .../src/avatar/MyCharacterController.cpp | 256 ++++++++++++++++-- interface/src/avatar/MyCharacterController.h | 29 +- libraries/physics/src/CharacterController.cpp | 7 +- libraries/physics/src/CharacterController.h | 7 +- .../physics/src/CharacterGhostObject.cpp | 34 +-- libraries/physics/src/CharacterGhostObject.h | 10 +- 7 files changed, 300 insertions(+), 55 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 8e4acd174d..a936e03943 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -1325,7 +1325,7 @@ void MyAvatar::updateMotors() { if (qApp->isHMDMode()) { // OUTOFBODY_HACK: only apply vertical component of _actionMotorVelocity to the characterController - _characterController.addMotor(glm::vec3(0, _actionMotorVelocity.y, 0), motorRotation, DEFAULT_MOTOR_TIMESCALE, INVALID_MOTOR_TIMESCALE); + _characterController.addMotor(glm::vec3(0.0f, _actionMotorVelocity.y, 0.0f), motorRotation, DEFAULT_MOTOR_TIMESCALE, INVALID_MOTOR_TIMESCALE); } else { if (_isPushing || _isBraking || !_isBeingPushed) { _characterController.addMotor(_actionMotorVelocity, motorRotation, DEFAULT_MOTOR_TIMESCALE, INVALID_MOTOR_TIMESCALE); @@ -1889,8 +1889,16 @@ void MyAvatar::updatePosition(float deltaTime) { float speed2 = glm::length2(velocity); _moving = speed2 > MOVING_SPEED_THRESHOLD_SQUARED; - if (qApp->isHMDMode()) { + if (_moving) { + // scan for walkability + glm::vec3 position = getPosition(); + MyCharacterController::RayShotgunResult result; + glm::vec3 step = deltaTime * (getRotation() * _actionMotorVelocity); + _characterController.testRayShotgun(position, step, result); + _characterController.setStepUpEnabled(result.walkable); + } + if (qApp->isHMDMode()) { // Apply _actionMotorVelocity directly to the sensorToWorld matrix. glm::quat motorRotation; glm::quat liftRotation; diff --git a/interface/src/avatar/MyCharacterController.cpp b/interface/src/avatar/MyCharacterController.cpp index 9b46aae84d..e2b96fd26d 100644 --- a/interface/src/avatar/MyCharacterController.cpp +++ b/interface/src/avatar/MyCharacterController.cpp @@ -15,11 +15,19 @@ #include "MyAvatar.h" -// TODO: improve walking up steps -// TODO: make avatars able to walk up and down steps/slopes +// DONE TODO: improve walking up steps +// DONE TODO: make avatars able to walk up and down steps/slopes // TODO: make avatars stand on steep slope // TODO: make avatars not snag on low ceilings + +void MyCharacterController::RayShotgunResult::reset() { + //hitNormal = glm::vec3(0.0f); + hitFraction = 1.0f; + //numHits = 0; + walkable = true; +} + MyCharacterController::MyCharacterController(MyAvatar* avatar) { assert(avatar); @@ -30,6 +38,12 @@ MyCharacterController::MyCharacterController(MyAvatar* avatar) { MyCharacterController::~MyCharacterController() { } +void MyCharacterController::setDynamicsWorld(btDynamicsWorld* world) { + CharacterController::setDynamicsWorld(world); + + initRayShotgun(world); +} + void MyCharacterController::updateShapeIfNecessary() { if (_pendingFlags & PENDING_FLAG_UPDATE_SHAPE) { _pendingFlags &= ~PENDING_FLAG_UPDATE_SHAPE; @@ -37,30 +51,10 @@ void MyCharacterController::updateShapeIfNecessary() { if (_radius > 0.0f) { // create RigidBody if it doesn't exist if (!_rigidBody) { - // HACK: the avatar collides using convex hull with a collision margin equal to - // the old capsule radius. Two points define a capsule and additional points are - // spread out at chest level to produce a slight taper toward the feet. This - // makes the avatar more likely to collide with vertical walls at a higher point - // and thus less likely to produce a single-point collision manifold below the - // _maxStepHeight when walking into against vertical surfaces --> fixes a bug - // where the "walk up steps" feature would allow the avatar to walk up vertical - // walls. - const int32_t NUM_POINTS = 6; - btVector3 points[NUM_POINTS]; - btVector3 xAxis = btVector3(1.0f, 0.0f, 0.0f); - btVector3 yAxis = btVector3(0.0f, 1.0f, 0.0f); - btVector3 zAxis = btVector3(0.0f, 0.0f, 1.0f); - points[0] = _halfHeight * yAxis; - points[1] = -_halfHeight * yAxis; - points[2] = (0.75f * _halfHeight) * yAxis - (0.1f * _radius) * zAxis; - points[3] = (0.75f * _halfHeight) * yAxis + (0.1f * _radius) * zAxis; - points[4] = (0.75f * _halfHeight) * yAxis - (0.1f * _radius) * xAxis; - points[5] = (0.75f * _halfHeight) * yAxis + (0.1f * _radius) * xAxis; - btCollisionShape* shape = new btConvexHullShape(reinterpret_cast(points), NUM_POINTS); - shape->setMargin(_radius); + btCollisionShape* shape = computeShape(); // HACK: use some simple mass property defaults for now - const float DEFAULT_AVATAR_MASS = 100.0f; + const btScalar DEFAULT_AVATAR_MASS = 100.0f; const btVector3 DEFAULT_AVATAR_INERTIA_TENSOR(30.0f, 8.0f, 30.0f); _rigidBody = new btRigidBody(DEFAULT_AVATAR_MASS, nullptr, shape, DEFAULT_AVATAR_INERTIA_TENSOR); @@ -69,7 +63,7 @@ void MyCharacterController::updateShapeIfNecessary() { if (shape) { delete shape; } - shape = new btCapsuleShape(_radius, 2.0f * _halfHeight); + shape = computeShape(); _rigidBody->setCollisionShape(shape); } @@ -96,3 +90,215 @@ void MyCharacterController::updateShapeIfNecessary() { } } +bool MyCharacterController::testRayShotgun(const glm::vec3& position, const glm::vec3& step, RayShotgunResult& result) { + // get _ghost ready for ray traces + btTransform transform = _rigidBody->getWorldTransform(); + btVector3 newPosition = glmToBullet(position); + transform.setOrigin(newPosition); + _ghost.setWorldTransform(transform); + btMatrix3x3 rotation = transform.getBasis(); + _ghost.refreshOverlappingPairCache(); + + CharacterRayResult rayResult(&_ghost); + CharacterRayResult closestRayResult(&_ghost); + btVector3 rayStart; + btVector3 rayEnd; + btVector3 rayDirection = glmToBullet(step); + + btScalar stepLength = rayDirection.length(); + if (stepLength < FLT_EPSILON) { + return false; + } + rayDirection /= stepLength; + const btScalar backSlop = 0.04f; + + // scan the top + // NOTE: if we scan an extra distance forward we can detect flat surfaces that are too steep to walk on. + // The approximate extra distance can be derived with trigonometry. + // + // minimumForward = [ (maxStepHeight + radius / sinTheta - radius) * (sinTheta / cosTheta) - radius ] + // + // where: theta = max angle between floor normal and vertical + // + // if stepLength is not long enough we can add the difference. + // + btScalar cosTheta = _minFloorNormalDotUp; + btScalar sinTheta = sqrtf(1.0f - cosTheta * cosTheta); + btScalar forwardSlop = (_maxStepHeight + _radius / sinTheta - _radius) * (sinTheta / cosTheta) - (_radius + stepLength); + bool needToRemoveForwardSlop = true; + if (forwardSlop < 0.0f) { + forwardSlop = 0.0f; + needToRemoveForwardSlop = false; + } + + for (int32_t i = 0; i < _topPoints.size(); ++i) { + rayStart = newPosition + rotation * _topPoints[i] - backSlop * rayDirection; + rayEnd = rayStart + (backSlop + stepLength + forwardSlop) * rayDirection; + if (_ghost.rayTest(rayStart, rayEnd, rayResult)) { + if (rayResult.m_closestHitFraction < closestRayResult.m_closestHitFraction) { + closestRayResult = rayResult; + } + if (result.walkable) { + if (rayResult.m_hitNormalWorld.dot(_currentUp) < _minFloorNormalDotUp) { + result.walkable = false; + break; + } + } + } + } + if (!result.walkable) { + // the top scan wasn't walkable so don't bother scanning the bottom + // remove both forwardSlop and backSlop + result.hitFraction = (closestRayResult.m_closestHitFraction * (backSlop + stepLength + forwardSlop) - forwardSlop) / (backSlop + stepLength); + } else { + if (needToRemoveForwardSlop && result.hitFraction < 1.0f) { + // remove forwardSlop + result.hitFraction = (closestRayResult.m_closestHitFraction * (stepLength + forwardSlop) - forwardSlop) / stepLength; + } + // scan the bottom + for (int32_t i = 0; i < _bottomPoints.size(); ++i) { + rayStart = newPosition + rotation * _bottomPoints[i] - backSlop * rayDirection; + rayEnd = rayStart + (backSlop + stepLength) * rayDirection; + if (_ghost.rayTest(rayStart, rayEnd, rayResult)) { + if (rayResult.m_closestHitFraction < closestRayResult.m_closestHitFraction) { + closestRayResult = rayResult; + } + } + } + // remove backSlop + // NOTE: backSlop removal can produce a NEGATIVE hitFraction! + // which means the shape is actually in interpenetration + result.hitFraction = (closestRayResult.m_closestHitFraction * (backSlop + stepLength) - backSlop) / stepLength; + } + return result.hitFraction < 1.0f; +} + +btConvexHullShape* MyCharacterController::computeShape() const { + // HACK: the avatar collides using convex hull with a collision margin equal to + // the old capsule radius. Two points define a capsule and additional points are + // spread out at chest level to produce a slight taper toward the feet. This + // makes the avatar more likely to collide with vertical walls at a higher point + // and thus less likely to produce a single-point collision manifold below the + // _maxStepHeight when walking into against vertical surfaces --> fixes a bug + // where the "walk up steps" feature would allow the avatar to walk up vertical + // walls. + const int32_t NUM_POINTS = 6; + btVector3 points[NUM_POINTS]; + btVector3 xAxis = btVector3(1.0f, 0.0f, 0.0f); + btVector3 yAxis = btVector3(0.0f, 1.0f, 0.0f); + btVector3 zAxis = btVector3(0.0f, 0.0f, 1.0f); + points[0] = _halfHeight * yAxis; + points[1] = -_halfHeight * yAxis; + points[2] = (0.75f * _halfHeight) * yAxis - (0.1f * _radius) * zAxis; + points[3] = (0.75f * _halfHeight) * yAxis + (0.1f * _radius) * zAxis; + points[4] = (0.75f * _halfHeight) * yAxis - (0.1f * _radius) * xAxis; + points[5] = (0.75f * _halfHeight) * yAxis + (0.1f * _radius) * xAxis; + btConvexHullShape* shape = new btConvexHullShape(reinterpret_cast(points), NUM_POINTS); + shape->setMargin(_radius); + return shape; +} + +void MyCharacterController::initRayShotgun(const btCollisionWorld* world) { + // In order to trace rays out from the avatar's shape surface we need to know where the start points are in + // the local-frame. Since the avatar shape is somewhat irregular computing these points by hand is a hassle + // so instead we ray-trace backwards to the avatar to find them. + // + // We trace back a regular grid (see below) of points against the shape and keep any that hit. + // ___ + // + / + \ + + // |+ +| + // +| + | + + // |+ +| + // +| + | + + // |+ +| + // + \ + / + + // --- + // The shotgun will send rays out from these same points to see if the avatar's shape can proceed through space. + + // helper class for simple ray-traces against character + class MeOnlyResultCallback : public btCollisionWorld::ClosestRayResultCallback { + public: + MeOnlyResultCallback (btRigidBody* me) : btCollisionWorld::ClosestRayResultCallback(btVector3(0.0f, 0.0f, 0.0f), btVector3(0.0f, 0.0f, 0.0f)) { + _me = me; + m_collisionFilterGroup = BULLET_COLLISION_GROUP_DYNAMIC; + m_collisionFilterMask = BULLET_COLLISION_MASK_DYNAMIC; + } + virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult,bool normalInWorldSpace) override { + if (rayResult.m_collisionObject != _me) { + return 1.0f; + } + return ClosestRayResultCallback::addSingleResult(rayResult, normalInWorldSpace); + } + btRigidBody* _me; + }; + + const btScalar fullHalfHeight = _radius + _halfHeight; + const btScalar divisionLine = -fullHalfHeight + _maxStepHeight; // line between top and bottom + const btScalar topHeight = fullHalfHeight - divisionLine; + const btScalar slop = 0.02f; + + const int32_t numRows = 3; // must be odd number > 1 + const int32_t numColumns = 3; // must be odd number > 1 + btVector3 reach = (2.0f * _radius) * btVector3(0.0f, 0.0f, 1.0f); + + { // top points + _topPoints.clear(); + _topPoints.reserve(numRows * numColumns); + btScalar stepY = (topHeight - slop) / (btScalar)(numRows - 1); + btScalar stepX = 2.0f * (_radius - slop) / (btScalar)(numColumns - 1); + + btTransform transform = _rigidBody->getWorldTransform(); + btVector3 position = transform.getOrigin(); + btMatrix3x3 rotation = transform.getBasis(); + + for (int32_t i = 0; i < numRows; ++i) { + int32_t maxJ = numColumns; + btScalar offsetX = -(btScalar)((numColumns - 1) / 2) * stepX; + if (i%2 == 1) { + // odd rows have one less point and start a halfStep closer + maxJ -= 1; + offsetX += 0.5f * stepX; + } + for (int32_t j = 0; j < maxJ; ++j) { + btVector3 localRayEnd(offsetX + (btScalar)(j) * stepX, divisionLine + (btScalar)(i) * stepY, 0.0f); + btVector3 localRayStart = localRayEnd - reach; + MeOnlyResultCallback result(_rigidBody); + world->rayTest(position + rotation * localRayStart, position + rotation * localRayEnd, result); + if (result.m_closestHitFraction < 1.0f) { + _topPoints.push_back(localRayStart + result.m_closestHitFraction * reach); + } + } + } + } + + { // bottom points + _bottomPoints.clear(); + _bottomPoints.reserve(numRows * numColumns); + + btScalar stepY = (_maxStepHeight - 2.0f * slop) / (btScalar)(numRows - 1); + btScalar stepX = 2.0f * (_radius - slop) / (btScalar)(numColumns - 1); + + btTransform transform = _rigidBody->getWorldTransform(); + btVector3 position = transform.getOrigin(); + btMatrix3x3 rotation = transform.getBasis(); + + for (int32_t i = 0; i < numRows; ++i) { + int32_t maxJ = numColumns; + btScalar offsetX = -(btScalar)((numColumns - 1) / 2) * stepX; + if (i%2 == 1) { + // odd rows have one less point and start a halfStep closer + maxJ -= 1; + offsetX += 0.5f * stepX; + } + for (int32_t j = 0; j < maxJ; ++j) { + btVector3 localRayEnd(offsetX + (btScalar)(j) * stepX, (divisionLine - slop) - (btScalar)(i) * stepY, 0.0f); + btVector3 localRayStart = localRayEnd - reach; + MeOnlyResultCallback result(_rigidBody); + world->rayTest(position + rotation * localRayStart, position + rotation * localRayEnd, result); + if (result.m_closestHitFraction < 1.0f) { + _bottomPoints.push_back(localRayStart + result.m_closestHitFraction * reach); + } + } + } + } +} diff --git a/interface/src/avatar/MyCharacterController.h b/interface/src/avatar/MyCharacterController.h index 265406bc6f..f57aa45050 100644 --- a/interface/src/avatar/MyCharacterController.h +++ b/interface/src/avatar/MyCharacterController.h @@ -24,10 +24,37 @@ public: explicit MyCharacterController(MyAvatar* avatar); ~MyCharacterController (); - virtual void updateShapeIfNecessary() override; + void setDynamicsWorld(btDynamicsWorld* world) override; + void updateShapeIfNecessary() override; + + // Sweeping a convex shape through the physics simulation can expensive when the obstacles are too complex + // (e.g. small 20k triangle static mesh) so instead as a fallback we cast several rays forward and if they + // don't hit anything we consider it a clean sweep. Hence the "Shotgun" code. + class RayShotgunResult { + public: + void reset(); + + //glm::vec3 hitNormal { 0.0f, 0.0f, 0.0f }; + float hitFraction { 1.0f }; + //int32_t numHits { 0 }; + bool walkable { true }; + }; + + /// return true if RayShotgun hits anything + bool testRayShotgun(const glm::vec3& position, const glm::vec3& step, RayShotgunResult& result); + +protected: + void initRayShotgun(const btCollisionWorld* world); + +private: + btConvexHullShape* computeShape() const; protected: MyAvatar* _avatar { nullptr }; + + // shotgun scan data + btAlignedObjectArray _topPoints; + btAlignedObjectArray _bottomPoints; }; #endif // hifi_MyCharacterController_h diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 64025449dd..c1e1af4235 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -152,7 +152,6 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld, btSc btDispatcher* dispatcher = collisionWorld->getDispatcher(); int numManifolds = dispatcher->getNumManifolds(); bool hasFloor = false; - const float COS_PI_OVER_THREE = cosf(PI / 3.0f); btTransform rotation = _rigidBody->getWorldTransform(); rotation.setOrigin(btVector3(0.0f, 0.0f, 0.0f)); // clear translation part @@ -170,7 +169,7 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld, btSc btVector3 pointOnCharacter = characterIsFirst ? contact.m_localPointA : contact.m_localPointB; // object-local-frame btVector3 normal = characterIsFirst ? contact.m_normalWorldOnB : -contact.m_normalWorldOnB; // points toward character btScalar hitHeight = _halfHeight + _radius + pointOnCharacter.dot(_currentUp); - if (hitHeight < _maxStepHeight && normal.dot(_currentUp) > COS_PI_OVER_THREE) { + if (hitHeight < _maxStepHeight && normal.dot(_currentUp) > _minFloorNormalDotUp) { hasFloor = true; if (!pushing) { // we're not pushing against anything so we can early exit @@ -180,7 +179,7 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld, btSc } if (pushing && _targetVelocity.dot(normal) < 0.0f) { // remember highest step obstacle - if (hitHeight > _maxStepHeight) { + if (!_stepUpEnabled || hitHeight > _maxStepHeight) { // this manifold is invalidated by point that is too high stepContactIndex = -1; break; @@ -199,7 +198,7 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld, btSc _stepHeight = highestStep; _stepPoint = rotation * pointOnCharacter; // rotate into world-frame } - if (hasFloor && !pushing) { + if (hasFloor && !(pushing && _stepUpEnabled)) { // early exit since all we need to know is that we're on a floor break; } diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h index feb01482f8..492b5c387e 100644 --- a/libraries/physics/src/CharacterController.h +++ b/libraries/physics/src/CharacterController.h @@ -19,6 +19,7 @@ #include #include +#include #include #include "BulletUtil.h" @@ -29,6 +30,7 @@ const uint32_t PENDING_FLAG_REMOVE_FROM_SIMULATION = 1U << 1; const uint32_t PENDING_FLAG_UPDATE_SHAPE = 1U << 2; const uint32_t PENDING_FLAG_JUMP = 1U << 3; const uint32_t PENDING_FLAG_UPDATE_COLLISION_GROUP = 1U << 4; +const float DEFAULT_MIN_FLOOR_NORMAL_DOT_UP = cosf(PI / 3.0f); const float DEFAULT_CHARACTER_GRAVITY = -5.0f; @@ -48,7 +50,7 @@ public: bool needsRemoval() const; bool needsAddition() const; - void setDynamicsWorld(btDynamicsWorld* world); + virtual void setDynamicsWorld(btDynamicsWorld* world); btCollisionObject* getCollisionObject() { return _rigidBody; } virtual void updateShapeIfNecessary() = 0; @@ -70,6 +72,7 @@ public: 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 setStepUpEnabled(bool enabled) { _stepUpEnabled = enabled; } void computeNewVelocity(btScalar dt, btVector3& velocity); void computeNewVelocity(btScalar dt, glm::vec3& velocity); @@ -172,11 +175,13 @@ protected: btScalar _stepHeight { 0.0f }; btScalar _minStepHeight { 0.0f }; btScalar _maxStepHeight { 0.0f }; + btScalar _minFloorNormalDotUp { DEFAULT_MIN_FLOOR_NORMAL_DOT_UP }; btScalar _halfHeight { 0.0f }; btScalar _radius { 0.0f }; btScalar _floorDistance; + bool _stepUpEnabled { true }; bool _hasSupport; btScalar _gravity { DEFAULT_CHARACTER_GRAVITY }; diff --git a/libraries/physics/src/CharacterGhostObject.cpp b/libraries/physics/src/CharacterGhostObject.cpp index f897711622..2cdcd2a879 100755 --- a/libraries/physics/src/CharacterGhostObject.cpp +++ b/libraries/physics/src/CharacterGhostObject.cpp @@ -239,6 +239,15 @@ bool CharacterGhostObject::sweepTest( return false; } +bool CharacterGhostObject::rayTest(const btVector3& start, + const btVector3& end, + CharacterRayResult& result) const { + if (_world && _inWorld) { + _world->rayTest(start, end, result); + } + return result.hasHit(); +} + void CharacterGhostObject::measurePenetration(btVector3& minBoxOut, btVector3& maxBoxOut) { // minBoxOut and maxBoxOut will be updated with penetration envelope. // If one of the corner points is <0,0,0> then the penetration is resolvable in a single step, @@ -315,6 +324,14 @@ void CharacterGhostObject::measurePenetration(btVector3& minBoxOut, btVector3& m } } +void CharacterGhostObject::refreshOverlappingPairCache() { + assert(_world && _inWorld); + btVector3 minAabb, maxAabb; + getCollisionShape()->getAabb(getWorldTransform(), minAabb, maxAabb); + // this updates both pairCaches: world broadphase and ghostobject + _world->getBroadphase()->setAabb(getBroadphaseHandle(), minAabb, maxAabb, _world->getDispatcher()); +} + void CharacterGhostObject::removeFromWorld() { if (_world && _inWorld) { _world->removeCollisionObject(this); @@ -331,15 +348,6 @@ void CharacterGhostObject::addToWorld() { } } -bool CharacterGhostObject::rayTest(const btVector3& start, - const btVector3& end, - CharacterRayResult& result) const { - if (_world && _inWorld) { - _world->rayTest(start, end, result); - } - return result.hasHit(); -} - bool CharacterGhostObject::resolvePenetration(int numTries) { btVector3 minBox, maxBox; measurePenetration(minBox, maxBox); @@ -353,14 +361,6 @@ bool CharacterGhostObject::resolvePenetration(int numTries) { return true; } -void CharacterGhostObject::refreshOverlappingPairCache() { - assert(_world && _inWorld); - btVector3 minAabb, maxAabb; - getCollisionShape()->getAabb(getWorldTransform(), minAabb, maxAabb); - // this updates both pairCaches: world broadphase and ghostobject - _world->getBroadphase()->setAabb(getBroadphaseHandle(), minAabb, maxAabb, _world->getDispatcher()); -} - void CharacterGhostObject::updateVelocity(btScalar dt, btScalar gravity) { if (!_motorOnly) { if (_hovering) { diff --git a/libraries/physics/src/CharacterGhostObject.h b/libraries/physics/src/CharacterGhostObject.h index 2567cae204..1da280124e 100755 --- a/libraries/physics/src/CharacterGhostObject.h +++ b/libraries/physics/src/CharacterGhostObject.h @@ -51,6 +51,10 @@ public: const btTransform& end, CharacterSweepResult& result) const; + bool rayTest(const btVector3& start, + const btVector3& end, + CharacterRayResult& result) const; + bool isHovering() const { return _hovering; } void setHovering(bool hovering) { _hovering = hovering; } void setMotorOnly(bool motorOnly) { _motorOnly = motorOnly; } @@ -60,17 +64,13 @@ public: const btVector3& getFloorNormal() const { return _floorNormal; } void measurePenetration(btVector3& minBoxOut, btVector3& maxBoxOut); + void refreshOverlappingPairCache(); protected: void removeFromWorld(); void addToWorld(); - bool rayTest(const btVector3& start, - const btVector3& end, - CharacterRayResult& result) const; - bool resolvePenetration(int numTries); - void refreshOverlappingPairCache(); void updateVelocity(btScalar dt, btScalar gravity); void updateTraction(const btVector3& position); btScalar measureAvailableStepHeight() const; From ecc6cb49aefca5afee63dca5b3b5ffcd99c6c409 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 18 Oct 2016 11:42:31 -0700 Subject: [PATCH 085/101] more correct hitFraction adjustments --- interface/src/avatar/MyCharacterController.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/interface/src/avatar/MyCharacterController.cpp b/interface/src/avatar/MyCharacterController.cpp index e2b96fd26d..17920dd415 100644 --- a/interface/src/avatar/MyCharacterController.cpp +++ b/interface/src/avatar/MyCharacterController.cpp @@ -149,11 +149,11 @@ bool MyCharacterController::testRayShotgun(const glm::vec3& position, const glm: if (!result.walkable) { // the top scan wasn't walkable so don't bother scanning the bottom // remove both forwardSlop and backSlop - result.hitFraction = (closestRayResult.m_closestHitFraction * (backSlop + stepLength + forwardSlop) - forwardSlop) / (backSlop + stepLength); + result.hitFraction = glm::min(1.0f, (closestRayResult.m_closestHitFraction * (backSlop + stepLength + forwardSlop) - backSlop) / stepLength); } else { if (needToRemoveForwardSlop && result.hitFraction < 1.0f) { // remove forwardSlop - result.hitFraction = (closestRayResult.m_closestHitFraction * (stepLength + forwardSlop) - forwardSlop) / stepLength; + result.hitFraction = (closestRayResult.m_closestHitFraction * (backSlop + stepLength + forwardSlop)) / (backSlop + stepLength); } // scan the bottom for (int32_t i = 0; i < _bottomPoints.size(); ++i) { @@ -168,7 +168,7 @@ bool MyCharacterController::testRayShotgun(const glm::vec3& position, const glm: // remove backSlop // NOTE: backSlop removal can produce a NEGATIVE hitFraction! // which means the shape is actually in interpenetration - result.hitFraction = (closestRayResult.m_closestHitFraction * (backSlop + stepLength) - backSlop) / stepLength; + result.hitFraction = ((closestRayResult.m_closestHitFraction * (backSlop + stepLength)) - backSlop) / stepLength; } return result.hitFraction < 1.0f; } From 5a04c7ceee1725fe84708deae558d7f81c4ff707 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 18 Oct 2016 13:09:42 -0700 Subject: [PATCH 086/101] remove tabs in formatting --- interface/src/avatar/MyCharacterController.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/interface/src/avatar/MyCharacterController.cpp b/interface/src/avatar/MyCharacterController.cpp index 17920dd415..a0ac8617b4 100644 --- a/interface/src/avatar/MyCharacterController.cpp +++ b/interface/src/avatar/MyCharacterController.cpp @@ -131,7 +131,7 @@ bool MyCharacterController::testRayShotgun(const glm::vec3& position, const glm: needToRemoveForwardSlop = false; } - for (int32_t i = 0; i < _topPoints.size(); ++i) { + for (int32_t i = 0; i < _topPoints.size(); ++i) { rayStart = newPosition + rotation * _topPoints[i] - backSlop * rayDirection; rayEnd = rayStart + (backSlop + stepLength + forwardSlop) * rayDirection; if (_ghost.rayTest(rayStart, rayEnd, rayResult)) { @@ -145,7 +145,7 @@ bool MyCharacterController::testRayShotgun(const glm::vec3& position, const glm: } } } - } + } if (!result.walkable) { // the top scan wasn't walkable so don't bother scanning the bottom // remove both forwardSlop and backSlop @@ -156,7 +156,7 @@ bool MyCharacterController::testRayShotgun(const glm::vec3& position, const glm: result.hitFraction = (closestRayResult.m_closestHitFraction * (backSlop + stepLength + forwardSlop)) / (backSlop + stepLength); } // scan the bottom - for (int32_t i = 0; i < _bottomPoints.size(); ++i) { + for (int32_t i = 0; i < _bottomPoints.size(); ++i) { rayStart = newPosition + rotation * _bottomPoints[i] - backSlop * rayDirection; rayEnd = rayStart + (backSlop + stepLength) * rayDirection; if (_ghost.rayTest(rayStart, rayEnd, rayResult)) { @@ -164,7 +164,7 @@ bool MyCharacterController::testRayShotgun(const glm::vec3& position, const glm: closestRayResult = rayResult; } } - } + } // remove backSlop // NOTE: backSlop removal can produce a NEGATIVE hitFraction! // which means the shape is actually in interpenetration @@ -263,8 +263,8 @@ void MyCharacterController::initRayShotgun(const btCollisionWorld* world) { btVector3 localRayEnd(offsetX + (btScalar)(j) * stepX, divisionLine + (btScalar)(i) * stepY, 0.0f); btVector3 localRayStart = localRayEnd - reach; MeOnlyResultCallback result(_rigidBody); - world->rayTest(position + rotation * localRayStart, position + rotation * localRayEnd, result); - if (result.m_closestHitFraction < 1.0f) { + world->rayTest(position + rotation * localRayStart, position + rotation * localRayEnd, result); + if (result.m_closestHitFraction < 1.0f) { _topPoints.push_back(localRayStart + result.m_closestHitFraction * reach); } } @@ -294,8 +294,8 @@ void MyCharacterController::initRayShotgun(const btCollisionWorld* world) { btVector3 localRayEnd(offsetX + (btScalar)(j) * stepX, (divisionLine - slop) - (btScalar)(i) * stepY, 0.0f); btVector3 localRayStart = localRayEnd - reach; MeOnlyResultCallback result(_rigidBody); - world->rayTest(position + rotation * localRayStart, position + rotation * localRayEnd, result); - if (result.m_closestHitFraction < 1.0f) { + world->rayTest(position + rotation * localRayStart, position + rotation * localRayEnd, result); + if (result.m_closestHitFraction < 1.0f) { _bottomPoints.push_back(localRayStart + result.m_closestHitFraction * reach); } } From 6a445a0a9856808dc8ca0a822638c436802e56c0 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 18 Oct 2016 16:48:33 -0700 Subject: [PATCH 087/101] more correct steepest walkable slope detection --- .../src/avatar/MyCharacterController.cpp | 38 +++++++++++++++---- 1 file changed, 30 insertions(+), 8 deletions(-) diff --git a/interface/src/avatar/MyCharacterController.cpp b/interface/src/avatar/MyCharacterController.cpp index a0ac8617b4..c33dac888f 100644 --- a/interface/src/avatar/MyCharacterController.cpp +++ b/interface/src/avatar/MyCharacterController.cpp @@ -112,11 +112,29 @@ bool MyCharacterController::testRayShotgun(const glm::vec3& position, const glm: rayDirection /= stepLength; const btScalar backSlop = 0.04f; + // compute rotation that will orient local ray start points to face step direction + btVector3 forward = rotation * btVector3(0.0f, 0.0f, -1.0f); + btVector3 adjustedDirection = rayDirection - rayDirection.dot(_currentUp) * _currentUp; + btVector3 axis = forward.cross(adjustedDirection); + btScalar lengthAxis = axis.length(); + if (lengthAxis > FLT_EPSILON) { + // we're walking sideways + btScalar angle = acosf(lengthAxis / adjustedDirection.length()); + if (rayDirection.dot(forward) < 0.0f) { + angle = PI - angle; + } + axis /= lengthAxis; + rotation = btMatrix3x3(btQuaternion(axis, angle)) * rotation; + } else if (rayDirection.dot(forward) < 0.0f) { + // we're walking backwards + rotation = btMatrix3x3(btQuaternion(_currentUp, PI)) * rotation; + } + // scan the top // NOTE: if we scan an extra distance forward we can detect flat surfaces that are too steep to walk on. // The approximate extra distance can be derived with trigonometry. // - // minimumForward = [ (maxStepHeight + radius / sinTheta - radius) * (sinTheta / cosTheta) - radius ] + // minimumForward = [ (maxStepHeight + radius / cosTheta - radius) * (cosTheta / sinTheta) - radius ] // // where: theta = max angle between floor normal and vertical // @@ -124,11 +142,16 @@ bool MyCharacterController::testRayShotgun(const glm::vec3& position, const glm: // btScalar cosTheta = _minFloorNormalDotUp; btScalar sinTheta = sqrtf(1.0f - cosTheta * cosTheta); - btScalar forwardSlop = (_maxStepHeight + _radius / sinTheta - _radius) * (sinTheta / cosTheta) - (_radius + stepLength); - bool needToRemoveForwardSlop = true; + const btScalar MIN_FORWARD_SLOP = 0.12f; // HACK: not sure why this is necessary to detect steepest walkable slope + btScalar forwardSlop = (_maxStepHeight + _radius / cosTheta - _radius) * (cosTheta / sinTheta) - (_radius + stepLength) + MIN_FORWARD_SLOP; + + btScalar adjacent = (_radius + stepLength + forwardSlop); + btScalar opposite = (_topPoints[0].dot(_currentUp) + (_radius + _halfHeight)) + _radius / cosTheta - _radius; + btScalar angle = atanf(opposite/adjacent); + if (forwardSlop < 0.0f) { + // BIG step, no slop necessary forwardSlop = 0.0f; - needToRemoveForwardSlop = false; } for (int32_t i = 0; i < _topPoints.size(); ++i) { @@ -151,10 +174,9 @@ bool MyCharacterController::testRayShotgun(const glm::vec3& position, const glm: // remove both forwardSlop and backSlop result.hitFraction = glm::min(1.0f, (closestRayResult.m_closestHitFraction * (backSlop + stepLength + forwardSlop) - backSlop) / stepLength); } else { - if (needToRemoveForwardSlop && result.hitFraction < 1.0f) { - // remove forwardSlop - result.hitFraction = (closestRayResult.m_closestHitFraction * (backSlop + stepLength + forwardSlop)) / (backSlop + stepLength); - } + // remove forwardSlop + result.hitFraction = (closestRayResult.m_closestHitFraction * (backSlop + stepLength + forwardSlop)) / (backSlop + stepLength); + // scan the bottom for (int32_t i = 0; i < _bottomPoints.size(); ++i) { rayStart = newPosition + rotation * _bottomPoints[i] - backSlop * rayDirection; From a4308f08a42b177b245a698e4b38394d11275b03 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 18 Oct 2016 16:50:19 -0700 Subject: [PATCH 088/101] remove debug cruft --- interface/src/avatar/MyCharacterController.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/interface/src/avatar/MyCharacterController.cpp b/interface/src/avatar/MyCharacterController.cpp index c33dac888f..42bae7ad2a 100644 --- a/interface/src/avatar/MyCharacterController.cpp +++ b/interface/src/avatar/MyCharacterController.cpp @@ -144,11 +144,6 @@ bool MyCharacterController::testRayShotgun(const glm::vec3& position, const glm: btScalar sinTheta = sqrtf(1.0f - cosTheta * cosTheta); const btScalar MIN_FORWARD_SLOP = 0.12f; // HACK: not sure why this is necessary to detect steepest walkable slope btScalar forwardSlop = (_maxStepHeight + _radius / cosTheta - _radius) * (cosTheta / sinTheta) - (_radius + stepLength) + MIN_FORWARD_SLOP; - - btScalar adjacent = (_radius + stepLength + forwardSlop); - btScalar opposite = (_topPoints[0].dot(_currentUp) + (_radius + _halfHeight)) + _radius / cosTheta - _radius; - btScalar angle = atanf(opposite/adjacent); - if (forwardSlop < 0.0f) { // BIG step, no slop necessary forwardSlop = 0.0f; From f414ffaf98815f208c62eb5cb095d224f25dae56 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 18 Oct 2016 21:03:02 -0700 Subject: [PATCH 089/101] fix crash in MyCharacterController on shutdown --- interface/src/avatar/MyCharacterController.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/interface/src/avatar/MyCharacterController.cpp b/interface/src/avatar/MyCharacterController.cpp index 42bae7ad2a..86c6bea81a 100644 --- a/interface/src/avatar/MyCharacterController.cpp +++ b/interface/src/avatar/MyCharacterController.cpp @@ -40,8 +40,9 @@ MyCharacterController::~MyCharacterController() { void MyCharacterController::setDynamicsWorld(btDynamicsWorld* world) { CharacterController::setDynamicsWorld(world); - - initRayShotgun(world); + if (world) { + initRayShotgun(world); + } } void MyCharacterController::updateShapeIfNecessary() { From 66479808ba420a0082ee3aef8308eaa12c7c2d48 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 19 Oct 2016 12:06:23 -0700 Subject: [PATCH 090/101] cleanup velocity calculation in HMD mode --- interface/src/avatar/MyAvatar.cpp | 25 ++++++++++++++----------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index a936e03943..6107fbc3a0 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -1324,8 +1324,7 @@ void MyAvatar::updateMotors() { } if (qApp->isHMDMode()) { - // OUTOFBODY_HACK: only apply vertical component of _actionMotorVelocity to the characterController - _characterController.addMotor(glm::vec3(0.0f, _actionMotorVelocity.y, 0.0f), motorRotation, DEFAULT_MOTOR_TIMESCALE, INVALID_MOTOR_TIMESCALE); + // OUTOFBODY_HACK: motors are applied differently in HMDMode } else { if (_isPushing || _isBraking || !_isBeingPushed) { _characterController.addMotor(_actionMotorVelocity, motorRotation, DEFAULT_MOTOR_TIMESCALE, INVALID_MOTOR_TIMESCALE); @@ -1346,8 +1345,7 @@ void MyAvatar::updateMotors() { motorRotation = glm::quat(); } if (qApp->isHMDMode()) { - // OUTOFBODY_HACK: only apply vertical component of _scriptedMotorVelocity to the characterController - _characterController.addMotor(glm::vec3(0, _scriptedMotorVelocity.y, 0), motorRotation, DEFAULT_MOTOR_TIMESCALE, INVALID_MOTOR_TIMESCALE); + // OUTOFBODY_HACK: motors are applied differently in HMDMode } else { _characterController.addMotor(_scriptedMotorVelocity, motorRotation, _scriptedMotorTimescale); } @@ -1901,10 +1899,12 @@ void MyAvatar::updatePosition(float deltaTime) { if (qApp->isHMDMode()) { // Apply _actionMotorVelocity directly to the sensorToWorld matrix. glm::quat motorRotation; - glm::quat liftRotation; - swingTwistDecomposition(glmExtractRotation(_sensorToWorldMatrix * getHMDSensorMatrix()), _worldUpDirection, liftRotation, motorRotation); - glm::vec3 worldVelocity = motorRotation * _actionMotorVelocity; - applyVelocityToSensorToWorldMatrix(worldVelocity, deltaTime); + glm::vec3 worldVelocity = glm::vec3(0.0f); + if (_motionBehaviors & AVATAR_MOTION_ACTION_MOTOR_ENABLED) { + glm::quat liftRotation; + swingTwistDecomposition(glmExtractRotation(_sensorToWorldMatrix * getHMDSensorMatrix()), _worldUpDirection, liftRotation, motorRotation); + worldVelocity = motorRotation * _actionMotorVelocity; + } // Apply _scriptedMotorVelocity to the sensorToWorld matrix. if (_motionBehaviors & AVATAR_MOTION_SCRIPTED_MOTOR_ENABLED) { @@ -1916,13 +1916,16 @@ void MyAvatar::updatePosition(float deltaTime) { // world-frame motorRotation = glm::quat(); } - worldVelocity = motorRotation * _scriptedMotorVelocity; - applyVelocityToSensorToWorldMatrix(worldVelocity, deltaTime); + worldVelocity += motorRotation * _scriptedMotorVelocity; } // OUTOFBODY_HACK: apply scaling factor to _thrust, to get the same behavior as an periodically applied motor. const float THRUST_DAMPING_FACTOR = 0.25f; - applyVelocityToSensorToWorldMatrix(THRUST_DAMPING_FACTOR * _thrust, deltaTime); + worldVelocity += THRUST_DAMPING_FACTOR * _thrust; + + if (glm::length2(worldVelocity) > FLT_EPSILON) { + applyVelocityToSensorToWorldMatrix(worldVelocity, deltaTime); + } } } From 06866957d01343cfc0277ab8eeb094a3edb94f14 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 19 Oct 2016 15:34:11 -0700 Subject: [PATCH 091/101] first pass (theoretical) HMD ray-traced step logic --- .../src/avatar/MyCharacterController.cpp | 166 +++++++++++++++++- interface/src/avatar/MyCharacterController.h | 2 + 2 files changed, 160 insertions(+), 8 deletions(-) mode change 100644 => 100755 interface/src/avatar/MyCharacterController.cpp diff --git a/interface/src/avatar/MyCharacterController.cpp b/interface/src/avatar/MyCharacterController.cpp old mode 100644 new mode 100755 index 86c6bea81a..4cd2f65cbd --- a/interface/src/avatar/MyCharacterController.cpp +++ b/interface/src/avatar/MyCharacterController.cpp @@ -92,6 +92,13 @@ void MyCharacterController::updateShapeIfNecessary() { } bool MyCharacterController::testRayShotgun(const glm::vec3& position, const glm::vec3& step, RayShotgunResult& result) { + btVector3 rayDirection = glmToBullet(step); + btScalar stepLength = rayDirection.length(); + if (stepLength < FLT_EPSILON) { + return false; + } + rayDirection /= stepLength; + // get _ghost ready for ray traces btTransform transform = _rigidBody->getWorldTransform(); btVector3 newPosition = glmToBullet(position); @@ -104,14 +111,6 @@ bool MyCharacterController::testRayShotgun(const glm::vec3& position, const glm: CharacterRayResult closestRayResult(&_ghost); btVector3 rayStart; btVector3 rayEnd; - btVector3 rayDirection = glmToBullet(step); - - btScalar stepLength = rayDirection.length(); - if (stepLength < FLT_EPSILON) { - return false; - } - rayDirection /= stepLength; - const btScalar backSlop = 0.04f; // compute rotation that will orient local ray start points to face step direction btVector3 forward = rotation * btVector3(0.0f, 0.0f, -1.0f); @@ -150,6 +149,7 @@ bool MyCharacterController::testRayShotgun(const glm::vec3& position, const glm: forwardSlop = 0.0f; } + const btScalar backSlop = 0.04f; for (int32_t i = 0; i < _topPoints.size(); ++i) { rayStart = newPosition + rotation * _topPoints[i] - backSlop * rayDirection; rayEnd = rayStart + (backSlop + stepLength + forwardSlop) * rayDirection; @@ -191,6 +191,156 @@ bool MyCharacterController::testRayShotgun(const glm::vec3& position, const glm: return result.hitFraction < 1.0f; } +glm::vec3 MyCharacterController::computeHMDStep(const glm::vec3& position, const glm::vec3& step) { + btVector3 stepDirection = glmToBullet(step); + btScalar stepLength = stepDirection.length(); + if (stepLength < FLT_EPSILON) { + return glm::vec3(0.0f); + } + stepDirection /= stepLength; + + // get _ghost ready for ray traces + btTransform transform = _rigidBody->getWorldTransform(); + btVector3 newPosition = glmToBullet(position); + transform.setOrigin(newPosition); + btMatrix3x3 rotation = transform.getBasis(); + _ghost.setWorldTransform(transform); + _ghost.refreshOverlappingPairCache(); + + // compute rotation that will orient local ray start points to face stepDirection + btVector3 forward = rotation * btVector3(0.0f, 0.0f, -1.0f); + btVector3 horizontalDirection = stepDirection - stepDirection.dot(_currentUp) * _currentUp; + btVector3 axis = forward.cross(horizontalDirection); + btScalar lengthAxis = axis.length(); + if (lengthAxis > FLT_EPSILON) { + // non-zero sideways component + btScalar angle = acosf(lengthAxis / horizontalDirection.length()); + if (stepDirection.dot(forward) < 0.0f) { + angle = PI - angle; + } + axis /= lengthAxis; + rotation = btMatrix3x3(btQuaternion(axis, angle)) * rotation; + } else if (stepDirection.dot(forward) < 0.0f) { + // backwards + rotation = btMatrix3x3(btQuaternion(_currentUp, PI)) * rotation; + } + + // scan the top + // NOTE: if we scan an extra distance forward we can detect flat surfaces that are too steep to walk on. + // The approximate extra distance can be derived with trigonometry. + // + // minimumForward = [ (maxStepHeight + radius / cosTheta - radius) * (cosTheta / sinTheta) - radius ] + // + // where: theta = max angle between floor normal and vertical + // + // if stepLength is not long enough we can add the difference. + // + btScalar cosTheta = _minFloorNormalDotUp; + btScalar sinTheta = sqrtf(1.0f - cosTheta * cosTheta); + const btScalar MIN_FORWARD_SLOP = 0.12f; // HACK: not sure why this is necessary to detect steepest walkable slope + btScalar forwardSlop = (_maxStepHeight + _radius / cosTheta - _radius) * (cosTheta / sinTheta) - (_radius + stepLength) + MIN_FORWARD_SLOP; + if (forwardSlop < 0.0f) { + // BIG step, no slop necessary + forwardSlop = 0.0f; + } + + // we push the step forward by stepMargin to help reduce accidental overlap + btScalar stepMargin = 0.04f; + btScalar expandedStepLength = stepLength + forwardSlop + stepMargin; + + // loop + CharacterRayResult rayResult(&_ghost); + CharacterRayResult closestRayResult(&_ghost); + btVector3 rayStart; + btVector3 rayEnd; + btVector3 overlap = btVector3(0.0f, 0.0f, 0.0f); + int32_t numOverlaps = 0; + bool walkable = true; + for (int32_t i = 0; i < _topPoints.size(); ++i) { + rayStart = newPosition + rotation * _topPoints[i]; + rayEnd = rayStart + expandedStepLength * stepDirection; + rayResult.m_closestHitFraction = 1.0f; // reset rayResult for next test + if (_ghost.rayTest(rayStart, rayEnd, rayResult)) { + // track closest hit + if (rayResult.m_closestHitFraction < closestRayResult.m_closestHitFraction) { + closestRayResult = rayResult; + } + // check if walkable + if (walkable) { + if (rayResult.m_hitNormalWorld.dot(_currentUp) < _minFloorNormalDotUp) { + walkable = false; + } + } + // sum any overlap + btScalar distanceToPlane = -rayResult.m_closestHitFraction * stepLength * stepDirection.dot(rayResult.m_hitNormalWorld); + if (distanceToPlane < stepMargin) { + overlap += (stepMargin - distanceToPlane) * rayResult.m_hitNormalWorld; + ++numOverlaps; + } + } + } + // remove expansion from the closestHitFraction + btScalar closestHitFraction = glm::min(1.0f, (closestRayResult.m_closestHitFraction * expandedStepLength - stepMargin) / stepLength); + + /* + // scan the bottom + closestRayResult.m_closestHitFraction = 1.0f; // reset closestRayResult for next barrage + btScalar stepHeight = _minStepHeight; + for (int32_t i = 0; i < _bottomPoints.size(); ++i) { + rayStart = newPosition + rotation * _bottomPoints[i]; + rayEnd = rayStart + (stepLength + stepMargin) * stepDirection; + rayResult.m_closestHitFraction = 1.0f; // reset rayResult for next test + if (_ghost.rayTest(rayStart, rayEnd, rayResult)) { + // track closest hit + if (rayResult.m_closestHitFraction < closestRayResult.m_closestHitFraction) { + closestRayResult = rayResult; + } + // sum any overlap + btScalar distanceToPlane = -rayResult.m_closestHitFraction * stepDirection * stepDirection.dot(rayResult.m_hitNormalWorld); + if (distanceToPlane < stepMargin) { + overlap += (stepMargin - distanceToPlane) * rayResult.m_hitNormalWorld; + ++numOverlaps; + } + } + } + btScalar adjustedHitFraction = (closestRayResult.m_closestHitFraction * (stepLength - stepMargin) - stepMargin) / stepLength; + if (adjustedHitFraction < closestHitFraction) { + closestHitFraction = adjustedHitFraction; + } + */ + + // compute the final step + btVector3 finalStep = (closestHitFraction * stepLength) * stepDirection; + if (numOverlaps > 0) { + // we have two independent measures of displacement: finalStep and overlap + if (numOverlaps > 1) { + overlap /= (btScalar)numOverlaps; + } + + // reconcile distinct displacements as follows: + // add components that point in opposite directions, otherwise take the component with largest absolute value + btVector3 product = finalStep; + product *= overlap; // component-wise multiplication --> negative components point in opposite directions + if (product.getX() < 0.0f) { + finalStep.setX(finalStep.getX() + overlap.getX()); + } else if (fabsf(overlap.getX()) > fabsf(finalStep.getX())) { + finalStep.setX(overlap.getX()); + } + if (product.getY() < 0.0f) { + finalStep.setY(finalStep.getY() + overlap.getY()); + } else if (fabsf(overlap.getY()) > fabsf(finalStep.getY())) { + finalStep.setY(overlap.getY()); + } + if (product.getZ() < 0.0f) { + finalStep.setZ(finalStep.getZ() + overlap.getZ()); + } else if (fabsf(overlap.getZ()) > fabsf(finalStep.getZ())) { + finalStep.setZ(overlap.getZ()); + } + } + + return bulletToGLM(finalStep); +} // foo + btConvexHullShape* MyCharacterController::computeShape() const { // HACK: the avatar collides using convex hull with a collision margin equal to // the old capsule radius. Two points define a capsule and additional points are diff --git a/interface/src/avatar/MyCharacterController.h b/interface/src/avatar/MyCharacterController.h index f57aa45050..6680aa41db 100644 --- a/interface/src/avatar/MyCharacterController.h +++ b/interface/src/avatar/MyCharacterController.h @@ -43,6 +43,8 @@ public: /// return true if RayShotgun hits anything bool testRayShotgun(const glm::vec3& position, const glm::vec3& step, RayShotgunResult& result); + glm::vec3 computeHMDStep(const glm::vec3& position, const glm::vec3& step); + protected: void initRayShotgun(const btCollisionWorld* world); From 8ecc23f62b6d91608ba92201c2274e72427ddfaf Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 21 Oct 2016 16:18:51 -0700 Subject: [PATCH 092/101] HMD drive stops against simple walls --- interface/src/avatar/MyAvatar.cpp | 6 +- .../src/avatar/MyCharacterController.cpp | 131 +++++++++--------- 2 files changed, 71 insertions(+), 66 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 6107fbc3a0..01e19fc6e9 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -1924,7 +1924,11 @@ void MyAvatar::updatePosition(float deltaTime) { worldVelocity += THRUST_DAMPING_FACTOR * _thrust; if (glm::length2(worldVelocity) > FLT_EPSILON) { - applyVelocityToSensorToWorldMatrix(worldVelocity, deltaTime); + glm::mat4 worldBodyMatrix = _sensorToWorldMatrix * _bodySensorMatrix; + glm::vec3 position = extractTranslation(worldBodyMatrix); + glm::vec3 step = deltaTime * worldVelocity; + glm::vec3 newVelocity = _characterController.computeHMDStep(position, step) / deltaTime; + applyVelocityToSensorToWorldMatrix(newVelocity, deltaTime); } } } diff --git a/interface/src/avatar/MyCharacterController.cpp b/interface/src/avatar/MyCharacterController.cpp index 4cd2f65cbd..99d2bc2032 100755 --- a/interface/src/avatar/MyCharacterController.cpp +++ b/interface/src/avatar/MyCharacterController.cpp @@ -224,7 +224,6 @@ glm::vec3 MyCharacterController::computeHMDStep(const glm::vec3& position, const // backwards rotation = btMatrix3x3(btQuaternion(_currentUp, PI)) * rotation; } - // scan the top // NOTE: if we scan an extra distance forward we can detect flat surfaces that are too steep to walk on. // The approximate extra distance can be derived with trigonometry. @@ -244,99 +243,101 @@ glm::vec3 MyCharacterController::computeHMDStep(const glm::vec3& position, const forwardSlop = 0.0f; } - // we push the step forward by stepMargin to help reduce accidental overlap - btScalar stepMargin = 0.04f; + // we push the step forward by stepMargin to help reduce accidental penetration + btScalar stepMargin = glm::max(_radius, 0.4f); btScalar expandedStepLength = stepLength + forwardSlop + stepMargin; + bool slideOnWalls = false; // HACK: hard coded for now, maybe we'll make it optional // loop CharacterRayResult rayResult(&_ghost); - CharacterRayResult closestRayResult(&_ghost); btVector3 rayStart; btVector3 rayEnd; - btVector3 overlap = btVector3(0.0f, 0.0f, 0.0f); - int32_t numOverlaps = 0; + btVector3 penetration = btVector3(0.0f, 0.0f, 0.0f); + btVector3 rescuePenetration = penetration; + int32_t numPenetrations = 0; + btScalar closestHitFraction = 1.0f; bool walkable = true; for (int32_t i = 0; i < _topPoints.size(); ++i) { rayStart = newPosition + rotation * _topPoints[i]; rayEnd = rayStart + expandedStepLength * stepDirection; rayResult.m_closestHitFraction = 1.0f; // reset rayResult for next test if (_ghost.rayTest(rayStart, rayEnd, rayResult)) { - // track closest hit - if (rayResult.m_closestHitFraction < closestRayResult.m_closestHitFraction) { - closestRayResult = rayResult; - } // check if walkable if (walkable) { if (rayResult.m_hitNormalWorld.dot(_currentUp) < _minFloorNormalDotUp) { walkable = false; } } - // sum any overlap - btScalar distanceToPlane = -rayResult.m_closestHitFraction * stepLength * stepDirection.dot(rayResult.m_hitNormalWorld); - if (distanceToPlane < stepMargin) { - overlap += (stepMargin - distanceToPlane) * rayResult.m_hitNormalWorld; - ++numOverlaps; + btScalar adjustedHitFraction = (rayResult.m_closestHitFraction * expandedStepLength - stepMargin) / stepLength; + if (adjustedHitFraction < 1.0f) { + if (slideOnWalls) { + // sum penetration + btScalar depth = ((1.0f - adjustedHitFraction) * stepLength) * stepDirection.dot(rayResult.m_hitNormalWorld); + penetration -= depth * rayResult.m_hitNormalWorld; + ++numPenetrations; + } else if (adjustedHitFraction < 0.0f) { + // evidence suggests we need to back out of penetration however there is a + // literal corner case where backing out may put us in deeper penetration, + // so we check for it by casting another ray straight out from center + rayEnd = rayStart; + rayStart = rayStart.dot(_currentUp) * _currentUp; + btVector3 segment = rayEnd - rayStart; + btScalar radius = segment.length(); + if (radius > FLT_EPSILON) { + rayEnd += (stepMargin / radius) * segment; + CharacterRayResult checkResult(&_ghost); + if (_ghost.rayTest(rayStart, rayEnd, checkResult)) { + if (checkResult.m_hitNormalWorld.dot(stepDirection) > 0.0f) { + // the second test says backing out would be a bad idea + continue; + } + } + } + } + if (adjustedHitFraction < closestHitFraction) { + closestHitFraction = adjustedHitFraction; + const btScalar STEEP_ENOUGH_TO_BACK_OUT = -0.3f; + if (adjustedHitFraction < 0.0f && stepDirection.dot(rayResult.m_hitNormalWorld) < STEEP_ENOUGH_TO_BACK_OUT) { + closestHitFraction = 0.0f; + } + } } } } - // remove expansion from the closestHitFraction - btScalar closestHitFraction = glm::min(1.0f, (closestRayResult.m_closestHitFraction * expandedStepLength - stepMargin) / stepLength); - /* - // scan the bottom - closestRayResult.m_closestHitFraction = 1.0f; // reset closestRayResult for next barrage - btScalar stepHeight = _minStepHeight; - for (int32_t i = 0; i < _bottomPoints.size(); ++i) { - rayStart = newPosition + rotation * _bottomPoints[i]; - rayEnd = rayStart + (stepLength + stepMargin) * stepDirection; - rayResult.m_closestHitFraction = 1.0f; // reset rayResult for next test - if (_ghost.rayTest(rayStart, rayEnd, rayResult)) { - // track closest hit - if (rayResult.m_closestHitFraction < closestRayResult.m_closestHitFraction) { - closestRayResult = rayResult; - } - // sum any overlap - btScalar distanceToPlane = -rayResult.m_closestHitFraction * stepDirection * stepDirection.dot(rayResult.m_hitNormalWorld); - if (distanceToPlane < stepMargin) { - overlap += (stepMargin - distanceToPlane) * rayResult.m_hitNormalWorld; - ++numOverlaps; + /* TODO: implement sliding along sloped floors + bool steppingUp = false; + if (walkable && closestHitFraction > 0.0f) { + // scan the bottom + for (int32_t i = 0; i < _bottomPoints.size(); ++i) { + rayStart = newPosition + rotation * _bottomPoints[i]; + rayEnd = rayStart + (stepLength + stepMargin) * stepDirection; + rayResult.m_closestHitFraction = 1.0f; // reset rayResult for next test + if (_ghost.rayTest(rayStart, rayEnd, rayResult)) { + btScalar adjustedHitFraction = (rayResult.m_closestHitFraction * expandedStepLength - stepMargin) / stepLength; + if (adjustedHitFraction < 1.0f) { + steppingUp = true; + break; + } } } } - btScalar adjustedHitFraction = (closestRayResult.m_closestHitFraction * (stepLength - stepMargin) - stepMargin) / stepLength; - if (adjustedHitFraction < closestHitFraction) { - closestHitFraction = adjustedHitFraction; - } */ - // compute the final step - btVector3 finalStep = (closestHitFraction * stepLength) * stepDirection; - if (numOverlaps > 0) { - // we have two independent measures of displacement: finalStep and overlap - if (numOverlaps > 1) { - overlap /= (btScalar)numOverlaps; - } - - // reconcile distinct displacements as follows: - // add components that point in opposite directions, otherwise take the component with largest absolute value - btVector3 product = finalStep; - product *= overlap; // component-wise multiplication --> negative components point in opposite directions - if (product.getX() < 0.0f) { - finalStep.setX(finalStep.getX() + overlap.getX()); - } else if (fabsf(overlap.getX()) > fabsf(finalStep.getX())) { - finalStep.setX(overlap.getX()); - } - if (product.getY() < 0.0f) { - finalStep.setY(finalStep.getY() + overlap.getY()); - } else if (fabsf(overlap.getY()) > fabsf(finalStep.getY())) { - finalStep.setY(overlap.getY()); - } - if (product.getZ() < 0.0f) { - finalStep.setZ(finalStep.getZ() + overlap.getZ()); - } else if (fabsf(overlap.getZ()) > fabsf(finalStep.getZ())) { - finalStep.setZ(overlap.getZ()); + btVector3 finalStep = stepLength * stepDirection; + if (slideOnWalls) { + if (numPenetrations > 1) { + penetration /= (btScalar)numPenetrations; } + finalStep += penetration; + } else { + finalStep *= closestHitFraction; } + /* TODO: implement sliding along sloped floors + if (steppingUp) { + finalStep += stepLength * _currentUp; + } + */ return bulletToGLM(finalStep); } // foo From ccc6f8eee2a2e00b158dbd28c963939f2f3b46e7 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 21 Oct 2016 16:42:25 -0700 Subject: [PATCH 093/101] fix teleport.js to work for out-of-body --- scripts/system/controllers/teleport.js | 14 +------------- 1 file changed, 1 insertion(+), 13 deletions(-) diff --git a/scripts/system/controllers/teleport.js b/scripts/system/controllers/teleport.js index a05d9eb220..ffead2eb6b 100644 --- a/scripts/system/controllers/teleport.js +++ b/scripts/system/controllers/teleport.js @@ -15,9 +15,6 @@ var inTeleportMode = false; var SMOOTH_ARRIVAL_SPACING = 33; var NUMBER_OF_STEPS_FOR_TELEPORT = 6; -var AVATAR_DRAG_SPACING = 33; -var NUMBER_OF_STEPS_FOR_AVATAR_DRAG = 12; - var TARGET_MODEL_URL = Script.resolvePath("../assets/models/teleport-destination.fbx"); var TOO_CLOSE_MODEL_URL = Script.resolvePath("../assets/models/teleport-cancel.fbx"); var TARGET_MODEL_DIMENSIONS = { @@ -91,10 +88,7 @@ function Teleporter() { this.cancelOverlay = null; this.updateConnected = null; this.smoothArrivalInterval = null; - this.dragAvatarInterval = null; - this.oldAvatarCollisionsEnabled = MyAvatar.avatarCollisionsEnabled; this.teleportHand = null; - this.distance = 0.0; this.teleportMode = "HMDAndAvatarTogether"; this.tooClose = false; this.inCoolIn = false; @@ -136,10 +130,6 @@ function Teleporter() { if (this.smoothArrivalInterval !== null) { Script.clearInterval(this.smoothArrivalInterval); } - if (this.dragAvatarInterval !== null) { - Script.clearInterval(this.dragAvatarInterval); - MyAvatar.avatarCollisionsEnabled = _this.oldAvatarCollisionsEnabled; - } if (activationTimeout !== null) { Script.clearInterval(activationTimeout); } @@ -503,7 +493,6 @@ function Teleporter() { z: intersection.intersection.z }; - this.distance = Vec3.distance(MyAvatar.position, position); this.tooClose = isValidTeleportLocation(position, intersection.surfaceNormal); var towardUs = Quat.fromPitchYawRollDegrees(0, euler.y, 0); @@ -526,7 +515,6 @@ function Teleporter() { z: intersection.intersection.z }; - this.distance = Vec3.distance(MyAvatar.position, position); this.tooClose = isValidTeleportLocation(position, intersection.surfaceNormal); var towardUs = Quat.fromPitchYawRollDegrees(0, euler.y, 0); @@ -558,7 +546,7 @@ function Teleporter() { this.intersection.intersection.y += offset; this.exitTeleportMode(); if (MyAvatar.hmdLeanRecenterEnabled) { - if (this.distance > MAX_HMD_AVATAR_SEPARATION) { + if (Vec3.distance(MyAvatar.position, _this.intersection.intersection) > MAX_HMD_AVATAR_SEPARATION) { this.teleportMode = "HMDAndAvatarTogether"; } else { this.teleportMode = "HMDFirstAvatarWillFollow"; From 42c6996aeeb04a401b3d47d92579212e67a60bfe Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 24 Oct 2016 14:05:28 -0700 Subject: [PATCH 094/101] fix warning about unused variable --- interface/src/avatar/MyCharacterController.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/interface/src/avatar/MyCharacterController.cpp b/interface/src/avatar/MyCharacterController.cpp index 99d2bc2032..7e799a1166 100755 --- a/interface/src/avatar/MyCharacterController.cpp +++ b/interface/src/avatar/MyCharacterController.cpp @@ -253,7 +253,6 @@ glm::vec3 MyCharacterController::computeHMDStep(const glm::vec3& position, const btVector3 rayStart; btVector3 rayEnd; btVector3 penetration = btVector3(0.0f, 0.0f, 0.0f); - btVector3 rescuePenetration = penetration; int32_t numPenetrations = 0; btScalar closestHitFraction = 1.0f; bool walkable = true; @@ -340,7 +339,7 @@ glm::vec3 MyCharacterController::computeHMDStep(const glm::vec3& position, const */ return bulletToGLM(finalStep); -} // foo +} btConvexHullShape* MyCharacterController::computeShape() const { // HACK: the avatar collides using convex hull with a collision margin equal to From 69bd4ccea262e09f2c08fae70a7bc199554cf851 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 24 Oct 2016 18:16:05 -0700 Subject: [PATCH 095/101] Show hand controllers when out-of-body for more then 3/4 of a second Without the timer, the hands can flicker in and out of visibility when lightly brushing against collision. --- interface/src/avatar/MyAvatar.cpp | 28 ++++++++++++++++++++++++++ interface/src/avatar/MyAvatar.h | 6 +++++- interface/src/avatar/SkeletonModel.cpp | 4 ++-- 3 files changed, 35 insertions(+), 3 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 01e19fc6e9..dabffb4307 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -88,6 +88,8 @@ const float MyAvatar::ZOOM_DEFAULT = 1.5f; extern glm::vec3 TRUNCATE_IK_CAPSULE_POSITION; extern float TRUNCATE_IK_CAPSULE_LENGTH; extern float TRUNCATE_IK_CAPSULE_RADIUS; +extern float MIN_OUT_OF_BODY_DISTANCE; +extern float MAX_OUT_OF_BODY_DISTANCE; MyAvatar::MyAvatar(RigPointer rig) : Avatar(rig), @@ -365,6 +367,27 @@ void MyAvatar::update(float deltaTime) { simulate(deltaTime); + // Request to show the hand controllers if we're out-of-body for more then HAND_CONTROLLER_SHOW_TIME. + // Similarlly request to hide the controllers when we return to our bodies. + const float HAND_CONTROLLER_SHOW_TIME = 0.75f; + auto hmdInterface = DependencyManager::get(); + if (isOutOfBody() != _handControllerShow) { + _handControllerShowTimer += deltaTime; + if (_handControllerShowTimer > HAND_CONTROLLER_SHOW_TIME) { + if (isOutOfBody()) { + hmdInterface->requestShowHandControllers(); + _handControllerShow = true; + _handControllerShowTimer = 0.0f; + } else { + hmdInterface->requestHideHandControllers(); + _handControllerShow = false; + _handControllerShowTimer = 0.0f; + } + } + } else { + _handControllerShowTimer = 0.0f; + } + currentEnergy += energyChargeRate; currentEnergy -= getAccelerationEnergy(); currentEnergy -= getAudioEnergy(); @@ -2210,6 +2233,10 @@ glm::quat MyAvatar::getOrientationForAudio() { return quat(); } +bool MyAvatar::isOutOfBody() const { + return _follow._isOutOfBody; +} + void MyAvatar::setAudioListenerMode(AudioListenerMode audioListenerMode) { if (_audioListenerMode != audioListenerMode) { _audioListenerMode = audioListenerMode; @@ -2361,6 +2388,7 @@ void MyAvatar::FollowHelper::postPhysicsUpdate(MyAvatar& myAvatar) { glm::vec3 capsuleStart = myAvatar.getPosition() + Vectors::UNIT_Y * (TRUNCATE_IK_CAPSULE_LENGTH / 2.0f); glm::vec3 capsuleEnd = myAvatar.getPosition() - Vectors::UNIT_Y * (TRUNCATE_IK_CAPSULE_LENGTH / 2.0f); _isOutOfBody = !pointIsInsideCapsule(worldHMDPosition, capsuleStart, capsuleEnd, TRUNCATE_IK_CAPSULE_RADIUS); + _outOfBodyDistance = distanceFromCapsule(worldHMDPosition, capsuleStart, capsuleEnd, TRUNCATE_IK_CAPSULE_RADIUS); } float MyAvatar::getAccelerationEnergy() { diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 340e877dd4..4ca7f65519 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -333,7 +333,7 @@ public slots: glm::vec3 getPositionForAudio(); glm::quat getOrientationForAudio(); - bool isOutOfBody() const { return _follow._isOutOfBody; } + bool isOutOfBody() const; signals: void audioListenerModeChanged(); @@ -467,6 +467,7 @@ private: }; uint8_t _activeBits { 0 }; bool _isOutOfBody { false }; + float _outOfBodyDistance { 0.0f }; void deactivate(); void deactivate(FollowType type); @@ -541,6 +542,9 @@ private: }; DebugDrawVertex _debugLineLoop[DEBUG_LINE_LOOP_SIZE]; size_t _debugLineLoopIndex { 0 }; + + bool _handControllerShow { false }; + float _handControllerShowTimer { 0.0f }; }; QScriptValue audioListenModeToScriptValue(QScriptEngine* engine, const AudioListenerMode& audioListenerMode); diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index 02ffb903a1..b1e0f31062 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -26,6 +26,8 @@ glm::vec3 TRUNCATE_IK_CAPSULE_POSITION(0.0f, 0.0f, 0.0f); float TRUNCATE_IK_CAPSULE_LENGTH = 1000.0f; float TRUNCATE_IK_CAPSULE_RADIUS = 0.25f; +float MIN_OUT_OF_BODY_DISTANCE = TRUNCATE_IK_CAPSULE_RADIUS - 0.1f; +float MAX_OUT_OF_BODY_DISTANCE = TRUNCATE_IK_CAPSULE_RADIUS + 0.1f; SkeletonModel::SkeletonModel(Avatar* owningAvatar, QObject* parent, RigPointer rig) : Model(rig, parent), @@ -165,8 +167,6 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { Rig::HandParameters handParams; // compute interp factor between in body and out of body hand positions. - const float MIN_OUT_OF_BODY_DISTANCE = TRUNCATE_IK_CAPSULE_RADIUS - 0.1f; - const float MAX_OUT_OF_BODY_DISTANCE = TRUNCATE_IK_CAPSULE_RADIUS + 0.1f; glm::vec3 capsuleStart = Vectors::UNIT_Y * (TRUNCATE_IK_CAPSULE_LENGTH / 2.0f); glm::vec3 capsuleEnd = -Vectors::UNIT_Y * (TRUNCATE_IK_CAPSULE_LENGTH / 2.0f); float outOfBodyAlpha = distanceFromCapsule(hmdPositionInRigSpace, capsuleStart, capsuleEnd, TRUNCATE_IK_CAPSULE_RADIUS); From 5e273664dbd19998c106a4b72b46896d133d73ec Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 26 Oct 2016 16:39:15 -0700 Subject: [PATCH 096/101] improved ray tests for HMD driving --- .../src/avatar/MyCharacterController.cpp | 189 +++++++++--------- 1 file changed, 96 insertions(+), 93 deletions(-) diff --git a/interface/src/avatar/MyCharacterController.cpp b/interface/src/avatar/MyCharacterController.cpp index 7e799a1166..69c0d3b2fe 100755 --- a/interface/src/avatar/MyCharacterController.cpp +++ b/interface/src/avatar/MyCharacterController.cpp @@ -214,7 +214,7 @@ glm::vec3 MyCharacterController::computeHMDStep(const glm::vec3& position, const btScalar lengthAxis = axis.length(); if (lengthAxis > FLT_EPSILON) { // non-zero sideways component - btScalar angle = acosf(lengthAxis / horizontalDirection.length()); + btScalar angle = asinf(lengthAxis / horizontalDirection.length()); if (stepDirection.dot(forward) < 0.0f) { angle = PI - angle; } @@ -224,6 +224,53 @@ glm::vec3 MyCharacterController::computeHMDStep(const glm::vec3& position, const // backwards rotation = btMatrix3x3(btQuaternion(_currentUp, PI)) * rotation; } + + CharacterRayResult rayResult(&_ghost); + btVector3 rayStart; + btVector3 rayEnd; + btVector3 penetration = btVector3(0.0f, 0.0f, 0.0f); + int32_t numPenetrations = 0; + + { // first we scan straight out from capsule center to see if we're stuck on anything + btScalar forwardRatio = 0.5f; + btScalar backRatio = 0.25f; + + btVector3 radial; + bool stuck = false; + for (int32_t i = 0; i < _topPoints.size(); ++i) { + rayStart = rotation * _topPoints[i]; + radial = rayStart - rayStart.dot(_currentUp) * _currentUp; + rayEnd = newPosition + rayStart + forwardRatio * radial; + rayStart += newPosition - backRatio * radial; + + // reset rayResult for next test + rayResult.m_closestHitFraction = 1.0f; + rayResult.m_collisionObject = nullptr; + + if (_ghost.rayTest(rayStart, rayEnd, rayResult)) { + btScalar totalRatio = backRatio + forwardRatio; + btScalar adjustedHitFraction = (rayResult.m_closestHitFraction * totalRatio - backRatio) / forwardRatio; + if (adjustedHitFraction < 0.0f) { + penetration += adjustedHitFraction * radial; + ++numPenetrations; + } else { + stuck = true; + } + } + } + if (numPenetrations > 0) { + if (numPenetrations > 1) { + penetration /= (btScalar)numPenetrations; + } + return bulletToGLM(penetration); + } else if (stuck) { + return glm::vec3(0.0f); + } + } + + // if we get here then we're not stuck pushing into any surface + // so now we scan to see if the way before us is "walkable" + // scan the top // NOTE: if we scan an extra distance forward we can detect flat surfaces that are too steep to walk on. // The approximate extra distance can be derived with trigonometry. @@ -236,7 +283,7 @@ glm::vec3 MyCharacterController::computeHMDStep(const glm::vec3& position, const // btScalar cosTheta = _minFloorNormalDotUp; btScalar sinTheta = sqrtf(1.0f - cosTheta * cosTheta); - const btScalar MIN_FORWARD_SLOP = 0.12f; // HACK: not sure why this is necessary to detect steepest walkable slope + const btScalar MIN_FORWARD_SLOP = 0.10f; // HACK: not sure why this is necessary to detect steepest walkable slope btScalar forwardSlop = (_maxStepHeight + _radius / cosTheta - _radius) * (cosTheta / sinTheta) - (_radius + stepLength) + MIN_FORWARD_SLOP; if (forwardSlop < 0.0f) { // BIG step, no slop necessary @@ -244,101 +291,56 @@ glm::vec3 MyCharacterController::computeHMDStep(const glm::vec3& position, const } // we push the step forward by stepMargin to help reduce accidental penetration - btScalar stepMargin = glm::max(_radius, 0.4f); + const btScalar MIN_STEP_MARGIN = 0.04f; + btScalar stepMargin = glm::max(_radius, MIN_STEP_MARGIN); btScalar expandedStepLength = stepLength + forwardSlop + stepMargin; - bool slideOnWalls = false; // HACK: hard coded for now, maybe we'll make it optional - // loop - CharacterRayResult rayResult(&_ghost); - btVector3 rayStart; - btVector3 rayEnd; - btVector3 penetration = btVector3(0.0f, 0.0f, 0.0f); - int32_t numPenetrations = 0; - btScalar closestHitFraction = 1.0f; + // loop over topPoints bool walkable = true; for (int32_t i = 0; i < _topPoints.size(); ++i) { rayStart = newPosition + rotation * _topPoints[i]; rayEnd = rayStart + expandedStepLength * stepDirection; - rayResult.m_closestHitFraction = 1.0f; // reset rayResult for next test + + // reset rayResult for next test + rayResult.m_closestHitFraction = 1.0f; + rayResult.m_collisionObject = nullptr; + if (_ghost.rayTest(rayStart, rayEnd, rayResult)) { - // check if walkable - if (walkable) { - if (rayResult.m_hitNormalWorld.dot(_currentUp) < _minFloorNormalDotUp) { - walkable = false; - } - } - btScalar adjustedHitFraction = (rayResult.m_closestHitFraction * expandedStepLength - stepMargin) / stepLength; - if (adjustedHitFraction < 1.0f) { - if (slideOnWalls) { - // sum penetration - btScalar depth = ((1.0f - adjustedHitFraction) * stepLength) * stepDirection.dot(rayResult.m_hitNormalWorld); - penetration -= depth * rayResult.m_hitNormalWorld; - ++numPenetrations; - } else if (adjustedHitFraction < 0.0f) { - // evidence suggests we need to back out of penetration however there is a - // literal corner case where backing out may put us in deeper penetration, - // so we check for it by casting another ray straight out from center - rayEnd = rayStart; - rayStart = rayStart.dot(_currentUp) * _currentUp; - btVector3 segment = rayEnd - rayStart; - btScalar radius = segment.length(); - if (radius > FLT_EPSILON) { - rayEnd += (stepMargin / radius) * segment; - CharacterRayResult checkResult(&_ghost); - if (_ghost.rayTest(rayStart, rayEnd, checkResult)) { - if (checkResult.m_hitNormalWorld.dot(stepDirection) > 0.0f) { - // the second test says backing out would be a bad idea - continue; - } - } - } - } - if (adjustedHitFraction < closestHitFraction) { - closestHitFraction = adjustedHitFraction; - const btScalar STEEP_ENOUGH_TO_BACK_OUT = -0.3f; - if (adjustedHitFraction < 0.0f && stepDirection.dot(rayResult.m_hitNormalWorld) < STEEP_ENOUGH_TO_BACK_OUT) { - closestHitFraction = 0.0f; - } - } + if (rayResult.m_hitNormalWorld.dot(_currentUp) < _minFloorNormalDotUp) { + walkable = false; + break; } } } - /* TODO: implement sliding along sloped floors + // scan the bottom + // TODO: implement sliding along sloped floors bool steppingUp = false; - if (walkable && closestHitFraction > 0.0f) { - // scan the bottom - for (int32_t i = 0; i < _bottomPoints.size(); ++i) { - rayStart = newPosition + rotation * _bottomPoints[i]; - rayEnd = rayStart + (stepLength + stepMargin) * stepDirection; - rayResult.m_closestHitFraction = 1.0f; // reset rayResult for next test - if (_ghost.rayTest(rayStart, rayEnd, rayResult)) { - btScalar adjustedHitFraction = (rayResult.m_closestHitFraction * expandedStepLength - stepMargin) / stepLength; - if (adjustedHitFraction < 1.0f) { - steppingUp = true; - break; - } + expandedStepLength = stepLength + MIN_FORWARD_SLOP + MIN_STEP_MARGIN; + for (int32_t i = _bottomPoints.size() - 1; i > -1; --i) { + rayStart = newPosition + rotation * _bottomPoints[i] - MIN_STEP_MARGIN * stepDirection; + rayEnd = rayStart + expandedStepLength * stepDirection; + + // reset rayResult for next test + rayResult.m_closestHitFraction = 1.0f; + rayResult.m_collisionObject = nullptr; + + if (_ghost.rayTest(rayStart, rayEnd, rayResult)) { + btScalar adjustedHitFraction = (rayResult.m_closestHitFraction * expandedStepLength - MIN_STEP_MARGIN) / (stepLength + MIN_FORWARD_SLOP); + if (adjustedHitFraction < 1.0f) { + steppingUp = true; + break; } } } - */ - btVector3 finalStep = stepLength * stepDirection; - if (slideOnWalls) { - if (numPenetrations > 1) { - penetration /= (btScalar)numPenetrations; - } - finalStep += penetration; - } else { - finalStep *= closestHitFraction; + if (!walkable && steppingUp ) { + return glm::vec3(0.0f); } - /* TODO: implement sliding along sloped floors - if (steppingUp) { - finalStep += stepLength * _currentUp; - } - */ + // else it might not be walkable, but we aren't steppingUp yet which means we can still move forward - return bulletToGLM(finalStep); + // TODO: slide up ramps and fall off edges (then we can remove the vertical follow of Avatar's RigidBody) + return step; } btConvexHullShape* MyCharacterController::computeShape() const { @@ -405,23 +407,23 @@ void MyCharacterController::initRayShotgun(const btCollisionWorld* world) { const btScalar topHeight = fullHalfHeight - divisionLine; const btScalar slop = 0.02f; - const int32_t numRows = 3; // must be odd number > 1 - const int32_t numColumns = 3; // must be odd number > 1 + const int32_t NUM_ROWS = 5; // must be odd number > 1 + const int32_t NUM_COLUMNS = 5; // must be odd number > 1 btVector3 reach = (2.0f * _radius) * btVector3(0.0f, 0.0f, 1.0f); { // top points _topPoints.clear(); - _topPoints.reserve(numRows * numColumns); - btScalar stepY = (topHeight - slop) / (btScalar)(numRows - 1); - btScalar stepX = 2.0f * (_radius - slop) / (btScalar)(numColumns - 1); + _topPoints.reserve(NUM_ROWS * NUM_COLUMNS); + btScalar stepY = (topHeight - slop) / (btScalar)(NUM_ROWS - 1); + btScalar stepX = 2.0f * (_radius - slop) / (btScalar)(NUM_COLUMNS - 1); btTransform transform = _rigidBody->getWorldTransform(); btVector3 position = transform.getOrigin(); btMatrix3x3 rotation = transform.getBasis(); - for (int32_t i = 0; i < numRows; ++i) { - int32_t maxJ = numColumns; - btScalar offsetX = -(btScalar)((numColumns - 1) / 2) * stepX; + for (int32_t i = 0; i < NUM_ROWS; ++i) { + int32_t maxJ = NUM_COLUMNS; + btScalar offsetX = -(btScalar)((NUM_COLUMNS - 1) / 2) * stepX; if (i%2 == 1) { // odd rows have one less point and start a halfStep closer maxJ -= 1; @@ -441,18 +443,19 @@ void MyCharacterController::initRayShotgun(const btCollisionWorld* world) { { // bottom points _bottomPoints.clear(); - _bottomPoints.reserve(numRows * numColumns); + _bottomPoints.reserve(NUM_ROWS * NUM_COLUMNS); - btScalar stepY = (_maxStepHeight - 2.0f * slop) / (btScalar)(numRows - 1); - btScalar stepX = 2.0f * (_radius - slop) / (btScalar)(numColumns - 1); + btScalar steepestStepHitHeight = (_radius + 0.04f) * (1.0f - DEFAULT_MIN_FLOOR_NORMAL_DOT_UP); + btScalar stepY = (_maxStepHeight - slop - steepestStepHitHeight) / (btScalar)(NUM_ROWS - 1); + btScalar stepX = 2.0f * (_radius - slop) / (btScalar)(NUM_COLUMNS - 1); btTransform transform = _rigidBody->getWorldTransform(); btVector3 position = transform.getOrigin(); btMatrix3x3 rotation = transform.getBasis(); - for (int32_t i = 0; i < numRows; ++i) { - int32_t maxJ = numColumns; - btScalar offsetX = -(btScalar)((numColumns - 1) / 2) * stepX; + for (int32_t i = 0; i < NUM_ROWS; ++i) { + int32_t maxJ = NUM_COLUMNS; + btScalar offsetX = -(btScalar)((NUM_COLUMNS - 1) / 2) * stepX; if (i%2 == 1) { // odd rows have one less point and start a halfStep closer maxJ -= 1; From a04b7ae297794644cda46d1caacfc3ca2f8dcc1e Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 15 Nov 2016 11:45:09 -0800 Subject: [PATCH 097/101] fix hover-drive in HMD --- interface/src/avatar/MyAvatar.cpp | 32 +++++++++++++------ libraries/physics/src/CharacterController.cpp | 14 +++++--- 2 files changed, 32 insertions(+), 14 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 431b971dc6..72675d7a48 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -465,7 +465,9 @@ void MyAvatar::simulate(float deltaTime) { // update sensorToWorldMatrix for camera and hand controllers // before we perform rig animations and IK. - updateSensorToWorldMatrix(_enableVerticalComfortMode ? SensorToWorldUpdateMode::VerticalComfort : SensorToWorldUpdateMode::Vertical); + if (_characterController.getState() != CharacterController::State::Hover) { + updateSensorToWorldMatrix(_enableVerticalComfortMode ? SensorToWorldUpdateMode::VerticalComfort : SensorToWorldUpdateMode::Vertical); + } { PerformanceTimer perfTimer("skeleton"); @@ -1356,7 +1358,8 @@ void MyAvatar::updateMotors() { } if (qApp->isHMDMode()) { - // OUTOFBODY_HACK: motors are applied differently in HMDMode + // OUTOFBODY_HACK: in HMDMode motors are applied differently: a "follow" motor is added + // during the CharacterController's substep } else { if (_isPushing || _isBraking || !_isBeingPushed) { _characterController.addMotor(_actionMotorVelocity, motorRotation, DEFAULT_MOTOR_TIMESCALE, INVALID_MOTOR_TIMESCALE); @@ -1887,12 +1890,17 @@ void MyAvatar::updateActionMotor(float deltaTime) { } void MyAvatar::applyVelocityToSensorToWorldMatrix(const glm::vec3& velocity, float deltaTime) { - glm::vec3 horizontalVelocity(velocity.x, 0.0f, velocity.z); - float speed2 = glm::length2(horizontalVelocity); + glm::vec3 newVelocity = velocity; + if (_characterController.getState() != CharacterController::State::Hover) { + newVelocity -= glm::dot(newVelocity, _worldUpDirection); + } + float speed2 = glm::length2(newVelocity); if (speed2 > MIN_AVATAR_SPEED_SQUARED) { - glm::vec3 position = extractTranslation(_sensorToWorldMatrix) + deltaTime * horizontalVelocity; + glm::vec3 position = extractTranslation(_sensorToWorldMatrix) + deltaTime * newVelocity; // update the position column of matrix - _sensorToWorldMatrix[3] = glm::vec4(position, 1); + glm::mat4 newSensorToWorldMatrix = _sensorToWorldMatrix; + newSensorToWorldMatrix[3] = glm::vec4(position, 1.0f); + setSensorToWorldMatrix(newSensorToWorldMatrix); } } @@ -1929,16 +1937,19 @@ void MyAvatar::updatePosition(float deltaTime) { } if (qApp->isHMDMode()) { - // Apply _actionMotorVelocity directly to the sensorToWorld matrix. glm::quat motorRotation; glm::vec3 worldVelocity = glm::vec3(0.0f); if (_motionBehaviors & AVATAR_MOTION_ACTION_MOTOR_ENABLED) { - glm::quat liftRotation; - swingTwistDecomposition(glmExtractRotation(_sensorToWorldMatrix * getHMDSensorMatrix()), _worldUpDirection, liftRotation, motorRotation); + if (_characterController.getState() == CharacterController::State::Hover || + _characterController.getCollisionGroup() == BULLET_COLLISION_GROUP_COLLISIONLESS) { + motorRotation = glmExtractRotation(_sensorToWorldMatrix * getHMDSensorMatrix()); + } else { + glm::quat liftRotation; + swingTwistDecomposition(glmExtractRotation(_sensorToWorldMatrix * getHMDSensorMatrix()), _worldUpDirection, liftRotation, motorRotation); + } worldVelocity = motorRotation * _actionMotorVelocity; } - // Apply _scriptedMotorVelocity to the sensorToWorld matrix. if (_motionBehaviors & AVATAR_MOTION_SCRIPTED_MOTOR_ENABLED) { if (_scriptedMotorFrame == SCRIPTED_MOTOR_CAMERA_FRAME) { motorRotation = getHead()->getCameraOrientation() * glm::angleAxis(PI, Vectors::UNIT_Y); @@ -1955,6 +1966,7 @@ void MyAvatar::updatePosition(float deltaTime) { const float THRUST_DAMPING_FACTOR = 0.25f; worldVelocity += THRUST_DAMPING_FACTOR * _thrust; + // apply velocity directly to _sensorToWorldMatrix. if (glm::length2(worldVelocity) > FLT_EPSILON) { glm::mat4 worldBodyMatrix = _sensorToWorldMatrix * _bodySensorMatrix; glm::vec3 position = extractTranslation(worldBodyMatrix); diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index b348a773e4..100905dc5b 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -277,10 +277,15 @@ void CharacterController::playerStep(btCollisionWorld* collisionWorld, btScalar } vel += _followVelocity; const float HORIZONTAL_FOLLOW_TIMESCALE = 0.05f; - const float VERTICAL_FOLLOW_TIMESCALE = (_state == State::Hover) ? HORIZONTAL_FOLLOW_TIMESCALE : 20.0f; + float verticalFollowTimescale = 20.0f; + if (_state == State::Hover) { + verticalFollowTimescale = HORIZONTAL_FOLLOW_TIMESCALE; + } else { + // remove vertical component + vel -= vel.dot(_currentUp) * _currentUp; + } glm::quat worldFrameRotation; // identity - vel.setY(0.0f); // don't allow any vertical component of the follow velocity to enter the _targetVelocity. - addMotor(bulletToGLM(vel), worldFrameRotation, HORIZONTAL_FOLLOW_TIMESCALE, VERTICAL_FOLLOW_TIMESCALE); + addMotor(bulletToGLM(vel), worldFrameRotation, HORIZONTAL_FOLLOW_TIMESCALE, verticalFollowTimescale); } // angular part uses incremental teleports @@ -773,7 +778,8 @@ void CharacterController::updateState() { const btScalar MAX_WALKING_SPEED = 2.5f; bool flyingFast = _state == State::Hover && actualHorizVelocity.length() > (MAX_WALKING_SPEED * 0.75f); - if ((_floorDistance < MIN_HOVER_HEIGHT) && !jumpButtonHeld && !flyingFast) { + if ((_floorDistance < MIN_HOVER_HEIGHT) && + !(jumpButtonHeld || flyingFast || (now - _jumpButtonDownStartTime) > JUMP_TO_HOVER_PERIOD)) { SET_STATE(State::InAir, "near ground"); } else if (((_floorDistance < FLY_TO_GROUND_THRESHOLD) || _hasSupport) && !flyingFast) { SET_STATE(State::Ground, "touching ground"); From d749697c46238ffd90f5de7800f52650a971b371 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 15 Nov 2016 12:05:44 -0800 Subject: [PATCH 098/101] scan bottom points during hover-HMD-drive --- .../src/avatar/MyCharacterController.cpp | 33 ++++++++++++++----- 1 file changed, 25 insertions(+), 8 deletions(-) diff --git a/interface/src/avatar/MyCharacterController.cpp b/interface/src/avatar/MyCharacterController.cpp index 69c0d3b2fe..2f6e92a098 100755 --- a/interface/src/avatar/MyCharacterController.cpp +++ b/interface/src/avatar/MyCharacterController.cpp @@ -15,8 +15,6 @@ #include "MyAvatar.h" -// DONE TODO: improve walking up steps -// DONE TODO: make avatars able to walk up and down steps/slopes // TODO: make avatars stand on steep slope // TODO: make avatars not snag on low ceilings @@ -160,20 +158,39 @@ bool MyCharacterController::testRayShotgun(const glm::vec3& position, const glm: if (result.walkable) { if (rayResult.m_hitNormalWorld.dot(_currentUp) < _minFloorNormalDotUp) { result.walkable = false; - break; + // the top scan wasn't walkable so don't bother scanning the bottom + // remove both forwardSlop and backSlop + result.hitFraction = glm::min(1.0f, (closestRayResult.m_closestHitFraction * (backSlop + stepLength + forwardSlop) - backSlop) / stepLength); + return result.hitFraction < 1.0f; } } } } - if (!result.walkable) { - // the top scan wasn't walkable so don't bother scanning the bottom - // remove both forwardSlop and backSlop - result.hitFraction = glm::min(1.0f, (closestRayResult.m_closestHitFraction * (backSlop + stepLength + forwardSlop) - backSlop) / stepLength); + if (_state == State::Hover) { + // scan the bottom just like the top + for (int32_t i = 0; i < _bottomPoints.size(); ++i) { + rayStart = newPosition + rotation * _bottomPoints[i] - backSlop * rayDirection; + rayEnd = rayStart + (backSlop + stepLength + forwardSlop) * rayDirection; + if (_ghost.rayTest(rayStart, rayEnd, rayResult)) { + if (rayResult.m_closestHitFraction < closestRayResult.m_closestHitFraction) { + closestRayResult = rayResult; + } + if (result.walkable) { + if (rayResult.m_hitNormalWorld.dot(_currentUp) < _minFloorNormalDotUp) { + result.walkable = false; + // the bottom scan wasn't walkable + // remove both forwardSlop and backSlop + result.hitFraction = glm::min(1.0f, (closestRayResult.m_closestHitFraction * (backSlop + stepLength + forwardSlop) - backSlop) / stepLength); + return result.hitFraction < 1.0f; + } + } + } + } } else { + // scan the bottom looking for nearest step point // remove forwardSlop result.hitFraction = (closestRayResult.m_closestHitFraction * (backSlop + stepLength + forwardSlop)) / (backSlop + stepLength); - // scan the bottom for (int32_t i = 0; i < _bottomPoints.size(); ++i) { rayStart = newPosition + rotation * _bottomPoints[i] - backSlop * rayDirection; rayEnd = rayStart + (backSlop + stepLength) * rayDirection; From 445c730ca37f84b3d5af6893aaaaa612b254f4a6 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 1 Dec 2016 10:38:13 -0800 Subject: [PATCH 099/101] remove cruft, fix formatting and spelling --- interface/src/avatar/MyAvatar.cpp | 3 --- interface/src/avatar/MyCharacterController.cpp | 6 ++---- interface/src/avatar/MyCharacterController.h | 2 -- libraries/physics/src/CharacterGhostObject.cpp | 4 ++-- libraries/physics/src/CharacterGhostObject.h | 2 +- 5 files changed, 5 insertions(+), 12 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 72675d7a48..59f1c9a1f6 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -1444,9 +1444,6 @@ void MyAvatar::harvestResultsFromPhysicsSimulation(float deltaTime) { } nextAttitude(position, orientation); - // compute new _bodyToSensorMatrix - //_bodySensorMatrix = deriveBodyFromHMDSensor(); - if (_characterController.isEnabledAndReady()) { setVelocity(_characterController.getLinearVelocity()); } diff --git a/interface/src/avatar/MyCharacterController.cpp b/interface/src/avatar/MyCharacterController.cpp index 2f6e92a098..4161352e01 100755 --- a/interface/src/avatar/MyCharacterController.cpp +++ b/interface/src/avatar/MyCharacterController.cpp @@ -20,9 +20,7 @@ void MyCharacterController::RayShotgunResult::reset() { - //hitNormal = glm::vec3(0.0f); hitFraction = 1.0f; - //numHits = 0; walkable = true; } @@ -441,7 +439,7 @@ void MyCharacterController::initRayShotgun(const btCollisionWorld* world) { for (int32_t i = 0; i < NUM_ROWS; ++i) { int32_t maxJ = NUM_COLUMNS; btScalar offsetX = -(btScalar)((NUM_COLUMNS - 1) / 2) * stepX; - if (i%2 == 1) { + if (i % 2 == 1) { // odd rows have one less point and start a halfStep closer maxJ -= 1; offsetX += 0.5f * stepX; @@ -473,7 +471,7 @@ void MyCharacterController::initRayShotgun(const btCollisionWorld* world) { for (int32_t i = 0; i < NUM_ROWS; ++i) { int32_t maxJ = NUM_COLUMNS; btScalar offsetX = -(btScalar)((NUM_COLUMNS - 1) / 2) * stepX; - if (i%2 == 1) { + if (i % 2 == 1) { // odd rows have one less point and start a halfStep closer maxJ -= 1; offsetX += 0.5f * stepX; diff --git a/interface/src/avatar/MyCharacterController.h b/interface/src/avatar/MyCharacterController.h index 6680aa41db..4c465185d6 100644 --- a/interface/src/avatar/MyCharacterController.h +++ b/interface/src/avatar/MyCharacterController.h @@ -34,9 +34,7 @@ public: public: void reset(); - //glm::vec3 hitNormal { 0.0f, 0.0f, 0.0f }; float hitFraction { 1.0f }; - //int32_t numHits { 0 }; bool walkable { true }; }; diff --git a/libraries/physics/src/CharacterGhostObject.cpp b/libraries/physics/src/CharacterGhostObject.cpp index 2cdcd2a879..563605cd16 100755 --- a/libraries/physics/src/CharacterGhostObject.cpp +++ b/libraries/physics/src/CharacterGhostObject.cpp @@ -1,6 +1,6 @@ // // CharacterGhostObject.cpp -// libraries/physcis/src +// libraries/physics/src // // Created by Andrew Meadows 2016.08.26 // Copyright 2016 High Fidelity, Inc. @@ -289,7 +289,7 @@ void CharacterGhostObject::measurePenetration(btVector3& minBoxOut, btVector3& m btScalar mostFloorPenetration = 0.0f; collisionPair->m_algorithm->getAllContactManifolds(manifoldArray); - for (int j = 0;j < manifoldArray.size(); j++) { + for (int j = 0; j < manifoldArray.size(); j++) { btPersistentManifold* manifold = manifoldArray[j]; btScalar directionSign = (manifold->getBody0() == this) ? btScalar(1.0) : btScalar(-1.0); for (int p = 0; p < manifold->getNumContacts(); p++) { diff --git a/libraries/physics/src/CharacterGhostObject.h b/libraries/physics/src/CharacterGhostObject.h index 1da280124e..feb132a53e 100755 --- a/libraries/physics/src/CharacterGhostObject.h +++ b/libraries/physics/src/CharacterGhostObject.h @@ -1,6 +1,6 @@ // // CharacterGhostObject.h -// libraries/physcis/src +// libraries/physics/src // // Created by Andrew Meadows 2016.08.26 // Copyright 2016 High Fidelity, Inc. From ae699e2a654e60bbfd6f60a3ba88328e32f18e5f Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 1 Dec 2016 10:41:40 -0800 Subject: [PATCH 100/101] add initializer for member variable --- libraries/physics/src/CharacterController.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h index 492b5c387e..1b54fd1917 100644 --- a/libraries/physics/src/CharacterController.h +++ b/libraries/physics/src/CharacterController.h @@ -169,7 +169,7 @@ protected: quint32 _takeoffJumpButtonID; // data for walking up steps - btVector3 _stepPoint; + btVector3 _stepPoint { 0.0f, 0.0f, 0.0f }; btVector3 _stepNormal { 0.0f, 0.0f, 0.0f }; btVector3 _stepUpVelocity { 0.0f, 0.0f, 0.0f }; btScalar _stepHeight { 0.0f }; From 2bbe2be51649049a9e6b150b1a4e8ec1bd9f41da Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 1 Dec 2016 10:50:10 -0800 Subject: [PATCH 101/101] fix spelling: physcis --> physics --- libraries/physics/src/BulletUtil.h | 2 +- libraries/physics/src/CharacterController.cpp | 2 +- libraries/physics/src/CharacterController.h | 2 +- libraries/physics/src/CharacterGhostShape.cpp | 2 +- libraries/physics/src/CharacterGhostShape.h | 2 +- libraries/physics/src/CharacterRayResult.cpp | 2 +- libraries/physics/src/CharacterRayResult.h | 2 +- libraries/physics/src/CharacterSweepResult.cpp | 2 +- libraries/physics/src/CharacterSweepResult.h | 2 +- libraries/physics/src/CollisionRenderMeshCache.cpp | 2 +- libraries/physics/src/CollisionRenderMeshCache.h | 2 +- libraries/physics/src/ContactInfo.cpp | 2 +- libraries/physics/src/ContactInfo.h | 2 +- libraries/physics/src/ObjectAction.cpp | 2 +- libraries/physics/src/ObjectAction.h | 2 +- libraries/physics/src/ObjectMotionState.cpp | 2 +- libraries/physics/src/ObjectMotionState.h | 2 +- libraries/physics/src/PhysicalEntitySimulation.cpp | 2 +- libraries/physics/src/PhysicalEntitySimulation.h | 2 +- libraries/physics/src/PhysicsEngine.cpp | 2 +- libraries/physics/src/PhysicsEngine.h | 2 +- libraries/physics/src/ShapeFactory.cpp | 2 +- libraries/physics/src/ShapeFactory.h | 2 +- libraries/physics/src/ShapeManager.cpp | 2 +- libraries/physics/src/ShapeManager.h | 2 +- libraries/shared/src/BackgroundMode.h | 2 +- libraries/shared/src/ShapeInfo.cpp | 2 +- libraries/shared/src/ShapeInfo.h | 2 +- 28 files changed, 28 insertions(+), 28 deletions(-) diff --git a/libraries/physics/src/BulletUtil.h b/libraries/physics/src/BulletUtil.h index b6fac74617..c456ed8af8 100644 --- a/libraries/physics/src/BulletUtil.h +++ b/libraries/physics/src/BulletUtil.h @@ -1,6 +1,6 @@ // // BulletUtil.h -// libraries/physcis/src +// libraries/physics/src // // Created by Andrew Meadows 2014.11.02 // Copyright 2014 High Fidelity, Inc. diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 100905dc5b..e15a1eeaf0 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -1,6 +1,6 @@ // // CharacterControllerInterface.cpp -// libraries/physcis/src +// libraries/physics/src // // Created by Andrew Meadows 2015.10.21 // Copyright 2015 High Fidelity, Inc. diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h index 1b54fd1917..75cbc0730b 100644 --- a/libraries/physics/src/CharacterController.h +++ b/libraries/physics/src/CharacterController.h @@ -1,6 +1,6 @@ // // CharacterControllerInterface.h -// libraries/physcis/src +// libraries/physics/src // // Created by Andrew Meadows 2015.10.21 // Copyright 2015 High Fidelity, Inc. diff --git a/libraries/physics/src/CharacterGhostShape.cpp b/libraries/physics/src/CharacterGhostShape.cpp index e5a5cb59de..09f4f0b80f 100644 --- a/libraries/physics/src/CharacterGhostShape.cpp +++ b/libraries/physics/src/CharacterGhostShape.cpp @@ -1,6 +1,6 @@ // // CharacterGhostShape.cpp -// libraries/physcis/src +// libraries/physics/src // // Created by Andrew Meadows 2016.09.14 // Copyright 2016 High Fidelity, Inc. diff --git a/libraries/physics/src/CharacterGhostShape.h b/libraries/physics/src/CharacterGhostShape.h index 614bc168af..dc75c148d5 100644 --- a/libraries/physics/src/CharacterGhostShape.h +++ b/libraries/physics/src/CharacterGhostShape.h @@ -1,6 +1,6 @@ // // CharacterGhostShape.h -// libraries/physcis/src +// libraries/physics/src // // Created by Andrew Meadows 2016.09.14 // Copyright 2016 High Fidelity, Inc. diff --git a/libraries/physics/src/CharacterRayResult.cpp b/libraries/physics/src/CharacterRayResult.cpp index e0d5212e52..7a81e9cca6 100755 --- a/libraries/physics/src/CharacterRayResult.cpp +++ b/libraries/physics/src/CharacterRayResult.cpp @@ -1,6 +1,6 @@ // // CharaterRayResult.cpp -// libraries/physcis/src +// libraries/physics/src // // Created by Andrew Meadows 2016.09.05 // Copyright 2016 High Fidelity, Inc. diff --git a/libraries/physics/src/CharacterRayResult.h b/libraries/physics/src/CharacterRayResult.h index b74959aa34..e8b0bb7f99 100644 --- a/libraries/physics/src/CharacterRayResult.h +++ b/libraries/physics/src/CharacterRayResult.h @@ -1,6 +1,6 @@ // // CharaterRayResult.h -// libraries/physcis/src +// libraries/physics/src // // Created by Andrew Meadows 2016.09.05 // Copyright 2016 High Fidelity, Inc. diff --git a/libraries/physics/src/CharacterSweepResult.cpp b/libraries/physics/src/CharacterSweepResult.cpp index 13937354ae..a5c4092b1d 100755 --- a/libraries/physics/src/CharacterSweepResult.cpp +++ b/libraries/physics/src/CharacterSweepResult.cpp @@ -1,6 +1,6 @@ // // CharaterSweepResult.cpp -// libraries/physcis/src +// libraries/physics/src // // Created by Andrew Meadows 2016.09.01 // Copyright 2016 High Fidelity, Inc. diff --git a/libraries/physics/src/CharacterSweepResult.h b/libraries/physics/src/CharacterSweepResult.h index f80bd0bb47..1e2898a3cf 100644 --- a/libraries/physics/src/CharacterSweepResult.h +++ b/libraries/physics/src/CharacterSweepResult.h @@ -1,6 +1,6 @@ // // CharaterSweepResult.h -// libraries/physcis/src +// libraries/physics/src // // Created by Andrew Meadows 2016.09.01 // Copyright 2016 High Fidelity, Inc. diff --git a/libraries/physics/src/CollisionRenderMeshCache.cpp b/libraries/physics/src/CollisionRenderMeshCache.cpp index 3a1c4d0ea4..40a8a4aff9 100644 --- a/libraries/physics/src/CollisionRenderMeshCache.cpp +++ b/libraries/physics/src/CollisionRenderMeshCache.cpp @@ -1,6 +1,6 @@ // // CollisionRenderMeshCache.cpp -// libraries/physcis/src +// libraries/physics/src // // Created by Andrew Meadows 2016.07.13 // Copyright 2016 High Fidelity, Inc. diff --git a/libraries/physics/src/CollisionRenderMeshCache.h b/libraries/physics/src/CollisionRenderMeshCache.h index 910b43996e..6a6857a5ae 100644 --- a/libraries/physics/src/CollisionRenderMeshCache.h +++ b/libraries/physics/src/CollisionRenderMeshCache.h @@ -1,6 +1,6 @@ // // CollisionRenderMeshCache.h -// libraries/physcis/src +// libraries/physics/src // // Created by Andrew Meadows 2016.07.13 // Copyright 2016 High Fidelity, Inc. diff --git a/libraries/physics/src/ContactInfo.cpp b/libraries/physics/src/ContactInfo.cpp index c2ea6e8671..9acf105d3a 100644 --- a/libraries/physics/src/ContactInfo.cpp +++ b/libraries/physics/src/ContactInfo.cpp @@ -1,6 +1,6 @@ // // ContactEvent.cpp -// libraries/physcis/src +// libraries/physics/src // // Created by Andrew Meadows 2015.01.20 // Copyright 2015 High Fidelity, Inc. diff --git a/libraries/physics/src/ContactInfo.h b/libraries/physics/src/ContactInfo.h index 11c908a414..bbd2fdb460 100644 --- a/libraries/physics/src/ContactInfo.h +++ b/libraries/physics/src/ContactInfo.h @@ -1,6 +1,6 @@ // // ContactEvent.h -// libraries/physcis/src +// libraries/physics/src // // Created by Andrew Meadows 2015.01.20 // Copyright 2015 High Fidelity, Inc. diff --git a/libraries/physics/src/ObjectAction.cpp b/libraries/physics/src/ObjectAction.cpp index ca64cabe5f..0d5948a793 100644 --- a/libraries/physics/src/ObjectAction.cpp +++ b/libraries/physics/src/ObjectAction.cpp @@ -1,6 +1,6 @@ // // ObjectAction.cpp -// libraries/physcis/src +// libraries/physics/src // // Created by Seth Alves 2015-6-2 // Copyright 2015 High Fidelity, Inc. diff --git a/libraries/physics/src/ObjectAction.h b/libraries/physics/src/ObjectAction.h index 43330269ac..1327fd7c6f 100644 --- a/libraries/physics/src/ObjectAction.h +++ b/libraries/physics/src/ObjectAction.h @@ -1,6 +1,6 @@ // // ObjectAction.h -// libraries/physcis/src +// libraries/physics/src // // Created by Seth Alves 2015-6-2 // Copyright 2015 High Fidelity, Inc. diff --git a/libraries/physics/src/ObjectMotionState.cpp b/libraries/physics/src/ObjectMotionState.cpp index 38f079c1d4..503b39dc1c 100644 --- a/libraries/physics/src/ObjectMotionState.cpp +++ b/libraries/physics/src/ObjectMotionState.cpp @@ -1,6 +1,6 @@ // // ObjectMotionState.cpp -// libraries/physcis/src +// libraries/physics/src // // Created by Andrew Meadows 2014.11.05 // Copyright 2014 High Fidelity, Inc. diff --git a/libraries/physics/src/ObjectMotionState.h b/libraries/physics/src/ObjectMotionState.h index a7894998a8..17d75c733b 100644 --- a/libraries/physics/src/ObjectMotionState.h +++ b/libraries/physics/src/ObjectMotionState.h @@ -1,6 +1,6 @@ // // ObjectMotionState.h -// libraries/physcis/src +// libraries/physics/src // // Created by Andrew Meadows 2014.11.05 // Copyright 2014 High Fidelity, Inc. diff --git a/libraries/physics/src/PhysicalEntitySimulation.cpp b/libraries/physics/src/PhysicalEntitySimulation.cpp index 903b160a5e..dbef9b1f7d 100644 --- a/libraries/physics/src/PhysicalEntitySimulation.cpp +++ b/libraries/physics/src/PhysicalEntitySimulation.cpp @@ -1,6 +1,6 @@ // // PhysicalEntitySimulation.cpp -// libraries/physcis/src +// libraries/physics/src // // Created by Andrew Meadows 2015.04.27 // Copyright 2015 High Fidelity, Inc. diff --git a/libraries/physics/src/PhysicalEntitySimulation.h b/libraries/physics/src/PhysicalEntitySimulation.h index 24b9ba95f0..79d16604c9 100644 --- a/libraries/physics/src/PhysicalEntitySimulation.h +++ b/libraries/physics/src/PhysicalEntitySimulation.h @@ -1,6 +1,6 @@ // // PhysicalEntitySimulation.h -// libraries/physcis/src +// libraries/physics/src // // Created by Andrew Meadows 2015.04.27 // Copyright 2015 High Fidelity, Inc. diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp index ba002d925c..fa626d0c91 100644 --- a/libraries/physics/src/PhysicsEngine.cpp +++ b/libraries/physics/src/PhysicsEngine.cpp @@ -1,6 +1,6 @@ // // PhysicsEngine.cpp -// libraries/physcis/src +// libraries/physics/src // // Created by Andrew Meadows 2014.10.29 // Copyright 2014 High Fidelity, Inc. diff --git a/libraries/physics/src/PhysicsEngine.h b/libraries/physics/src/PhysicsEngine.h index bbafbb06b6..ae78802a64 100644 --- a/libraries/physics/src/PhysicsEngine.h +++ b/libraries/physics/src/PhysicsEngine.h @@ -1,6 +1,6 @@ // // PhysicsEngine.h -// libraries/physcis/src +// libraries/physics/src // // Created by Andrew Meadows 2014.10.29 // Copyright 2014 High Fidelity, Inc. diff --git a/libraries/physics/src/ShapeFactory.cpp b/libraries/physics/src/ShapeFactory.cpp index 100dab0fd1..5268278be0 100644 --- a/libraries/physics/src/ShapeFactory.cpp +++ b/libraries/physics/src/ShapeFactory.cpp @@ -1,6 +1,6 @@ // // ShapeFactory.cpp -// libraries/physcis/src +// libraries/physics/src // // Created by Andrew Meadows 2014.12.01 // Copyright 2014 High Fidelity, Inc. diff --git a/libraries/physics/src/ShapeFactory.h b/libraries/physics/src/ShapeFactory.h index a1022104dd..2bf79f390c 100644 --- a/libraries/physics/src/ShapeFactory.h +++ b/libraries/physics/src/ShapeFactory.h @@ -1,6 +1,6 @@ // // ShapeFactory.h -// libraries/physcis/src +// libraries/physics/src // // Created by Andrew Meadows 2014.12.01 // Copyright 2014 High Fidelity, Inc. diff --git a/libraries/physics/src/ShapeManager.cpp b/libraries/physics/src/ShapeManager.cpp index b61fb0037b..fd3e35d28a 100644 --- a/libraries/physics/src/ShapeManager.cpp +++ b/libraries/physics/src/ShapeManager.cpp @@ -1,6 +1,6 @@ // // ShapeManager.cpp -// libraries/physcis/src +// libraries/physics/src // // Created by Andrew Meadows 2014.10.29 // Copyright 2014 High Fidelity, Inc. diff --git a/libraries/physics/src/ShapeManager.h b/libraries/physics/src/ShapeManager.h index 261c06ddb9..ed81b5e8f8 100644 --- a/libraries/physics/src/ShapeManager.h +++ b/libraries/physics/src/ShapeManager.h @@ -1,6 +1,6 @@ // // ShapeManager.h -// libraries/physcis/src +// libraries/physics/src // // Created by Andrew Meadows 2014.10.29 // Copyright 2014 High Fidelity, Inc. diff --git a/libraries/shared/src/BackgroundMode.h b/libraries/shared/src/BackgroundMode.h index e6e585d9d8..0e0d684e62 100644 --- a/libraries/shared/src/BackgroundMode.h +++ b/libraries/shared/src/BackgroundMode.h @@ -1,6 +1,6 @@ // // BackgroundMode.h -// libraries/physcis/src +// libraries/physics/src // // Copyright 2015 High Fidelity, Inc. // diff --git a/libraries/shared/src/ShapeInfo.cpp b/libraries/shared/src/ShapeInfo.cpp index b8ea3a4272..583bceeaf2 100644 --- a/libraries/shared/src/ShapeInfo.cpp +++ b/libraries/shared/src/ShapeInfo.cpp @@ -1,6 +1,6 @@ // // ShapeInfo.cpp -// libraries/physcis/src +// libraries/physics/src // // Created by Andrew Meadows 2014.10.29 // Copyright 2014 High Fidelity, Inc. diff --git a/libraries/shared/src/ShapeInfo.h b/libraries/shared/src/ShapeInfo.h index a6ff8d6d4a..732b62ebbf 100644 --- a/libraries/shared/src/ShapeInfo.h +++ b/libraries/shared/src/ShapeInfo.h @@ -1,6 +1,6 @@ // // ShapeInfo.h -// libraries/physcis/src +// libraries/physics/src // // Created by Andrew Meadows 2014.10.29 // Copyright 2014 High Fidelity, Inc.