help avatar walk up steps

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) {

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,

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";

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) {
+        if (!ghostingAllowed && _characterController.getCollisionGroup() == BULLET_COLLISION_GROUP_COLLISIONLESS) {
-            _characterController.setEnabled(true);
+            _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;
     }
-
+    setAvatarCollisionsEnabled(menu->isOptionChecked(MenuOption::EnableAvatarCollisions));
-    setCharacterControllerEnabled(menu->isOptionChecked(MenuOption::EnableCharacterController));
 }
 
-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() {
+bool MyAvatar::getAvatarCollisionsEnabled() {
-    return _characterController.isEnabled();
+    return _characterController.getCollisionGroup() != BULLET_COLLISION_GROUP_COLLISIONLESS;
 }
 
 void MyAvatar::clearDriveKeys() {

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 void setAvatarCollisionsEnabled(bool enabled);
-    Q_INVOKABLE bool getCharacterControllerEnabled();
+    Q_INVOKABLE bool getAvatarCollisionsEnabled();
 
     virtual glm::quat getAbsoluteJointRotationInObjectFrame(int index) const override;
     virtual glm::vec3 getAbsoluteJointTranslationInObjectFrame(int index) const override;

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<btScalar*>(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);
+                }
+            }
+        }
+    }
+}

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<btVector3> _topPoints;
+    btAlignedObjectArray<btVector3> _bottomPoints;
 };
 
 #endif // hifi_MyCharacterController_h

libraries/physics/src/CharacterController.cpp

@@ -13,8 +13,8 @@
 
 #include <NumericalConstants.h>
 
