diff --git a/examples/entityScripts/lightController.js b/examples/entityScripts/lightController.js
new file mode 100644
index 0000000000..624fc929b3
--- /dev/null
+++ b/examples/entityScripts/lightController.js
@@ -0,0 +1,190 @@
+(function() {
+    this.entityID = null;
+    this.properties = null;
+    this.lightID = null;
+    this.sound = null;
+
+    function copyObject(object) {
+        return JSON.parse(JSON.stringify(object));
+    }
+    function didEntityExist(entityID) {
+        return entityID && entityID.isKnownID;
+    }
+    function doesEntityExistNow(entityID) {
+        return entityID && getTrueID(entityID).isKnownID;
+    }
+    function getTrueID(entityID) {
+        var properties = Entities.getEntityProperties(entityID);
+        return { id: properties.id, creatorTokenID: properties.creatorTokenID, isKnownID: properties.isKnownID };
+    }
+    function getUserData(entityID) {
+        var properties = Entities.getEntityProperties(entityID);
+        if (properties.userData) {
+            return JSON.parse(properties.userData);
+        } else {
+            print("Warning: light controller has no user data.");
+            return null;
+        }
+    }
+    function updateUserData(entityID, userData) {
+        Entities.editEntity(entityID, { userData: JSON.stringify(userData) });
+    }
+    
+    // Download sound if needed
+    this.maybeDownloadSound = function() {
+        if (this.sound === null) {
+            this.sound = SoundCache.getSound("http://public.highfidelity.io/sounds/Footsteps/FootstepW3Left-12db.wav");
+        }
+    }
+    // Play switch sound
+    this.playSound = function() {
+        if (this.sound && this.sound.downloaded) {
+            Audio.playSound(this.sound, {
+                position: Entities.getEntityProperties(this.entityID).position,
+                volume: 0.2
+            });
+        } else {
+            print("Warning: Couldn't play sound.");
+        }
+    }
+
+    // Toggles the associated light entity
+    this.toggleLight = function() {
+        if (this.lightID) {
+            var lightProperties = Entities.getEntityProperties(this.lightID);
+            Entities.editEntity(this.lightID, { visible: !lightProperties.visible });
+        } else {
+            print("Warning: No light to turn on/off");
+        }
+    }
+    
+    this.createLight = function(userData) {
+        var lightProperties = copyObject(userData.lightDefaultProperties);
+        if (lightProperties) {
+            var entityProperties = Entities.getEntityProperties(this.entityID);
+            
+            lightProperties.visible = false;
+            lightProperties.position = Vec3.sum(entityProperties.position,
+                                                Vec3.multiplyQbyV(entityProperties.rotation,
+                                                                  lightProperties.position));
+            return Entities.addEntity(lightProperties);
+        } else {
+            print("Warning: light controller has no default light.");
+            return null;
+        }
+    }
+    
+    this.updateLightID = function() {
+        var userData = getUserData(this.entityID);
+        if (!userData) {
+            userData = {
+                lightID: null,
+                lightDefaultProperties: {
+                    type: "Light",
+                    position: { x: 0, y: 0, z: 0 },
+                    dimensions: { x: 5, y: 5, z: 5 },
+                    isSpotlight: false,
+                    color: { red: 255, green: 48, blue: 0 },
+                    diffuseColor: { red: 255, green: 255, blue: 255 },
+                    ambientColor: { red: 255, green: 255, blue: 255 },
+                    specularColor: { red: 0, green: 0, blue: 0 },
+                    constantAttenuation: 1,
+                    linearAttenuation: 0,
+                    quadraticAttenuation: 0,
+                    intensity: 10,
+                    exponent: 0,
+                    cutoff: 180, // in degrees
+                }
+            };
+            updateUserData(this.entityID, userData);
+        }
+        
+        // Find valid light
+        if (doesEntityExistNow(this.lightID)) {
+            if (!didEntityExist(this.lightID)) {
+                // Light now has an ID, so update it in userData
+                this.lightID = getTrueID(this.lightID);
+                userData.lightID = this.lightID;
+                updateUserData(this.entityID, userData);
+            }
+            return;
+        }
+        
+        if (doesEntityExistNow(userData.lightID)) {
+            this.lightID = getTrueID(userData.lightID);
+            return;
+        }
+
+        // No valid light, create one
+        this.lightID = this.createLight(userData);
+        print("Created new light entity");
+        
+        // Update user data with new ID
+        userData.lightID = this.lightID;
+        updateUserData(this.entityID, userData);
+    }
+    
+    this.maybeMoveLight = function() {
+        var entityProperties = Entities.getEntityProperties(this.entityID);
+        var lightProperties = Entities.getEntityProperties(this.lightID);
+        var lightDefaultProperties = getUserData(this.entityID).lightDefaultProperties;
+        
+        var position = Vec3.sum(entityProperties.position,
+                                Vec3.multiplyQbyV(entityProperties.rotation,
+                                                  lightDefaultProperties.position));
+                                                              
+        if (!Vec3.equal(position, lightProperties.position)) {
+            print("Lamp entity moved, moving light entity as well");
+            Entities.editEntity(this.lightID, { position: position });
+        }
+    }
+
+    this.updateRelativeLightPosition = function() {
+        if (!doesEntityExistNow(this.entityID) || !doesEntityExistNow(this.lightID)) {
+            print("Warning: ID invalid, couldn't save relative position.");
+            return;
+        }
+
+        var userData = getUserData(this.entityID);
+        var entityProperties = Entities.getEntityProperties(this.entityID);
+        var lightProperties = Entities.getEntityProperties(this.lightID);
+        var newProperties = {};
+
+        // Copy only meaningful properties (trying to save space in userData here)
+        for (var key in userData.lightDefaultProperties) {
+            if (userData.lightDefaultProperties.hasOwnProperty(key)) {
+                newProperties[key] = lightProperties[key];
+            }
+        }
+
+        // Compute new relative position
+        newProperties.position = Quat.multiply(Quat.inverse(entityProperties.rotation),
+                                                 Vec3.subtract(lightProperties.position,
+                                                               entityProperties.position));
+        // inverse "visible" because right after we loaded the properties, the light entity is toggled.
+        newProperties.visible = !lightProperties.visible;
+
+        userData.lightDefaultProperties = copyObject(newProperties);
+        updateUserData(this.entityID, userData);
+        print("Relative properties of light entity saved.");
+    }
+
+    this.preload = function(entityID) {
+        this.entityID = entityID;
+        this.maybeDownloadSound();
+    };
+    
+    this.clickReleaseOnEntity = function(entityID, mouseEvent) {
+        this.entityID = entityID;
+        this.maybeDownloadSound();
+        
+        if (mouseEvent.isLeftButton) {
+            this.updateLightID();
+            this.maybeMoveLight();
+            this.toggleLight();
+            this.playSound();
+        } else if (mouseEvent.isRightButton) {
+            this.updateRelativeLightPosition();
+        }
+    };
+})
\ No newline at end of file
diff --git a/interface/external/sixense/readme.txt b/interface/external/sixense/readme.txt
index 29d1bbc2f9..a4790caa5e 100644
--- a/interface/external/sixense/readme.txt
+++ b/interface/external/sixense/readme.txt
@@ -2,7 +2,7 @@
 Instructions for adding the Sixense driver to Interface
 Andrzej Kapolka, November 18, 2013
 
-1. Copy the Sixense sdk folders (lib, include) into the interface/external/Sixense folder. This readme.txt should be there as well.
+1. Copy the Sixense sdk folders (bin, include, lib, and samples) into the interface/external/Sixense folder. This readme.txt should be there as well.
     
    You may optionally choose to copy the SDK folders to a location outside the repository (so you can re-use with different checkouts and different projects).
    If so our CMake find module expects you to set the ENV variable 'HIFI_LIB_DIR' to a directory containing a subfolder 'sixense' that contains the folders mentioned above.
