adding kinematic character controller demo

interface/src/Menu.cpp

@@ -506,6 +506,11 @@ Menu::Menu() {
         avatar, SLOT(updateMotionBehaviorFromMenu()),
         UNSPECIFIED_POSITION, "Developer");
 
+    // KINEMATIC_CONTROLLER_HACK
+    addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::MoveKinematically, 0, false,
+        avatar, SLOT(updateMotionBehaviorFromMenu()),
+        UNSPECIFIED_POSITION, "Developer");
+
     // Developer > Hands >>>
     MenuWrapper* handOptionsMenu = developerMenu->addMenu("Hands");
     addCheckableActionToQMenuAndActionHash(handOptionsMenu, MenuOption::DisplayHandTargets, 0, false,

interface/src/Menu.h

@@ -189,6 +189,7 @@ namespace MenuOption {
     const QString UseAudioForMouth = "Use Audio for Mouth";
     const QString UseCamera = "Use Camera";
     const QString UseAnimPreAndPostRotations = "Use Anim Pre and Post Rotations";
+    const QString MoveKinematically = "Move Kinematically"; // KINEMATIC_CONTROLLER_HACK
     const QString VelocityFilter = "Velocity Filter";
     const QString VisibleToEveryone = "Everyone";
     const QString VisibleToFriends = "Friends";

interface/src/avatar/MyAvatar.cpp

@@ -1933,6 +1933,10 @@ void MyAvatar::updateMotionBehaviorFromMenu() {
         _motionBehaviors &= ~AVATAR_MOTION_SCRIPTED_MOTOR_ENABLED;
     }
 
+    // KINEMATIC_CONTROLLER_HACK
+    bool moveKinematically = menu->isOptionChecked(MenuOption::MoveKinematically);
+    _characterController.setMoveKinematically(moveKinematically);
+
     setAvatarCollisionsEnabled(menu->isOptionChecked(MenuOption::EnableAvatarCollisions));
 }
 

interface/src/avatar/MyAvatar.h

@@ -492,6 +492,7 @@ private:
     ThreadSafeValueCache<controller::Pose> _rightHandControllerPoseInSensorFrameCache { controller::Pose() };
 
     bool _hmdLeanRecenterEnabled = true;
+    bool _moveKinematically { false }; // KINEMATIC_CONTROLLER_HACK
 
     float AVATAR_MOVEMENT_ENERGY_CONSTANT { 0.001f };
     float AUDIO_ENERGY_CONSTANT { 0.000001f };

interface/src/avatar/MyCharacterController.cpp

@@ -37,7 +37,7 @@ void MyCharacterController::updateShapeIfNecessary() {
         // compute new dimensions from avatar's bounding box
         float x = _boxScale.x;
         float z = _boxScale.z;
-        _radius = 0.5f * sqrtf(0.5f * (x * x + z * z));
+        setCapsuleRadius(0.5f * sqrtf(0.5f * (x * x + z * z)));
         _halfHeight = 0.5f * _boxScale.y - _radius;
         float MIN_HALF_HEIGHT = 0.1f;
         if (_halfHeight < MIN_HALF_HEIGHT) {
@@ -74,7 +74,13 @@ void MyCharacterController::updateShapeIfNecessary() {
             } else {
                 _rigidBody->setGravity(DEFAULT_CHARACTER_GRAVITY * _currentUp);
             }
-            //_rigidBody->setCollisionFlags(btCollisionObject::CF_CHARACTER_OBJECT);
+            // KINEMATIC_CONTROLLER_HACK
+            if (_moveKinematically) {
+                _rigidBody->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT);
+            } else {
+                _rigidBody->setCollisionFlags(_rigidBody->getCollisionFlags() &
+                        ~(btCollisionObject::CF_KINEMATIC_OBJECT | btCollisionObject::CF_STATIC_OBJECT));
+            }
         } else {
             // TODO: handle this failure case
         }

libraries/physics/src/CharacterController.cpp

@@ -102,6 +102,7 @@ bool CharacterController::needsAddition() const {
 
 void CharacterController::setDynamicsWorld(btDynamicsWorld* world) {
     if (_dynamicsWorld != world) {
+        // remove from old world
         if (_dynamicsWorld) {
             if (_rigidBody) {
                 _dynamicsWorld->removeRigidBody(_rigidBody);
@@ -110,6 +111,7 @@ void CharacterController::setDynamicsWorld(btDynamicsWorld* world) {
             _dynamicsWorld = nullptr;
         }
         if (world && _rigidBody) {
+            // add to new world
             _dynamicsWorld = world;
             _pendingFlags &= ~PENDING_FLAG_JUMP;
             // Before adding the RigidBody to the world we must save its oldGravity to the side
@@ -119,7 +121,18 @@ void CharacterController::setDynamicsWorld(btDynamicsWorld* world) {
             _dynamicsWorld->addAction(this);
             // restore gravity settings
             _rigidBody->setGravity(oldGravity);
+            _ghost.setCollisionShape(_rigidBody->getCollisionShape()); // KINEMATIC_CONTROLLER_HACK
         }
+        // KINEMATIC_CONTROLLER_HACK
+        int16_t group = BULLET_COLLISION_GROUP_MY_AVATAR;
+        int16_t mask = BULLET_COLLISION_MASK_MY_AVATAR & (~ group);
+        _ghost.setCollisionGroupAndMask(group, mask);
+        _ghost.setCollisionWorld(_dynamicsWorld);
+        _ghost.setDistanceToFeet(_radius + _halfHeight);
+        _ghost.setMaxStepHeight(0.75f * (_radius + _halfHeight)); // HACK
+        _ghost.setMinWallAngle(PI / 4.0f); // HACK
+        _ghost.setUpDirection(_currentUp);
+        _ghost.setGravity(DEFAULT_CHARACTER_GRAVITY);
     }
     if (_dynamicsWorld) {
         if (_pendingFlags & PENDING_FLAG_UPDATE_SHAPE) {
@@ -188,54 +201,67 @@ const btScalar MIN_TARGET_SPEED_SQUARED = MIN_TARGET_SPEED * MIN_TARGET_SPEED;
 void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) {
     btVector3 velocity = _rigidBody->getLinearVelocity() - _parentVelocity;
     computeNewVelocity(dt, velocity);
-    _rigidBody->setLinearVelocity(velocity + _parentVelocity);
 
-    // Dynamicaly compute a follow velocity to move this body toward the _followDesiredBodyTransform.
+    if (_moveKinematically) {
-    // Rather than add this velocity to velocity the RigidBody, we explicitly teleport the RigidBody towards its goal.
+        // KINEMATIC_CONTROLLER_HACK
-    // This mirrors the computation done in MyAvatar::FollowHelper::postPhysicsUpdate().
+        btTransform transform = _rigidBody->getWorldTransform();
+        transform.setOrigin(_ghost.getWorldTransform().getOrigin());
+        _ghost.setWorldTransform(transform);
+        _ghost.setMotorVelocity(_simpleMotorVelocity);
+        float overshoot = 1.0f * _radius;
+        _ghost.move(dt, overshoot);
+        _rigidBody->setWorldTransform(_ghost.getWorldTransform());
+        _rigidBody->setLinearVelocity(_ghost.getLinearVelocity());
+    } else {
+        // Dynamicaly compute a follow velocity to move this body toward the _followDesiredBodyTransform.
+        // Rather than add this velocity to velocity the RigidBody, we explicitly teleport the RigidBody towards its goal.
+        // This mirrors the computation done in MyAvatar::FollowHelper::postPhysicsUpdate().
 
-    if (_following) {
+        _rigidBody->setLinearVelocity(velocity + _parentVelocity);
-        // OUTOFBODY_HACK -- these consts were copied from elsewhere, and then tuned
+        if (_following) {
-        const float NORMAL_WALKING_SPEED = 0.5f;
+            // OUTOFBODY_HACK -- these consts were copied from elsewhere, and then tuned
-        const float FOLLOW_TIME = 0.8f;
+            const float NORMAL_WALKING_SPEED = 0.5f;
-        const float FOLLOW_ROTATION_THRESHOLD = cosf(PI / 6.0f);
+            const float FOLLOW_TIME = 0.8f;
+            const float FOLLOW_ROTATION_THRESHOLD = cosf(PI / 6.0f);
 
-        const float MAX_ANGULAR_SPEED = FOLLOW_ROTATION_THRESHOLD / FOLLOW_TIME;
+            const float MAX_ANGULAR_SPEED = FOLLOW_ROTATION_THRESHOLD / FOLLOW_TIME;
 
-        btTransform bodyTransform = _rigidBody->getWorldTransform();
+            btTransform bodyTransform = _rigidBody->getWorldTransform();
 
-        btVector3 startPos = bodyTransform.getOrigin();
+            btVector3 startPos = bodyTransform.getOrigin();
-        btVector3 deltaPos = _followDesiredBodyTransform.getOrigin() - startPos;
+            btVector3 deltaPos = _followDesiredBodyTransform.getOrigin() - startPos;
-        btVector3 vel = deltaPos * (0.5f / dt);
+            btVector3 vel = deltaPos * (0.5f / dt);
-        btScalar speed = vel.length();
+            btScalar speed = vel.length();
-        if (speed > NORMAL_WALKING_SPEED) {
+            if (speed > NORMAL_WALKING_SPEED) {
-            vel *= NORMAL_WALKING_SPEED / speed;
+                vel *= NORMAL_WALKING_SPEED / speed;
+            }
+            btVector3 linearDisplacement = vel * dt;
+            btVector3 endPos = startPos + linearDisplacement;
+
+            btQuaternion startRot = bodyTransform.getRotation();
+            glm::vec2 currentFacing = getFacingDir2D(bulletToGLM(startRot));
+            glm::vec2 currentRight(currentFacing.y, -currentFacing.x);
+            glm::vec2 desiredFacing = getFacingDir2D(bulletToGLM(_followDesiredBodyTransform.getRotation()));
+            float deltaAngle = acosf(glm::clamp(glm::dot(currentFacing, desiredFacing), -1.0f, 1.0f));
+            float angularSpeed = 0.5f * deltaAngle / dt;
+            if (angularSpeed > MAX_ANGULAR_SPEED) {
+                angularSpeed *= MAX_ANGULAR_SPEED / angularSpeed;
+            }
+            float sign = copysignf(1.0f, glm::dot(desiredFacing, currentRight));
+            btQuaternion angularDisplacement = btQuaternion(btVector3(0.0f, 1.0f, 0.0f), sign * angularSpeed * dt);
+            btQuaternion endRot = angularDisplacement * startRot;
+
+            // in order to accumulate displacement of avatar position, we need to take _shapeLocalOffset into account.
+            btVector3 shapeLocalOffset = glmToBullet(_shapeLocalOffset);
+            btVector3 swingDisplacement = rotateVector(endRot, -shapeLocalOffset) - rotateVector(startRot, -shapeLocalOffset);
+
+            _followLinearDisplacement = linearDisplacement + swingDisplacement + _followLinearDisplacement;
+            _followAngularDisplacement = angularDisplacement * _followAngularDisplacement;
+
+            _rigidBody->setWorldTransform(btTransform(endRot, endPos));
         }
-        btVector3 linearDisplacement = vel * dt;
-        btVector3 endPos = startPos + linearDisplacement;
-
-        btQuaternion startRot = bodyTransform.getRotation();
-        glm::vec2 currentFacing = getFacingDir2D(bulletToGLM(startRot));
-        glm::vec2 currentRight(currentFacing.y, -currentFacing.x);
-        glm::vec2 desiredFacing = getFacingDir2D(bulletToGLM(_followDesiredBodyTransform.getRotation()));
-        float deltaAngle = acosf(glm::clamp(glm::dot(currentFacing, desiredFacing), -1.0f, 1.0f));
-        float angularSpeed = 0.5f * deltaAngle / dt;
-        if (angularSpeed > MAX_ANGULAR_SPEED) {
-            angularSpeed *= MAX_ANGULAR_SPEED / angularSpeed;
-        }
-        float sign = copysignf(1.0f, glm::dot(desiredFacing, currentRight));
-        btQuaternion angularDisplacement = btQuaternion(btVector3(0.0f, 1.0f, 0.0f), sign * angularSpeed * dt);
-        btQuaternion endRot = angularDisplacement * startRot;
-
-        // in order to accumulate displacement of avatar position, we need to take _shapeLocalOffset into account.
-        btVector3 shapeLocalOffset = glmToBullet(_shapeLocalOffset);
-        btVector3 swingDisplacement = rotateVector(endRot, -shapeLocalOffset) - rotateVector(startRot, -shapeLocalOffset);
-
-        _followLinearDisplacement = linearDisplacement + swingDisplacement + _followLinearDisplacement;
-        _followAngularDisplacement = angularDisplacement * _followAngularDisplacement;
-
-        _rigidBody->setWorldTransform(btTransform(endRot, endPos));
         _followTime += dt;
+        _ghost.setWorldTransform(_rigidBody->getWorldTransform());
     }
 }
 
@@ -353,6 +379,7 @@ void CharacterController::handleChangedCollisionGroup() {
 
 void CharacterController::updateUpAxis(const glm::quat& rotation) {
     _currentUp = quatRotate(glmToBullet(rotation), LOCAL_UP_AXIS);
+    _ghost.setUpDirection(_currentUp);
     if (_state != State::Hover && _rigidBody) {
         if (_collisionGroup == BULLET_COLLISION_GROUP_COLLISIONLESS) {
             _rigidBody->setGravity(btVector3(0.0f, 0.0f, 0.0f));
@@ -414,6 +441,10 @@ glm::vec3 CharacterController::getLinearVelocity() const {
     return velocity;
 }
 
+void CharacterController::setCapsuleRadius(float radius) {
+    _radius = radius;
+}
+
 glm::vec3 CharacterController::getVelocityChange() const {
     if (_rigidBody) {
         return bulletToGLM(_velocityChange);
@@ -490,6 +521,7 @@ void CharacterController::applyMotor(int index, btScalar dt, btVector3& worldVel
 
         // add components back together and rotate into world-frame
         velocity = (hVelocity + vVelocity).rotate(axis, angle);
+        _simpleMotorVelocity += maxTau * (hTargetVelocity + vTargetVelocity).rotate(axis, angle);
 
         // store velocity and weights
         velocities.push_back(velocity);
@@ -507,6 +539,7 @@ void CharacterController::computeNewVelocity(btScalar dt, btVector3& velocity) {
     velocities.reserve(_motors.size());
     std::vector<btScalar> weights;
     weights.reserve(_motors.size());
+    _simpleMotorVelocity = btVector3(0.0f, 0.0f, 0.0f);
     for (int i = 0; i < (int)_motors.size(); ++i) {
         applyMotor(i, dt, velocity, velocities, weights);
     }
@@ -522,6 +555,7 @@ void CharacterController::computeNewVelocity(btScalar dt, btVector3& velocity) {
         for (size_t i = 0; i < velocities.size(); ++i) {
             velocity += (weights[i] / totalWeight) * velocities[i];
         }
+        _simpleMotorVelocity /= totalWeight;
     }
     if (velocity.length2() < MIN_TARGET_SPEED_SQUARED) {
         velocity = btVector3(0.0f, 0.0f, 0.0f);
@@ -686,3 +720,10 @@ void CharacterController::setFlyingAllowed(bool value) {
         }
     }
 }
+
+void CharacterController::setMoveKinematically(bool kinematic) {
+    if (kinematic != _moveKinematically) {
+        _moveKinematically = kinematic;
+        _pendingFlags |= PENDING_FLAG_UPDATE_SHAPE;
+    }
+}

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>
@@ -22,6 +22,7 @@
 #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;
@@ -95,6 +96,7 @@ public:
     glm::vec3 getLinearVelocity() const;
     glm::vec3 getVelocityChange() const;
 
+    virtual void setCapsuleRadius(float radius);
     float getCapsuleRadius() const { return _radius; }
     float getCapsuleHalfHeight() const { return _halfHeight; }
     glm::vec3 getCapsuleLocalOffset() const { return _shapeLocalOffset; }
@@ -110,10 +112,6 @@ public:
 
     void setLocalBoundingBox(const glm::vec3& corner, const glm::vec3& scale);
 
-    /*
-    bool isEnabled() const { return _enabled; }  // thread-safe
-    void setEnabled(bool enabled);
-    */
     bool isEnabledAndReady() const { return _dynamicsWorld; }
 
     void setCollisionGroup(int16_t group);
@@ -124,6 +122,7 @@ public:
 
     void setFlyingAllowed(bool value);
 
+    void setMoveKinematically(bool kinematic); // KINEMATIC_CONTROLLER_HACK
 
 protected:
 #ifdef DEBUG_STATE_CHANGE
@@ -146,11 +145,13 @@ protected:
     };
 
     std::vector<CharacterMotor> _motors;
+    CharacterGhostObject _ghost; // KINEMATIC_CONTROLLER_HACK
     btVector3 _currentUp;
     btVector3 _targetVelocity;
     btVector3 _parentVelocity;
     btVector3 _preSimulationVelocity;
     btVector3 _velocityChange;
+    btVector3 _simpleMotorVelocity; // KINEMATIC_CONTROLLER_HACK
     btTransform _followDesiredBodyTransform;
     btTransform _characterBodyTransform;
 
@@ -188,7 +189,8 @@ protected:
     uint32_t _previousFlags { 0 };
 
     bool _flyingAllowed { true };
+    bool _moveKinematically { false }; // KINEMATIC_CONTROLLER_HACK
     int16_t _collisionGroup { BULLET_COLLISION_GROUP_MY_AVATAR };
 };
 
-#endif // hifi_CharacterControllerInterface_h
+#endif // hifi_CharacterController_h

libraries/physics/src/CharacterGhostObject.cpp

@@ -0,0 +1,390 @@
+//
+//  CharacterGhostObject.cpp
+//  libraries/physcis/src
+//
+//  Created by Andrew Meadows 2016.08.26
+//  Copyright 2016 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+#include "CharacterGhostObject.h"
+
+#include <assert.h>
+
+#include "CharacterRayResult.h"
+
+const btScalar DEFAULT_STEP_UP_HEIGHT = 0.5f;
+
+
+CharacterGhostObject::~CharacterGhostObject() {
+    removeFromWorld();
+    setCollisionShape(nullptr);
+}
+
+void CharacterGhostObject::setCollisionGroupAndMask(int16_t group, int16_t mask) {
+    _collisionFilterGroup = group;
+    _collisionFilterMask = mask;
+    // TODO: if this probe is in the world reset ghostObject overlap cache
+}
+
+void CharacterGhostObject::getCollisionGroupAndMask(int16_t& group, int16_t& mask) const {
+    group = _collisionFilterGroup;
+    mask = _collisionFilterMask;
+}
+
+
+void CharacterGhostObject::setUpDirection(const btVector3& up) {
+    btScalar length = up.length();
+    if (length > FLT_EPSILON) {
+        _upDirection /= length;
+    } else {
+        _upDirection = btVector3(0.0f, 1.0f, 0.0f);
+    }
+}
+
+void CharacterGhostObject::setMotorVelocity(const btVector3& velocity) {
+    _motorVelocity = velocity;
+    _motorSpeed = _motorVelocity.length();
+}
+
+// override of btCollisionObject::setCollisionShape()
+void CharacterGhostObject::setCollisionShape(btCollisionShape* shape) {
+    assert(!shape || shape->isConvex()); // if shape is valid then please make it convex
+    if (shape != getCollisionShape()) {
+        bool wasInWorld = _inWorld;
+        removeFromWorld();
+        btCollisionObject::setCollisionShape(shape);
+        if (wasInWorld) {
+            assert(shape); // please remove from world before setting null shape
+            addToWorld();
+        }
+    }
+}
+
+void CharacterGhostObject::setCollisionWorld(btCollisionWorld* world) {
+    if (world != _world) {
+        removeFromWorld();
+        _world = world;
+        addToWorld();
+    }
+}
+
+void CharacterGhostObject::move(btScalar dt, btScalar overshoot) {
+    _onFloor = false;
+    assert(_world && _inWorld);
+    updateVelocity(dt);
+
+    // resolve any penetrations before sweeping
+    int32_t MAX_LOOPS = 4;
+    int32_t numExtractions = 0;
+    btVector3 totalPosition(0.0f, 0.0f, 0.0f);
+    while (numExtractions < MAX_LOOPS) {
+        if (resolvePenetration(numExtractions)) {
+            numExtractions = 0;
+            break;
+        }
+        totalPosition += getWorldTransform().getOrigin();
+        ++numExtractions;
+    }
+    if (numExtractions > 1) {
+        // penetration resolution was probably oscillating between opposing objects
+        // so we use the average of the solutions
+        totalPosition /= btScalar(numExtractions);
+        btTransform transform = getWorldTransform();
+        transform.setOrigin(totalPosition);
+        setWorldTransform(transform);
+
+        // TODO: figure out how to untrap character
+    }
+    if (_onFloor) {
+        // a floor was identified during resolvePenetration()
+        _hovering = false;
+        updateTraction();
+    }
+
+    btVector3 forwardSweep = dt * _linearVelocity;
+    btScalar stepDistance = forwardSweep.length();
+    btScalar MIN_SWEEP_DISTANCE = 0.0001f;
+    if (stepDistance < MIN_SWEEP_DISTANCE) {
+        // not moving, no need to sweep
+        updateHoverState(getWorldTransform());
+        return;
+    }
+
+    const btCollisionShape* shape = getCollisionShape();
+    assert(shape->isConvex());
+    const btConvexShape* convexShape= static_cast<const btConvexShape*>(shape);
+
+    // augment forwardSweep to help slow moving sweeps get over steppable ledges
+    btScalar margin = shape->getMargin();
+    if (overshoot < margin) {
+        overshoot = margin;
+    }
+    btScalar longSweepDistance = stepDistance + overshoot;
+    forwardSweep *= longSweepDistance / stepDistance;
+
+    // expand this object's Aabb in the broadphase and
+    // update the pairCache for the sweepTests we intend to do
+    btVector3 minAabb, maxAabb;
+    getCollisionShape()->getAabb(getWorldTransform(), minAabb, maxAabb);
+    minAabb.setMin(minAabb - btVector3(margin, margin, margin));
+    maxAabb.setMax(maxAabb + btVector3(margin, margin, margin));
+    minAabb.setMin(minAabb + forwardSweep);
+    maxAabb.setMax(maxAabb + forwardSweep);
+    minAabb.setMin(minAabb + _maxStepHeight * _upDirection);
+    maxAabb.setMax(maxAabb + _maxStepHeight * _upDirection);
+
+    // this updates both pairCaches: world broadphase and ghostobject
+    _world->getBroadphase()->setAabb(getBroadphaseHandle(), minAabb, maxAabb, _world->getDispatcher());
+
+    // step forward
+    CharacterSweepResult result(this);
+    btTransform startTransform = getWorldTransform();
+    btTransform transform = startTransform;
+    btTransform nextTransform = transform;
+    nextTransform.setOrigin(transform.getOrigin() + forwardSweep);
+    sweepTest(convexShape, transform, nextTransform, result); // forward
+
+    if (!result.hasHit()) {
+        nextTransform.setOrigin(transform.getOrigin() + (stepDistance / longSweepDistance) * forwardSweep);
+        setWorldTransform(nextTransform);
+        updateHoverState(nextTransform);
+        updateTraction();
+        return;
+    }
+
+    // check if this hit is obviously unsteppable
+    btVector3 hitFromBase = result.m_hitPointWorld - (transform.getOrigin() - (_distanceToFeet * _upDirection));
+    btScalar hitHeight = hitFromBase.dot(_upDirection);
+    if (hitHeight > _maxStepHeight) {
+        // capsule can't step over the obstacle so move forward as much as possible before we bail
+        btVector3 forwardTranslation = result.m_closestHitFraction * forwardSweep;
+        btScalar forwardDistance = forwardTranslation.length();
+        if (forwardDistance > stepDistance) {
+            forwardTranslation *= stepDistance / forwardDistance;
+        }
+        transform.setOrigin(transform.getOrigin() + forwardTranslation);
+        setWorldTransform(transform);
+        return;
+    }
+    // if we get here then we hit something that might be steppable
+
+    // remember the forward sweep hit fraction for later
+    btScalar forwardSweepHitFraction = result.m_closestHitFraction;
+
+    // figure out how high we can step up
+    btScalar availableStepHeight = measureAvailableStepHeight();
+
+    // raise by availableStepHeight before sweeping forward
+    result.resetHitHistory();
+    transform.setOrigin(startTransform.getOrigin() + availableStepHeight * _upDirection);
+    nextTransform.setOrigin(transform.getOrigin() + forwardSweep);
+    sweepTest(convexShape, transform, nextTransform, result);
+    if (result.hasHit()) {
+        transform.setOrigin(transform.getOrigin() + result.m_closestHitFraction * forwardSweep);
+    } else {
+        transform = nextTransform;
+    }
+
+    // sweep down in search of future landing spot
+    result.resetHitHistory();
+    btVector3 downSweep = (dt * _linearVelocity.dot(_upDirection) - availableStepHeight) * _upDirection;
+    nextTransform.setOrigin(transform.getOrigin() + downSweep);
+    sweepTest(convexShape, transform, nextTransform, result);
+    if (result.hasHit() && result.m_hitNormalWorld.dot(_upDirection) > _maxWallNormalUpComponent) {
+        // can stand on future landing spot, so we interpolate toward it
+        _floorNormal = result.m_hitNormalWorld;
+        _onFloor = true;
+        _hovering = false;
+        nextTransform.setOrigin(transform.getOrigin() + result.m_closestHitFraction * downSweep);
+        btVector3 totalStep = nextTransform.getOrigin() - startTransform.getOrigin();
+        transform.setOrigin(startTransform.getOrigin() + (stepDistance / totalStep.length()) * totalStep);
+    } else {
+        // either there is no future landing spot, or there is but we can't stand on it
+        // in any case: we go forward as much as possible
+        transform.setOrigin(startTransform.getOrigin() + forwardSweepHitFraction * (stepDistance / longSweepDistance) * forwardSweep);
+    }
+    setWorldTransform(transform);
+    updateTraction();
+}
+
+void CharacterGhostObject::removeFromWorld() {
+    if (_world && _inWorld) {
+        _world->removeCollisionObject(this);
+        _inWorld = false;
+    }
+}
+
+void CharacterGhostObject::addToWorld() {
+    if (_world && !_inWorld) {
+        assert(getCollisionShape());
+        setCollisionFlags(getCollisionFlags() | btCollisionObject::CF_NO_CONTACT_RESPONSE);
+        //assert(getBroadphaseHandle());
+        //int16_t group = getBroadphaseHandle()->m_collisionFilterGroup;
+        //int16_t mask = getBroadphaseHandle()->m_collisionFilterMask;
+        _world->addCollisionObject(this, _collisionFilterGroup, _collisionFilterMask);
+        _inWorld = true;
+    }
+}
+
+bool CharacterGhostObject::sweepTest(
+        const btConvexShape* shape,
+        const btTransform& start,
+        const btTransform& end,
+        CharacterSweepResult& result) const {
+    if (_world && _inWorld) {
+        assert(shape);
+
+        btScalar allowedPenetration = _world->getDispatchInfo().m_allowedCcdPenetration;
+        convexSweepTest(shape, start, end, result, allowedPenetration);
+
+        if (result.hasHit()) {
+            return true;
+        }
+    }
+    return false;
+}
+
+bool CharacterGhostObject::rayTest(const btVector3& start,
+        const btVector3& end,
+        CharacterRayResult& result) const {
+    if (_world && _inWorld) {
+        _world->rayTest(start, end, result);
+	}
+    return result.hasHit();
+}
+
+bool CharacterGhostObject::resolvePenetration(int numTries) {
+    assert(_world);
+    // We refresh the overlapping pairCache because any previous movement may have pushed us
+    // into an overlap that was not in the cache.
+    refreshOverlappingPairCache();
+
+    // compute collision details
+    btHashedOverlappingPairCache* pairCache = getOverlappingPairCache();
+    _world->getDispatcher()->dispatchAllCollisionPairs(pairCache, _world->getDispatchInfo(), _world->getDispatcher());
+
+    // loop over contact manifolds
+    btTransform transform = getWorldTransform();
+    btVector3 position = transform.getOrigin();
+    btVector3 minBox =btVector3(0.0f, 0.0f, 0.0f);
+    btVector3 maxBox = btVector3(0.0f, 0.0f, 0.0f);
+    btManifoldArray manifoldArray;
+    const btScalar PENETRATION_RESOLUTION_FUDGE_FACTOR = 0.0001f; // speeds up resolvation
+
+    int numPairs = pairCache->getNumOverlappingPairs();
+    for (int i = 0; i < numPairs; i++) {
+        manifoldArray.resize(0);
+        btBroadphasePair* collisionPair = &(pairCache->getOverlappingPairArray()[i]);
+
+        btCollisionObject* obj0 = static_cast<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;
+                        _onFloor = true;
+                    }
+                }
+
+                btVector3 penetration = (-penetrationDepth + PENETRATION_RESOLUTION_FUDGE_FACTOR) * normal;
+                minBox.setMin(penetration);
+                maxBox.setMax(penetration);
+            }
+        }
+    }
+
+    btVector3 restore = maxBox + minBox;
+    if (restore.length2() > 0.0f) {
+        transform.setOrigin(position + restore);
+        setWorldTransform(transform);
+        return false;
+    }
+    return true;
+}
+
+void CharacterGhostObject::refreshOverlappingPairCache() {
+    assert(_world && _inWorld);
+    btVector3 minAabb, maxAabb;
+    getCollisionShape()->getAabb(getWorldTransform(), minAabb, maxAabb);
+
+    // this updates both pairCaches: world broadphase and ghostobject
+    _world->getBroadphase()->setAabb(getBroadphaseHandle(), minAabb, maxAabb, _world->getDispatcher());
+}
+
+void CharacterGhostObject::updateVelocity(btScalar dt) {
+    if (_hovering) {
+        _linearVelocity *= 0.99f; // HACK damping
+    } else {
+        _linearVelocity += (dt * _gravity) * _upDirection;
+    }
+}
+
+void CharacterGhostObject::updateTraction() {
+    if (_hovering) {
+        _linearVelocity = _motorVelocity;
+    } else if (_onFloor) {
+        btVector3 pathDirection = _floorNormal.cross(_motorVelocity).cross(_floorNormal);
+        btScalar pathLength = pathDirection.length();
+        if (pathLength > FLT_EPSILON) {
+            _linearVelocity = (_motorSpeed / pathLength) * pathDirection;
+        } else {
+            _linearVelocity = btVector3(0.0f, 0.0f, 0.0f);
+        }
+    }
+}
+
+btScalar CharacterGhostObject::measureAvailableStepHeight() const {
+    const btCollisionShape* shape = getCollisionShape();
+    assert(shape->isConvex());
+    const btConvexShape* convexShape= static_cast<const btConvexShape*>(shape);
+
+    CharacterSweepResult result(this);
+    btTransform transform = getWorldTransform();
+    btTransform nextTransform = transform;
+    nextTransform.setOrigin(transform.getOrigin() + _maxStepHeight * _upDirection);
+    sweepTest(convexShape, transform, nextTransform, result);
+    return result.m_closestHitFraction * _maxStepHeight;
+}
+
+void CharacterGhostObject::updateHoverState(const btTransform& transform) {
+    // cast a ray down looking for floor support
+    CharacterRayResult rayResult(this);
+    btVector3 startPos = transform.getOrigin() - ((_distanceToFeet - 0.1f) * _upDirection); // 0.1 HACK to make ray hit
+    btVector3 endPos = startPos - (2.0f * _distanceToFeet) * _upDirection;
+    rayTest(startPos, endPos, rayResult);
+    // we're hovering if the ray didn't hit an object we can stand on
+    _hovering = !(rayResult.hasHit() && rayResult.m_hitNormalWorld.dot(_upDirection) > _maxWallNormalUpComponent);
+}
+

libraries/physics/src/CharacterGhostObject.h

@@ -0,0 +1,83 @@
+//
+//  CharacterGhostObject.h
+//  libraries/physcis/src
+//
+//  Created by Andrew Meadows 2016.08.26
+//  Copyright 2016 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+#ifndef hifi_CharacterGhostObject_h
+#define hifi_CharacterGhostObject_h
+
+#include <btBulletDynamicsCommon.h>
+#include <BulletCollision/CollisionDispatch/btGhostObject.h>
+
+#include "CharacterSweepResult.h"
+#include "CharacterRayResult.h"
+
+
+class CharacterGhostObject : public btPairCachingGhostObject {
+public:
+    CharacterGhostObject() { }
+    ~CharacterGhostObject();
+
+    void setCollisionGroupAndMask(int16_t group, int16_t mask);
+    void getCollisionGroupAndMask(int16_t& group, int16_t& mask) const;
+
+    void setDistanceToFeet(btScalar distance) { _distanceToFeet = distance; }
+    void setUpDirection(const btVector3& up);
+    void setMotorVelocity(const btVector3& velocity);
+    void setGravity(btScalar gravity) { _gravity = gravity; } // NOTE: we expect _gravity to be negative (in _upDirection)
+    void setMinWallAngle(btScalar angle) { _maxWallNormalUpComponent = cosf(angle); }
+    void setMaxStepHeight(btScalar height) { _maxStepHeight = height; }
+
+    const btVector3& getLinearVelocity() const { return _linearVelocity; }
+
+    void setCollisionShape(btCollisionShape* shape) override;
+
+    void setCollisionWorld(btCollisionWorld* world);
+
+    void move(btScalar dt, btScalar overshoot);
+
+protected:
+    void removeFromWorld();
+    void addToWorld();
+
+    bool sweepTest(const btConvexShape* shape,
+            const btTransform& start,
+            const btTransform& end,
+            CharacterSweepResult& result) const;
+    bool rayTest(const btVector3& start,
+            const btVector3& end,
+            CharacterRayResult& result) const;
+
+    bool resolvePenetration(int numTries);
+    void refreshOverlappingPairCache();
+    void updateVelocity(btScalar dt);
+    void updateTraction();
+    btScalar measureAvailableStepHeight() const;
+    void updateHoverState(const btTransform& transform);
+
+protected:
+    btVector3 _upDirection { 0.0f, 1.0f, 0.0f }; // input, up in world-frame
+    btVector3 _motorVelocity { 0.0f, 0.0f, 0.0f }; // input, velocity character is trying to achieve
+    btVector3 _linearVelocity { 0.0f, 0.0f, 0.0f }; // internal, actual character velocity
+    btVector3 _floorNormal { 0.0f, 0.0f, 0.0f }; // internal, probable floor normal
+    btCollisionWorld* _world { nullptr }; // input, pointer to world
+    btScalar _distanceToFeet { 0.0f }; // input, distance from object center to lowest point on shape
+    btScalar _motorSpeed { 0.0f }; // internal, cached for speed
+    btScalar _gravity { 0.0f }; // input, amplitude of gravity along _upDirection (should be negative)
+    btScalar _maxWallNormalUpComponent { 0.0f }; // input: max vertical component of wall normal
+    btScalar _maxStepHeight { 0.0f }; // input, max step height the character can climb
+    int16_t _collisionFilterGroup { 0 };
+    int16_t _collisionFilterMask { 0 };
+    bool _inWorld { false }; // internal, was added to world
+    bool _hovering { false }; // internal, 
+    bool _onFloor { false }; // output, is actually standing on floor
+    bool _hasFloor { false }; // output, has floor underneath to fall on
+};
+
+#endif // hifi_CharacterGhostObject_h

libraries/physics/src/CharacterRayResult.cpp

@@ -0,0 +1,29 @@
+//
+//  CharaterRayResult.cpp
+//  libraries/physcis/src
+//
+//  Created by Andrew Meadows 2016.09.05
+//  Copyright 2016 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+#include "CharacterRayResult.h"
+
+#include "CharacterGhostObject.h"
+
+CharacterRayResult::CharacterRayResult (const CharacterGhostObject* character) :
+    btCollisionWorld::ClosestRayResultCallback(btVector3(0.0f, 0.0f, 0.0f), btVector3(0.0f, 0.0f, 0.0f)),
+    _character(character)
+{
+    assert(_character);
+    _character->getCollisionGroupAndMask(m_collisionFilterGroup, m_collisionFilterMask);
+}
+
+btScalar CharacterRayResult::addSingleResult(btCollisionWorld::LocalRayResult& rayResult, bool normalInWorldSpace) {
+    if (rayResult.m_collisionObject == _character) {
+        return 1.0f;
+    }
+    return ClosestRayResultCallback::addSingleResult (rayResult, normalInWorldSpace);
+}

libraries/physics/src/CharacterRayResult.h

@@ -0,0 +1,44 @@
+//
+//  CharaterRayResult.h
+//  libraries/physcis/src
+//
+//  Created by Andrew Meadows 2016.09.05
+//  Copyright 2016 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+#ifndef hifi_CharacterRayResult_h
+#define hifi_CharacterRayResult_h
+
+#include <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,40 @@
+//
+//  CharaterSweepResult.cpp
+//  libraries/physcis/src
+//
+//  Created by Andrew Meadows 2016.09.01
+//  Copyright 2016 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+#include "CharacterSweepResult.h"
+
+#include "CharacterGhostObject.h"
+
+CharacterSweepResult::CharacterSweepResult(const CharacterGhostObject* character)
+    :   btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0f, 0.0f, 0.0f), btVector3(0.0f, 0.0f, 0.0f)),
+        _character(character)
+{
+    // set collision group and mask to match _character
+    assert(_character);
+    _character->getCollisionGroupAndMask(m_collisionFilterGroup, m_collisionFilterMask);
+}
+
+btScalar CharacterSweepResult::addSingleResult(btCollisionWorld::LocalConvexResult& convexResult, bool useWorldFrame) {
+	// skip objects that we shouldn't collide with
+    if (!convexResult.m_hitCollisionObject->hasContactResponse()) {
+        return btScalar(1.0);
+    }
+    if (convexResult.m_hitCollisionObject == _character) {
+        return btScalar(1.0);
+    }
+
+    return ClosestConvexResultCallback::addSingleResult(convexResult, useWorldFrame);
+}
+
+void CharacterSweepResult::resetHitHistory() {
+	m_hitCollisionObject = nullptr;
+	m_closestHitFraction = btScalar(1.0f);
+}

libraries/physics/src/CharacterSweepResult.h

@@ -0,0 +1,45 @@
+//
+//  CharaterSweepResult.h
+//  libraries/physcis/src
+//
+//  Created by Andrew Meadows 2016.09.01
+//  Copyright 2016 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+#ifndef hifi_CharacterSweepResult_h
+#define hifi_CharacterSweepResult_h
+
+#include <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