From e21bd7a67adfe76a3c4d57b2e766040a6eeda76d Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 30 Mar 2017 11:21:34 -0700 Subject: [PATCH] help avatar walk up steps --- interface/src/Application.cpp | 7 - interface/src/Menu.cpp | 4 + interface/src/Menu.h | 2 +- interface/src/avatar/MyAvatar.cpp | 36 +- interface/src/avatar/MyAvatar.h | 6 +- .../src/avatar/MyCharacterController.cpp | 438 +++++++++++++++++- interface/src/avatar/MyCharacterController.h | 28 +- libraries/physics/src/CharacterController.cpp | 394 ++++++++++------ libraries/physics/src/CharacterController.h | 54 ++- .../physics/src/CharacterGhostObject.cpp | 415 +++++++++++++++++ libraries/physics/src/CharacterGhostObject.h | 103 ++++ libraries/physics/src/CharacterGhostShape.cpp | 31 ++ libraries/physics/src/CharacterGhostShape.h | 25 + libraries/physics/src/CharacterRayResult.cpp | 31 ++ libraries/physics/src/CharacterRayResult.h | 44 ++ .../physics/src/CharacterSweepResult.cpp | 42 ++ libraries/physics/src/CharacterSweepResult.h | 45 ++ 17 files changed, 1501 insertions(+), 204 deletions(-) mode change 100644 => 100755 interface/src/avatar/MyAvatar.cpp mode change 100644 => 100755 interface/src/avatar/MyCharacterController.cpp mode change 100644 => 100755 libraries/physics/src/CharacterController.cpp create mode 100755 libraries/physics/src/CharacterGhostObject.cpp create mode 100755 libraries/physics/src/CharacterGhostObject.h create mode 100644 libraries/physics/src/CharacterGhostShape.cpp create mode 100644 libraries/physics/src/CharacterGhostShape.h create mode 100755 libraries/physics/src/CharacterRayResult.cpp create mode 100644 libraries/physics/src/CharacterRayResult.h create mode 100755 libraries/physics/src/CharacterSweepResult.cpp create mode 100644 libraries/physics/src/CharacterSweepResult.h diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index 3be55e82cd..20d85f76cb 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -4331,13 +4331,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); - } } } } else if (domainLoadingInProgress) { diff --git a/interface/src/Menu.cpp b/interface/src/Menu.cpp index 9688694287..7ac03ebd2e 100644 --- a/interface/src/Menu.cpp +++ b/interface/src/Menu.cpp @@ -532,6 +532,10 @@ Menu::Menu() { avatar.get(), SLOT(updateMotionBehaviorFromMenu()), UNSPECIFIED_POSITION, "Developer"); + addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::EnableAvatarCollisions, 0, true, + avatar.get(), 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 250d2241ac..72f823d3bd 100644 --- a/interface/src/Menu.h +++ b/interface/src/Menu.h @@ -96,7 +96,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 = "Collide with world"; + const QString EnableAvatarCollisions = "Enable Avatar Collisions"; const QString EnableInverseKinematics = "Enable Inverse Kinematics"; const QString EntityScriptServerLog = "Entity Script Server Log"; const QString ExpandMyAvatarSimulateTiming = "Expand /myAvatar/simulation"; diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp old mode 100644 new mode 100755 index 3f3ce7d9e9..3de69d0d86 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -150,8 +150,6 @@ MyAvatar::MyAvatar(QThread* thread, RigPointer rig) : // when we leave a domain we lift whatever restrictions that domain may have placed on our scale connect(&domainHandler, &DomainHandler::disconnectedFromDomain, this, &MyAvatar::clearScaleRestriction); - _characterController.setEnabled(true); - _bodySensorMatrix = deriveBodyFromHMDSensor(); using namespace recording; @@ -588,8 +586,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); } } @@ -1449,7 +1447,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 = getMyHead()->getCameraOrientation(); } else { // non-hovering = walking: follow camera twist about vertical but not lift @@ -1495,6 +1494,7 @@ void MyAvatar::prepareForPhysicsSimulation() { qDebug() << "Warning: getParentVelocity failed" << getID(); parentVelocity = glm::vec3(); } + _characterController.handleChangedCollisionGroup(); _characterController.setParentVelocity(parentVelocity); _characterController.setPositionAndOrientation(getPosition(), getOrientation()); @@ -1906,7 +1906,7 @@ void MyAvatar::updateActionMotor(float deltaTime) { 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; + motorSpeed *= 1.0f + glm::clamp(deltaTime / speedGrowthTimescale, 0.0f, 1.0f) * speedIncreaseFactor; const float maxBoostSpeed = getUniformScale() * MAX_BOOST_SPEED; if (_isPushing) { @@ -1949,9 +1949,17 @@ 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; + + 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); + } } // capture the head rotation, in sensor space, when the user first indicates they would like to move/fly. @@ -2188,14 +2196,13 @@ void MyAvatar::updateMotionBehaviorFromMenu() { } else { _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; } @@ -2207,11 +2214,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() { diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 7c510f0556..a20730d87a 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -128,7 +128,7 @@ class MyAvatar : public Avatar { Q_PROPERTY(float isAway READ getIsAway WRITE setAway) Q_PROPERTY(bool hmdLeanRecenterEnabled READ getHMDLeanRecenterEnabled WRITE setHMDLeanRecenterEnabled) - Q_PROPERTY(bool characterControllerEnabled READ getCharacterControllerEnabled WRITE setCharacterControllerEnabled) + Q_PROPERTY(bool avatarCollisionsEnabled READ getAvatarCollisionsEnabled WRITE setAvatarCollisionsEnabled) Q_PROPERTY(bool useAdvancedMovementControls READ useAdvancedMovementControls WRITE setUseAdvancedMovementControls) public: @@ -470,8 +470,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/interface/src/avatar/MyCharacterController.cpp b/interface/src/avatar/MyCharacterController.cpp old mode 100644 new mode 100755 index 6e52f4a949..e90022b2c5 --- a/interface/src/avatar/MyCharacterController.cpp +++ b/interface/src/avatar/MyCharacterController.cpp @@ -15,11 +15,15 @@ #include "MyAvatar.h" -// TODO: improve walking up steps -// 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() { + hitFraction = 1.0f; + walkable = true; +} + MyCharacterController::MyCharacterController(MyAvatar* avatar) { assert(avatar); @@ -30,37 +34,33 @@ MyCharacterController::MyCharacterController(MyAvatar* avatar) { MyCharacterController::~MyCharacterController() { } +void MyCharacterController::setDynamicsWorld(btDynamicsWorld* world) { + CharacterController::setDynamicsWorld(world); + if (world) { + initRayShotgun(world); + } +} + 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; - _radius = 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) { + 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); - 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(); if (shape) { delete shape; } - shape = new btCapsuleShape(_radius, 2.0f * _halfHeight); + shape = computeShape(); _rigidBody->setCollisionShape(shape); } @@ -72,12 +72,414 @@ 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); } - //_rigidBody->setCollisionFlags(btCollisionObject::CF_CHARACTER_OBJECT); + _rigidBody->setCollisionFlags(_rigidBody->getCollisionFlags() & + ~(btCollisionObject::CF_KINEMATIC_OBJECT | btCollisionObject::CF_STATIC_OBJECT)); } else { // TODO: handle this failure case } } } +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); + transform.setOrigin(newPosition); + _ghost.setWorldTransform(transform); + btMatrix3x3 rotation = transform.getBasis(); + _ghost.refreshOverlappingPairCache(); + + CharacterRayResult rayResult(&_ghost); + CharacterRayResult closestRayResult(&_ghost); + btVector3 rayStart; + btVector3 rayEnd; + + // 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 / 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; + } + + 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; + 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 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 (_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); + + 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; +} + +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 = asinf(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; + } + + 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. + // + // 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.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 + forwardSlop = 0.0f; + } + + // we push the step forward by stepMargin to help reduce accidental penetration + const btScalar MIN_STEP_MARGIN = 0.04f; + btScalar stepMargin = glm::max(_radius, MIN_STEP_MARGIN); + btScalar expandedStepLength = stepLength + forwardSlop + stepMargin; + + // loop over topPoints + bool walkable = true; + for (int32_t i = 0; i < _topPoints.size(); ++i) { + rayStart = newPosition + rotation * _topPoints[i]; + rayEnd = rayStart + expandedStepLength * stepDirection; + + // reset rayResult for next test + rayResult.m_closestHitFraction = 1.0f; + rayResult.m_collisionObject = nullptr; + + if (_ghost.rayTest(rayStart, rayEnd, rayResult)) { + if (rayResult.m_hitNormalWorld.dot(_currentUp) < _minFloorNormalDotUp) { + walkable = false; + break; + } + } + } + + // scan the bottom + // TODO: implement sliding along sloped floors + bool steppingUp = false; + 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; + } + } + } + + if (!walkable && steppingUp ) { + return glm::vec3(0.0f); + } + // else it might not be walkable, but we aren't steppingUp yet which means we can still move forward + + // 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 { + // 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 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(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 < 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; + 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(NUM_ROWS * NUM_COLUMNS); + + 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 < 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; + 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..df9d31d3c5 100644 --- a/interface/src/avatar/MyCharacterController.h +++ b/interface/src/avatar/MyCharacterController.h @@ -24,10 +24,36 @@ 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 be expensive when the obstacles are too + // complex (e.g. small 20k triangle static mesh) so instead we cast several rays forward and if they + // don't hit anything we consider it a clean sweep. Hence this "Shotgun" code. + class RayShotgunResult { + public: + void reset(); + float hitFraction { 1.0f }; + bool walkable { true }; + }; + + /// 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); + +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 old mode 100644 new mode 100755 index 751524c40b..40ca3a0826 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -13,8 +13,8 @@ #include -#include "PhysicsCollisionGroups.h" #include "ObjectMotionState.h" +#include "PhysicsHelpers.h" #include "PhysicsLogging.h" const btVector3 LOCAL_UP_AXIS(0.0f, 1.0f, 0.0f); @@ -62,10 +62,6 @@ CharacterController::CharacterMotor::CharacterMotor(const glm::vec3& vel, const } CharacterController::CharacterController() { - _halfHeight = 1.0f; - - _enabled = false; - _floorDistance = MAX_FALL_HEIGHT; _targetVelocity.setValue(0.0f, 0.0f, 0.0f); @@ -107,6 +103,7 @@ bool CharacterController::needsAddition() const { void CharacterController::setDynamicsWorld(btDynamicsWorld* world) { if (_dynamicsWorld != world) { + // remove from old world if (_dynamicsWorld) { if (_rigidBody) { _dynamicsWorld->removeRigidBody(_rigidBody); @@ -115,16 +112,25 @@ 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 - // 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); + // 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() == CONVEX_HULL_SHAPE_PROXYTYPE); + _ghost.setCharacterShape(static_cast(shape)); } + _ghost.setCollisionGroupAndMask(_collisionGroup, BULLET_COLLISION_MASK_MY_AVATAR & (~ _collisionGroup)); + _ghost.setCollisionWorld(_dynamicsWorld); + _ghost.setRadiusAndHalfHeight(_radius, _halfHeight); + _ghost.setMaxStepHeight(0.75f * (_radius + _halfHeight)); + _ghost.setMinWallAngle(PI / 4.0f); + _ghost.setUpDirection(_currentUp); + _ghost.setMotorOnly(true); + _ghost.setWorldTransform(_rigidBody->getWorldTransform()); } if (_dynamicsWorld) { if (_pendingFlags & PENDING_FLAG_UPDATE_SHAPE) { @@ -138,38 +144,78 @@ void CharacterController::setDynamicsWorld(btDynamicsWorld* world) { } } -static const float COS_PI_OVER_THREE = cosf(PI / 3.0f); +bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) { + bool pushing = _targetVelocity.length2() > FLT_EPSILON; + + btDispatcher* dispatcher = collisionWorld->getDispatcher(); + int numManifolds = dispatcher->getNumManifolds(); + bool hasFloor = false; + + btTransform rotation = _rigidBody->getWorldTransform(); + rotation.setOrigin(btVector3(0.0f, 0.0f, 0.0f)); // clear translation part -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(); + int stepContactIndex = -1; + float highestStep = _minStepHeight; 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 < _maxStepHeight && normal.dot(_currentUp) > _minFloorNormalDotUp) { + hasFloor = true; + 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; + } } + if (pushing && _targetVelocity.dot(normal) < 0.0f) { + // remember highest step obstacle + if (!_stepUpEnabled || 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) { + // remember step info for later + btManifoldPoint& contact = contactManifold->getContactPoint(stepContactIndex); + 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 && !(pushing && _stepUpEnabled)) { + // early exit since all we need to know is that we're on a floor + break; } } } - return false; + return hasFloor; +} + +void CharacterController::updateAction(btCollisionWorld* collisionWorld, btScalar deltaTime) { + 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& 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; @@ -183,21 +229,16 @@ 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) { + _stepHeight = _minStepHeight; // clears memory of last step obstacle + _hasSupport = checkForSupport(collisionWorld); 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(). const float MINIMUM_TIME_REMAINING = 0.005f; const float MAX_DISPLACEMENT = 0.5f * _radius; @@ -231,6 +272,28 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) { _rigidBody->setWorldTransform(btTransform(endRot, endPos)); } _followTime += dt; + + float stepUpSpeed2 = _stepUpVelocity.length2(); + if (stepUpSpeed2 > FLT_EPSILON) { + // we step up with micro-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()); } void CharacterController::jump() { @@ -272,95 +335,96 @@ 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(_gravity * _currentUp); + } } } _state = desiredState; } } -void CharacterController::setLocalBoundingBox(const glm::vec3& corner, const glm::vec3& scale) { - _boxScale = scale; - - float x = _boxScale.x; - float z = _boxScale.z; +void CharacterController::setLocalBoundingBox(const glm::vec3& minCorner, const glm::vec3& scale) { + 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; + 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 _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; + _shapeLocalOffset = minCorner + 0.5f * scale; } -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; + _ghost.setCollisionGroupAndMask(_collisionGroup, BULLET_COLLISION_MASK_MY_AVATAR & (~ _collisionGroup)); + } +} + +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) { + _gravity = _collisionGroup == BULLET_COLLISION_GROUP_COLLISIONLESS ? 0.0f : DEFAULT_CHARACTER_GRAVITY; + _rigidBody->setGravity(_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) { - _rigidBody->setGravity(DEFAULT_CHARACTER_GRAVITY * _currentUp); - } + _ghost.setUpDirection(_currentUp); + if (_state != State::Hover && _rigidBody) { + _rigidBody->setGravity(_gravity * _currentUp); } } void CharacterController::setPositionAndOrientation( const glm::vec3& position, const glm::quat& orientation) { - // 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 { - if (_enabled && _rigidBody) { + if (_rigidBody) { const btTransform& avatarTransform = _rigidBody->getWorldTransform(); rotation = bulletToGLM(avatarTransform.getRotation()); position = bulletToGLM(avatarTransform.getOrigin()) - rotation * _shapeLocalOffset; @@ -428,16 +492,18 @@ 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) { 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); @@ -445,12 +511,32 @@ void CharacterController::applyMotor(int index, btScalar dt, btVector3& worldVel } else { // compute local UP btVector3 up = _currentUp.rotate(axis, -angle); + btVector3 motorVelocity = motor.velocity; + + // save these non-adjusted components for later + 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; - btVector3 vTargetVelocity = motor.velocity.dot(up) * up; - btVector3 hTargetVelocity = motor.velocity - vTargetVelocity; + btVector3 vMotorVelocity = motorVelocity.dot(up) * up; + btVector3 hMotorVelocity = motorVelocity - vMotorVelocity; // modify each component separately btScalar maxTau = 0.0f; @@ -460,7 +546,7 @@ void CharacterController::applyMotor(int index, btScalar dt, btVector3& worldVel tau = 1.0f; } maxTau = tau; - hVelocity += (hTargetVelocity - hVelocity) * tau; + hVelocity += (hMotorVelocity - hVelocity) * tau; } if (motor.vTimescale < MAX_CHARACTER_MOTOR_TIMESCALE) { btScalar tau = dt / motor.vTimescale; @@ -470,11 +556,12 @@ 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 velocity = (hVelocity + vVelocity).rotate(axis, angle); + _targetVelocity += maxTau * (hTargetVelocity + vTargetVelocity).rotate(axis, angle); // store velocity and weights velocities.push_back(velocity); @@ -492,6 +579,8 @@ void CharacterController::computeNewVelocity(btScalar dt, btVector3& velocity) { velocities.reserve(_motors.size()); 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); } @@ -507,14 +596,18 @@ void CharacterController::computeNewVelocity(btScalar dt, btVector3& velocity) { for (size_t i = 0; i < velocities.size(); ++i) { velocity += (weights[i] / totalWeight) * velocities[i]; } + _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) { @@ -523,57 +616,54 @@ void CharacterController::computeNewVelocity(btScalar dt, glm::vec3& velocity) { velocity = bulletToGLM(btVelocity); } -void CharacterController::preSimulation() { - if (_enabled && _dynamicsWorld && _rigidBody) { - quint64 now = usecTimestampNow(); +void CharacterController::updateState() { + if (!_dynamicsWorld) { + return; + } + 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 to where it is supposed to be - _rigidBody->setWorldTransform(_characterBodyTransform); - 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 = _characterBodyTransform.getOrigin(); + // 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 actualHorizVelocity = velocity - velocity.dot(_currentUp) * _currentUp; - bool flyingFast = _state == State::Hover && actualHorizVelocity.length() > (MAX_WALKING_SPEED * 0.75f); + btVector3 velocity = _preSimulationVelocity; + // disable normal state transitions while collisionless + if (_collisionGroup == BULLET_COLLISION_GROUP_MY_AVATAR) { switch (_state) { case State::Ground: if (!rayHasHit && !_hasSupport) { @@ -613,32 +703,47 @@ void CharacterController::preSimulation() { break; } case State::Hover: - if ((_floorDistance < MIN_HOVER_HEIGHT) && !jumpButtonHeld && !flyingFast) { + 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 || (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"); } break; } + } else { + // 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 (_rigidBody) { + // slam body transform and remember velocity + _rigidBody->setWorldTransform(btTransform(btTransform(_rotation, _position))); + _preSimulationVelocity = _rigidBody->getLinearVelocity(); + + updateState(); } _previousFlags = _pendingFlags; _pendingFlags &= ~PENDING_FLAG_JUMP; - - _followTime = 0.0f; - _followLinearDisplacement = btVector3(0, 0, 0); - _followAngularDisplacement = btQuaternion::getIdentity(); } void CharacterController::postSimulation() { - // postSimulation() exists for symmetry and just in case we need to do something here later - if (_enabled && _dynamicsWorld && _rigidBody) { - btVector3 velocity = _rigidBody->getLinearVelocity(); - _velocityChange = velocity - _preSimulationVelocity; + if (_rigidBody) { + _velocityChange = _rigidBody->getLinearVelocity() - _preSimulationVelocity; } } - bool CharacterController::getRigidBodyLocation(glm::vec3& avatarRigidBodyPosition, glm::quat& avatarRigidBodyRotation) { if (!_rigidBody) { return false; @@ -655,7 +760,10 @@ void CharacterController::setFlyingAllowed(bool value) { _flyingAllowed = value; if (!_flyingAllowed && _state == State::Hover) { - SET_STATE(State::InAir, "flying not allowed"); + // 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 aaae1c6492..8323284315 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 @@ -19,12 +19,18 @@ #include #include +#include +#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; 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; @@ -44,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; @@ -56,10 +62,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 @@ -69,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); @@ -103,12 +107,15 @@ public: }; State getState() const { return _state; } + void updateState(); - void setLocalBoundingBox(const glm::vec3& corner, const glm::vec3& scale); + void setLocalBoundingBox(const glm::vec3& minCorner, 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); @@ -123,7 +130,7 @@ protected: #endif void updateUpAxis(const glm::quat& rotation); - bool checkForSupport(btCollisionWorld* collisionWorld) const; + bool checkForSupport(btCollisionWorld* collisionWorld); protected: struct CharacterMotor { @@ -136,6 +143,7 @@ protected: }; std::vector _motors; + CharacterGhostObject _ghost; btVector3 _currentUp; btVector3 _targetVelocity; btVector3 _parentVelocity; @@ -144,6 +152,8 @@ protected: btTransform _followDesiredBodyTransform; btScalar _followTimeRemaining; btTransform _characterBodyTransform; + btVector3 _position; + btQuaternion _rotation; glm::vec3 _shapeLocalOffset; @@ -155,13 +165,23 @@ protected: quint32 _jumpButtonDownCount; quint32 _takeoffJumpButtonID; - btScalar _halfHeight; - btScalar _radius; + // data for walking up steps + 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 }; + 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; + btScalar _gravity { DEFAULT_CHARACTER_GRAVITY }; btScalar _jumpSpeed; btScalar _followTime; @@ -169,7 +189,6 @@ protected: btQuaternion _followAngularDisplacement; btVector3 _linearAcceleration; - std::atomic_bool _enabled; State _state; bool _isPushingUp; @@ -179,6 +198,7 @@ protected: uint32_t _previousFlags { 0 }; bool _flyingAllowed { true }; + 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 100755 index 0000000000..563605cd16 --- /dev/null +++ b/libraries/physics/src/CharacterGhostObject.cpp @@ -0,0 +1,415 @@ +// +// CharacterGhostObject.cpp +// libraries/physics/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 + +#include + +#include "CharacterRayResult.h" +#include "CharacterGhostShape.h" + + +CharacterGhostObject::~CharacterGhostObject() { + removeFromWorld(); + if (_ghostShape) { + delete _ghostShape; + _ghostShape = nullptr; + 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::setRadiusAndHalfHeight(btScalar radius, btScalar halfHeight) { + _radius = radius; + _halfHeight = halfHeight; +} + +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; + if (_motorOnly) { + _linearVelocity = _motorVelocity; + } +} + +// override of btCollisionObject::setCollisionShape() +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(static_cast(shape)); + setCollisionShape(_ghostShape); +} + +void CharacterGhostObject::setCollisionWorld(btCollisionWorld* world) { + if (world != _world) { + removeFromWorld(); + _world = world; + addToWorld(); + } +} + +void CharacterGhostObject::move(btScalar dt, btScalar overshoot, btScalar gravity) { + bool oldOnFloor = _onFloor; + _onFloor = false; + _steppingUp = false; + assert(_world && _inWorld); + updateVelocity(dt, gravity); + + // 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 + } + btTransform startTransform = getWorldTransform(); + btVector3 startPosition = startTransform.getOrigin(); + if (_onFloor) { + // resolvePenetration() pushed the avatar out of a floor so + // we must updateTraction() before using _linearVelocity + updateTraction(startPosition); + } + + btScalar speed = _linearVelocity.length(); + btVector3 forwardSweep = dt * _linearVelocity; + btScalar stepDistance = dt * speed; + btScalar MIN_SWEEP_DISTANCE = 0.0001f; + if (stepDistance < MIN_SWEEP_DISTANCE) { + // not moving, no need to sweep + updateTraction(startPosition); + return; + } + + // augment forwardSweep to help slow moving sweeps get over steppable ledges + const btScalar MIN_OVERSHOOT = 0.04f; // default margin + if (overshoot < MIN_OVERSHOOT) { + overshoot = MIN_OVERSHOOT; + } + btScalar longSweepDistance = stepDistance + overshoot; + forwardSweep *= longSweepDistance / stepDistance; + + // step forward + CharacterSweepResult result(this); + btTransform nextTransform = startTransform; + nextTransform.setOrigin(startPosition + forwardSweep); + sweepTest(_characterShape, startTransform, nextTransform, result); // forward + + if (!result.hasHit()) { + nextTransform.setOrigin(startPosition + (stepDistance / longSweepDistance) * forwardSweep); + setWorldTransform(nextTransform); + updateTraction(nextTransform.getOrigin()); + return; + } + 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); + 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)); + btScalar hitHeight = hitFromBase.dot(_upDirection); + if (hitHeight > _maxStepHeight) { + // 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) { + forwardTranslation *= stepDistance / forwardDistance; + } + nextTransform.setOrigin(startPosition + forwardTranslation); + setWorldTransform(nextTransform); + _onFloor = _onFloor || oldOnFloor; + 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(); + startTransform.setOrigin(startPosition + availableStepHeight * _upDirection); + nextTransform.setOrigin(startTransform.getOrigin() + forwardSweep); + sweepTest(_characterShape, startTransform, nextTransform, result); + if (result.hasHit()) { + startTransform.setOrigin(startTransform.getOrigin() + result.m_closestHitFraction * forwardSweep); + } else { + startTransform = nextTransform; + } + + // sweep down in search of future landing spot + result.resetHitHistory(); + btVector3 downSweep = (- availableStepHeight) * _upDirection; + nextTransform.setOrigin(startTransform.getOrigin() + downSweep); + 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; + _floorContact = result.m_hitPointWorld; + _steppingUp = true; + _onFloor = true; + _hovering = false; + 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 + nextTransform.setOrigin(startPosition + forwardSweepHitFraction * (stepDistance / longSweepDistance) * forwardSweep); + _onFloor = _onFloor || oldOnFloor; + updateTraction(nextTransform.getOrigin()); + } + setWorldTransform(nextTransform); +} + +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); + return result.hasHit(); + } + 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, + // 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. + refreshOverlappingPairCache(); + + // compute collision details + btHashedOverlappingPairCache* pairCache = getOverlappingPairCache(); + _world->getDispatcher()->dispatchAllCollisionPairs(pairCache, _world->getDispatchInfo(), _world->getDispatcher()); + + // 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; + + 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; + if (directionSign > 0.0f) { + _floorContact = pt.m_positionWorldOnA; + } else { + _floorContact = pt.m_positionWorldOnB; + } + _onFloor = true; + } + } + + btVector3 penetration = (-penetrationDepth) * normal; + minBoxOut.setMin(penetration); + maxBoxOut.setMax(penetration); + } + } + } +} + +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); + _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::resolvePenetration(int numTries) { + btVector3 minBox, maxBox; + measurePenetration(minBox, maxBox); + btVector3 restore = maxBox + minBox; + if (restore.length2() > 0.0f) { + btTransform transform = getWorldTransform(); + transform.setOrigin(transform.getOrigin() + restore); + setWorldTransform(transform); + return false; + } + return true; +} + +void CharacterGhostObject::updateVelocity(btScalar dt, btScalar gravity) { + if (!_motorOnly) { + if (_hovering) { + _linearVelocity *= 0.999f; // HACK damping + } else { + _linearVelocity += (dt * gravity) * _upDirection; + } + } +} + +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 || _motorOnly) { + _linearVelocity = _motorVelocity; + } else if (_onFloor) { + // 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(); + if (pathLength > FLT_EPSILON) { + _linearVelocity = (_motorVelocity.length() / pathLength) * pathDirection; + } else { + _linearVelocity = btVector3(0.0f, 0.0f, 0.0f); + } + } +} + +btScalar CharacterGhostObject::measureAvailableStepHeight() const { + CharacterSweepResult result(this); + btTransform transform = getWorldTransform(); + btTransform nextTransform = transform; + nextTransform.setOrigin(transform.getOrigin() + _maxStepHeight * _upDirection); + sweepTest(_characterShape, transform, nextTransform, result); + return result.m_closestHitFraction * _maxStepHeight; +} + diff --git a/libraries/physics/src/CharacterGhostObject.h b/libraries/physics/src/CharacterGhostObject.h new file mode 100755 index 0000000000..feb132a53e --- /dev/null +++ b/libraries/physics/src/CharacterGhostObject.h @@ -0,0 +1,103 @@ +// +// CharacterGhostObject.h +// libraries/physics/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 +#include + +#include "CharacterSweepResult.h" +#include "CharacterRayResult.h" + +class CharacterGhostShape; + +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 setRadiusAndHalfHeight(btScalar radius, btScalar halfHeight); + void setUpDirection(const btVector3& up); + void setMotorVelocity(const btVector3& 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 setCharacterShape(btConvexHullShape* shape); + + void setCollisionWorld(btCollisionWorld* world); + + void move(btScalar dt, btScalar overshoot, btScalar gravity); + + 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 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; } + + void measurePenetration(btVector3& minBoxOut, btVector3& maxBoxOut); + void refreshOverlappingPairCache(); + +protected: + void removeFromWorld(); + void addToWorld(); + + bool resolvePenetration(int numTries); + void updateVelocity(btScalar dt, btScalar gravity); + void updateTraction(const btVector3& position); + btScalar measureAvailableStepHeight() const; + 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 _halfHeight { 0.0f }; + 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 + 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 + 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 + bool _motorOnly { false }; // input, _linearVelocity slaves to _motorVelocity +}; + +#endif // hifi_CharacterGhostObject_h diff --git a/libraries/physics/src/CharacterGhostShape.cpp b/libraries/physics/src/CharacterGhostShape.cpp new file mode 100644 index 0000000000..09f4f0b80f --- /dev/null +++ b/libraries/physics/src/CharacterGhostShape.cpp @@ -0,0 +1,31 @@ +// +// CharacterGhostShape.cpp +// libraries/physics/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" + +#include + + +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 { + 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; + aabbMax += expansion; +} diff --git a/libraries/physics/src/CharacterGhostShape.h b/libraries/physics/src/CharacterGhostShape.h new file mode 100644 index 0000000000..dc75c148d5 --- /dev/null +++ b/libraries/physics/src/CharacterGhostShape.h @@ -0,0 +1,25 @@ +// +// CharacterGhostShape.h +// libraries/physics/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 btConvexHullShape { + // Same as btConvexHullShape but reports an expanded Aabb for larger ghost overlap cache +public: + CharacterGhostShape(const btConvexHullShape* shape); + + virtual void getAabb (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const override; +}; + +#endif // hifi_CharacterGhostShape_h diff --git a/libraries/physics/src/CharacterRayResult.cpp b/libraries/physics/src/CharacterRayResult.cpp new file mode 100755 index 0000000000..7a81e9cca6 --- /dev/null +++ b/libraries/physics/src/CharacterRayResult.cpp @@ -0,0 +1,31 @@ +// +// CharaterRayResult.cpp +// libraries/physics/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 + +#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..e8b0bb7f99 --- /dev/null +++ b/libraries/physics/src/CharacterRayResult.h @@ -0,0 +1,44 @@ +// +// CharaterRayResult.h +// libraries/physics/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 100755 index 0000000000..a5c4092b1d --- /dev/null +++ b/libraries/physics/src/CharacterSweepResult.cpp @@ -0,0 +1,42 @@ +// +// CharaterSweepResult.cpp +// libraries/physics/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 + +#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..1e2898a3cf --- /dev/null +++ b/libraries/physics/src/CharacterSweepResult.h @@ -0,0 +1,45 @@ +// +// CharaterSweepResult.h +// libraries/physics/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