diff --git a/interface/src/avatar/FaceModel.cpp b/interface/src/avatar/FaceModel.cpp
index c80772ef49..722f998f86 100644
--- a/interface/src/avatar/FaceModel.cpp
+++ b/interface/src/avatar/FaceModel.cpp
@@ -87,7 +87,8 @@ void FaceModel::maybeUpdateEyeRotation(Model* model, const JointState& parentSta
 void FaceModel::updateJointState(int index) {
     JointState& state = _jointStates[index];
     const FBXJoint& joint = state.getFBXJoint();
-    if (joint.parentIndex != -1) {
+    // guard against out-of-bounds access to _jointStates
+    if (joint.parentIndex != -1 && joint.parentIndex >= 0 && joint.parentIndex < _jointStates.size()) {
         const JointState& parentState = _jointStates.at(joint.parentIndex);
         const FBXGeometry& geometry = _geometry->getFBXGeometry();
         if (index == geometry.neckJointIndex) {
diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp
index 5173b368c1..fb200b2c57 100644
--- a/libraries/physics/src/CharacterController.cpp
+++ b/libraries/physics/src/CharacterController.cpp
@@ -16,10 +16,9 @@ subject to the following restrictions:
 */
 
 
-//#include <stdio.h>
-
 #include "BulletCollision/CollisionDispatch/btGhostObject.h"
 
+#include "BulletUtil.h"
 #include "CharacterController.h"
 
 
@@ -54,7 +53,7 @@ m_me = me;
 
 virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult, bool normalInWorldSpace)
 {
-if(rayResult.m_collisionObject == m_me)
+if (rayResult.m_collisionObject == m_me)
 return 1.0;
 
 return ClosestRayResultCallback::addSingleResult(rayResult, normalInWorldSpace);
@@ -64,8 +63,7 @@ btCollisionObject* m_me;
 };
 */
 
-class btKinematicClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
-{
+class btKinematicClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback {
     public:
         btKinematicClosestNotMeConvexResultCallback(btCollisionObject* me, const btVector3& up, btScalar minSlopeDot)
             : btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0))
@@ -92,6 +90,9 @@ class btKinematicClosestNotMeConvexResultCallback : public btCollisionWorld::Clo
             hitNormalWorld = convexResult.m_hitCollisionObject->getWorldTransform().getBasis()*convexResult.m_hitNormalLocal;
         }
 
+        // Note: hitNormalWorld points into character, away from object
+        // and m_up points opposite to movement
+
         btScalar dotUp = m_up.dot(hitNormalWorld);
         if (dotUp < m_minSlopeDot) {
             return btScalar(1.0);
@@ -106,21 +107,104 @@ protected:
     btScalar m_minSlopeDot;
 };
 
+class StepDownConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback {
+    // special convex sweep callback for character during the stepDown() phase
+    public:
+        StepDownConvexResultCallback(btCollisionObject* me, 
+                const btVector3& up, 
+                const btVector3& start,
+                const btVector3& step,
+                const btVector3& pushDirection,
+                btScalar minSlopeDot,
+                btScalar radius,
+                btScalar halfHeight)
+            : btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0))
+              , m_me(me)
+              , m_up(up)
+              , m_start(start)
+              , m_step(step)
+              , m_pushDirection(pushDirection)
+              , m_minSlopeDot(minSlopeDot)
+              , m_radius(radius)
+              , m_halfHeight(halfHeight)
+    {
+    }
+
+    virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult, bool normalInWorldSpace) {
+        if (convexResult.m_hitCollisionObject == m_me) {
+            return btScalar(1.0);
+        }
+
+        if (!convexResult.m_hitCollisionObject->hasContactResponse()) {
+            return btScalar(1.0);
+        }
+
+        btVector3 hitNormalWorld;
+        if (normalInWorldSpace) {
+            hitNormalWorld = convexResult.m_hitNormalLocal;
+        } else {
+            ///need to transform normal into worldspace
+            hitNormalWorld = convexResult.m_hitCollisionObject->getWorldTransform().getBasis() * convexResult.m_hitNormalLocal;
+        }
+
+        // Note: hitNormalWorld points into character, away from object
+        // and m_up points opposite to movement
+
+        btScalar dotUp = m_up.dot(hitNormalWorld);
+        if (dotUp < m_minSlopeDot) {
+            if (hitNormalWorld.dot(m_pushDirection) > 0.0f) {
+                // ignore hits that push in same direction as character is moving
+                // which helps character NOT snag when stepping off ledges
+                return btScalar(1.0f);
+            }
+
+            // compute the angle between "down" and the line from character center to "hit" point
+            btVector3 fractionalStep = convexResult.m_hitFraction * m_step;
+            btVector3 localHit = convexResult.m_hitPointLocal - m_start + fractionalStep;
+            btScalar angle = localHit.angle(-m_up);
+
+            // compute a maxAngle based on size of m_step
+            btVector3 side(m_radius, - (m_halfHeight - m_step.length() + fractionalStep.dot(m_up)), 0.0f);
+            btScalar maxAngle = side.angle(-m_up);
+
+            // Ignore hits that are larger than maxAngle. Effectively what is happening here is: 
+            // we're ignoring hits at contacts that have non-vertical normals... if they hit higher
+            // than the character's "feet".  Ignoring the contact allows the character to slide down
+            // for these hits.  In other words, vertical walls against the character's torso will
+            // not prevent them from "stepping down" to find the floor.
+            if (angle > maxAngle) {
+                return btScalar(1.0f);
+            }
+        }
+
+        btScalar fraction = ClosestConvexResultCallback::addSingleResult(convexResult, normalInWorldSpace);
+        return fraction;
+    }
+
+protected:
+    btCollisionObject* m_me;
+    const btVector3 m_up;
+    btVector3 m_start;
+    btVector3 m_step;
+    btVector3 m_pushDirection;
+    btScalar m_minSlopeDot;
+    btScalar m_radius;
+    btScalar m_halfHeight;
+};
+
 /*
  * Returns the reflection direction of a ray going 'direction' hitting a surface with normal 'normal'
  *
  * from: http://www-cs-students.stanford.edu/~adityagp/final/node3.html
  */
