-diff -urN a/src/BulletDynamics/Character/btCharacterControllerInterface.h b/src/BulletDynamics/Character/btCharacterControllerInterface.h
---- a/src/BulletDynamics/Character/btCharacterControllerInterface.h 2013-01-22 17:54:25.468670949 -0600
-+++ b/src/BulletDynamics/Character/btCharacterControllerInterface.h 2011-04-28 12:53:37.000000000 -0500
-@@ -31,7 +31,7 @@
-
- virtual void setWalkDirection(const btVector3& walkDirection) = 0;
- virtual void setVelocityForTimeInterval(const btVector3& velocity, btScalar timeInterval) = 0;
-- virtual void reset () = 0;
-+ virtual void reset ( btCollisionWorld* collisionWorld ) = 0;
- virtual void warp (const btVector3& origin) = 0;
-
- virtual void preStep ( btCollisionWorld* collisionWorld) = 0;
-@@ -40,6 +40,7 @@
- virtual void jump () = 0;
-
- virtual bool onGround () const = 0;
-+ virtual void setUpInterpolate (bool value) = 0;
- };
-
- #endif //BT_CHARACTER_CONTROLLER_INTERFACE_H
-diff -urN a/src/BulletDynamics/Character/btKinematicCharacterController.cpp b/src/BulletDynamics/Character/btKinematicCharacterController.cpp
---- a/src/BulletDynamics/Character/btKinematicCharacterController.cpp 2013-01-22 17:54:25.444670455 -0600
-+++ b/src/BulletDynamics/Character/btKinematicCharacterController.cpp 2013-01-22 17:17:47.583541659 -0600
-@@ -14,6 +14,7 @@
- */
-
-
-+#include <stdio.h>
- #include "LinearMath/btIDebugDraw.h"
- #include "BulletCollision/CollisionDispatch/btGhostObject.h"
- #include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
-@@ -77,6 +78,9 @@
- if (convexResult.m_hitCollisionObject == m_me)
- return btScalar(1.0);
+diff -urN bullet.orig/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp bullet.new/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
+--- bullet.orig/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp 2014-01-10 15:49:31.000000000 -0600
++++ bullet.new/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp 2015-03-26 20:16:59.766935213 -0500
+@@ -51,7 +51,7 @@
+ m_ignoreMargin(false),
+ m_lastUsedMethod(-1),
+ m_catchDegeneracies(1),
+-m_fixContactNormalDirection(1)
++m_fixContactNormalDirection(0)
+ {
+ }
+ btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver)
+@@ -67,7 +67,7 @@
+ m_ignoreMargin(false),
+ m_lastUsedMethod(-1),
+ m_catchDegeneracies(1),
+-m_fixContactNormalDirection(1)
++m_fixContactNormalDirection(0)
+ {
+ }
-+ if (!convexResult.m_hitCollisionObject->hasContactResponse())
-+ return btScalar(1.0);
-+
- btVector3 hitNormalWorld;
- if (normalInWorldSpace)
- {
-@@ -146,7 +150,11 @@
- m_jumpSpeed = 10.0; // ?
- m_wasOnGround = false;
- m_wasJumping = false;
-+ m_interpolateUp = true;
- setMaxSlope(btRadians(45.0));
-+ m_currentStepOffset = 0;
-+ full_drop = false;
-+ bounce_fix = false;
+diff -urN bullet.orig/src/BulletDynamics/Character/btKinematicCharacterController.cpp bullet.new/src/BulletDynamics/Character/btKinematicCharacterController.cpp
+--- bullet.orig/src/BulletDynamics/Character/btKinematicCharacterController.cpp 2015-03-26 13:15:36.992897379 -0500
++++ bullet.new/src/BulletDynamics/Character/btKinematicCharacterController.cpp 2015-12-22 17:59:06.345343465 -0600
+@@ -136,6 +136,7 @@
+ m_upAxis = upAxis;
+ m_addedMargin = 0.02;
+ m_walkDirection.setValue(0,0,0);
++ m_normalizedDirection.setValue(0,0,0);
+ m_useGhostObjectSweepTest = true;
+ m_ghostObject = ghostObject;
+ m_stepHeight = stepHeight;
+@@ -187,7 +188,7 @@
+
+ collisionWorld->getDispatcher()->dispatchAllCollisionPairs(m_ghostObject->getOverlappingPairCache(), collisionWorld->getDispatchInfo(), collisionWorld->getDispatcher());
+
+- m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
++ //m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
+
+ btScalar maxPen = btScalar(0.0);
+ for (int i = 0; i < m_ghostObject->getOverlappingPairCache()->getNumOverlappingPairs(); i++)
+@@ -234,10 +235,13 @@
+ //manifold->clearManifold();
+ }
+ }
+- btTransform newTrans = m_ghostObject->getWorldTransform();
+- newTrans.setOrigin(m_currentPosition);
+- m_ghostObject->setWorldTransform(newTrans);
+-// printf("m_touchingNormal = %f,%f,%f\n",m_touchingNormal[0],m_touchingNormal[1],m_touchingNormal[2]);
++ if (penetration == true)
++ {
++ 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;
}
- btKinematicCharacterController::~btKinematicCharacterController ()
-@@ -187,6 +195,12 @@
- m_manifoldArray.resize(0);
+@@ -338,17 +342,19 @@
+ btScalar distance2 = (m_currentPosition-m_targetPosition).length2();
+ // printf("distance2=%f\n",distance2);
- btBroadphasePair* collisionPair = &m_ghostObject->getOverlappingPairCache()->getOverlappingPairArray()[i];
-+
-+ btCollisionObject* obj0 = static_cast<btCollisionObject*>(collisionPair->m_pProxy0->m_clientObject);
-+ btCollisionObject* obj1 = static_cast<btCollisionObject*>(collisionPair->m_pProxy1->m_clientObject);
-+
-+ if ((obj0 && !obj0->hasContactResponse()) || (obj1 && !obj1->hasContactResponse()))
-+ continue;
-
- if (collisionPair->m_algorithm)
- collisionPair->m_algorithm->getAllContactManifolds(m_manifoldArray);
-@@ -260,7 +274,10 @@
- {
- // we moved up only a fraction of the step height
- m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction;
-- m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
-+ if (m_interpolateUp == true)
-+ m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
-+ else
-+ m_currentPosition = m_targetPosition;
- }
- m_verticalVelocity = 0.0;
- m_verticalOffset = 0.0;
-@@ -325,7 +342,8 @@
+- if (m_touchingContact)
++ /*if (m_touchingContact)
{
if (m_normalizedDirection.dot(m_touchingNormal) > btScalar(0.0))
{
-- updateTargetPositionBasedOnCollision (m_touchingNormal);
-+ //interferes with step movement
-+ //updateTargetPositionBasedOnCollision (m_touchingNormal);
+ //interferes with step movement
+- //updateTargetPositionBasedOnCollision (m_touchingNormal);
++ updateTargetPositionBasedOnCollision (m_touchingNormal);
}
- }
-
-@@ -397,7 +415,8 @@
-
- void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld, btScalar dt)
- {
-- btTransform start, end;
-+ btTransform start, end, end_double;
-+ bool runonce = false;
-
- // phase 3: down
- /*btScalar additionalDownStep = (m_wasOnGround && !onGround()) ? m_stepHeight : 0.0;
-@@ -406,44 +425,124 @@
- btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * downVelocity;
- 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_stepHeight
-+
-+ if(downVelocity > 0.0 && downVelocity > m_fallSpeed
- && (m_wasOnGround || !m_wasJumping))
-- {
-- downVelocity = m_stepHeight;
- }
-+ downVelocity = m_fallSpeed;
-
- btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
- m_targetPosition -= step_drop;
++ }*/
-- start.setIdentity ();
-- end.setIdentity ();
-+ btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
-+ callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
-+ callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
+ int maxIter = 10;
-- start.setOrigin (m_currentPosition);
-- end.setOrigin (m_targetPosition);
-+ btKinematicClosestNotMeConvexResultCallback callback2 (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
-+ callback2.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
-+ callback2.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
-
-- btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
-- callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
-- callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
--
-- if (m_useGhostObjectSweepTest)
-+ while (1)
- {
-- m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
-- } else
-- {
-- collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
-+ start.setIdentity ();
-+ end.setIdentity ();
-+
-+ end_double.setIdentity ();
-+
-+ start.setOrigin (m_currentPosition);
-+ end.setOrigin (m_targetPosition);
-+
-+ //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);
-+ }
-+ } else
-+ {
-+ collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
++ doPenetrationRecovery(collisionWorld);
+
-+ 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);
-+ }
-+ }
-+
-+ btScalar downVelocity2 = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
-+ bool has_hit = false;
-+ if (bounce_fix == true)
-+ has_hit = callback.hasHit() || callback2.hasHit();
-+ else
-+ has_hit = callback2.hasHit();
-+
-+ if(downVelocity2 > 0.0 && downVelocity2 < m_stepHeight && has_hit == true && runonce == false
-+ && (m_wasOnGround || !m_wasJumping))
-+ {
-+ //redo the velocity calculation when falling a small amount, for fast stairs motion
-+ //for larger falls, use the smoother/slower interpolated movement by not touching the target position
-+
-+ m_targetPosition = orig_position;
-+ downVelocity = m_stepHeight;
-+
-+ btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
-+ m_targetPosition -= step_drop;
-+ runonce = true;
-+ continue; //re-run previous tests
-+ }
-+ break;
- }
-
-- if (callback.hasHit())
-+ if (callback.hasHit() || runonce == true)
+ while (fraction > btScalar(0.01) && maxIter-- > 0)
{
- // we dropped a fraction of the height -> hit floor
-- m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
-+
-+ btScalar fraction = (m_currentPosition.getY() - callback.m_hitPointWorld.getY()) / 2;
-+
-+ //printf("hitpoint: %g - pos %g\n", callback.m_hitPointWorld.getY(), m_currentPosition.getY());
-+
-+ if (bounce_fix == true)
-+ {
-+ if (full_drop == true)
-+ m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
-+ else
-+ //due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually
-+ m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, fraction);
-+ }
-+ else
-+ m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
-+
-+ full_drop = false;
-+
- m_verticalVelocity = 0.0;
- m_verticalOffset = 0.0;
- m_wasJumping = false;
- } else {
- // we dropped the full height
-
-+ full_drop = true;
-+
-+ if (bounce_fix == true)
-+ {
-+ downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
-+ if (downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping))
-+ {
-+ m_targetPosition += step_drop; //undo previous target change
-+ downVelocity = m_fallSpeed;
-+ step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
-+ m_targetPosition -= step_drop;
-+ }
-+ }
-+ //printf("full drop - %g, %g\n", m_currentPosition.getY(), m_targetPosition.getY());
-+
- m_currentPosition = m_targetPosition;
- }
- }
-@@ -479,10 +578,21 @@
- m_velocityTimeInterval = timeInterval;
- }
-
+ start.setOrigin (m_currentPosition);
+@@ -607,24 +613,9 @@
+ void btKinematicCharacterController::preStep ( btCollisionWorld* collisionWorld)
+ {
+
+- int numPenetrationLoops = 0;
+- m_touchingContact = false;
+- while (recoverFromPenetration (collisionWorld))
+- {
+- numPenetrationLoops++;
+- m_touchingContact = true;
+- if (numPenetrationLoops > 4)
+- {
+- //printf("character could not recover from penetration = %d\n", numPenetrationLoops);
+- break;
+- }
+- }
-
+ m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
+ m_targetPosition = m_currentPosition;
+ // printf("m_targetPosition=%f,%f,%f\n",m_targetPosition[0],m_targetPosition[1],m_targetPosition[2]);
-
--void btKinematicCharacterController::reset ()
-+void btKinematicCharacterController::reset ( btCollisionWorld* collisionWorld )
- {
-+ m_verticalVelocity = 0.0;
-+ m_verticalOffset = 0.0;
-+ m_wasOnGround = false;
-+ m_wasJumping = false;
-+ m_walkDirection.setValue(0,0,0);
-+ m_velocityTimeInterval = 0.0;
-+
-+ //clear pair cache
-+ btHashedOverlappingPairCache *cache = m_ghostObject->getOverlappingPairCache();
-+ while (cache->getOverlappingPairArray().size() > 0)
-+ {
-+ cache->removeOverlappingPair(cache->getOverlappingPairArray()[0].m_pProxy0, cache->getOverlappingPairArray()[0].m_pProxy1, collisionWorld->getDispatcher());
-+ }
+-
}
- void btKinematicCharacterController::warp (const btVector3& origin)
-@@ -653,3 +763,8 @@
- void btKinematicCharacterController::debugDraw(btIDebugDraw* debugDrawer)
+ #include <stdio.h>
+@@ -768,3 +759,20 @@
{
+ m_interpolateUp = value;
}
+
-+void btKinematicCharacterController::setUpInterpolate(bool value)
++void btKinematicCharacterController::doPenetrationRecovery(btCollisionWorld* collisionWorld)
+{
-+ m_interpolateUp = value;
++ int numPenetrationLoops = 0;
++ m_touchingContact = false;
++
++ while (recoverFromPenetration (collisionWorld))
++ {
++ numPenetrationLoops++;
++ m_touchingContact = true;
++ if (numPenetrationLoops > 4)
++ {
++ //printf("character could not recover from penetration = %d\n", numPenetrationLoops);
++ break;
++ }
++ }
+}
-diff -urN a/src/BulletDynamics/Character/btKinematicCharacterController.h b/src/BulletDynamics/Character/btKinematicCharacterController.h
---- a/src/BulletDynamics/Character/btKinematicCharacterController.h 2013-01-22 17:54:25.440670373 -0600
-+++ b/src/BulletDynamics/Character/btKinematicCharacterController.h 2013-01-13 14:33:42.262892731 -0600
-@@ -81,6 +81,9 @@
- int m_upAxis;
-
- static btVector3* getUpAxisDirections();
-+ bool m_interpolateUp;
-+ bool full_drop;
-+ bool bounce_fix;
-
- btVector3 computeReflectionDirection (const btVector3& direction, const btVector3& normal);
- btVector3 parallelComponent (const btVector3& direction, const btVector3& normal);
-@@ -133,7 +136,7 @@
- virtual void setVelocityForTimeInterval(const btVector3& velocity,
- btScalar timeInterval);
-
-- void reset ();
-+ void reset ( btCollisionWorld* collisionWorld );
- void warp (const btVector3& origin);
-
- void preStep ( btCollisionWorld* collisionWorld);
-@@ -161,6 +164,7 @@
- }
-
- bool onGround () const;
-+ void setUpInterpolate (bool value);
- };
-
- #endif // BT_KINEMATIC_CHARACTER_CONTROLLER_H
+diff -urN bullet.orig/src/BulletDynamics/Character/btKinematicCharacterController.h bullet.new/src/BulletDynamics/Character/btKinematicCharacterController.h
+--- bullet.orig/src/BulletDynamics/Character/btKinematicCharacterController.h 2015-03-26 13:15:36.992897379 -0500
++++ bullet.new/src/BulletDynamics/Character/btKinematicCharacterController.h 2015-03-26 01:23:54.895928312 -0500
+@@ -94,6 +94,7 @@
+ 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 doPenetrationRecovery(btCollisionWorld* collisionWorld);
+ public:
+
+ BT_DECLARE_ALIGNED_ALLOCATOR();