From 78b614f8553d956de855ddda35a275ad123350d1 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Mon, 16 Mar 2015 16:24:32 -0700
Subject: [PATCH 01/23] move avatar details into CharacterController

---
 libraries/physics/src/CharacterController.cpp | 254 +++++++++++-------
 libraries/physics/src/CharacterController.h   |  24 +-
 libraries/physics/src/PhysicsEngine.cpp       | 125 +++------
 libraries/physics/src/PhysicsEngine.h         |   3 -
 4 files changed, 200 insertions(+), 206 deletions(-)

diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp
index 5173b368c1..994c95e074 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"
 
 
@@ -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))
@@ -111,16 +109,14 @@ protected:
  *
  * 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 +124,34 @@ 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);
+CharacterController::CharacterController(AvatarData* avatarData) {
+    assert(avatarData);
+    m_avatarData = avatarData;
+
+    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_useGhostObjectSweepTest = true;
-    m_ghostObject = ghostObject;
-    m_stepHeight = stepHeight;
-    m_turnAngle = btScalar(0.0);
-    m_convexShape = convexShape;
+    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_currentStepOffset = 0.0f;
 
     // internal state data members
     full_drop = false;
@@ -226,8 +220,6 @@ bool CharacterController::recoverFromPenetration(btCollisionWorld* collisionWorl
                     }
                     m_currentPosition += pt.m_normalWorldOnB * directionSign * dist * btScalar(0.2);
                     penetration = true;
-                } else {
-                    //printf("touching %f\n", dist);
                 }
             }
 
@@ -237,14 +229,13 @@ bool CharacterController::recoverFromPenetration(btCollisionWorld* collisionWorl
     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
     btTransform start, end;
-    m_targetPosition = m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_stepHeight + (m_verticalOffset > 0.f?m_verticalOffset:0.f));
+    m_targetPosition = m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_stepHeight + (m_verticalOffset > 0.0f ? m_verticalOffset : 0.0f));
 
     start.setIdentity();
     end.setIdentity();
@@ -301,22 +292,17 @@ void CharacterController::updateTargetPositionBasedOnCollision(const btVector3&
         //if (tangentMag != 0.0) {
         if (0) {
             btVector3 parComponent = parallelDir * btScalar(tangentMag*movementLength);
-            //printf("parComponent=%f,%f,%f\n", parComponent[0], parComponent[1], parComponent[2]);
             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]);
             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;
@@ -327,7 +313,6 @@ void CharacterController::stepForwardAndStrafe( btCollisionWorld* collisionWorld
 
     btScalar fraction = 1.0;
     btScalar distance2 = (m_currentPosition-m_targetPosition).length2();
-    //printf("distance2=%f\n", distance2);
 
     if (m_touchingContact) {
         if (m_normalizedDirection.dot(m_touchingNormal) > btScalar(0.0)) {
@@ -380,7 +365,6 @@ void CharacterController::stepForwardAndStrafe( btCollisionWorld* collisionWorld
                     break;
                 }
             } else {
-                //printf("currentDir: don't normalize a zero vector\n");
                 break;
             }
         } else {
@@ -388,7 +372,7 @@ void CharacterController::stepForwardAndStrafe( btCollisionWorld* collisionWorld
             m_currentPosition = m_targetPosition;
         }
 
-        //if (callback.m_closestHitFraction == 0.f) {
+        //if (callback.m_closestHitFraction == 0.0f) {
         //  break;
         //}
 
@@ -397,24 +381,23 @@ void CharacterController::stepForwardAndStrafe( btCollisionWorld* collisionWorld
 
 void CharacterController::stepDown( btCollisionWorld* collisionWorld, btScalar dt) {
     btTransform start, end, end_double;
-    bool runonce = false;
+    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; 
+      btScalar downSpeed = (additionalDownStep == 0.0 && m_verticalVelocity < 0.0 ? -m_verticalVelocity : 0.0);
+      btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * downSpeed; 
       m_targetPosition -= (step_drop + gravity_drop);*/
 
     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);
+    btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downSpeed * dt);
     m_targetPosition -= step_drop;
 
     btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
@@ -453,7 +436,7 @@ void CharacterController::stepDown( btCollisionWorld* collisionWorld, btScalar d
             }
         }
 
-        btScalar downVelocity2 = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
+        btScalar downDistance = (m_verticalVelocity < 0.0f ? -m_verticalVelocity : 0.0f) * dt;
         bool has_hit = false;
         if(bounce_fix == true) {
             has_hit = callback.hasHit() || callback2.hasHit();
@@ -461,29 +444,25 @@ void CharacterController::stepDown( btCollisionWorld* collisionWorld, btScalar d
             has_hit = callback2.hasHit();
         }
 
-        if(downVelocity2 > 0.0 && downVelocity2 < m_stepHeight && has_hit == true && runonce == false
+        if(downDistance > 0.0 && downDistance < 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);
+            btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + m_stepHeight);
             m_targetPosition -= step_drop;
-            runonce = true;
+            runOnce = true;
             continue; //re-run previous tests
         }
         break;
     }
 
-    if (callback.hasHit() || runonce == true) {
+    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);
@@ -502,39 +481,28 @@ void CharacterController::stepDown( btCollisionWorld* collisionWorld, btScalar d
         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)) {
+            downSpeed = (m_verticalVelocity < 0.0f) ? -m_verticalVelocity : 0.0f;
+            if (downSpeed > m_maxFallSpeed && (m_wasOnGround || !m_wasJumping)) {
                 m_targetPosition += step_drop; //undo previous target change
-                downVelocity = m_fallSpeed;
-                step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
+                // use fallSpeed instead of downSpeed
+                step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + m_maxFallSpeed * dt);
                 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);
@@ -571,23 +539,16 @@ void CharacterController::preStep(  btCollisionWorld* 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");
         return; // no motion
     }
 
@@ -595,49 +556,36 @@ 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);
-
     stepUp (collisionWorld);
     if (m_useWalkDirection) {
         stepForwardAndStrafe(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);
     }
     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 +610,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();