-btVector3 CharacterController::computeReflectionDirection(const btVector3& direction, const btVector3& normal)
-{
+btVector3 CharacterController::computeReflectionDirection(const btVector3& direction, const btVector3& normal) {
     return direction - (btScalar(2.0) * direction.dot(normal)) * normal;
 }
 
 /*
  * Returns the portion of 'direction' that is parallel to 'normal'
  */
-btVector3 CharacterController::parallelComponent(const btVector3& direction, const btVector3& normal)
-{
+btVector3 CharacterController::parallelComponent(const btVector3& direction, const btVector3& normal) {
     btScalar magnitude = direction.dot(normal);
     return normal * magnitude;
 }
@@ -128,36 +212,38 @@ btVector3 CharacterController::parallelComponent(const btVector3& direction, con
 /*
  * Returns the portion of 'direction' that is perpindicular to 'normal'
  */
-btVector3 CharacterController::perpindicularComponent(const btVector3& direction, const btVector3& normal)
-{
+btVector3 CharacterController::perpindicularComponent(const btVector3& direction, const btVector3& normal) {
     return direction - parallelComponent(direction, normal);
 }
 
-CharacterController::CharacterController(
-        btPairCachingGhostObject* ghostObject, 
-        btConvexShape* convexShape, 
-        btScalar stepHeight, 
-        int upAxis) {
-    m_upAxis = upAxis;
-    m_addedMargin = 0.02;
-    m_walkDirection.setValue(0,0,0);
-    m_useGhostObjectSweepTest = true;
-    m_ghostObject = ghostObject;
-    m_stepHeight = stepHeight;
-    m_turnAngle = btScalar(0.0);
-    m_convexShape = convexShape;
+CharacterController::CharacterController(AvatarData* avatarData) {
+    assert(avatarData);
+    m_avatarData = avatarData;
+
+    // cache the "PhysicsEnabled" state of m_avatarData
+    m_avatarData->lockForRead();
+    m_enabled = m_avatarData->isPhysicsEnabled();
+    m_avatarData->unlock();
+
+    createShapeAndGhost();
+
+    m_upAxis = 1; // HACK: hard coded to be 1 for now (yAxis)
+
+    m_addedMargin = 0.02f;
+    m_walkDirection.setValue(0.0f,0.0f,0.0f);
+    m_turnAngle = btScalar(0.0f);
     m_useWalkDirection = true; // use walk direction by default, legacy behavior
-    m_velocityTimeInterval = 0.0;
-    m_verticalVelocity = 0.0;
-    m_verticalOffset = 0.0;
-    m_gravity = 9.8 * 3 ; // 3G acceleration.
-    m_fallSpeed = 55.0; // Terminal velocity of a sky diver in m/s.
-    m_jumpSpeed = 10.0; // ?
+    m_velocityTimeInterval = 0.0f;
+    m_verticalVelocity = 0.0f;
+    m_verticalOffset = 0.0f;
+    m_gravity = 9.8f;
+    m_maxFallSpeed = 55.0f; // Terminal velocity of a sky diver in m/s.
+    m_jumpSpeed = 10.0f; // ?
     m_wasOnGround = false;
     m_wasJumping = false;
     m_interpolateUp = true;
-    setMaxSlope(btRadians(45.0));
-    m_currentStepOffset = 0;
+    setMaxSlope(btRadians(45.0f));
+    m_lastStepUp = 0.0f;
 
     // internal state data members
     full_drop = false;
@@ -192,6 +278,9 @@ bool CharacterController::recoverFromPenetration(btCollisionWorld* collisionWorl
     collisionWorld->getDispatcher()->dispatchAllCollisionPairs(m_ghostObject->getOverlappingPairCache(), collisionWorld->getDispatchInfo(), collisionWorld->getDispatcher());
 
     m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
+    btVector3 up = getUpAxisDirections()[m_upAxis];
+
+    btVector3 currentPosition = m_currentPosition;
 
     btScalar maxPen = btScalar(0.0);
     for (int i = 0; i < m_ghostObject->getOverlappingPairCache()->getNumOverlappingPairs(); i++) {
@@ -210,83 +299,109 @@ bool CharacterController::recoverFromPenetration(btCollisionWorld* collisionWorl
             collisionPair->m_algorithm->getAllContactManifolds(m_manifoldArray);
         }
 
-        for (int j=0;j<m_manifoldArray.size();j++) {
+        for (int j = 0;j < m_manifoldArray.size(); j++) {
             btPersistentManifold* manifold = m_manifoldArray[j];
-            btScalar directionSign = manifold->getBody0() == m_ghostObject ? btScalar(-1.0) : btScalar(1.0);
-            for (int p=0;p<manifold->getNumContacts();p++) {
+            btScalar directionSign = (manifold->getBody0() == m_ghostObject) ? btScalar(1.0) : btScalar(-1.0);
+            for (int p = 0;p < manifold->getNumContacts(); p++) {
                 const btManifoldPoint&pt = manifold->getContactPoint(p);
 
                 btScalar dist = pt.getDistance();
 
                 if (dist < 0.0) {
-                    if (dist < maxPen) {
-                        maxPen = dist;
-                        m_touchingNormal = pt.m_normalWorldOnB * directionSign;//??
+                    bool useContact = true;
+                    btVector3 normal = pt.m_normalWorldOnB;
+                    normal *= directionSign; // always points from object to character
 
+                    btScalar normalDotUp = normal.dot(up);
+                    if (normalDotUp < m_maxSlopeCosine) {
+                        // this contact has a non-vertical normal... might need to ignored
+                        btVector3 collisionPoint;
+                        if (directionSign > 0.0) {
+                            collisionPoint = pt.getPositionWorldOnB();
+                        } else {
+                            collisionPoint = pt.getPositionWorldOnA();
+                        }
+
+                        // we do math in frame where character base is origin
+                        btVector3 characterBase = currentPosition - (m_radius + m_halfHeight) * up;
+                        collisionPoint -= characterBase;
+                        btScalar collisionHeight = collisionPoint.dot(up);
+
+                        if (collisionHeight < m_lastStepUp) {
+                            // This contact is below the lastStepUp, so we ignore it for penetration resolution,
+                            // otherwise it may prevent the character from getting close enough to find any available 
+                            // horizontal foothold that would allow it to climbe the ledge.  In other words, we're
+                            // making the character's "feet" soft for collisions against steps, but not floors.
+                            useContact = false;
+                        } 
+                    }
+                    if (useContact) {
+    
+                        if (dist < maxPen) {
+                            maxPen = dist;
+                            m_floorNormal = normal;
+                        }
+                        const btScalar INCREMENTAL_RESOLUTION_FACTOR = 0.2f;
+                        m_currentPosition += normal * (fabsf(dist) * INCREMENTAL_RESOLUTION_FACTOR);
+                        penetration = true;
                     }
-                    m_currentPosition += pt.m_normalWorldOnB * directionSign * dist * btScalar(0.2);
-                    penetration = true;
-                } else {
-                    //printf("touching %f\n", dist);
                 }
             }
-
-            //manifold->clearManifold();
         }
     }
     btTransform newTrans = m_ghostObject->getWorldTransform();
     newTrans.setOrigin(m_currentPosition);
     m_ghostObject->setWorldTransform(newTrans);
-    //printf("m_touchingNormal = %f,%f,%f\n", m_touchingNormal[0], m_touchingNormal[1], m_touchingNormal[2]);
     return penetration;
 }
 
 void CharacterController::stepUp( btCollisionWorld* world) {
     // phase 1: up
+
+    // compute start and end
     btTransform start, end;
-    m_targetPosition = m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_stepHeight + (m_verticalOffset > 0.f?m_verticalOffset:0.f));
-
     start.setIdentity();
-    end.setIdentity();
-
-    /* FIXME: Handle penetration properly */
     start.setOrigin(m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_convexShape->getMargin() + m_addedMargin));
+
+    //m_targetPosition = m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_stepHeight + (m_verticalOffset > 0.0f ? m_verticalOffset : 0.0f));
+    m_targetPosition = m_currentPosition + getUpAxisDirections()[m_upAxis] * m_stepHeight;
+    end.setIdentity();
     end.setOrigin(m_targetPosition);
 
-    btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, -getUpAxisDirections()[m_upAxis], btScalar(0.7071));
+    // sweep up
+    btVector3 sweepDirNegative = -getUpAxisDirections()[m_upAxis];
+    btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, sweepDirNegative, btScalar(0.7071));
     callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
     callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
-
-    if (m_useGhostObjectSweepTest) {
-        m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration);
-    }
-    else {
-        world->convexSweepTest(m_convexShape, start, end, callback);
-    }
+    m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration);
 
     if (callback.hasHit()) {
+        // we hit something, so zero our vertical velocity
+        m_verticalVelocity = 0.0;
+        m_verticalOffset = 0.0;
+
         // Only modify the position if the hit was a slope and not a wall or ceiling.
         if (callback.m_hitNormalWorld.dot(getUpAxisDirections()[m_upAxis]) > 0.0) {
-            // we moved up only a fraction of the step height
-            m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction;
+            m_lastStepUp = m_stepHeight * callback.m_closestHitFraction;
             if (m_interpolateUp == true) {
                 m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
             } else {
                 m_currentPosition = m_targetPosition;
             }
+        } else {
+            m_lastStepUp = m_stepHeight;
+            m_currentPosition = m_targetPosition;
         }
-        m_verticalVelocity = 0.0;
-        m_verticalOffset = 0.0;
     } else {
-        m_currentStepOffset = m_stepHeight;
         m_currentPosition = m_targetPosition;
+        m_lastStepUp = m_stepHeight;
     }
 }
 
 void CharacterController::updateTargetPositionBasedOnCollision(const btVector3& hitNormal, btScalar tangentMag, btScalar normalMag) {
     btVector3 movementDirection = m_targetPosition - m_currentPosition;
     btScalar movementLength = movementDirection.length();
-    if (movementLength>SIMD_EPSILON) {
+    if (movementLength > SIMD_EPSILON) {
         movementDirection.normalize();
 
         btVector3 reflectDir = computeReflectionDirection(movementDirection, hitNormal);
@@ -300,241 +415,152 @@ void CharacterController::updateTargetPositionBasedOnCollision(const btVector3&
         m_targetPosition = m_currentPosition;
         //if (tangentMag != 0.0) {
         if (0) {
-            btVector3 parComponent = parallelDir * btScalar(tangentMag*movementLength);
-            //printf("parComponent=%f,%f,%f\n", parComponent[0], parComponent[1], parComponent[2]);
+            btVector3 parComponent = parallelDir * btScalar(tangentMag * movementLength);
             m_targetPosition +=  parComponent;
         }
 
         if (normalMag != 0.0) {
-            btVector3 perpComponent = perpindicularDir * btScalar(normalMag*movementLength);
-            //printf("perpComponent=%f,%f,%f\n", perpComponent[0], perpComponent[1], perpComponent[2]);
+            btVector3 perpComponent = perpindicularDir * btScalar(normalMag * movementLength);
             m_targetPosition += perpComponent;
         }
-    } else {
-        //printf("movementLength don't normalize a zero vector\n");
     }
 }
 
-void CharacterController::stepForwardAndStrafe( btCollisionWorld* collisionWorld, const btVector3& walkMove) {
-    //printf("m_normalizedDirection=%f,%f,%f\n",
-    // m_normalizedDirection[0], m_normalizedDirection[1], m_normalizedDirection[2]);
-    // phase 2: forward and strafe
-    btTransform start, end;
-    m_targetPosition = m_currentPosition + walkMove;
+void CharacterController::stepForward( btCollisionWorld* collisionWorld, const btVector3& movement) {
+    // phase 2: forward
+    m_targetPosition = m_currentPosition + movement;
 
+    btTransform start, end;
     start.setIdentity();
     end.setIdentity();
 
-    btScalar fraction = 1.0;
-    btScalar distance2 = (m_currentPosition-m_targetPosition).length2();
-    //printf("distance2=%f\n", distance2);
-
+    /* TODO: experiment with this to see if we can use this to help direct motion when a floor is available
     if (m_touchingContact) {
-        if (m_normalizedDirection.dot(m_touchingNormal) > btScalar(0.0)) {
-            //interferes with step movement
-            //updateTargetPositionBasedOnCollision(m_touchingNormal);
+        if (m_normalizedDirection.dot(m_floorNormal) < btScalar(0.0)) {
+            updateTargetPositionBasedOnCollision(m_floorNormal, 1.0f, 1.0f);
         }
-    }
+    }*/
 
+    // modify shape's margin for the sweeps
+    btScalar margin = m_convexShape->getMargin();
+    m_convexShape->setMargin(margin + m_addedMargin);
+
+    const btScalar MIN_STEP_DISTANCE = 0.0001f;
+    btVector3 step = m_targetPosition - m_currentPosition;
+    btScalar stepLength2 = step.length2();
     int maxIter = 10;
 
-    while (fraction > btScalar(0.01) && maxIter-- > 0) {
+    while (stepLength2 > MIN_STEP_DISTANCE && maxIter-- > 0) {
         start.setOrigin(m_currentPosition);
         end.setOrigin(m_targetPosition);
-        btVector3 sweepDirNegative(m_currentPosition - m_targetPosition);
 
+        // sweep forward
+        btVector3 sweepDirNegative(m_currentPosition - m_targetPosition);
         btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, sweepDirNegative, btScalar(0.0));
         callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
         callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
-
-
-        btScalar margin = m_convexShape->getMargin();
-        m_convexShape->setMargin(margin + m_addedMargin);
-
-
-        if (m_useGhostObjectSweepTest) {
-            m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
-        } else {
-            collisionWorld->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
-        }
-
-        m_convexShape->setMargin(margin);
-
-
-        fraction -= callback.m_closestHitFraction;
+        m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
 
         if (callback.hasHit()) {
-            // we moved only a fraction
-            //btScalar hitDistance;
-            //hitDistance = (callback.m_hitPointWorld - m_currentPosition).length();
+            // we hit soemthing!
+            // Compute new target position by removing portion cut-off by collision, which will produce a new target
+            // that is the closest approach of the the obstacle plane to the original target.
+            step = m_targetPosition - m_currentPosition;
+            btScalar stepDotNormal = step.dot(callback.m_hitNormalWorld); // we expect this dot to be negative
+            step += (stepDotNormal * (1.0f - callback.m_closestHitFraction)) * callback.m_hitNormalWorld;
+            m_targetPosition = m_currentPosition + step;
 
-            //m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
-
-            updateTargetPositionBasedOnCollision(callback.m_hitNormalWorld);
-            btVector3 currentDir = m_targetPosition - m_currentPosition;
-            distance2 = currentDir.length2();
-            if (distance2 > SIMD_EPSILON) {
-                currentDir.normalize();
-                /* See Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners." */
-                if (currentDir.dot(m_normalizedDirection) <= btScalar(0.0)) { 
-                    break;
-                }
-            } else {
-                //printf("currentDir: don't normalize a zero vector\n");
-                break;
-            }
+            stepLength2 = step.length2();
         } else {
-            // we moved whole way
+            // we swept to the end without hitting anything
             m_currentPosition = m_targetPosition;
+            break;
         }
-
-        //if (callback.m_closestHitFraction == 0.f) {
-        //  break;
-        //}
-
     }
+
+    // restore shape's margin
+    m_convexShape->setMargin(margin);
 }
 
 void CharacterController::stepDown( btCollisionWorld* collisionWorld, btScalar dt) {
-    btTransform start, end, end_double;
-    bool runonce = false;
-
     // phase 3: down
-    /*btScalar additionalDownStep = (m_wasOnGround && !onGround()) ? m_stepHeight : 0.0;
-      btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + additionalDownStep);
-      btScalar downVelocity = (additionalDownStep == 0.0 && m_verticalVelocity<0.0?-m_verticalVelocity:0.0) * dt;
-      btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * downVelocity; 
-      m_targetPosition -= (step_drop + gravity_drop);*/
+    //
+    // The "stepDown" phase first makes a normal sweep down that cancels the lift from the "stepUp" phase.  
+    // If it hits a ledge then it stops otherwise it makes another sweep down in search of a floor within
+    // reach of the character's feet.
 
-    btVector3 orig_position = m_targetPosition;
-
-    btScalar downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
-
-    if (downVelocity > 0.0 && downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping)) {
-        downVelocity = m_fallSpeed;
+    btScalar downSpeed = (m_verticalVelocity < 0.0f) ? -m_verticalVelocity : 0.0f;
+    if (downSpeed > 0.0f && downSpeed > m_maxFallSpeed && (m_wasOnGround || !m_wasJumping)) {
+        downSpeed = m_maxFallSpeed;
     }
 
-    btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
-    m_targetPosition -= step_drop;
+    // first sweep for ledge
+    btVector3 step = getUpAxisDirections()[m_upAxis] * (-(m_lastStepUp + downSpeed * dt));
 
-    btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
+    StepDownConvexResultCallback callback(m_ghostObject, 
+            getUpAxisDirections()[m_upAxis], 
+            m_currentPosition, step, 
+            m_walkDirection,
+            m_maxSlopeCosine, 
+            m_radius, m_halfHeight);
     callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
     callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
 
-    btKinematicClosestNotMeConvexResultCallback callback2 (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
-    callback2.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
-    callback2.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
+    btTransform start, end;
+    start.setIdentity();
+    end.setIdentity();
 
-    while (1) {
-        start.setIdentity();
-        end.setIdentity();
+    start.setOrigin(m_currentPosition);
+    m_targetPosition = m_currentPosition + step;
+    end.setOrigin(m_targetPosition);
+    m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
 
-        end_double.setIdentity();
+    if (callback.hasHit()) {
+        m_currentPosition += callback.m_closestHitFraction * step;
+        m_verticalVelocity = 0.0f;
+        m_verticalOffset = 0.0f;
+        m_wasJumping = false;
+    } else {
+        // sweep again for floor within downStep threshold
+        StepDownConvexResultCallback callback2 (m_ghostObject, 
+                getUpAxisDirections()[m_upAxis], 
+                m_currentPosition, step,
+                m_walkDirection,
+                m_maxSlopeCosine, 
+                m_radius, m_halfHeight);
+
+        callback2.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
+        callback2.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
+
+        m_currentPosition = m_targetPosition;
+        btVector3 oldPosition = m_currentPosition;
+        step = (- m_stepHeight) * getUpAxisDirections()[m_upAxis];
+        m_targetPosition = m_currentPosition + step;
 
         start.setOrigin(m_currentPosition);
         end.setOrigin(m_targetPosition);
+        m_ghostObject->convexSweepTest(m_convexShape, start, end, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
 
-        //set double test for 2x the step drop, to check for a large drop vs small drop
-        end_double.setOrigin(m_targetPosition - step_drop);
-
-        if (m_useGhostObjectSweepTest) {
-            m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
-
-            if (!callback.hasHit()) {
-                //test a double fall height, to see if the character should interpolate it's fall (full) or not (partial)
-                m_ghostObject->convexSweepTest(m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
-            }
+        if (callback2.hasHit()) {
+            m_currentPosition += callback2.m_closestHitFraction * step;
+            m_verticalVelocity = 0.0f;
+            m_verticalOffset = 0.0f;
+            m_wasJumping = false;
         } else {
-            collisionWorld->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
-
-            if (!callback.hasHit()) {
-                //test a double fall height, to see if the character should interpolate it's fall (large) or not (small)
-                collisionWorld->convexSweepTest(m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
-            }
+            // nothing to step down on, so remove the stepUp effect
+            m_currentPosition = oldPosition - m_lastStepUp * getUpAxisDirections()[m_upAxis];
+            m_lastStepUp = 0.0f;
         }
-
-        btScalar downVelocity2 = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
-        bool has_hit = false;
-        if(bounce_fix == true) {
-            has_hit = callback.hasHit() || callback2.hasHit();
-        } else {
-            has_hit = callback2.hasHit();
-        }
-
-        if(downVelocity2 > 0.0 && downVelocity2 < m_stepHeight && has_hit == true && runonce == false
-                && (m_wasOnGround || !m_wasJumping)) {
-            //redo the velocity calculation when falling a small amount, for fast stairs motion
-            //for larger falls, use the smoother/slower interpolated movement by not touching the target position
-
-            m_targetPosition = orig_position;
-            downVelocity = m_stepHeight;
-
-            btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
-            m_targetPosition -= step_drop;
-            runonce = true;
-            continue; //re-run previous tests
-        }
-        break;
-    }
-
-    if (callback.hasHit() || runonce == true) {
-        // we dropped a fraction of the height -> hit floor
-
-        btScalar fraction = (m_currentPosition.getY() - callback.m_hitPointWorld.getY()) / 2;
-
-        //printf("hitpoint: %g - pos %g\n", callback.m_hitPointWorld.getY(), m_currentPosition.getY());
-
-        if (bounce_fix == true) {
-            if (full_drop == true) {
-                m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
-            } else {
-                //due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually
-                m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, fraction);
-            }
-        }
-        else
-            m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
-
-        full_drop = false;
-
-        m_verticalVelocity = 0.0;
-        m_verticalOffset = 0.0;
-        m_wasJumping = false;
-    } else {
-        // we dropped the full height
-
-        full_drop = true;
-
-        if (bounce_fix == true) {
-            downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
-            if (downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping)) {
-                m_targetPosition += step_drop; //undo previous target change
-                downVelocity = m_fallSpeed;
-                step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
-                m_targetPosition -= step_drop;
-            }
-        }
-        //printf("full drop - %g, %g\n", m_currentPosition.getY(), m_targetPosition.getY());
-
-        m_currentPosition = m_targetPosition;
     }
 }
 
-
-
 void CharacterController::setWalkDirection(const btVector3& walkDirection) {
     m_useWalkDirection = true;
     m_walkDirection = walkDirection;
     m_normalizedDirection = getNormalizedVector(m_walkDirection);
 }
 
-
-
 void CharacterController::setVelocityForTimeInterval(const btVector3& velocity, btScalar timeInterval) {
-    //printf("setVelocity!\n");
-    //printf("  interval: %f\n", timeInterval);
-    //printf("  velocity: (%f, %f, %f)\n", velocity.x(), velocity.y(), velocity.z());
-
     m_useWalkDirection = false;
     m_walkDirection = velocity;
     m_normalizedDirection = getNormalizedVector(m_walkDirection);
@@ -565,29 +591,25 @@ void CharacterController::warp(const btVector3& origin) {
 
 
 void CharacterController::preStep(  btCollisionWorld* collisionWorld) {
+    if (!m_enabled) {
+        return;
+    }
     int numPenetrationLoops = 0;
     m_touchingContact = false;
     while (recoverFromPenetration(collisionWorld)) {
         numPenetrationLoops++;
         m_touchingContact = true;
         if (numPenetrationLoops > 4) {
-            //printf("character could not recover from penetration = %d\n", numPenetrationLoops);
             break;
         }
     }
 
     m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
     m_targetPosition = m_currentPosition;
-    //printf("m_targetPosition=%f,%f,%f\n", m_targetPosition[0], m_targetPosition[1], m_targetPosition[2]);
 }
 
 void CharacterController::playerStep(  btCollisionWorld* collisionWorld, btScalar dt) {
-    //printf("playerStep(): ");
-    //printf("  dt = %f", dt);
-
-    // quick check...
-    if (!m_useWalkDirection && m_velocityTimeInterval <= 0.0) {
-        //printf("\n");
+    if (!m_enabled || (!m_useWalkDirection && m_velocityTimeInterval <= 0.0)) {
         return; // no motion
     }
 
@@ -595,49 +617,41 @@ void CharacterController::playerStep(  btCollisionWorld* collisionWorld, btScala
 
     // Update fall velocity.
     m_verticalVelocity -= m_gravity * dt;
-    if (m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed) {
+    if (m_verticalVelocity > m_jumpSpeed) {
         m_verticalVelocity = m_jumpSpeed;
-    }
-    if (m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed)) {
-        m_verticalVelocity = -btFabs(m_fallSpeed);
+    } else if (m_verticalVelocity < -m_maxFallSpeed) {
+        m_verticalVelocity = -m_maxFallSpeed;
     }
     m_verticalOffset = m_verticalVelocity * dt;
 
-
     btTransform xform;
     xform = m_ghostObject->getWorldTransform();
 
-    //printf("walkDirection(%f,%f,%f)\n", walkDirection[0], walkDirection[1], walkDirection[2]);
-    //printf("walkSpeed=%f\n", walkSpeed);
+    // the algorithm is as follows:
+    // (1) step the character up a little bit so that its forward step doesn't hit the floor
+    // (2) step the character forward
+    // (3) step the character down looking for new ledges, the original floor, or a floor one step below where we started
 
-    stepUp (collisionWorld);
+    stepUp(collisionWorld);
     if (m_useWalkDirection) {
-        stepForwardAndStrafe(collisionWorld, m_walkDirection);
+        stepForward(collisionWorld, m_walkDirection);
     } else {
-        //printf("  time: %f", m_velocityTimeInterval);
-        // still have some time left for moving!
-        btScalar dtMoving =
-            (dt < m_velocityTimeInterval) ? dt : m_velocityTimeInterval;
+        // compute substep and decrement total interval
+        btScalar dtMoving = (dt < m_velocityTimeInterval) ? dt : m_velocityTimeInterval;
         m_velocityTimeInterval -= dt;
 
-        // how far will we move while we are moving?
+        // stepForward substep
         btVector3 move = m_walkDirection * dtMoving;
-
-        //printf("  dtMoving: %f", dtMoving);
-
-        // okay, step
-        stepForwardAndStrafe(collisionWorld, move);
+        stepForward(collisionWorld, move);
     }
     stepDown(collisionWorld, dt);
 
-    //printf("\n");
-
     xform.setOrigin(m_currentPosition);
     m_ghostObject->setWorldTransform(xform);
 }
 
-void CharacterController::setFallSpeed(btScalar fallSpeed) {
-    m_fallSpeed = fallSpeed;
+void CharacterController::setMaxFallSpeed(btScalar speed) {
+    m_maxFallSpeed = speed;
 }
 
 void CharacterController::setJumpSpeed(btScalar jumpSpeed) {
@@ -662,7 +676,7 @@ void CharacterController::jump() {
 
 #if 0
     currently no jumping.
-        btTransform xform;
+    btTransform xform;
     m_rigidBody->getMotionState()->getWorldTransform(xform);
     btVector3 up = xform.getBasis()[1];
     up.normalize();
@@ -689,7 +703,7 @@ btScalar CharacterController::getMaxSlope() const {
 }
 
 bool CharacterController::onGround() const {
-    return m_verticalVelocity == 0.0 && m_verticalOffset == 0.0;
+    return m_enabled && m_verticalVelocity == 0.0 && m_verticalOffset == 0.0;
 }
 
 btVector3* CharacterController::getUpAxisDirections() {
@@ -704,3 +718,109 @@ void CharacterController::debugDraw(btIDebugDraw* debugDrawer) {
 void CharacterController::setUpInterpolate(bool value) {
     m_interpolateUp = value;
 }
+
+// protected
+void CharacterController::createShapeAndGhost() {
+    // get new dimensions from avatar
+    m_avatarData->lockForRead();
+    AABox box = m_avatarData->getLocalAABox();
+
+    // create new ghost
+    m_ghostObject = new btPairCachingGhostObject();
+    m_ghostObject->setWorldTransform(btTransform(glmToBullet(m_avatarData->getOrientation()),
+                                                    glmToBullet(m_avatarData->getPosition())));
+    m_avatarData->unlock();
+
+    const glm::vec3& diagonal = box.getScale();
+    m_radius = 0.5f * sqrtf(0.5f * (diagonal.x * diagonal.x + diagonal.z * diagonal.z));
+    m_halfHeight = 0.5f * diagonal.y - m_radius;
+    float MIN_HALF_HEIGHT = 0.1f;
+    if (m_halfHeight < MIN_HALF_HEIGHT) {
+        m_halfHeight = MIN_HALF_HEIGHT;
+    }
+    glm::vec3 offset = box.getCorner() + 0.5f * diagonal;
+    m_shapeLocalOffset = offset;
+
+    // stepHeight affects the heights of ledges that the character can ascend
+    // however the actual ledge height is some function of m_stepHeight 
+    // due to character shape and this CharacterController algorithm 
+    // (the function is approximately 2*m_stepHeight)
+    m_stepHeight = 0.1f * (m_radius + m_halfHeight) + 0.1f;
+
+    // create new shape
+    m_convexShape = new btCapsuleShape(m_radius, 2.0f * m_halfHeight);
+    m_ghostObject->setCollisionShape(m_convexShape);
+    m_ghostObject->setCollisionFlags(btCollisionObject::CF_CHARACTER_OBJECT);
+}
+
+bool CharacterController::needsShapeUpdate() {
+    // get new dimensions from avatar
+    m_avatarData->lockForRead();
+    AABox box = m_avatarData->getLocalAABox();
+    m_avatarData->unlock();
+
+    const glm::vec3& diagonal = box.getScale();
+    float radius = 0.5f * sqrtf(0.5f * (diagonal.x * diagonal.x + diagonal.z * diagonal.z));
+    float halfHeight = 0.5f * diagonal.y - radius;
+    float MIN_HALF_HEIGHT = 0.1f;
+    if (halfHeight < MIN_HALF_HEIGHT) {
+        halfHeight = MIN_HALF_HEIGHT;
+    }
+    glm::vec3 offset = box.getCorner() + 0.5f * diagonal;
+
+    // compare dimensions (and offset)
+    float radiusDelta = glm::abs(radius - m_radius);
+    float heightDelta = glm::abs(halfHeight - m_halfHeight);
+    if (radiusDelta < FLT_EPSILON && heightDelta < FLT_EPSILON) {
+        // shape hasn't changed --> nothing to do
+        float offsetDelta = glm::distance(offset, m_shapeLocalOffset);
+        if (offsetDelta > FLT_EPSILON) {
+            // if only the offset changes then we can update it --> no need to rebuild shape
+            m_shapeLocalOffset = offset;
+        }
+        return false;
+    }
+    return true;
+}
+
+void CharacterController::updateShape() {
+    // DANGER: make sure this CharacterController and its GhostShape have been removed from
+    // the PhysicsEngine before calling this.
+
+    // delete shape and GhostObject
+    delete m_ghostObject;
+    m_ghostObject = NULL;
+    delete m_convexShape;
+    m_convexShape = NULL;
+   
+    createShapeAndGhost();
+}
+
+void CharacterController::preSimulation(btScalar timeStep) {
+    m_avatarData->lockForRead();
+
+    // cache the "PhysicsEnabled" state of m_avatarData here 
+    // and use the cached value for the rest of the simulation step
+    m_enabled = m_avatarData->isPhysicsEnabled();
+
+    glm::quat rotation = m_avatarData->getOrientation();
+    glm::vec3 position = m_avatarData->getPosition() + rotation * m_shapeLocalOffset;
+    m_ghostObject->setWorldTransform(btTransform(glmToBullet(rotation), glmToBullet(position)));
+    btVector3 walkVelocity = glmToBullet(m_avatarData->getVelocity());
+    setVelocityForTimeInterval(walkVelocity, timeStep);
+
+    m_avatarData->unlock();
+}
+
+void CharacterController::postSimulation() {
+    if (m_enabled) {
+        m_avatarData->lockForWrite();
+        const btTransform& avatarTransform = m_ghostObject->getWorldTransform();            
+        glm::quat rotation = bulletToGLM(avatarTransform.getRotation());
+        glm::vec3 offset = rotation * m_shapeLocalOffset;
+        m_avatarData->setOrientation(rotation);
+        m_avatarData->setPosition(bulletToGLM(avatarTransform.getOrigin()) - offset);             
+        m_avatarData->unlock();
+    }
+}
+
diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h
index 301253b2bd..7969fffd9d 100644
--- a/libraries/physics/src/CharacterController.h
+++ b/libraries/physics/src/CharacterController.h
@@ -19,6 +19,8 @@ subject to the following restrictions:
 #ifndef hifi_CharacterController_h
 #define hifi_CharacterController_h
 
+#include <AvatarData.h>
+
 #include <btBulletDynamicsCommon.h>
 #include <BulletDynamics/Character/btCharacterControllerInterface.h>
 #include <BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h>
@@ -39,14 +41,17 @@ ATTRIBUTE_ALIGNED16(class) CharacterController : public btCharacterControllerInt
 {
 protected:
 
+    AvatarData* m_avatarData = NULL;
+    btPairCachingGhostObject* m_ghostObject;
+    glm::vec3 m_shapeLocalOffset;
+
+    btConvexShape* m_convexShape;//is also in m_ghostObject, but it needs to be convex, so we store it here to avoid upcast
+    btScalar m_radius;
     btScalar m_halfHeight;
 
-    btPairCachingGhostObject* m_ghostObject;
-    btConvexShape* m_convexShape;//is also in m_ghostObject, but it needs to be convex, so we store it here to avoid upcast
-
     btScalar m_verticalVelocity;
-    btScalar m_verticalOffset;
-    btScalar m_fallSpeed;
+    btScalar m_verticalOffset; // fall distance from velocity this frame
+    btScalar m_maxFallSpeed;
     btScalar m_jumpSpeed;
     btScalar m_maxJumpHeight;
     btScalar m_maxSlopeRadians; // Slope angle that is set (used for returning the exact value)
@@ -55,7 +60,7 @@ protected:
 
     btScalar m_turnAngle;
 
-    btScalar m_stepHeight;
+    btScalar m_stepHeight; // height of stepUp prior to stepForward
 
     btScalar m_addedMargin;//@todo: remove this and fix the code
 
@@ -65,18 +70,18 @@ protected:
 
     //some internal variables
     btVector3 m_currentPosition;
-    btScalar  m_currentStepOffset;
     btVector3 m_targetPosition;
+    btScalar  m_lastStepUp;
 
     ///keep track of the contact manifolds
     btManifoldArray m_manifoldArray;
 
     bool m_touchingContact;
-    btVector3 m_touchingNormal;
+    btVector3 m_floorNormal; // points from object to character
 
-    bool  m_wasOnGround;
-    bool  m_wasJumping;
-    bool m_useGhostObjectSweepTest;
+    bool m_enabled;
+    bool m_wasOnGround;
+    bool m_wasJumping;
     bool m_useWalkDirection;
     btScalar m_velocityTimeInterval;
     int m_upAxis;
@@ -93,17 +98,14 @@ protected:
     bool recoverFromPenetration(btCollisionWorld* collisionWorld);
     void stepUp(btCollisionWorld* collisionWorld);
     void updateTargetPositionBasedOnCollision(const btVector3& hit_normal, btScalar tangentMag = btScalar(0.0), btScalar normalMag = btScalar(1.0));
-    void stepForwardAndStrafe(btCollisionWorld* collisionWorld, const btVector3& walkMove);
+    void stepForward(btCollisionWorld* collisionWorld, const btVector3& walkMove);
     void stepDown(btCollisionWorld* collisionWorld, btScalar dt);
+    void createShapeAndGhost();
 public:
 
     BT_DECLARE_ALIGNED_ALLOCATOR();
 
-    CharacterController(
-            btPairCachingGhostObject* ghostObject, 
-            btConvexShape* convexShape, 
-            btScalar stepHeight, 
-            int upAxis = 1);
+    CharacterController(AvatarData* avatarData);
     ~CharacterController();
 
 
@@ -145,7 +147,7 @@ public:
     void preStep(btCollisionWorld* collisionWorld);
     void playerStep(btCollisionWorld* collisionWorld, btScalar dt);
 
-    void setFallSpeed(btScalar fallSpeed);
+    void setMaxFallSpeed(btScalar speed);
     void setJumpSpeed(btScalar jumpSpeed);
     void setMaxJumpHeight(btScalar maxJumpHeight);
     bool canJump() const;
@@ -161,12 +163,15 @@ public:
     btScalar getMaxSlope() const;
 
     btPairCachingGhostObject* getGhostObject();
-    void setUseGhostSweepTest(bool useGhostObjectSweepTest) {
-        m_useGhostObjectSweepTest = useGhostObjectSweepTest;
-    }
 
     bool onGround() const;
     void setUpInterpolate(bool value);
+
+    bool needsShapeUpdate();
+    void updateShape();
+
+    void preSimulation(btScalar timeStep);
+    void postSimulation();
 };
 
 #endif // hifi_CharacterController_h
diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp
index 90224caa0c..a46ba9f819 100644
--- a/libraries/physics/src/PhysicsEngine.cpp
+++ b/libraries/physics/src/PhysicsEngine.cpp
@@ -24,8 +24,7 @@ uint32_t PhysicsEngine::getNumSubsteps() {
 }
 
 PhysicsEngine::PhysicsEngine(const glm::vec3& offset)
-    : _originOffset(offset),
-    _avatarShapeLocalOffset(0.0f) {
+    : _originOffset(offset) {
 }
 
 PhysicsEngine::~PhysicsEngine() {
@@ -279,9 +278,6 @@ void PhysicsEngine::init(EntityEditPacketSender* packetSender) {
 }
 
 void PhysicsEngine::stepSimulation() {
-    // expect the engine to have an avatar (and hence: a character controller)
-    assert(_avatarData);
-
     lock();
     // NOTE: the grand order of operations is:
     // (1) relay incoming changes
@@ -298,19 +294,11 @@ void PhysicsEngine::stepSimulation() {
     _clock.reset();
     float timeStep = btMin(dt, MAX_TIMESTEP);
 
-    _avatarData->lockForRead();
-    if (_avatarData->isPhysicsEnabled()) {
-        // update character controller
-        glm::quat rotation = _avatarData->getOrientation();
-        glm::vec3 position = _avatarData->getPosition() + rotation * _avatarShapeLocalOffset;
-        _avatarGhostObject->setWorldTransform(btTransform(glmToBullet(rotation), glmToBullet(position)));
-        
-        btVector3 walkVelocity = glmToBullet(_avatarData->getVelocity());
-        _characterController->setVelocityForTimeInterval(walkVelocity, timeStep);
-    }
-    _avatarData->unlock();
-
     // This is step (2).
+    if (_characterController) {
+        _characterController->preSimulation(timeStep);
+    }
+
     int numSubsteps = _dynamicsWorld->stepSimulation(timeStep, MAX_NUM_SUBSTEPS, PHYSICS_ENGINE_FIXED_SUBSTEP);
     _numSubsteps += (uint32_t)numSubsteps;
     stepNonPhysicalKinematics(usecTimestampNow());
@@ -326,21 +314,11 @@ void PhysicsEngine::stepSimulation() {
         //
         // TODO: untangle these lock sequences.
         _entityTree->lockForWrite();
-
         lock();
         _dynamicsWorld->synchronizeMotionStates();
 
-        _avatarData->lockForRead();
-        bool avatarHasPhysicsEnabled = _avatarData->isPhysicsEnabled();
-        _avatarData->unlock();
-        if (avatarHasPhysicsEnabled) {
-            const btTransform& avatarTransform = _avatarGhostObject->getWorldTransform();
-            glm::quat rotation = bulletToGLM(avatarTransform.getRotation());
-            glm::vec3 offset = rotation * _avatarShapeLocalOffset;
-            _avatarData->lockForWrite();
-            _avatarData->setOrientation(rotation);
-            _avatarData->setPosition(bulletToGLM(avatarTransform.getOrigin()) - offset);
-            _avatarData->unlock();
+        if (_characterController) {
+            _characterController->postSimulation();
         }
 
         unlock();
@@ -620,76 +598,35 @@ bool PhysicsEngine::updateObjectHard(btRigidBody* body, ObjectMotionState* motio
     return true;
 }
 
-
-
 void PhysicsEngine::setAvatarData(AvatarData *avatarData) {
-    assert(avatarData); // don't pass NULL argument
-
-    // compute capsule dimensions
-    AABox box = avatarData->getLocalAABox();
-    const glm::vec3& diagonal = box.getScale();
-    float radius = 0.5f * sqrtf(0.5f * (diagonal.x * diagonal.x + diagonal.z * diagonal.z));
-    float halfHeight = 0.5f * diagonal.y - radius;
-    float MIN_HALF_HEIGHT = 0.1f;
-    if (halfHeight < MIN_HALF_HEIGHT) {
-        halfHeight = MIN_HALF_HEIGHT;
-    }
-    glm::vec3 offset = box.getCorner() + 0.5f * diagonal;
-
-    if (!_avatarData) {
-        // _avatarData is being initialized
-        _avatarData = avatarData;
-    } else {
-        // _avatarData is being updated
-        assert(_avatarData == avatarData);
-
-        // get old dimensions from shape
-        btCapsuleShape* capsule = static_cast<btCapsuleShape*>(_avatarGhostObject->getCollisionShape());
-        btScalar oldRadius = capsule->getRadius();
-        btScalar oldHalfHeight = capsule->getHalfHeight();
-
-        // compare dimensions (and offset)
-        float radiusDelta = glm::abs(radius - oldRadius);
-        float heightDelta = glm::abs(halfHeight - oldHalfHeight);
-        if (radiusDelta < FLT_EPSILON && heightDelta < FLT_EPSILON) {
-            // shape hasn't changed --> nothing to do
-            float offsetDelta = glm::distance(offset, _avatarShapeLocalOffset);
-            if (offsetDelta > FLT_EPSILON) {
-                // if only the offset changes then we can update it --> no need to rebuild shape
-                _avatarShapeLocalOffset = offset;
-            }
-            return;
+    if (_characterController) {
+        bool needsShapeUpdate = _characterController->needsShapeUpdate();
+        if (needsShapeUpdate) {
+            lock();
+            // remove old info
+            _dynamicsWorld->removeCollisionObject(_characterController->getGhostObject());
+            _dynamicsWorld->removeAction(_characterController);
+            // update shape
+            _characterController->updateShape();
+            // insert new info
+            _dynamicsWorld->addCollisionObject(_characterController->getGhostObject(), 
+                    btBroadphaseProxy::CharacterFilter,
+                    btBroadphaseProxy::StaticFilter | btBroadphaseProxy::DefaultFilter);
+            _dynamicsWorld->addAction(_characterController);
+            _characterController->reset(_dynamicsWorld);
+            unlock();
         }
-
-        // delete old controller and friends
-        _dynamicsWorld->removeCollisionObject(_avatarGhostObject);
-        _dynamicsWorld->removeAction(_characterController);
-        delete _characterController;
-        _characterController = NULL;
-        delete _avatarGhostObject;
-        _avatarGhostObject = NULL;
-        delete capsule;
+    } else {
+        // initialize _characterController
+        assert(avatarData); // don't pass NULL argument
+        lock();
+        _characterController = new CharacterController(avatarData);
+        _dynamicsWorld->addCollisionObject(_characterController->getGhostObject(), 
+                btBroadphaseProxy::CharacterFilter,
+                btBroadphaseProxy::StaticFilter | btBroadphaseProxy::DefaultFilter);
+        _dynamicsWorld->addAction(_characterController);
+        _characterController->reset(_dynamicsWorld);
+        unlock();
     }
-
-    // set offset
-    _avatarShapeLocalOffset = offset;
-
-    // build ghost, shape, and controller
-    _avatarGhostObject = new btPairCachingGhostObject();
-    _avatarGhostObject->setWorldTransform(btTransform(glmToBullet(_avatarData->getOrientation()),
-                                                    glmToBullet(_avatarData->getPosition())));
-    // ?TODO: use ShapeManager to get avatar's shape?
-    btCapsuleShape* capsule = new btCapsuleShape(radius, 2.0f * halfHeight);
-
-    _avatarGhostObject->setCollisionShape(capsule);
-    _avatarGhostObject->setCollisionFlags(btCollisionObject::CF_CHARACTER_OBJECT);
-
-    const float MIN_STEP_HEIGHT = 0.35f;
-    btScalar stepHeight = glm::max(MIN_STEP_HEIGHT, radius + 0.5f * halfHeight);
-    _characterController = new CharacterController(_avatarGhostObject, capsule, stepHeight);
-
-    _dynamicsWorld->addCollisionObject(_avatarGhostObject, btBroadphaseProxy::CharacterFilter,
-                                    btBroadphaseProxy::StaticFilter | btBroadphaseProxy::DefaultFilter);
-    _dynamicsWorld->addAction(_characterController);
-    _characterController->reset(_dynamicsWorld);
 }
+
diff --git a/libraries/physics/src/PhysicsEngine.h b/libraries/physics/src/PhysicsEngine.h
index acf1617b16..cb637c60b9 100644
--- a/libraries/physics/src/PhysicsEngine.h
+++ b/libraries/physics/src/PhysicsEngine.h
@@ -123,9 +123,6 @@ private:
 
     /// character collisions
     CharacterController* _characterController = NULL;
-    class btPairCachingGhostObject* _avatarGhostObject = NULL;
-    AvatarData* _avatarData = NULL;
-    glm::vec3 _avatarShapeLocalOffset;
 };
 
 #endif // hifi_PhysicsEngine_h
diff --git a/libraries/render-utils/src/Model.cpp b/libraries/render-utils/src/Model.cpp
index 0e0f081ec8..d640876086 100644
--- a/libraries/render-utils/src/Model.cpp
+++ b/libraries/render-utils/src/Model.cpp
@@ -727,19 +727,19 @@ bool Model::renderCore(float alpha, RenderMode mode, RenderArgs* args) {
 
     //renderMeshes(RenderMode mode, bool translucent, float alphaThreshold, bool hasTangents, bool hasSpecular, book isSkinned, args);
     int opaqueMeshPartsRendered = 0;
-    opaqueMeshPartsRendered += renderMeshes(batch, mode, false, DEFAULT_ALPHA_THRESHOLD, false, false, false, false, args);
-    opaqueMeshPartsRendered += renderMeshes(batch, mode, false, DEFAULT_ALPHA_THRESHOLD, false, false, false, true, args);
-    opaqueMeshPartsRendered += renderMeshes(batch, mode, false, DEFAULT_ALPHA_THRESHOLD, false, false, true, false, args);
-    opaqueMeshPartsRendered += renderMeshes(batch, mode, false, DEFAULT_ALPHA_THRESHOLD, false, false, true, true, args);
-    opaqueMeshPartsRendered += renderMeshes(batch, mode, false, DEFAULT_ALPHA_THRESHOLD, false, true, false, false, args);
-    opaqueMeshPartsRendered += renderMeshes(batch, mode, false, DEFAULT_ALPHA_THRESHOLD, false, true, false, true, args);
-    opaqueMeshPartsRendered += renderMeshes(batch, mode, false, DEFAULT_ALPHA_THRESHOLD, false, true, true, false, args);
-    opaqueMeshPartsRendered += renderMeshes(batch, mode, false, DEFAULT_ALPHA_THRESHOLD, false, true, true, true, args);
+    opaqueMeshPartsRendered += renderMeshes(batch, mode, false, DEFAULT_ALPHA_THRESHOLD, false, false, false, false, args, true);
+    opaqueMeshPartsRendered += renderMeshes(batch, mode, false, DEFAULT_ALPHA_THRESHOLD, false, false, false, true, args, true);
+    opaqueMeshPartsRendered += renderMeshes(batch, mode, false, DEFAULT_ALPHA_THRESHOLD, false, false, true, false, args, true);
+    opaqueMeshPartsRendered += renderMeshes(batch, mode, false, DEFAULT_ALPHA_THRESHOLD, false, false, true, true, args, true);
+    opaqueMeshPartsRendered += renderMeshes(batch, mode, false, DEFAULT_ALPHA_THRESHOLD, false, true, false, false, args, true);
+    opaqueMeshPartsRendered += renderMeshes(batch, mode, false, DEFAULT_ALPHA_THRESHOLD, false, true, false, true, args, true);
+    opaqueMeshPartsRendered += renderMeshes(batch, mode, false, DEFAULT_ALPHA_THRESHOLD, false, true, true, false, args, true);
+    opaqueMeshPartsRendered += renderMeshes(batch, mode, false, DEFAULT_ALPHA_THRESHOLD, false, true, true, true, args, true);
 
-    opaqueMeshPartsRendered += renderMeshes(batch, mode, false, DEFAULT_ALPHA_THRESHOLD, true, false, false, false, args);
-    opaqueMeshPartsRendered += renderMeshes(batch, mode, false, DEFAULT_ALPHA_THRESHOLD, true, false, true, false, args);
-    opaqueMeshPartsRendered += renderMeshes(batch, mode, false, DEFAULT_ALPHA_THRESHOLD, true, true, false, false, args);
-    opaqueMeshPartsRendered += renderMeshes(batch, mode, false, DEFAULT_ALPHA_THRESHOLD, true, true, true, false, args);
+    opaqueMeshPartsRendered += renderMeshes(batch, mode, false, DEFAULT_ALPHA_THRESHOLD, true, false, false, false, args, true);
+    opaqueMeshPartsRendered += renderMeshes(batch, mode, false, DEFAULT_ALPHA_THRESHOLD, true, false, true, false, args, true);
+    opaqueMeshPartsRendered += renderMeshes(batch, mode, false, DEFAULT_ALPHA_THRESHOLD, true, true, false, false, args, true);
+    opaqueMeshPartsRendered += renderMeshes(batch, mode, false, DEFAULT_ALPHA_THRESHOLD, true, true, true, false, args, true);
 
     // render translucent meshes afterwards
     //DependencyManager::get<TextureCache>()->setPrimaryDrawBuffers(false, true, true);
@@ -842,10 +842,67 @@ bool Model::renderCore(float alpha, RenderMode mode, RenderArgs* args) {
         args->_translucentMeshPartsRendered = translucentMeshPartsRendered;
         args->_opaqueMeshPartsRendered = opaqueMeshPartsRendered;
     }
-
+    
+    #ifdef WANT_DEBUG_MESHBOXES
+    renderDebugMeshBoxes();
+    #endif
+    
     return true;
 }
 
+void Model::renderDebugMeshBoxes() {
+    int colorNdx = 0;
+    foreach(AABox box, _calculatedMeshBoxes) {
+        if (_debugMeshBoxesID == GeometryCache::UNKNOWN_ID) {
+            _debugMeshBoxesID = DependencyManager::get<GeometryCache>()->allocateID();
+        }
+        QVector<glm::vec3> points;
+        
+        glm::vec3 brn = box.getCorner();
+        glm::vec3 bln = brn + glm::vec3(box.getDimensions().x, 0, 0);
+        glm::vec3 brf = brn + glm::vec3(0, 0, box.getDimensions().z);
+        glm::vec3 blf = brn + glm::vec3(box.getDimensions().x, 0, box.getDimensions().z);
+
+        glm::vec3 trn = brn + glm::vec3(0, box.getDimensions().y, 0);
+        glm::vec3 tln = bln + glm::vec3(0, box.getDimensions().y, 0);
+        glm::vec3 trf = brf + glm::vec3(0, box.getDimensions().y, 0);
+        glm::vec3 tlf = blf + glm::vec3(0, box.getDimensions().y, 0);
+
+        points << brn << bln;
+        points << brf << blf;
+        points << brn << brf;
+        points << bln << blf;
+
+        points << trn << tln;
+        points << trf << tlf;
+        points << trn << trf;
+        points << tln << tlf;
+
+        points << brn << trn;
+        points << brf << trf;
+        points << bln << tln;
+        points << blf << tlf;
+
+        glm::vec4 color[] = {
+            { 1.0f, 0.0f, 0.0f, 1.0f }, // red
+            { 0.0f, 1.0f, 0.0f, 1.0f }, // green
+            { 0.0f, 0.0f, 1.0f, 1.0f }, // blue
+            { 1.0f, 0.0f, 1.0f, 1.0f }, // purple
+            { 1.0f, 1.0f, 0.0f, 1.0f }, // yellow
+            { 0.0f, 1.0f, 1.0f, 1.0f }, // cyan
+            { 1.0f, 1.0f, 1.0f, 1.0f }, // white
+            { 0.0f, 0.5f, 0.0f, 1.0f }, 
+            { 0.0f, 0.0f, 0.5f, 1.0f }, 
+            { 0.5f, 0.0f, 0.5f, 1.0f }, 
+            { 0.5f, 0.5f, 0.0f, 1.0f }, 
+            { 0.0f, 0.5f, 0.5f, 1.0f } };
+            
+        DependencyManager::get<GeometryCache>()->updateVertices(_debugMeshBoxesID, points, color[colorNdx]);
+        DependencyManager::get<GeometryCache>()->renderVertices(gpu::LINES, _debugMeshBoxesID);
+        colorNdx++;
+    }
+}
+
 Extents Model::getBindExtents() const {
     if (!isActive()) {
         return Extents();
@@ -1306,8 +1363,11 @@ void Model::updateJointState(int index) {
         glm::mat4 parentTransform = glm::scale(_scale) * glm::translate(_offset) * geometry.offset;
         state.computeTransform(parentTransform);
     } else {
-        const JointState& parentState = _jointStates.at(parentIndex);
-        state.computeTransform(parentState.getTransform(), parentState.getTransformChanged());
+        // guard against out-of-bounds access to _jointStates
+        if (joint.parentIndex >= 0 && joint.parentIndex < _jointStates.size()) {
+            const JointState& parentState = _jointStates.at(parentIndex);
+            state.computeTransform(parentState.getTransform(), parentState.getTransformChanged());
+        }
     }
 }
 
@@ -2299,7 +2359,8 @@ int Model::renderMeshesForModelsInScene(gpu::Batch& batch, RenderMode mode, bool
 }
 
 int Model::renderMeshes(gpu::Batch& batch, RenderMode mode, bool translucent, float alphaThreshold,
-                            bool hasLightmap, bool hasTangents, bool hasSpecular, bool isSkinned, RenderArgs* args) {
+                            bool hasLightmap, bool hasTangents, bool hasSpecular, bool isSkinned, RenderArgs* args, 
+                            bool forceRenderSomeMeshes) {
 
     PROFILE_RANGE(__FUNCTION__);
     int meshPartsRendered = 0;
@@ -2319,8 +2380,10 @@ int Model::renderMeshes(gpu::Batch& batch, RenderMode mode, bool translucent, fl
 
     Locations* locations;
     SkinLocations* skinLocations;
-    pickPrograms(batch, mode, translucent, alphaThreshold, hasLightmap, hasTangents, hasSpecular, isSkinned, args, locations, skinLocations);
-    meshPartsRendered = renderMeshesFromList(list, batch, mode, translucent, alphaThreshold, args, locations, skinLocations);
+    pickPrograms(batch, mode, translucent, alphaThreshold, hasLightmap, hasTangents, hasSpecular, isSkinned, 
+                                args, locations, skinLocations);
+    meshPartsRendered = renderMeshesFromList(list, batch, mode, translucent, alphaThreshold, 
+                                args, locations, skinLocations, forceRenderSomeMeshes);
     GLBATCH(glUseProgram)(0);
 
     return meshPartsRendered;
@@ -2328,7 +2391,7 @@ int Model::renderMeshes(gpu::Batch& batch, RenderMode mode, bool translucent, fl
 
 
 int Model::renderMeshesFromList(QVector<int>& list, gpu::Batch& batch, RenderMode mode, bool translucent, float alphaThreshold, RenderArgs* args,
-                                        Locations* locations, SkinLocations* skinLocations) {
+                                        Locations* locations, SkinLocations* skinLocations, bool forceRenderSomeMeshes) {
     PROFILE_RANGE(__FUNCTION__);
 
     auto textureCache = DependencyManager::get<TextureCache>();
@@ -2364,11 +2427,21 @@ int Model::renderMeshesFromList(QVector<int>& list, gpu::Batch& batch, RenderMod
         // if we got here, then check to see if this mesh is in view
         if (args) {
             bool shouldRender = true;
+            bool forceRender = false;
             args->_meshesConsidered++;
 
             if (args->_viewFrustum) {
-                shouldRender = args->_viewFrustum->boxInFrustum(_calculatedMeshBoxes.at(i)) != ViewFrustum::OUTSIDE;
-                if (shouldRender) {
+            
+                // NOTE: This is a hack to address the fact that for avatar meshes, the _calculatedMeshBoxes can be wrong
+                // for some meshes. Those meshes where the mesh's modelTransform is the identity matrix, and will have
+                // incorrectly calculated mesh boxes. In this case, we will ignore the box and assume it's visible.
+                if (forceRenderSomeMeshes && (geometry.meshes.at(i).modelTransform == glm::mat4())) {
+                    forceRender = true;
+                }
+                
+                shouldRender = forceRender || args->_viewFrustum->boxInFrustum(_calculatedMeshBoxes.at(i)) != ViewFrustum::OUTSIDE;
+            
+                if (shouldRender && !forceRender) {
                     float distance = args->_viewFrustum->distanceToCamera(_calculatedMeshBoxes.at(i).calcCenter());
                     shouldRender = !_viewState ? false : _viewState->shouldRenderMesh(_calculatedMeshBoxes.at(i).getLargestDimension(),
                                                                             distance);
diff --git a/libraries/render-utils/src/Model.h b/libraries/render-utils/src/Model.h
index 7468ddb288..2e3f358635 100644
--- a/libraries/render-utils/src/Model.h
+++ b/libraries/render-utils/src/Model.h
@@ -467,6 +467,9 @@ private:
     QVector<int> _meshesOpaqueLightmapTangentsSpecular;
     QVector<int> _meshesOpaqueLightmapSpecular;
 
+    // debug rendering support
+    void renderDebugMeshBoxes();
+    int _debugMeshBoxesID = GeometryCache::UNKNOWN_ID;
 
     // Scene rendering support
     static QVector<Model*> _modelsInScene;
@@ -479,12 +482,15 @@ private:
     void renderSetup(RenderArgs* args);
     bool renderCore(float alpha, RenderMode mode, RenderArgs* args);
     int renderMeshes(gpu::Batch& batch, RenderMode mode, bool translucent, float alphaThreshold, 
-                        bool hasLightmap, bool hasTangents, bool hasSpecular, bool isSkinned, RenderArgs* args = NULL);
+                        bool hasLightmap, bool hasTangents, bool hasSpecular, bool isSkinned, RenderArgs* args = NULL, 
+                        bool forceRenderSomeMeshes = false);
+                        
     void setupBatchTransform(gpu::Batch& batch);
     QVector<int>* pickMeshList(bool translucent, float alphaThreshold, bool hasLightmap, bool hasTangents, bool hasSpecular, bool isSkinned);
 
     int renderMeshesFromList(QVector<int>& list, gpu::Batch& batch, RenderMode mode, bool translucent, float alphaThreshold,
-                                        RenderArgs* args, Locations* locations, SkinLocations* skinLocations);
+                                        RenderArgs* args, Locations* locations, SkinLocations* skinLocations, 
+                                        bool forceRenderSomeMeshes = false);
 
     static void pickPrograms(gpu::Batch& batch, RenderMode mode, bool translucent, float alphaThreshold,
                             bool hasLightmap, bool hasTangents, bool hasSpecular, bool isSkinned, RenderArgs* args,