-#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
+            _dynamicsWorld->addRigidBody(_rigidBody, _collisionGroup, BULLET_COLLISION_MASK_MY_AVATAR);
-            // 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->addAction(this);
-            // restore gravity settings
+            // restore gravity settings because adding an object to the world overwrites its gravity setting
-            _rigidBody->setGravity(oldGravity);
+            _rigidBody->setGravity(_gravity * _currentUp);
+            btCollisionShape* shape = _rigidBody->getCollisionShape();
+            assert(shape && shape->getShapeType() == CONVEX_HULL_SHAPE_PROXYTYPE);
+            _ghost.setCharacterShape(static_cast<btConvexHullShape*>(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);
+        btPersistentManifold* contactManifold = dispatcher->getManifoldByIndexInternal(i);
-        const btCollisionObject* obA = static_cast<const btCollisionObject*>(contactManifold->getBody0());
+        if (_rigidBody == contactManifold->getBody1() || _rigidBody == contactManifold->getBody0()) {
-        const btCollisionObject* obB = static_cast<const btCollisionObject*>(contactManifold->getBody1());
+            bool characterIsFirst = _rigidBody == contactManifold->getBody0();
-        if (obA == _rigidBody || obB == _rigidBody) {
             int numContacts = contactManifold->getNumContacts();
+            int stepContactIndex = -1;
+            float highestStep = _minStepHeight;
             for (int j = 0; j < numContacts; j++) {
-                btManifoldPoint& pt = contactManifold->getContactPoint(j);
+                // check for "floor"
-
+                btManifoldPoint& contact = contactManifold->getContactPoint(j);
-                // check to see if contact point is touching the bottom sphere of the capsule.
+                btVector3 pointOnCharacter = characterIsFirst ? contact.m_localPointA : contact.m_localPointB; // object-local-frame
-                // and the contact normal is not slanted too much.
+                btVector3 normal = characterIsFirst ? contact.m_normalWorldOnB : -contact.m_normalWorldOnB; // points toward character
-                float contactPointY = (obA == _rigidBody) ? pt.m_localPointA.getY() : pt.m_localPointB.getY();
+                btScalar hitHeight = _halfHeight + _radius + pointOnCharacter.dot(_currentUp);
-                btVector3 normal = (obA == _rigidBody) ? pt.m_normalWorldOnB : -pt.m_normalWorldOnB;
+                if (hitHeight < _maxStepHeight && normal.dot(_currentUp) > _minFloorNormalDotUp) {
-                if (contactPointY < -_halfHeight && normal.dot(_currentUp) > COS_PI_OVER_THREE) {
+                    hasFloor = true;
-                    return 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) {
+        if (_rigidBody) {
-            // hover enter
+            if (desiredState == State::Hover && _state != State::Hover) {
-            if (_rigidBody) {
+                // hover enter
                 _rigidBody->setGravity(btVector3(0.0f, 0.0f, 0.0f));
-            }
+            } else if (_state == State::Hover && desiredState != State::Hover) {
-        } else if (_state == State::Hover && desiredState != State::Hover) {
+                // hover exit
-            // hover exit
+                if (_collisionGroup == BULLET_COLLISION_GROUP_COLLISIONLESS) {
-            if (_rigidBody) {
+                    _rigidBody->setGravity(btVector3(0.0f, 0.0f, 0.0f));
-                _rigidBody->setGravity(DEFAULT_CHARACTER_GRAVITY * _currentUp);
+                } else {
+                    _rigidBody->setGravity(_gravity * _currentUp);
+                }
             }
         }
         _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 = scale.x;
-
+    float z = scale.z;
-    float x = _boxScale.x;
-    float z = _boxScale.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);
+    if (glm::abs(radius - _radius) > FLT_EPSILON || glm::abs(halfHeight - _halfHeight) > FLT_EPSILON) {
-    float heightDelta = glm::abs(halfHeight - _halfHeight);
+        _radius = radius;
-    if (radiusDelta < FLT_EPSILON && heightDelta < FLT_EPSILON) {
+        _halfHeight = halfHeight;
-        // shape hasn't changed --> nothing to do
+        const btScalar DEFAULT_MIN_STEP_HEIGHT = 0.041f; // HACK: hardcoded now but should just larger than shape margin
-    } else {
+        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
+        _pendingFlags |= PENDING_FLAG_ADD_TO_SIMULATION;
-        if (_enabled) {
-            _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) {
+void CharacterController::setCollisionGroup(int16_t group) {
-    if (enabled != _enabled) {
+    if (_collisionGroup != group) {
-        if (enabled) {
+        _collisionGroup = group;
-            // Don't bother clearing REMOVE bit since it might be paired with an UPDATE_SHAPE bit.
+        _pendingFlags |= PENDING_FLAG_UPDATE_COLLISION_GROUP;
-            // Setting the ADD bit here works for all cases so we don't even bother checking other bits.
+        _ghost.setCollisionGroupAndMask(_collisionGroup, BULLET_COLLISION_MASK_MY_AVATAR & (~ _collisionGroup));
-            _pendingFlags |= PENDING_FLAG_ADD_TO_SIMULATION;
+    }
-        } else {
+}
-            if (_dynamicsWorld) {
+
-                _pendingFlags |= PENDING_FLAG_REMOVE_FROM_SIMULATION;
+void CharacterController::handleChangedCollisionGroup() {
-            }
+    if (_pendingFlags & PENDING_FLAG_UPDATE_COLLISION_GROUP) {
-            _pendingFlags &= ~ PENDING_FLAG_ADD_TO_SIMULATION;
+        // 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) {
+    _ghost.setUpDirection(_currentUp);
-        const btScalar MIN_UP_ERROR = 0.01f;
+    if (_state != State::Hover && _rigidBody) {
-        if (oldUp.distance(_currentUp) > MIN_UP_ERROR) {
+        _rigidBody->setGravity(_gravity * _currentUp);
-            _rigidBody->setGravity(DEFAULT_CHARACTER_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);
-    btQuaternion bodyOrientation = glmToBullet(orientation);
+    _position = glmToBullet(position + orientation * _shapeLocalOffset);
-    btVector3 bodyPosition = glmToBullet(position + orientation * _shapeLocalOffset);
-    _characterBodyTransform = btTransform(bodyOrientation, bodyPosition);
 }
 
 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 vMotorVelocity = motorVelocity.dot(up) * up;
-        btVector3 hTargetVelocity = motor.velocity - vTargetVelocity;
+        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<btScalar> 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() {
+void CharacterController::updateState() {
-    if (_enabled && _dynamicsWorld && _rigidBody) {
+    if (!_dynamicsWorld) {
-        quint64 now = usecTimestampNow();
+        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
+    // scan for distant floor
-        _rigidBody->setWorldTransform(_characterBodyTransform);
+    // rayStart is at center of bottom sphere
-        btVector3 velocity = _rigidBody->getLinearVelocity();
+    btVector3 rayStart = _position;
-        _preSimulationVelocity = velocity;
 
-        // scan for distant floor
+    // rayEnd is straight down MAX_FALL_HEIGHT
-        // rayStart is at center of bottom sphere
+    btScalar rayLength = _radius + MAX_FALL_HEIGHT;
-        btVector3 rayStart = _characterBodyTransform.getOrigin();
+    btVector3 rayEnd = rayStart - rayLength * _currentUp;
 
-        // rayEnd is straight down MAX_FALL_HEIGHT
+    ClosestNotMe rayCallback(_rigidBody);
-        btScalar rayLength = _radius + MAX_FALL_HEIGHT;
+    rayCallback.m_closestHitFraction = 1.0f;
-        btVector3 rayEnd = rayStart - rayLength * _currentUp;
+    _dynamicsWorld->rayTest(rayStart, rayEnd, rayCallback);
-
+    bool rayHasHit = rayCallback.hasHit();
-        const btScalar FLY_TO_GROUND_THRESHOLD = 0.1f * _radius;
+    quint64 now = usecTimestampNow();
-        const btScalar GROUND_TO_FLY_THRESHOLD = 0.8f * _radius + _halfHeight;
+    if (rayHasHit) {
-        const quint64 TAKE_OFF_TO_IN_AIR_PERIOD = 250 * MSECS_PER_SECOND;
+        _rayHitStartTime = now;
-        const btScalar MIN_HOVER_HEIGHT = 2.5f;
+        _floorDistance = rayLength * rayCallback.m_closestHitFraction - (_radius + _halfHeight);
-        const quint64 JUMP_TO_HOVER_PERIOD = 1100 * MSECS_PER_SECOND;
+    } else {
-        const btScalar MAX_WALKING_SPEED = 2.5f;
         const quint64 RAY_HIT_START_PERIOD = 500 * MSECS_PER_SECOND;
-
+        if ((now - _rayHitStartTime) < RAY_HIT_START_PERIOD) {
-        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) {
             rayHasHit = true;
         } else {
             _floorDistance = FLT_MAX;
         }
+    }
 
-        // record a time stamp when the jump button was first pressed.
+    // record a time stamp when the jump button was first pressed.
-        if ((_previousFlags & PENDING_FLAG_JUMP) != (_pendingFlags & PENDING_FLAG_JUMP)) {
+    bool jumpButtonHeld = _pendingFlags & PENDING_FLAG_JUMP;
-            if (_pendingFlags & PENDING_FLAG_JUMP) {
+    if ((_previousFlags & PENDING_FLAG_JUMP) != (_pendingFlags & PENDING_FLAG_JUMP)) {
-                _jumpButtonDownStartTime = now;
+        if (_pendingFlags & PENDING_FLAG_JUMP) {
-                _jumpButtonDownCount++;
+            _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);
 
+    // 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 (_rigidBody) {
-    if (_enabled && _dynamicsWorld && _rigidBody) {
+        _velocityChange = _rigidBody->getLinearVelocity() - _preSimulationVelocity;
-        btVector3 velocity = _rigidBody->getLinearVelocity();
-        _velocityChange = velocity - _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");
+            }
         }
     }
 }

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
+#ifndef hifi_CharacterController_h
-#define hifi_CharacterControllerInterface_h
+#define hifi_CharacterController_h
 
 #include <assert.h>
 #include <stdint.h>
@@ -19,12 +19,18 @@
 #include <BulletDynamics/Character/btCharacterControllerInterface.h>
 
 #include <GLMHelpers.h>
+#include <NumericalConstants.h>
+#include <PhysicsCollisionGroups.h>
+
 #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 {
+    virtual void updateAction(btCollisionWorld* collisionWorld, btScalar deltaTime) override;
-        preStep(collisionWorld);
-        playerStep(collisionWorld, deltaTime);
-    }
     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<btVector3>& velocities, std::vector<btScalar>& 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
+    bool isEnabledAndReady() const { return _dynamicsWorld; }
-    void setEnabled(bool enabled);
+
-    bool isEnabledAndReady() const { return _enabled && _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<CharacterMotor> _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;
+    // data for walking up steps
-    btScalar _radius;
+    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

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 <stdint.h>
+#include <assert.h>
+
+#include <PhysicsHelpers.h>
+
+#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<const btConvexHullShape*>(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<btCollisionObject*>(collisionPair->m_pProxy0->m_clientObject);
+        btCollisionObject* obj1 = static_cast<btCollisionObject*>(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;
+}
+

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 <stdint.h>
+
+#include <btBulletDynamicsCommon.h>
+#include <BulletCollision/CollisionDispatch/btGhostObject.h>
+#include <stdint.h>
+
+#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

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 <assert.h>
+
+
+CharacterGhostShape::CharacterGhostShape(const btConvexHullShape* shape) :
+        btConvexHullShape(reinterpret_cast<const btScalar*>(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;
+}

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 <BulletCollision/CollisionShapes/btConvexHullShape.h>
+
+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

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 <assert.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);
+}

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 <btBulletDynamicsCommon.h>
+#include <BulletCollision/CollisionDispatch/btCollisionWorld.h>
+
+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

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 <assert.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);
+}

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 <btBulletDynamicsCommon.h>
+#include <BulletCollision/CollisionDispatch/btCollisionWorld.h>
+
+
+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