@@ -704,3 +652,103 @@ void CharacterController::debugDraw(btIDebugDraw* debugDrawer) {
 void CharacterController::setUpInterpolate(bool value) {
     m_interpolateUp = value;
 }
+
+// protected
+void CharacterController::createShapeAndGhost() {
+    // get new dimensions from avatar
+    AABox box = m_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;
+    m_shapeLocalOffset = offset;
+
+    const float MIN_STEP_HEIGHT = 0.35f;
+    m_stepHeight = glm::max(MIN_STEP_HEIGHT, radius + 0.5f * halfHeight);
+
+    // create new shape
+    m_convexShape = new btCapsuleShape(radius, 2.0f * halfHeight);
+
+    // create new ghost
+    m_ghostObject = new btPairCachingGhostObject();
+    m_ghostObject->setWorldTransform(btTransform(glmToBullet(m_avatarData->getOrientation()),
+                                                    glmToBullet(m_avatarData->getPosition())));
+    m_ghostObject->setCollisionShape(m_convexShape);
+    m_ghostObject->setCollisionFlags(btCollisionObject::CF_CHARACTER_OBJECT);
+}
+
+bool CharacterController::needsShapeUpdate() {
+    // get new dimensions from avatar
+    AABox box = m_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;
+
+    // get old dimensions from shape
+    btCapsuleShape* capsule = static_cast<btCapsuleShape*>(m_convexShape);
+    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, 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) {
+    if (m_avatarData->isPhysicsEnabled()) { 
+         m_avatarData->lockForRead();
+        // update character controller
+        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_avatarData->isPhysicsEnabled()) {
+        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..8e1116f7ea 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,15 @@ ATTRIBUTE_ALIGNED16(class) CharacterController : public btCharacterControllerInt
 {
 protected:
 
-    btScalar m_halfHeight;
-
+    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_verticalVelocity;
     btScalar m_verticalOffset;
-    btScalar m_fallSpeed;
+    btScalar m_maxFallSpeed;
     btScalar m_jumpSpeed;
     btScalar m_maxJumpHeight;
     btScalar m_maxSlopeRadians; // Slope angle that is set (used for returning the exact value)
@@ -95,15 +98,12 @@ protected:
     void updateTargetPositionBasedOnCollision(const btVector3& hit_normal, btScalar tangentMag = btScalar(0.0), btScalar normalMag = btScalar(1.0));
     void stepForwardAndStrafe(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 +145,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;
@@ -167,6 +167,12 @@ public:
 
     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 3e2fabfd89..ba42163a0a 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() {
@@ -277,9 +276,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
@@ -296,17 +292,11 @@ void PhysicsEngine::stepSimulation() {
     _clock.reset();
     float timeStep = btMin(dt, MAX_TIMESTEP);
 
-    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);
+    // This is step (2).
+    if (_characterController) {
+        _characterController->preSimulation(timeStep);
     }
 
-    // This is step (2).
     int numSubsteps = _dynamicsWorld->stepSimulation(timeStep, MAX_NUM_SUBSTEPS, PHYSICS_ENGINE_FIXED_SUBSTEP);
     _numSubsteps += (uint32_t)numSubsteps;
     stepNonPhysicalKinematics(usecTimestampNow());
@@ -322,20 +312,14 @@ void PhysicsEngine::stepSimulation() {
         //
         // TODO: untangle these lock sequences.
         _entityTree->lockForWrite();
-        _avatarData->lockForWrite();
         lock();
         _dynamicsWorld->synchronizeMotionStates();
 
-        if (_avatarData->isPhysicsEnabled()) {
-            const btTransform& avatarTransform = _avatarGhostObject->getWorldTransform();
-            glm::quat rotation = bulletToGLM(avatarTransform.getRotation());
-            glm::vec3 offset = rotation * _avatarShapeLocalOffset;
-            _avatarData->setOrientation(rotation);
-            _avatarData->setPosition(bulletToGLM(avatarTransform.getOrigin()) - offset);
+        if (_characterController) {
+            _characterController->postSimulation();
         }
 
         unlock();
-        _avatarData->unlock();
         _entityTree->unlock();
     
         computeCollisionEvents();
@@ -614,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

From b4263cd2ebf4129b4cd9bd272a63603cfeb2f013 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Mon, 16 Mar 2015 17:10:49 -0700
Subject: [PATCH 02/23] allow objs to collide agains char controller

but char controller only responds to penetration when enabled
---
 libraries/physics/src/CharacterController.cpp | 54 ++++++++++++-------
 libraries/physics/src/CharacterController.h   |  5 +-
 2 files changed, 38 insertions(+), 21 deletions(-)

diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp
index 994c95e074..8fb3125ad0 100644
--- a/libraries/physics/src/CharacterController.cpp
+++ b/libraries/physics/src/CharacterController.cpp
@@ -132,6 +132,11 @@ 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)
@@ -533,6 +538,9 @@ void CharacterController::warp(const btVector3& origin) {
 
 
 void CharacterController::preStep(  btCollisionWorld* collisionWorld) {
+    if (!m_enabled) {
+        return;
+    }
     int numPenetrationLoops = 0;
     m_touchingContact = false;
     while (recoverFromPenetration(collisionWorld)) {
@@ -548,7 +556,7 @@ void CharacterController::preStep(  btCollisionWorld* collisionWorld) {
 }
 
 void CharacterController::playerStep(  btCollisionWorld* collisionWorld, btScalar dt) {
-    if (!m_useWalkDirection && m_velocityTimeInterval <= 0.0) {
+    if (!m_enabled || (!m_useWalkDirection && m_velocityTimeInterval <= 0.0)) {
         return; // no motion
     }
 
@@ -637,7 +645,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() {
@@ -656,7 +664,15 @@ void CharacterController::setUpInterpolate(bool 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();
     float radius = 0.5f * sqrtf(0.5f * (diagonal.x * diagonal.x + diagonal.z * diagonal.z));
     float halfHeight = 0.5f * diagonal.y - radius;
@@ -672,18 +688,16 @@ void CharacterController::createShapeAndGhost() {
 
     // create new shape
     m_convexShape = new btCapsuleShape(radius, 2.0f * halfHeight);
-
-    // create new ghost
-    m_ghostObject = new btPairCachingGhostObject();
-    m_ghostObject->setWorldTransform(btTransform(glmToBullet(m_avatarData->getOrientation()),
-                                                    glmToBullet(m_avatarData->getPosition())));
     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;
@@ -727,21 +741,23 @@ void CharacterController::updateShape() {
 }
 
 void CharacterController::preSimulation(btScalar timeStep) {
-    if (m_avatarData->isPhysicsEnabled()) { 
-         m_avatarData->lockForRead();
-        // update character controller
-        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();
-    } 
+    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_avatarData->isPhysicsEnabled()) {
+    if (m_enabled) {
         m_avatarData->lockForWrite();
         const btTransform& avatarTransform = m_ghostObject->getWorldTransform();            
         glm::quat rotation = bulletToGLM(avatarTransform.getRotation());
diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h
index 8e1116f7ea..436f9a7277 100644
--- a/libraries/physics/src/CharacterController.h
+++ b/libraries/physics/src/CharacterController.h
@@ -77,8 +77,9 @@ protected:
     bool m_touchingContact;
     btVector3 m_touchingNormal;
 
-    bool  m_wasOnGround;
-    bool  m_wasJumping;
+    bool m_enabled;
+    bool m_wasOnGround;
+    bool m_wasJumping;
     bool m_useGhostObjectSweepTest;
     bool m_useWalkDirection;
     btScalar m_velocityTimeInterval;

From 9ea13fac378673cb5982f1158e56af1aaf90d31b Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Tue, 17 Mar 2015 09:01:36 -0700
Subject: [PATCH 03/23] cleanup formatting

---
 libraries/physics/src/CharacterController.cpp | 20 ++++++++-----------
 1 file changed, 8 insertions(+), 12 deletions(-)

diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp
index 8fb3125ad0..ae408a9aea 100644
--- a/libraries/physics/src/CharacterController.cpp
+++ b/libraries/physics/src/CharacterController.cpp
@@ -53,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);
@@ -221,7 +221,6 @@ bool CharacterController::recoverFromPenetration(btCollisionWorld* collisionWorl
                     if (dist < maxPen) {
                         maxPen = dist;
                         m_touchingNormal = pt.m_normalWorldOnB * directionSign;//??
-
                     }
                     m_currentPosition += pt.m_normalWorldOnB * directionSign * dist * btScalar(0.2);
                     penetration = true;
@@ -282,7 +281,7 @@ void CharacterController::stepUp( btCollisionWorld* world) {
 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);
@@ -296,12 +295,12 @@ void CharacterController::updateTargetPositionBasedOnCollision(const btVector3&
         m_targetPosition = m_currentPosition;
         //if (tangentMag != 0.0) {
         if (0) {
-            btVector3 parComponent = parallelDir * btScalar(tangentMag*movementLength);
+            btVector3 parComponent = parallelDir * btScalar(tangentMag * movementLength);
             m_targetPosition +=  parComponent;
         }
 
         if (normalMag != 0.0) {
-            btVector3 perpComponent = perpindicularDir * btScalar(normalMag*movementLength);
+            btVector3 perpComponent = perpindicularDir * btScalar(normalMag * movementLength);
             m_targetPosition += perpComponent;
         }
     }
@@ -337,11 +336,9 @@ void CharacterController::stepForwardAndStrafe( btCollisionWorld* collisionWorld
         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 {
@@ -350,7 +347,6 @@ void CharacterController::stepForwardAndStrafe( btCollisionWorld* collisionWorld
 
         m_convexShape->setMargin(margin);
 
-
         fraction -= callback.m_closestHitFraction;
 
         if (callback.hasHit()) {
@@ -443,13 +439,13 @@ void CharacterController::stepDown( btCollisionWorld* collisionWorld, btScalar d
 
         btScalar downDistance = (m_verticalVelocity < 0.0f ? -m_verticalVelocity : 0.0f) * dt;
         bool has_hit = false;
-        if(bounce_fix == true) {
+        if (bounce_fix == true) {
             has_hit = callback.hasHit() || callback2.hasHit();
         } else {
             has_hit = callback2.hasHit();
         }
 
-        if(downDistance > 0.0 && downDistance < m_stepHeight && has_hit == true && runOnce == false
+        if (downDistance > 0.0 && downDistance < 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
@@ -475,9 +471,9 @@ void CharacterController::stepDown( btCollisionWorld* collisionWorld, btScalar d
                 //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
+        } else {
             m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
+        }
 
         full_drop = false;
 

From 8eec83c14489008c8c5de35de14d18bab08bccf1 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Tue, 17 Mar 2015 22:28:38 -0700
Subject: [PATCH 04/23] comments and formatting

---
 libraries/physics/src/CharacterController.cpp | 17 ++++++++++++-----
 libraries/physics/src/CharacterController.h   |  4 ++--
 2 files changed, 14 insertions(+), 7 deletions(-)

diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp
index ae408a9aea..6fdf55abed 100644
--- a/libraries/physics/src/CharacterController.cpp
+++ b/libraries/physics/src/CharacterController.cpp
@@ -90,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);
@@ -209,10 +212,10 @@ 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();
@@ -570,6 +573,11 @@ void CharacterController::playerStep(  btCollisionWorld* collisionWorld, btScala
     btTransform xform;
     xform = m_ghostObject->getWorldTransform();
 
+    // 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 so that its back in contact with the ground
+
     stepUp (collisionWorld);
     if (m_useWalkDirection) {
         stepForwardAndStrafe(collisionWorld, m_walkDirection);
@@ -679,8 +687,7 @@ void CharacterController::createShapeAndGhost() {
     glm::vec3 offset = box.getCorner() + 0.5f * diagonal;
     m_shapeLocalOffset = offset;
 
-    const float MIN_STEP_HEIGHT = 0.35f;
-    m_stepHeight = glm::max(MIN_STEP_HEIGHT, radius + 0.5f * halfHeight);
+    m_stepHeight = 0.1f;
 
     // create new shape
     m_convexShape = new btCapsuleShape(radius, 2.0f * halfHeight);
diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h
index 436f9a7277..1805dcba74 100644
--- a/libraries/physics/src/CharacterController.h
+++ b/libraries/physics/src/CharacterController.h
@@ -58,7 +58,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
 
@@ -75,7 +75,7 @@ protected:
     btManifoldArray m_manifoldArray;
 
     bool m_touchingContact;
-    btVector3 m_touchingNormal;
+    btVector3 m_touchingNormal; // points from character to object
 
     bool m_enabled;
     bool m_wasOnGround;

From 3cd2ce82d4238e39b88f132cda19738e31028a93 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Thu, 19 Mar 2015 15:31:34 -0700
Subject: [PATCH 05/23] tuning character so it can walk up ledges

---
 libraries/physics/src/CharacterController.cpp | 424 ++++++++++--------
 libraries/physics/src/CharacterController.h   |  14 +-
 2 files changed, 241 insertions(+), 197 deletions(-)

diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp
index 6fdf55abed..2c01e2ae32 100644
--- a/libraries/physics/src/CharacterController.cpp
+++ b/libraries/physics/src/CharacterController.cpp
@@ -107,6 +107,91 @@ 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, 
+                btScalar minSlopeDot,
+                const btVector3& start,
+                const btVector3& step,
+                btScalar radius,
+                btScalar halfHeight,
+                btVector3 pushDirection)
+            : btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0))
+              , m_me(me)
+              , m_up(up)
+              , m_minSlopeDot(minSlopeDot)
+              , m_start(start)
+              , m_step(step)
+              , m_radius(radius)
+              , m_halfHeight(halfHeight)
+              , m_pushDirection(pushDirection)
+    {
+    }
+
+    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;
+    btScalar m_minSlopeDot;
+    btVector3 m_start;
+    btVector3 m_step;
+    btScalar m_radius;
+    btScalar m_halfHeight;
+    btVector3 m_pushDirection;
+};
+
 /*
  * Returns the reflection direction of a ray going 'direction' hitting a surface with normal 'normal'
  *
@@ -146,7 +231,6 @@ CharacterController::CharacterController(AvatarData* avatarData) {
 
     m_addedMargin = 0.02f;
     m_walkDirection.setValue(0.0f,0.0f,0.0f);
-    m_useGhostObjectSweepTest = true;
     m_turnAngle = btScalar(0.0f);
     m_useWalkDirection = true; // use walk direction by default, legacy behavior
     m_velocityTimeInterval = 0.0f;
@@ -159,7 +243,7 @@ CharacterController::CharacterController(AvatarData* avatarData) {
     m_wasJumping = false;
     m_interpolateUp = true;
     setMaxSlope(btRadians(45.0f));
-    m_currentStepOffset = 0.0f;
+    m_lastStepUp = 0.0f;
 
     // internal state data members
     full_drop = false;
@@ -194,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++) {
@@ -214,23 +301,52 @@ bool CharacterController::recoverFromPenetration(btCollisionWorld* collisionWorl
 
         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);
+            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;
                 }
             }
-
-            //manifold->clearManifold();
         }
     }
     btTransform newTrans = m_ghostObject->getWorldTransform();
@@ -241,43 +357,44 @@ bool CharacterController::recoverFromPenetration(btCollisionWorld* collisionWorl
 
 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.0f ? m_verticalOffset : 0.0f));
-
     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;
     }
 }
 
@@ -309,194 +426,124 @@ void CharacterController::updateTargetPositionBasedOnCollision(const btVector3&
     }
 }
 
-void CharacterController::stepForwardAndStrafe( btCollisionWorld* collisionWorld, const btVector3& walkMove) {
-    // 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();
-
+    /* 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 {
-                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.0f) {
-        //  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 downSpeed = (additionalDownStep == 0.0 && m_verticalVelocity < 0.0 ? -m_verticalVelocity : 0.0);
-      btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * downSpeed; 
-      m_targetPosition -= (step_drop + gravity_drop);*/
-
-    btVector3 orig_position = m_targetPosition;
+    //
+    // 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.
 
     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 + downSpeed * dt);
-    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_maxSlopeCosine, m_currentPosition, 
+            step, m_radius, m_halfHeight, m_walkDirection);
     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_maxSlopeCosine, m_currentPosition, step, m_radius, m_halfHeight, m_walkDirection);
+
+        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 downDistance = (m_verticalVelocity < 0.0f ? -m_verticalVelocity : 0.0f) * dt;
-        bool has_hit = false;
-        if (bounce_fix == true) {
-            has_hit = callback.hasHit() || callback2.hasHit();
-        } else {
-            has_hit = callback2.hasHit();
-        }
-
-        if (downDistance > 0.0 && downDistance < 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;
-            btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + m_stepHeight);
-            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;
-
-        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) {
-            downSpeed = (m_verticalVelocity < 0.0f) ? -m_verticalVelocity : 0.0f;
-            if (downSpeed > m_maxFallSpeed && (m_wasOnGround || !m_wasJumping)) {
-                m_targetPosition += step_drop; //undo previous target change
-                // use fallSpeed instead of downSpeed
-                step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + m_maxFallSpeed * dt);
-                m_targetPosition -= step_drop;
-            }
-        }
-        m_currentPosition = m_targetPosition;
     }
 }
 
@@ -576,11 +623,11 @@ void CharacterController::playerStep(  btCollisionWorld* collisionWorld, btScala
     // 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 so that its back in contact with the ground
+    // (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 {
         // compute substep and decrement total interval
         btScalar dtMoving = (dt < m_velocityTimeInterval) ? dt : m_velocityTimeInterval;
@@ -588,7 +635,7 @@ void CharacterController::playerStep(  btCollisionWorld* collisionWorld, btScala
 
         // stepForward substep
         btVector3 move = m_walkDirection * dtMoving;
-        stepForwardAndStrafe(collisionWorld, move);
+        stepForward(collisionWorld, move);
     }
     stepDown(collisionWorld, dt);
 
@@ -678,19 +725,23 @@ void CharacterController::createShapeAndGhost() {
     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;
+    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 (halfHeight < MIN_HALF_HEIGHT) {
-        halfHeight = MIN_HALF_HEIGHT;
+    if (m_halfHeight < MIN_HALF_HEIGHT) {
+        m_halfHeight = MIN_HALF_HEIGHT;
     }
     glm::vec3 offset = box.getCorner() + 0.5f * diagonal;
     m_shapeLocalOffset = offset;
 
-    m_stepHeight = 0.1f;
+    // 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(radius, 2.0f * halfHeight);
+    m_convexShape = new btCapsuleShape(m_radius, 2.0f * m_halfHeight);
     m_ghostObject->setCollisionShape(m_convexShape);
     m_ghostObject->setCollisionFlags(btCollisionObject::CF_CHARACTER_OBJECT);
 }
@@ -710,14 +761,9 @@ bool CharacterController::needsShapeUpdate() {
     }
     glm::vec3 offset = box.getCorner() + 0.5f * diagonal;
 
-    // get old dimensions from shape
-    btCapsuleShape* capsule = static_cast<btCapsuleShape*>(m_convexShape);
-    btScalar oldRadius = capsule->getRadius();
-    btScalar oldHalfHeight = capsule->getHalfHeight();
-
     // compare dimensions (and offset)
-    float radiusDelta = glm::abs(radius - oldRadius);
-    float heightDelta = glm::abs(halfHeight - oldHalfHeight);
+    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);
diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h
index 1805dcba74..7969fffd9d 100644
--- a/libraries/physics/src/CharacterController.h
+++ b/libraries/physics/src/CharacterController.h
@@ -46,9 +46,11 @@ protected:
     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;
 
     btScalar m_verticalVelocity;
-    btScalar m_verticalOffset;
+    btScalar m_verticalOffset; // fall distance from velocity this frame
     btScalar m_maxFallSpeed;
     btScalar m_jumpSpeed;
     btScalar m_maxJumpHeight;
@@ -68,19 +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; // points from character to object
+    btVector3 m_floorNormal; // points from object to character
 
     bool m_enabled;
     bool m_wasOnGround;
     bool m_wasJumping;
-    bool m_useGhostObjectSweepTest;
     bool m_useWalkDirection;
     btScalar m_velocityTimeInterval;
     int m_upAxis;
@@ -97,7 +98,7 @@ 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:
@@ -162,9 +163,6 @@ public:
     btScalar getMaxSlope() const;
 
     btPairCachingGhostObject* getGhostObject();
-    void setUseGhostSweepTest(bool useGhostObjectSweepTest) {
-        m_useGhostObjectSweepTest = useGhostObjectSweepTest;
-    }
 
     bool onGround() const;
     void setUpInterpolate(bool value);

From ab9d8eb345b2801f1d85b5e25b212281162ad6c0 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Fri, 20 Mar 2015 08:42:35 -0700
Subject: [PATCH 06/23] reorder arguments to StepDownConvexResultCallback ctor

---
 libraries/physics/src/CharacterController.cpp | 27 ++++++++++++-------
 1 file changed, 17 insertions(+), 10 deletions(-)

diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp
index 2c01e2ae32..fb200b2c57 100644
--- a/libraries/physics/src/CharacterController.cpp
+++ b/libraries/physics/src/CharacterController.cpp
@@ -112,21 +112,21 @@ class StepDownConvexResultCallback : public btCollisionWorld::ClosestConvexResul
     public:
         StepDownConvexResultCallback(btCollisionObject* me, 
                 const btVector3& up, 
-                btScalar minSlopeDot,
                 const btVector3& start,
                 const btVector3& step,
+                const btVector3& pushDirection,
+                btScalar minSlopeDot,
                 btScalar radius,
-                btScalar halfHeight,
-                btVector3 pushDirection)
+                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_minSlopeDot(minSlopeDot)
               , m_start(start)
               , m_step(step)
+              , m_pushDirection(pushDirection)
+              , m_minSlopeDot(minSlopeDot)
               , m_radius(radius)
               , m_halfHeight(halfHeight)
-              , m_pushDirection(pushDirection)
     {
     }
 
@@ -184,12 +184,12 @@ class StepDownConvexResultCallback : public btCollisionWorld::ClosestConvexResul
 protected:
     btCollisionObject* m_me;
     const btVector3 m_up;
-    btScalar m_minSlopeDot;
     btVector3 m_start;
     btVector3 m_step;
+    btVector3 m_pushDirection;
+    btScalar m_minSlopeDot;
     btScalar m_radius;
     btScalar m_halfHeight;
-    btVector3 m_pushDirection;
 };
 
 /*
@@ -499,8 +499,10 @@ void CharacterController::stepDown( btCollisionWorld* collisionWorld, btScalar d
 
     StepDownConvexResultCallback callback(m_ghostObject, 
             getUpAxisDirections()[m_upAxis], 
-            m_maxSlopeCosine, m_currentPosition, 
-            step, m_radius, m_halfHeight, m_walkDirection);
+            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;
 
@@ -520,7 +522,12 @@ void CharacterController::stepDown( btCollisionWorld* collisionWorld, btScalar d
         m_wasJumping = false;
     } else {
         // sweep again for floor within downStep threshold
-        StepDownConvexResultCallback callback2 (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine, m_currentPosition, step, m_radius, m_halfHeight, m_walkDirection);
+        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;

From 34ab47062420f0cad7a4aa8b293fc0d6c19eab07 Mon Sep 17 00:00:00 2001
From: Atlante45 <clement.brisset@gmail.com>
Date: Fri, 20 Mar 2015 21:21:18 +0100
Subject: [PATCH 07/23] lightController first draft

---
 examples/entityScripts/lightController.js | 119 ++++++++++++++++++++++
 1 file changed, 119 insertions(+)
 create mode 100644 examples/entityScripts/lightController.js

diff --git a/examples/entityScripts/lightController.js b/examples/entityScripts/lightController.js
new file mode 100644
index 0000000000..50e32e30f3
--- /dev/null
+++ b/examples/entityScripts/lightController.js
@@ -0,0 +1,119 @@
+(function() {
+    this.entityID = null;
+    this.lightID = null;
+    this.sound = null;
+
+    function checkEntity(entityID) {
+        return entityID && Entities.identifyEntity(entityID).isKnownID;
+    }
+    function getUserData(entityID) {
+        var properties = Entities.getEntityProperties(entityID);
+        if (properties.userData) {
+            return JSON.parse(properties.userData);
+        }
+        print("Warning: light controller has no user data.");
+        // TODO: Remove before merge
+        this.DO_NOT_MERGE();
+        return getUserData(entityID);
+        ////////////////////////////
+        return null;
+    }
+    // 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 });
+        } 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() {
+        var lightProperties = getUserData(this.entityID).lightDefaultProperties;
+        if (lightProperties) {
+            var properties = Entities.getEntityProperties(this.entityID);
+            
+            lightProperties.rotation = Quat.multiply(properties.rotation, lightProperties.rotation);
+            lightProperties.position = Vec3.sum(properties.position,
+                                                Vec3.multiplyQbyV(properties.rotation, lightProperties.position));
+            print("Created light");
+            return Entities.addEntity(lightProperties);
+        } else {
+            print("Warning: light controller has no default light.");
+            return null;
+        }
+    }
+    
+    this.updateLight = function() {
+        // Find valid light
+        if (!checkEntity(this.lightID)) {
+            var userData = getUserData(this.entityID);
+            if (userData.lightID && checkEntity(userData.lightID)) {
+                this.lightID = userData.lightID;
+            } else {
+                this.lightID = null;
+            }
+        }
+
+        // No valid light, create one
+        if (!this.lightID) {
+            this.lightID = this.createLight();
+        }
+    }
+
+    this.preload = function(entityID) {
+        this.entityID = entityID;
+        this.maybeDownloadSound();
+    };
+    
+    this.clickReleaseOnEntity = function(entityID, mouseEvent) {
+        if (mouseEvent.isLeftButton) {
+            this.updateLight();
+            this.toggleLight();
+            this.playSound();
+        } else if (mouseEvent.isRightButton) {
+            print("Right button");
+        }
+    };
+    
+    this.DO_NOT_MERGE = function() {
+        var userData = {
+            lightID: null,
+            lightDefaultProperties: {
+                type: "Light",  
+                position: { x: 0, y: 0.5, z: 0 },
+                dimensions: { x: 2, y: 2, z: 2 },
+                angularVelocity: { x: 0, y: 0, z: 0 },
+                angularDamping: 0,
+
+                isSpotlight: true,
+                color: { red: 255, green: 0, blue: 0 },
+                diffuseColor: { red: 255, green: 0, blue: 0 },
+                ambientColor: { red: 0, green: 0, blue: 255 },
+                specularColor: { red: 0, green: 255, blue: 0 },
+
+                intensity: 10,
+                constantAttenuation: 0,
+                linearAttenuation: 1,
+                quadraticAttenuation: 0,
+                exponent: 0,
+                cutoff: 180, // in degrees
+            }
+        };
+        Entities.editEntity(this.entityID, { userData: JSON.stringify(userData) });
+    }
+})
\ No newline at end of file

From 7dbe92f56fb516486f9bfda20814bd495d0d0d50 Mon Sep 17 00:00:00 2001
From: David Rowe <david@ctrlaltstudio.com>
Date: Sat, 21 Mar 2015 14:22:09 -0700
Subject: [PATCH 08/23] Building with Sixense build requires four of the SDK
 libraries

---
 interface/external/sixense/readme.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

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.

From 19c8e526e1fb630095f6ff4d0763b20ebacef784 Mon Sep 17 00:00:00 2001
From: ZappoMan <bradh@konamoxt.com>
Date: Mon, 23 Mar 2015 08:47:06 -0700
Subject: [PATCH 09/23] add guards to _jointStates array size

---
 interface/src/avatar/FaceModel.cpp   | 2 +-
 libraries/render-utils/src/Model.cpp | 6 ++++--
 2 files changed, 5 insertions(+), 3 deletions(-)

diff --git a/interface/src/avatar/FaceModel.cpp b/interface/src/avatar/FaceModel.cpp
index c80772ef49..6dd97f067a 100644
--- a/interface/src/avatar/FaceModel.cpp
+++ b/interface/src/avatar/FaceModel.cpp
@@ -87,7 +87,7 @@ 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) {
+    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/render-utils/src/Model.cpp b/libraries/render-utils/src/Model.cpp
index 0e0f081ec8..f7a4257de7 100644
--- a/libraries/render-utils/src/Model.cpp
+++ b/libraries/render-utils/src/Model.cpp
@@ -1306,8 +1306,10 @@ 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());
+        if (joint.parentIndex >= 0 && joint.parentIndex < _jointStates.size()) {
+            const JointState& parentState = _jointStates.at(parentIndex);
+            state.computeTransform(parentState.getTransform(), parentState.getTransformChanged());
+        }
     }
 }
 

From 6b4845abf3dea25ba982333c644e52610c6a3fc7 Mon Sep 17 00:00:00 2001
From: Atlante45 <clement.brisset@gmail.com>
Date: Mon, 23 Mar 2015 16:47:24 +0100
Subject: [PATCH 10/23] Some improvements to lightController

---
 examples/entityScripts/lightController.js | 179 +++++++++++++++-------
 1 file changed, 126 insertions(+), 53 deletions(-)

diff --git a/examples/entityScripts/lightController.js b/examples/entityScripts/lightController.js
index 50e32e30f3..76d0b33eb3 100644
--- a/examples/entityScripts/lightController.js
+++ b/examples/entityScripts/lightController.js
@@ -1,23 +1,31 @@
 (function() {
     this.entityID = null;
+    this.properties = null;
     this.lightID = null;
     this.sound = null;
 
-    function checkEntity(entityID) {
-        return entityID && Entities.identifyEntity(entityID).isKnownID;
+    function copyObject(object) {
+        return JSON.parse(JSON.stringify(object));
+    }
+    function didEntityExist(entityID) {
+        return entityID && entityID.isKnownID;
+    }
+    function doesEntityExistNow(entityID) {
+        return entityID && Entities.getEntityProperties(entityID).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;
         }
-        print("Warning: light controller has no user data.");
-        // TODO: Remove before merge
-        this.DO_NOT_MERGE();
-        return getUserData(entityID);
-        ////////////////////////////
-        return null;
     }
+    function updateUserData(entityID, userData) {
+        Entities.editEntity(entityID, { userData: JSON.stringify(userData) });
+    }
+    
     // Download sound if needed
     this.maybeDownloadSound = function() {
         if (this.sound === null) {
@@ -32,6 +40,7 @@
             print("Warning: Couldn't play sound.");
         }
     }
+
     // Toggles the associated light entity
     this.toggleLight = function() {
         if (this.lightID) {
@@ -42,39 +51,126 @@
         }
     }
     
-    this.createLight = function() {
-        var lightProperties = getUserData(this.entityID).lightDefaultProperties;
+    this.createLight = function(userData) {
+        var lightProperties = copyObject(userData.lightDefaultProperties);
         if (lightProperties) {
-            var properties = Entities.getEntityProperties(this.entityID);
+            var entityProperties = Entities.getEntityProperties(this.entityID);
             
-            lightProperties.rotation = Quat.multiply(properties.rotation, lightProperties.rotation);
-            lightProperties.position = Vec3.sum(properties.position,
-                                                Vec3.multiplyQbyV(properties.rotation, lightProperties.position));
-            print("Created light");
-            return Entities.addEntity(lightProperties);
+            lightProperties.visible = false;
+            lightProperties.position = Vec3.sum(entityProperties.position,
+                                                Vec3.multiplyQbyV(entityProperties.rotation,
+                                                                  lightProperties.position));
+
+            print(lightProperties);
+            print(JSON.stringify(lightProperties));
+            var newLight = Entities.addEntity(lightProperties);
+            print(newLight);
+            print(JSON.stringify(newLight));
+            return newLight;
         } else {
             print("Warning: light controller has no default light.");
             return null;
         }
     }
     
-    this.updateLight = function() {
+    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: 2, y: 2, z: 2 },
+                    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 (!checkEntity(this.lightID)) {
-            var userData = getUserData(this.entityID);
-            if (userData.lightID && checkEntity(userData.lightID)) {
-                this.lightID = userData.lightID;
-            } else {
-                this.lightID = null;
+        if (doesEntityExistNow(this.lightID)) {
+            if (!didEntityExist(this.lightID)) {
+                // Light now has an ID, so update it in userData
+                this.lightID = Entities.identifyEntity(this.lightID);
+                userData.lightID = this.lightID.id;
+                updateUserData(this.entityID, userData);
             }
+            return;
+        }
+        
+        if (doesEntityExistNow(userData.lightID)) {
+            this.lightID = Entities.identifyEntity(userData.lightID);
+            return;
         }
 
         // No valid light, create one
-        if (!this.lightID) {
-            this.lightID = this.createLight();
+        this.lightID = this.createLight(userData);
+        print("Created new light entity");
+        
+        // Update user data with new ID
+        userData.lightID = this.lightID.id;
+        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 entityProperties = Entities.getEntityProperties(this.entityID);
+        var lightProperties = Entities.getEntityProperties(this.lightID);
+        var newProperties = {};
+
+        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;
+       
+       // Copy only meaningful properties (trying to save space in userData here)
+       newProperties.dimensions = lightProperties.dimensions;
+       newProperties.color = lightProperties.color;
+       newProperties.lifetime = lightProperties.lifetime;
+       newProperties.isSpotlight = lightProperties.isSpotlight;
+       newProperties.intensity = lightProperties.intensity;
+       newProperties.exponent = lightProperties.exponent;
+       newProperties.cutoff = lightProperties.cutoff;
+
+        var userData = getUserData(this.entityID);
+        userData.lightDefaultProperties = copyObject(lightProperties);
+        updateUserData(this.entityID, userData);
+        
+        print("Relative properties of light entity saved.");
+        print(JSON.stringify(userData));
+    }
+
     this.preload = function(entityID) {
         this.entityID = entityID;
         this.maybeDownloadSound();
@@ -82,38 +178,15 @@
     
     this.clickReleaseOnEntity = function(entityID, mouseEvent) {
         if (mouseEvent.isLeftButton) {
-            this.updateLight();
+            this.updateLightID();
+            this.maybeMoveLight();
             this.toggleLight();
             this.playSound();
         } else if (mouseEvent.isRightButton) {
-            print("Right button");
+            this.updateRelativeLightPosition();
         }
     };
     
-    this.DO_NOT_MERGE = function() {
-        var userData = {
-            lightID: null,
-            lightDefaultProperties: {
-                type: "Light",  
-                position: { x: 0, y: 0.5, z: 0 },
-                dimensions: { x: 2, y: 2, z: 2 },
-                angularVelocity: { x: 0, y: 0, z: 0 },
-                angularDamping: 0,
-
-                isSpotlight: true,
-                color: { red: 255, green: 0, blue: 0 },
-                diffuseColor: { red: 255, green: 0, blue: 0 },
-                ambientColor: { red: 0, green: 0, blue: 255 },
-                specularColor: { red: 0, green: 255, blue: 0 },
-
-                intensity: 10,
-                constantAttenuation: 0,
-                linearAttenuation: 1,
-                quadraticAttenuation: 0,
-                exponent: 0,
-                cutoff: 180, // in degrees
-            }
-        };
-        Entities.editEntity(this.entityID, { userData: JSON.stringify(userData) });
-    }
+    // file:///Users/clement/hifi/examples/entityScripts/lightController.js
+    // file:///Users/clement/Downloads/japan-lamp.fbx
 })
\ No newline at end of file

From 5fadcbe7d9c4a3051f845a26f91631c08425f2ea Mon Sep 17 00:00:00 2001
From: Atlante45 <clement.brisset@gmail.com>
Date: Mon, 23 Mar 2015 17:05:46 +0100
Subject: [PATCH 11/23] Prints cleanup

---
 examples/entityScripts/lightController.js | 9 +--------
 1 file changed, 1 insertion(+), 8 deletions(-)

diff --git a/examples/entityScripts/lightController.js b/examples/entityScripts/lightController.js
index 76d0b33eb3..68844b5605 100644
--- a/examples/entityScripts/lightController.js
+++ b/examples/entityScripts/lightController.js
@@ -60,13 +60,7 @@
             lightProperties.position = Vec3.sum(entityProperties.position,
                                                 Vec3.multiplyQbyV(entityProperties.rotation,
                                                                   lightProperties.position));
-
-            print(lightProperties);
-            print(JSON.stringify(lightProperties));
-            var newLight = Entities.addEntity(lightProperties);
-            print(newLight);
-            print(JSON.stringify(newLight));
-            return newLight;
+            return Entities.addEntity(lightProperties);
         } else {
             print("Warning: light controller has no default light.");
             return null;
@@ -168,7 +162,6 @@
         updateUserData(this.entityID, userData);
         
         print("Relative properties of light entity saved.");
-        print(JSON.stringify(userData));
     }
 
     this.preload = function(entityID) {

From 66f954142a6d290124a0087250a32175fa917caf Mon Sep 17 00:00:00 2001
From: Atlante45 <clement.brisset@gmail.com>
Date: Mon, 23 Mar 2015 17:06:12 +0100
Subject: [PATCH 12/23] Fix light multiple creations

---
 examples/entityScripts/lightController.js | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/examples/entityScripts/lightController.js b/examples/entityScripts/lightController.js
index 68844b5605..7086cc1ce8 100644
--- a/examples/entityScripts/lightController.js
+++ b/examples/entityScripts/lightController.js
@@ -97,7 +97,7 @@
             if (!didEntityExist(this.lightID)) {
                 // Light now has an ID, so update it in userData
                 this.lightID = Entities.identifyEntity(this.lightID);
-                userData.lightID = this.lightID.id;
+                userData.lightID = this.lightID;
                 updateUserData(this.entityID, userData);
             }
             return;
@@ -113,7 +113,7 @@
         print("Created new light entity");
         
         // Update user data with new ID
-        userData.lightID = this.lightID.id;
+        userData.lightID = this.lightID;
         updateUserData(this.entityID, userData);
     }
     

From efd177d80d79bb4cf4d043741dd7a144462fac2b Mon Sep 17 00:00:00 2001
From: Atlante45 <clement.brisset@gmail.com>
Date: Mon, 23 Mar 2015 17:06:34 +0100
Subject: [PATCH 13/23] Fix property save

---
 examples/entityScripts/lightController.js | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/examples/entityScripts/lightController.js b/examples/entityScripts/lightController.js
index 7086cc1ce8..d8dfd36c38 100644
--- a/examples/entityScripts/lightController.js
+++ b/examples/entityScripts/lightController.js
@@ -158,7 +158,7 @@
        newProperties.cutoff = lightProperties.cutoff;
 
         var userData = getUserData(this.entityID);
-        userData.lightDefaultProperties = copyObject(lightProperties);
+        userData.lightDefaultProperties = copyObject(newProperties);
         updateUserData(this.entityID, userData);
         
         print("Relative properties of light entity saved.");

From 30ea2eb722892b5bbcf57444cd1a9606fe7fbd52 Mon Sep 17 00:00:00 2001
From: Atlante45 <clement.brisset@gmail.com>
Date: Mon, 23 Mar 2015 17:20:21 +0100
Subject: [PATCH 14/23] Nicer way to update light Properties

---
 examples/entityScripts/lightController.js | 24 +++++++++++------------
 1 file changed, 11 insertions(+), 13 deletions(-)

diff --git a/examples/entityScripts/lightController.js b/examples/entityScripts/lightController.js
index d8dfd36c38..44350da277 100644
--- a/examples/entityScripts/lightController.js
+++ b/examples/entityScripts/lightController.js
@@ -138,29 +138,27 @@
             return;
         }
 
+        var userData = getUserData(this.entityID);
+        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] = userData.lightDefaultProperties[key];
+            }
+        }
+        
+        // Compute new relative position
         var entityProperties = Entities.getEntityProperties(this.entityID);
         var lightProperties = Entities.getEntityProperties(this.lightID);
-        var newProperties = {};
-
         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;
-       
-       // Copy only meaningful properties (trying to save space in userData here)
-       newProperties.dimensions = lightProperties.dimensions;
-       newProperties.color = lightProperties.color;
-       newProperties.lifetime = lightProperties.lifetime;
-       newProperties.isSpotlight = lightProperties.isSpotlight;
-       newProperties.intensity = lightProperties.intensity;
-       newProperties.exponent = lightProperties.exponent;
-       newProperties.cutoff = lightProperties.cutoff;
 
-        var userData = getUserData(this.entityID);
         userData.lightDefaultProperties = copyObject(newProperties);
         updateUserData(this.entityID, userData);
-        
         print("Relative properties of light entity saved.");
     }
 

From 40ed0146e2a533e7738c7ea465e26ef55710c5c2 Mon Sep 17 00:00:00 2001
From: Atlante45 <clement.brisset@gmail.com>
Date: Mon, 23 Mar 2015 17:26:43 +0100
Subject: [PATCH 15/23] Better handling of erased lights

---
 examples/entityScripts/lightController.js | 10 +++++++---
 1 file changed, 7 insertions(+), 3 deletions(-)

diff --git a/examples/entityScripts/lightController.js b/examples/entityScripts/lightController.js
index 44350da277..a012ff45f8 100644
--- a/examples/entityScripts/lightController.js
+++ b/examples/entityScripts/lightController.js
@@ -11,7 +11,11 @@
         return entityID && entityID.isKnownID;
     }
     function doesEntityExistNow(entityID) {
-        return entityID && Entities.getEntityProperties(entityID).isKnownID;
+        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);
@@ -96,7 +100,7 @@
         if (doesEntityExistNow(this.lightID)) {
             if (!didEntityExist(this.lightID)) {
                 // Light now has an ID, so update it in userData
-                this.lightID = Entities.identifyEntity(this.lightID);
+                this.lightID = getTrueID(this.lightID);
                 userData.lightID = this.lightID;
                 updateUserData(this.entityID, userData);
             }
@@ -104,7 +108,7 @@
         }
         
         if (doesEntityExistNow(userData.lightID)) {
-            this.lightID = Entities.identifyEntity(userData.lightID);
+            this.lightID = getTrueID(userData.lightID);
             return;
         }
 

From 963be820d059da0b7a75129a5a6b28d8cbb062b0 Mon Sep 17 00:00:00 2001
From: Atlante45 <clement.brisset@gmail.com>
Date: Mon, 23 Mar 2015 17:28:11 +0100
Subject: [PATCH 16/23] Remove comments

---
 examples/entityScripts/lightController.js | 3 ---
 1 file changed, 3 deletions(-)

diff --git a/examples/entityScripts/lightController.js b/examples/entityScripts/lightController.js
index a012ff45f8..981099377c 100644
--- a/examples/entityScripts/lightController.js
+++ b/examples/entityScripts/lightController.js
@@ -181,7 +181,4 @@
             this.updateRelativeLightPosition();
         }
     };
-    
-    // file:///Users/clement/hifi/examples/entityScripts/lightController.js
-    // file:///Users/clement/Downloads/japan-lamp.fbx
 })
\ No newline at end of file

From fdfdba5d2d36a40b7d81b0dc5ffce216884ab106 Mon Sep 17 00:00:00 2001
From: Atlante45 <clement.brisset@gmail.com>
Date: Mon, 23 Mar 2015 17:46:57 +0100
Subject: [PATCH 17/23] Couple tweaks

---
 examples/entityScripts/lightController.js | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/examples/entityScripts/lightController.js b/examples/entityScripts/lightController.js
index 981099377c..dd1a50cd84 100644
--- a/examples/entityScripts/lightController.js
+++ b/examples/entityScripts/lightController.js
@@ -79,7 +79,7 @@
                 lightDefaultProperties: {
                     type: "Light",
                     position: { x: 0, y: 0, z: 0 },
-                    dimensions: { x: 2, y: 2, z: 2 },
+                    dimensions: { x: 5, y: 5, z: 5 },
                     isSpotlight: false,
                     color: { red: 255, green: 48, blue: 0 },
                     diffuseColor: { red: 255, green: 255, blue: 255 },
@@ -172,6 +172,9 @@
     };
     
     this.clickReleaseOnEntity = function(entityID, mouseEvent) {
+        this.entityID = entityID;
+        this.maybeDownloadSound();
+        
         if (mouseEvent.isLeftButton) {
             this.updateLightID();
             this.maybeMoveLight();

From ea0f3f05e7e1fdccb2c38618048c2b8c57f45c8c Mon Sep 17 00:00:00 2001
From: Atlante45 <clement.brisset@gmail.com>
Date: Mon, 23 Mar 2015 17:55:23 +0100
Subject: [PATCH 18/23] Tune sound down

---
 examples/entityScripts/lightController.js | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/examples/entityScripts/lightController.js b/examples/entityScripts/lightController.js
index dd1a50cd84..ba4fd60b03 100644
--- a/examples/entityScripts/lightController.js
+++ b/examples/entityScripts/lightController.js
@@ -39,7 +39,10 @@
     // Play switch sound
     this.playSound = function() {
         if (this.sound && this.sound.downloaded) {
-            Audio.playSound(this.sound, { position: Entities.getEntityProperties(this.entityID).position });
+            Audio.playSound(this.sound, {
+                position: Entities.getEntityProperties(this.entityID).position,
+                volume: 0.2
+            });
         } else {
             print("Warning: Couldn't play sound.");
         }

From fe8ad9517f788bbadb9c13b95d4a9c4983ae2130 Mon Sep 17 00:00:00 2001
From: ZappoMan <bradh@konamoxt.com>
Date: Mon, 23 Mar 2015 10:08:08 -0700
Subject: [PATCH 19/23] added debugging for model mesh boxes

---
 libraries/render-utils/src/Model.cpp | 59 +++++++++++++++++++++++++++-
 libraries/render-utils/src/Model.h   |  3 ++
 2 files changed, 61 insertions(+), 1 deletion(-)

diff --git a/libraries/render-utils/src/Model.cpp b/libraries/render-utils/src/Model.cpp
index f7a4257de7..d91c972c45 100644
--- a/libraries/render-utils/src/Model.cpp
+++ b/libraries/render-utils/src/Model.cpp
@@ -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();
diff --git a/libraries/render-utils/src/Model.h b/libraries/render-utils/src/Model.h
index 5114ef1c9f..e7cb0aa8e5 100644
--- a/libraries/render-utils/src/Model.h
+++ b/libraries/render-utils/src/Model.h
@@ -444,6 +444,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;

From c26f17e16b6fee518912880c13dfe0923a979b8f Mon Sep 17 00:00:00 2001
From: Atlante45 <clement.brisset@gmail.com>
Date: Mon, 23 Mar 2015 18:36:40 +0100
Subject: [PATCH 20/23] Update from light properties

---
 examples/entityScripts/lightController.js | 14 +++++++-------
 1 file changed, 7 insertions(+), 7 deletions(-)

diff --git a/examples/entityScripts/lightController.js b/examples/entityScripts/lightController.js
index ba4fd60b03..624fc929b3 100644
--- a/examples/entityScripts/lightController.js
+++ b/examples/entityScripts/lightController.js
@@ -146,23 +146,23 @@
         }
 
         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] = userData.lightDefaultProperties[key];
+                newProperties[key] = lightProperties[key];
             }
         }
-        
+
         // Compute new relative position
-        var entityProperties = Entities.getEntityProperties(this.entityID);
-        var lightProperties = Entities.getEntityProperties(this.lightID);
         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;
+        // 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);

From b705d7633685c7f4ff81af45ffed3364632cb13b Mon Sep 17 00:00:00 2001
From: ZappoMan <bradh@konamoxt.com>
Date: Mon, 23 Mar 2015 10:43:21 -0700
Subject: [PATCH 21/23] total hack to ignore frustum culling in case of avatar
 mesh parts with no transform

---
 libraries/render-utils/src/Model.cpp | 49 ++++++++++++++++++----------
 libraries/render-utils/src/Model.h   |  7 ++--
 2 files changed, 36 insertions(+), 20 deletions(-)

diff --git a/libraries/render-utils/src/Model.cpp b/libraries/render-utils/src/Model.cpp
index d91c972c45..0b7fd3dcbf 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);
@@ -2358,7 +2358,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;
@@ -2378,8 +2379,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;
@@ -2387,7 +2390,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>();
@@ -2423,11 +2426,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 e7cb0aa8e5..05db20b056 100644
--- a/libraries/render-utils/src/Model.h
+++ b/libraries/render-utils/src/Model.h
@@ -459,12 +459,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,

From 3e8508f1493c654fde86d917facf68af35a70da3 Mon Sep 17 00:00:00 2001
From: ZappoMan <bradh@konamoxt.com>
Date: Mon, 23 Mar 2015 11:13:35 -0700
Subject: [PATCH 22/23] CR feedback

---
 libraries/render-utils/src/Model.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/libraries/render-utils/src/Model.cpp b/libraries/render-utils/src/Model.cpp
index 0b7fd3dcbf..d640876086 100644
--- a/libraries/render-utils/src/Model.cpp
+++ b/libraries/render-utils/src/Model.cpp
@@ -1363,6 +1363,7 @@ void Model::updateJointState(int index) {
         glm::mat4 parentTransform = glm::scale(_scale) * glm::translate(_offset) * geometry.offset;
         state.computeTransform(parentTransform);
     } else {
+        // 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());

From 9fb53e3d72e8618401a80424f97e5756ecb6500f Mon Sep 17 00:00:00 2001
From: ZappoMan <bradh@konamoxt.com>
Date: Mon, 23 Mar 2015 11:13:42 -0700
Subject: [PATCH 23/23] CR feedback

---
 interface/src/avatar/FaceModel.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/interface/src/avatar/FaceModel.cpp b/interface/src/avatar/FaceModel.cpp
index 6dd97f067a..722f998f86 100644
--- a/interface/src/avatar/FaceModel.cpp
+++ b/interface/src/avatar/FaceModel.cpp
@@ -87,6 +87,7 @@ void FaceModel::maybeUpdateEyeRotation(Model* model, const JointState& parentSta
 void FaceModel::updateJointState(int index) {
     JointState& state = _jointStates[index];
     const FBXJoint& joint = state.getFBXJoint();
+    // 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();