tuning character so it can walk up ledges

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) {
+                    bool useContact = true;
-                        maxPen = dist;
+                    btVector3 normal = pt.m_normalWorldOnB;
-                        m_touchingNormal = pt.m_normalWorldOnB * directionSign;//??
+                    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;
-
+    m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration);
-    if (m_useGhostObjectSweepTest) {
-        m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration);
-    }
-    else {
-        world->convexSweepTest(m_convexShape, start, end, callback);
-    }
 
     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_lastStepUp = m_stepHeight * callback.m_closestHitFraction;
-            m_currentStepOffset = 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) {
+void CharacterController::stepForward( btCollisionWorld* collisionWorld, const btVector3& movement) {
-    // m_normalizedDirection[0], m_normalizedDirection[1], m_normalizedDirection[2]);
+    // phase 2: forward
-    // phase 2: forward and strafe
+    m_targetPosition = m_currentPosition + movement;
-    btTransform start, end;
-    m_targetPosition = m_currentPosition + walkMove;
 
+    btTransform start, end;
     start.setIdentity();
     end.setIdentity();
 
-    btScalar fraction = 1.0;
+    /* TODO: experiment with this to see if we can use this to help direct motion when a floor is available
-    btScalar distance2 = (m_currentPosition-m_targetPosition).length2();
-
     if (m_touchingContact) {
-        if (m_normalizedDirection.dot(m_touchingNormal) > btScalar(0.0)) {
+        if (m_normalizedDirection.dot(m_floorNormal) < btScalar(0.0)) {
-            //interferes with step movement
+            updateTargetPositionBasedOnCollision(m_floorNormal, 1.0f, 1.0f);
-            //updateTargetPositionBasedOnCollision(m_touchingNormal);
         }
-    }
+    }*/
 
+    // 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;
-
+        m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
-        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;
 
         if (callback.hasHit()) {
-            // we moved only a fraction
+            // we hit soemthing!
-            //btScalar hitDistance;
+            // Compute new target position by removing portion cut-off by collision, which will produce a new target
-            //hitDistance = (callback.m_hitPointWorld - m_currentPosition).length();
+            // 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);
+            stepLength2 = step.length2();
-
-            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;
-            }
         } 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);
+    // The "stepDown" phase first makes a normal sweep down that cancels the lift from the "stepUp" phase.  
-      btScalar downSpeed = (additionalDownStep == 0.0 && m_verticalVelocity < 0.0 ? -m_verticalVelocity : 0.0);
+    // If it hits a ledge then it stops otherwise it makes another sweep down in search of a floor within
-      btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * downSpeed; 
+    // reach of the character's feet.
-      m_targetPosition -= (step_drop + gravity_drop);*/
-
-    btVector3 orig_position = m_targetPosition;
 
     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);
+    // first sweep for ledge
-    m_targetPosition -= step_drop;
+    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);
+    btTransform start, end;
-    callback2.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
+    start.setIdentity();
-    callback2.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
+    end.setIdentity();
 
-    while (1) {
+    start.setOrigin(m_currentPosition);
-        start.setIdentity();
+    m_targetPosition = m_currentPosition + step;
-        end.setIdentity();
+    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
+        if (callback2.hasHit()) {
-        end_double.setOrigin(m_targetPosition - step_drop);
+            m_currentPosition += callback2.m_closestHitFraction * step;
-
+            m_verticalVelocity = 0.0f;
-        if (m_useGhostObjectSweepTest) {
+            m_verticalOffset = 0.0f;
-            m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+            m_wasJumping = false;
-
-            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);
-            }
         } else {
-            collisionWorld->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+            // nothing to step down on, so remove the stepUp effect
-
+            m_currentPosition = oldPosition - m_lastStepUp * getUpAxisDirections()[m_upAxis];
-            if (!callback.hasHit()) {
+            m_lastStepUp = 0.0f;
-                //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);
-            }
         }
-
-        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));
+    m_radius = 0.5f * sqrtf(0.5f * (diagonal.x * diagonal.x + diagonal.z * diagonal.z));
-    float halfHeight = 0.5f * diagonal.y - radius;
+    m_halfHeight = 0.5f * diagonal.y - m_radius;
     float MIN_HALF_HEIGHT = 0.1f;
-    if (halfHeight < MIN_HALF_HEIGHT) {
+    if (m_halfHeight < MIN_HALF_HEIGHT) {
-        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 radiusDelta = glm::abs(radius - m_radius);
-    float heightDelta = glm::abs(halfHeight - oldHalfHeight);
+    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);

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