improved isStuck detection and MyAvatar::safeLanding() trigger

interface/src/Application.cpp

@@ -6282,11 +6282,13 @@ void Application::tryToEnablePhysics() {
         // process octree stats packets are sent in between full sends of a scene (this isn't currently true).
         // We keep physics disabled until we've received a full scene and everything near the avatar in that
         // scene is ready to compute its collision shape.
-        if (getMyAvatar()->isReadyForPhysics()) {
+        auto myAvatar = getMyAvatar();
+        if (myAvatar->isReadyForPhysics()) {
+            myAvatar->getCharacterController()->setPhysicsEngine(_physicsEngine);
             _octreeProcessor.resetSafeLanding();
             _physicsEnabled = true;
             setIsInterstitialMode(false);
-            getMyAvatar()->updateMotionBehaviorFromMenu();
+            myAvatar->updateMotionBehaviorFromMenu();
         }
     }
 }
@@ -6577,7 +6579,7 @@ void Application::update(float deltaTime) {
                 avatarManager->handleProcessedPhysicsTransaction(transaction);
 
                 myAvatar->prepareForPhysicsSimulation();
-                _physicsEngine->enableGlobalContactAddedCallback(myAvatar->isFlying());
+                myAvatar->getCharacterController()->preSimulation();
             }
         }
 
@@ -6630,6 +6632,7 @@ void Application::update(float deltaTime) {
 
                         {
                             PROFILE_RANGE(simulation_physics, "MyAvatar");
+                            myAvatar->getCharacterController()->postSimulation();
                             myAvatar->harvestResultsFromPhysicsSimulation(deltaTime);
                         }
 

interface/src/avatar/MyAvatar.cpp

@@ -733,7 +733,7 @@ void MyAvatar::update(float deltaTime) {
         // When needed and ready, arrange to check and fix.
         _physicsSafetyPending = false;
         if (_goToSafe) {
-            safeLanding(_goToPosition); // no-op if already safe
+            safeLanding(_goToPosition); // no-op if safeLanding logic determines already safe
         }
     }
 
@@ -2733,24 +2733,21 @@ void MyAvatar::nextAttitude(glm::vec3 position, glm::quat orientation) {
 void MyAvatar::harvestResultsFromPhysicsSimulation(float deltaTime) {
     glm::vec3 position;
     glm::quat orientation;
-    if (_characterController.isEnabledAndReady()) {
+    if (_characterController.isEnabledAndReady() && !(_characterController.needsSafeLandingSupport() || _goToPending)) {
         _characterController.getPositionAndOrientation(position, orientation);
+        setWorldVelocity(_characterController.getLinearVelocity() + _characterController.getFollowVelocity());
     } else {
         position = getWorldPosition();
         orientation = getWorldOrientation();
+        if (_characterController.needsSafeLandingSupport() && !_goToPending) {
+            _goToPending = true;
+            _goToSafe = true;
+            _goToPosition = position;
+        }
+        setWorldVelocity(getWorldVelocity() + _characterController.getFollowVelocity());
     }
     nextAttitude(position, orientation);
     _bodySensorMatrix = _follow.postPhysicsUpdate(*this, _bodySensorMatrix);
-
-    if (_characterController.isEnabledAndReady()) {
-        setWorldVelocity(_characterController.getLinearVelocity() + _characterController.getFollowVelocity());
-        if (_characterController.isStuck()) {
-            _physicsSafetyPending = true;
-            _goToPosition = position;
-        }
-    } else {
-        setWorldVelocity(getWorldVelocity() + _characterController.getFollowVelocity());
-    }
 }
 
 QString MyAvatar::getScriptedMotorFrame() const {

interface/src/avatar/MyCharacterController.cpp

@@ -36,10 +36,10 @@ MyCharacterController::MyCharacterController(std::shared_ptr<MyAvatar> avatar) {
 MyCharacterController::~MyCharacterController() {
 }
 
-void MyCharacterController::setDynamicsWorld(btDynamicsWorld* world) {
+void MyCharacterController::addToWorld() {
-    CharacterController::setDynamicsWorld(world);
+    CharacterController::addToWorld();
-    if (world && _rigidBody) {
+    if (_rigidBody) {
-        initRayShotgun(world);
+        initRayShotgun(_physicsEngine->getDynamicsWorld());
     }
 }
 
@@ -204,7 +204,7 @@ bool MyCharacterController::testRayShotgun(const glm::vec3& position, const glm:
 }
 
 int32_t MyCharacterController::computeCollisionMask() const {
-    int32_t collisionMask = BULLET_COLLISION_MASK_MY_AVATAR; 
+    int32_t collisionMask = BULLET_COLLISION_MASK_MY_AVATAR;
     if (_collisionless && _collisionlessAllowed) {
         collisionMask = BULLET_COLLISION_MASK_COLLISIONLESS;
     } else if (!_collideWithOtherAvatars) {
@@ -216,16 +216,17 @@ int32_t MyCharacterController::computeCollisionMask() const {
 void MyCharacterController::handleChangedCollisionMask() {
     if (_pendingFlags & PENDING_FLAG_UPDATE_COLLISION_MASK) {
         // ATM the easiest way to update collision groups/masks is to remove/re-add the RigidBody
-        if (_dynamicsWorld) {
+        // but we don't do it here.  Instead we set some flags to remind us to do it later.
-            _dynamicsWorld->removeRigidBody(_rigidBody);
+        _pendingFlags |= (PENDING_FLAG_REMOVE_FROM_SIMULATION | PENDING_FLAG_ADD_TO_SIMULATION);
-            int32_t collisionMask = computeCollisionMask();
-            _dynamicsWorld->addRigidBody(_rigidBody, BULLET_COLLISION_GROUP_MY_AVATAR, collisionMask);
-        }
         _pendingFlags &= ~PENDING_FLAG_UPDATE_COLLISION_MASK;
         updateCurrentGravity();
     }
 }
 
+bool MyCharacterController::needsSafeLandingSupport() const {
+    return _isStuck && 0 == (_numStuckFrames % NUM_FRAMES_FOR_SAFE_LANDING_RETRY);
+}
+
 btConvexHullShape* MyCharacterController::computeShape() const {
     // HACK: the avatar collides using convex hull with a collision margin equal to
     // the old capsule radius.  Two points define a capsule and additional points are
@@ -447,12 +448,12 @@ public:
 std::vector<MyCharacterController::RayAvatarResult> MyCharacterController::rayTest(const btVector3& origin, const btVector3& direction,
                                                                                    const btScalar& length, const QVector<uint>& jointsToExclude) const {
     std::vector<RayAvatarResult> foundAvatars;
-    if (_dynamicsWorld) {
+    if (_physicsEngine) {
         btVector3 end = origin + length * direction;
         DetailedRayResultCallback rayCallback = DetailedRayResultCallback();
         rayCallback.m_flags |= btTriangleRaycastCallback::kF_KeepUnflippedNormal;
         rayCallback.m_flags |= btTriangleRaycastCallback::kF_UseSubSimplexConvexCastRaytest;
-        _dynamicsWorld->rayTest(origin, end, rayCallback);
+        _physicsEngine->getDynamicsWorld()->rayTest(origin, end, rayCallback);
         if (rayCallback.m_hitFractions.size() > 0) {
             foundAvatars.reserve(rayCallback.m_hitFractions.size());
             for (int32_t i = 0; i < rayCallback.m_hitFractions.size(); i++) {

interface/src/avatar/MyCharacterController.h

@@ -26,7 +26,7 @@ public:
     explicit MyCharacterController(std::shared_ptr<MyAvatar> avatar);
     ~MyCharacterController ();
 
-    void setDynamicsWorld(btDynamicsWorld* world) override;
+    void addToWorld() override;
     void updateShapeIfNecessary() override;
 
     // Sweeping a convex shape through the physics simulation can be expensive when the obstacles are too
@@ -69,9 +69,10 @@ public:
     int32_t computeCollisionMask() const override;
     void handleChangedCollisionMask() override;
 
-    bool _collideWithOtherAvatars{ true };
     void setCollideWithOtherAvatars(bool collideWithOtherAvatars) { _collideWithOtherAvatars = collideWithOtherAvatars; }
 
+    bool needsSafeLandingSupport() const;
+
 protected:
     void initRayShotgun(const btCollisionWorld* world);
     void updateMassProperties() override;
@@ -88,6 +89,7 @@ protected:
     btScalar _density { 1.0f };
 
     std::vector<DetailedMotionState*> _detailedMotionStates;
+    bool _collideWithOtherAvatars { true };
 };
 
 #endif // hifi_MyCharacterController_h

libraries/physics/src/CharacterController.cpp

@@ -11,14 +11,136 @@
 
 #include "CharacterController.h"
 
-#include <NumericalConstants.h>
 #include <AvatarConstants.h>
+#include <NumericalConstants.h>
+#include <PhysicsCollisionGroups.h>
 
 #include "ObjectMotionState.h"
 #include "PhysicsHelpers.h"
 #include "PhysicsLogging.h"
 
 const btVector3 LOCAL_UP_AXIS(0.0f, 1.0f, 0.0f);
+static bool _flippedThisFrame = false;
+
+// Note: flipBackfaceTriangleNormals is registered as a sub-callback to Bullet's gContactAddedCallback feature
+// when we detect MyAvatar is "stuck".  It will reverse the triangles on the backface of mesh shapes, unless it thinks
+// the old normal would be better at extracting MyAvatar out along its UP direction.
+bool flipBackfaceTriangleNormals(btManifoldPoint& cp,
+        const btCollisionObjectWrapper* colObj0Wrap, int partId0, int index0,
+        const btCollisionObjectWrapper* colObj1Wrap, int partId1, int index1) {
+    static int32_t numCalls = 0;
+    ++numCalls;
+    // This callback is ONLY called on objects with btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK flag
+    // and the flagged object will always be sorted to Obj0.  Hence the "other" is always Obj1.
+    const btCollisionObject* other = colObj1Wrap->m_collisionObject;
+
+    if (other->getCollisionShape()->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) {
+        // access the meshInterface
+        auto meshShape = static_cast<const btBvhTriangleMeshShape*>(other->getCollisionShape());
+        const btStridingMeshInterface* meshInterface = meshShape->getMeshInterface();
+
+        // figure out about how to navigate meshInterface
+        const uint8_t* vertexBase;
+        int32_t numverts;
+        PHY_ScalarType vertexType;
+        int32_t vertexStride;
+        const uint8_t* indexBase;
+        int32_t indexStride;
+        int32_t numFaces;
+        PHY_ScalarType indicesType;
+        int32_t subPart = colObj1Wrap->m_partId;
+        // NOTE: all arguments are being passed by reference except the bases (passed by pointer) and subPart (true input)
+        meshInterface->getLockedReadOnlyVertexIndexBase(&vertexBase, numverts, vertexType, vertexStride, &indexBase, indexStride, numFaces, indicesType, subPart);
+
+        // fetch the triangle vertices
+        int32_t triangleIndex = colObj1Wrap->m_index;
+        assert(vertexType == PHY_FLOAT); // all mesh vertex data is float...
+        // ...but indicesType can vary
+        btVector3 triangleVertex[3];
+        switch (indicesType) {
+            case PHY_INTEGER: {
+                uint32_t* triangleIndices = (uint32_t*)(indexBase + triangleIndex * indexStride);
+                float* triangleBase;
+                triangleBase = (float*)(vertexBase + triangleIndices[0] * vertexStride);
+                triangleVertex[0].setValue(triangleBase[0], triangleBase[1], triangleBase[2]);
+                triangleBase = (float*)(vertexBase + triangleIndices[1] * vertexStride);
+                triangleVertex[1].setValue(triangleBase[0], triangleBase[1], triangleBase[2]);
+                triangleBase = (float*)(vertexBase + triangleIndices[2] * vertexStride);
+                triangleVertex[2].setValue(triangleBase[0], triangleBase[1], triangleBase[2]);
+            }
+            break;
+            case PHY_SHORT: {
+                uint16_t* triangleIndices = (uint16_t*)(indexBase + triangleIndex * indexStride);
+                float* triangleBase;
+                triangleBase = (float*)(vertexBase + triangleIndices[0] * vertexStride);
+                triangleVertex[0].setValue(triangleBase[0], triangleBase[1], triangleBase[2]);
+                triangleBase = (float*)(vertexBase + triangleIndices[1] * vertexStride);
+                triangleVertex[1].setValue(triangleBase[0], triangleBase[1], triangleBase[2]);
+                triangleBase = (float*)(vertexBase + triangleIndices[2] * vertexStride);
+                triangleVertex[2].setValue(triangleBase[0], triangleBase[1], triangleBase[2]);
+            }
+            break;
+            case PHY_UCHAR: {
+                uint8_t* triangleIndices = (uint8_t*)(indexBase + triangleIndex * indexStride);
+                float* triangleBase;
+                triangleBase = (float*)(vertexBase + triangleIndices[0] * vertexStride);
+                triangleVertex[0].setValue(triangleBase[0], triangleBase[1], triangleBase[2]);
+                triangleBase = (float*)(vertexBase + triangleIndices[1] * vertexStride);
+                triangleVertex[1].setValue(triangleBase[0], triangleBase[1], triangleBase[2]);
+                triangleBase = (float*)(vertexBase + triangleIndices[2] * vertexStride);
+                triangleVertex[2].setValue(triangleBase[0], triangleBase[1], triangleBase[2]);
+            }
+            break;
+            default:
+                return false;
+        }
+
+        // compute faceNormal
+        btVector3 meshScaling = meshInterface->getScaling();
+        triangleVertex[0] *= meshScaling;
+        triangleVertex[1] *= meshScaling;
+        triangleVertex[2] *= meshScaling;
+        btVector3 faceNormal = other->getWorldTransform().getBasis() * btCross(triangleVertex[1] - triangleVertex[0], triangleVertex[2] - triangleVertex[0]);
+        float nDotF = btDot(faceNormal, cp.m_normalWorldOnB);
+        if (nDotF <= 0.0f && faceNormal.length2() > EPSILON) {
+            faceNormal.normalize();
+            // flip the contact normal to be aligned with the face normal...
+            // ...but only if old normal does NOT point along obj0's UP
+            // (because we're "stuck" and UP is the likely path out)
+            btVector3 up = colObj0Wrap->m_collisionObject->getWorldTransform().getBasis() * LOCAL_UP_AXIS;
+            if (cp.m_normalWorldOnB.dot(up) <= 0.0f) {
+                nDotF = btDot(faceNormal, cp.m_normalWorldOnB);
+                cp.m_normalWorldOnB -= 2.0f * nDotF * faceNormal;
+                _flippedThisFrame = true;
+            }
+            // Note: if we're flipping normals it means the "Are we stuck?" logic is concluding "Yes, we are".
+            // But when we flip the normals it typically causes the ContactManifold to discard the modified ManifoldPoint
+            // which in turn causes the "Are we stuck?" logic to incorrectly conclude "No, we are not".
+            // So we set '_flippedThisFrame = true' here and use it later to stay in "stuck" state until flipping stops
+        }
+    }
+
+    // KEEP THIS: in case we add support for concave shapes which delegate to temporary btTriangleShapes (such as btHeightfieldTerrainShape)
+    //else if (other->getCollisionShape()->getShapeType() == TRIANGLE_SHAPE_PROXYTYPE) {
+    //    auto triShape = static_cast<const btTriangleShape*>(other->getCollisionShape());
+    //    const btVector3* v = triShape->m_vertices1;
+    //    btVector3 faceNormal = other->getWorldTransform().getBasis() * btCross(v[1] - v[0], v[2] - v[0]);
+    //    float nDotF = btDot(faceNormal, cp.m_normalWorldOnB);
+    //    if (nDotF <= 0.0f && faceNormal.length2() > EPSILON) {
+    //        faceNormal.normalize();
+    //        // flip the contact normal to be aligned with the face normal
+    //        cp.m_normalWorldOnB += -2.0f * nDotF * faceNormal;
+    //    }
+    //}
+    // KEEP THIS
+
+    // NOTE: this ManifoldPoint is a candidate and hasn't been accepted yet into the final ContactManifold yet.
+    // So when we modify its parameters we can convince Bullet to discard it.
+    //
+    // by our own convention:
+    // return true when this ManifoldPoint has been modified in a way that would disable it
+    return false;
+};
 
 #ifdef DEBUG_STATE_CHANGE
 #define SET_STATE(desiredState, reason) setState(desiredState, reason)
@@ -92,64 +214,70 @@ CharacterController::~CharacterController() {
 }
 
 bool CharacterController::needsRemoval() const {
-    return ((_pendingFlags & PENDING_FLAG_REMOVE_FROM_SIMULATION) == PENDING_FLAG_REMOVE_FROM_SIMULATION);
+    return (_physicsEngine && (_pendingFlags & PENDING_FLAG_REMOVE_FROM_SIMULATION) == PENDING_FLAG_REMOVE_FROM_SIMULATION);
 }
 
 bool CharacterController::needsAddition() const {
-    return ((_pendingFlags & PENDING_FLAG_ADD_TO_SIMULATION) == PENDING_FLAG_ADD_TO_SIMULATION);
+    return (_physicsEngine && (_pendingFlags & PENDING_FLAG_ADD_TO_SIMULATION) == PENDING_FLAG_ADD_TO_SIMULATION);
 }
 
-void CharacterController::setDynamicsWorld(btDynamicsWorld* world) {
+void CharacterController::removeFromWorld() {
-    if (_dynamicsWorld != world) {
+    if (_inWorld) {
-        // remove from old world
-        if (_dynamicsWorld) {
-            if (_rigidBody) {
-                _dynamicsWorld->removeRigidBody(_rigidBody);
-                _dynamicsWorld->removeAction(this);
-            }
-            _dynamicsWorld = nullptr;
-        }
-        int32_t collisionMask = computeCollisionMask();
-        int32_t collisionGroup = BULLET_COLLISION_GROUP_MY_AVATAR; 
         if (_rigidBody) {
-            updateMassProperties();
+            _physicsEngine->getDynamicsWorld()->removeRigidBody(_rigidBody);
-        }
+            _physicsEngine->getDynamicsWorld()->removeAction(this);
-        if (world && _rigidBody) {
-            // add to new world
-            _dynamicsWorld = world;
-            _pendingFlags &= ~PENDING_FLAG_JUMP;
-            _dynamicsWorld->addRigidBody(_rigidBody, collisionGroup, collisionMask); 
-            _dynamicsWorld->addAction(this);
-            // restore gravity settings because adding an object to the world overwrites its gravity setting
-            _rigidBody->setGravity(_currentGravity * _currentUp);
-            // set flag to enable custom contactAddedCallback
-            _rigidBody->setCollisionFlags(btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
-
-            // enable CCD
-            _rigidBody->setCcdSweptSphereRadius(_radius);
-            _rigidBody->setCcdMotionThreshold(_radius);
-
-            btCollisionShape* shape = _rigidBody->getCollisionShape();
-            assert(shape && shape->getShapeType() == CONVEX_HULL_SHAPE_PROXYTYPE);
-            _ghost.setCharacterShape(static_cast<btConvexHullShape*>(shape));
-        }
-        _ghost.setCollisionGroupAndMask(collisionGroup, collisionMask & (~ collisionGroup)); 
-        _ghost.setCollisionWorld(_dynamicsWorld);
-        _ghost.setRadiusAndHalfHeight(_radius, _halfHeight);
-        if (_rigidBody) {
-            _ghost.setWorldTransform(_rigidBody->getWorldTransform());
         }
+        _inWorld = false;
     }
-    if (_dynamicsWorld) {
+    _pendingFlags &= ~PENDING_FLAG_REMOVE_FROM_SIMULATION;
-        if (_pendingFlags & PENDING_FLAG_UPDATE_SHAPE) {
+}
-            // shouldn't fall in here, but if we do make sure both ADD and REMOVE bits are still set
+
-            _pendingFlags |= PENDING_FLAG_ADD_TO_SIMULATION | PENDING_FLAG_REMOVE_FROM_SIMULATION | 
+void CharacterController::addToWorld() {
-                             PENDING_FLAG_ADD_DETAILED_TO_SIMULATION | PENDING_FLAG_REMOVE_DETAILED_FROM_SIMULATION;
+    if (!_rigidBody) {
-        } else {
+        return;
-            _pendingFlags &= ~PENDING_FLAG_ADD_TO_SIMULATION;
+    }
-        }
+    if (_inWorld) {
+        _pendingFlags &= ~PENDING_FLAG_ADD_DETAILED_TO_SIMULATION;
+        return;
+    }
+    btDiscreteDynamicsWorld* world = _physicsEngine->getDynamicsWorld();
+    int32_t collisionMask = computeCollisionMask();
+    int32_t collisionGroup = BULLET_COLLISION_GROUP_MY_AVATAR;
+
+    updateMassProperties();
+    _pendingFlags &= ~PENDING_FLAG_ADD_DETAILED_TO_SIMULATION;
+
+    // add to new world
+    _pendingFlags &= ~PENDING_FLAG_JUMP;
+    world->addRigidBody(_rigidBody, collisionGroup, collisionMask);
+    world->addAction(this);
+    _inWorld = true;
+
+    // restore gravity settings because adding an object to the world overwrites its gravity setting
+    _rigidBody->setGravity(_currentGravity * _currentUp);
+    // set flag to enable custom contactAddedCallback
+    _rigidBody->setCollisionFlags(btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
+
+    // enable CCD
+    _rigidBody->setCcdSweptSphereRadius(_radius);
+    _rigidBody->setCcdMotionThreshold(_radius);
+
+    btCollisionShape* shape = _rigidBody->getCollisionShape();
+    assert(shape && shape->getShapeType() == CONVEX_HULL_SHAPE_PROXYTYPE);
+    _ghost.setCharacterShape(static_cast<btConvexHullShape*>(shape));
+
+    _ghost.setCollisionGroupAndMask(collisionGroup, collisionMask & (~ collisionGroup));
+    _ghost.setCollisionWorld(world);
+    _ghost.setRadiusAndHalfHeight(_radius, _halfHeight);
+    if (_rigidBody) {
+        _ghost.setWorldTransform(_rigidBody->getWorldTransform());
+    }
+    if (_pendingFlags & PENDING_FLAG_UPDATE_SHAPE) {
+        // shouldn't fall in here, but if we do make sure both ADD and REMOVE bits are still set
+        _pendingFlags |= PENDING_FLAG_ADD_TO_SIMULATION | PENDING_FLAG_REMOVE_FROM_SIMULATION |
+                         PENDING_FLAG_ADD_DETAILED_TO_SIMULATION | PENDING_FLAG_REMOVE_DETAILED_FROM_SIMULATION;
     } else {
-        _pendingFlags &= ~PENDING_FLAG_REMOVE_FROM_SIMULATION;
+        _pendingFlags &= ~PENDING_FLAG_ADD_TO_SIMULATION;
     }
 }
 
@@ -159,17 +287,21 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) {
     btDispatcher* dispatcher = collisionWorld->getDispatcher();
     int numManifolds = dispatcher->getNumManifolds();
     bool hasFloor = false;
-    bool isStuck = false;
+    bool probablyStuck = _isStuck && _flippedThisFrame;
 
     btTransform rotation = _rigidBody->getWorldTransform();
     rotation.setOrigin(btVector3(0.0f, 0.0f, 0.0f)); // clear translation part
 
+    float deepestDistance = 0.0f;
+    float strongestImpulse = 0.0f;
+
     for (int i = 0; i < numManifolds; i++) {
         btPersistentManifold* contactManifold = dispatcher->getManifoldByIndexInternal(i);
         if (_rigidBody == contactManifold->getBody1() || _rigidBody == contactManifold->getBody0()) {
             bool characterIsFirst = _rigidBody == contactManifold->getBody0();
             int numContacts = contactManifold->getNumContacts();
             int stepContactIndex = -1;
+            bool stepValid = true;
             float highestStep = _minStepHeight;
             for (int j = 0; j < numContacts; j++) {
                 // check for "floor"
@@ -177,28 +309,24 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) {
                 btVector3 pointOnCharacter = characterIsFirst ? contact.m_localPointA : contact.m_localPointB; // object-local-frame
                 btVector3 normal = characterIsFirst ? contact.m_normalWorldOnB : -contact.m_normalWorldOnB; // points toward character
                 btScalar hitHeight = _halfHeight + _radius + pointOnCharacter.dot(_currentUp);
-                // If there's non-trivial penetration with a big impulse for several steps, we're probably stuck.
+
-                // Note it here in the controller, and let MyAvatar figure out what to do about it.
+                float distance = contact.getDistance();
-                const float STUCK_PENETRATION = -0.05f; // always negative into the object.
+                if (distance < deepestDistance) {
-                const float STUCK_IMPULSE = 500.0f;
+                    deepestDistance = distance;
-                const int STUCK_LIFETIME = 3;
-                if ((contact.getDistance() < STUCK_PENETRATION) && (contact.getAppliedImpulse() > STUCK_IMPULSE) && (contact.getLifeTime() > STUCK_LIFETIME)) {
-                    isStuck = true; // latch on
                 }
+                float impulse = contact.getAppliedImpulse();
+                if (impulse > strongestImpulse) {
+                    strongestImpulse = impulse;
+                }
+
                 if (hitHeight < _maxStepHeight && normal.dot(_currentUp) > _minFloorNormalDotUp) {
                     hasFloor = true;
-                    if (!pushing && isStuck) {
-                        // we're not pushing against anything and we're stuck so we can early exit
-                        // (all we need to know is that there is a floor)
-                        break;
-                    }
                 }
-                if (pushing && _targetVelocity.dot(normal) < 0.0f) {
+                if (stepValid && pushing && _targetVelocity.dot(normal) < 0.0f) {
                     // remember highest step obstacle
                     if (!_stepUpEnabled || hitHeight > _maxStepHeight) {
                         // this manifold is invalidated by point that is too high
-                        stepContactIndex = -1;
+                        stepValid = false;
-                        break;
                     } else if (hitHeight > highestStep && normal.dot(_targetVelocity) < 0.0f ) {
                         highestStep = hitHeight;
                         stepContactIndex = j;
@@ -206,7 +334,7 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) {
                     }
                 }
             }
-            if (stepContactIndex > -1 && highestStep > _stepHeight) {
+            if (stepValid && stepContactIndex > -1 && highestStep > _stepHeight) {
                 // remember step info for later
                 btManifoldPoint& contact = contactManifold->getContactPoint(stepContactIndex);
                 btVector3 pointOnCharacter = characterIsFirst ? contact.m_localPointA : contact.m_localPointB; // object-local-frame
@@ -214,13 +342,37 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) {
                 _stepHeight = highestStep;
                 _stepPoint = rotation * pointOnCharacter; // rotate into world-frame
             }
-            if (hasFloor && isStuck && !(pushing && _stepUpEnabled)) {
-                // early exit since all we need to know is that we're on a floor
-                break;
-            }
         }
     }
-    _isStuck = isStuck;
+
+    // If there's deep penetration and big impulse we're probably stuck.
+    const float STUCK_PENETRATION = -0.05f; // always negative into the object.
+    const float STUCK_IMPULSE = 500.0f;
+    probablyStuck = probablyStuck || (deepestDistance < STUCK_PENETRATION && strongestImpulse > STUCK_IMPULSE);
+
+    if (_isStuck != probablyStuck) {
+        ++_stuckTransitionCount;
+        if (_stuckTransitionCount == NUM_FRAMES_FOR_STUCK_TRANSITION) {
+            // we've been in this "probablyStuck" state for several consecutive frames
+            // --> make it official by changing state
+            _isStuck = probablyStuck;
+            // start _numStuckFrames at NUM_FRAMES_FOR_SAFE_LANDING_RETRY so SafeLanding tries to help immediately
+            _numStuckFrames = NUM_FRAMES_FOR_SAFE_LANDING_RETRY;
+            _stuckTransitionCount = 0;
+            if (_isStuck) {
+                _physicsEngine->addContactAddedCallback(flipBackfaceTriangleNormals);
+            } else {
+                _physicsEngine->removeContactAddedCallback(flipBackfaceTriangleNormals);
+                _flippedThisFrame = false;
+            }
+        }
+    } else {
+        _stuckTransitionCount = 0;
+        if (_isStuck) {
+            ++_numStuckFrames;
+            _flippedThisFrame = false;
+        }
+    }
     return hasFloor;
 }
 
@@ -452,7 +604,7 @@ void CharacterController::setLocalBoundingBox(const glm::vec3& minCorner, const
         _minStepHeight = DEFAULT_MIN_STEP_HEIGHT_FACTOR * (_halfHeight + _radius);
         _maxStepHeight = DEFAULT_MAX_STEP_HEIGHT_FACTOR * (_halfHeight + _radius);
 
-        if (_dynamicsWorld) {
+        if (_physicsEngine) {
             // must REMOVE from world prior to shape update
             _pendingFlags |= PENDING_FLAG_REMOVE_FROM_SIMULATION | PENDING_FLAG_REMOVE_DETAILED_FROM_SIMULATION;
         }
@@ -470,6 +622,15 @@ void CharacterController::setLocalBoundingBox(const glm::vec3& minCorner, const
     }
 }
 
+void CharacterController::setPhysicsEngine(const PhysicsEnginePointer& engine) {
+    if (!_physicsEngine && engine) {
+        // ATM there is only one PhysicsEngine: it is a singleton, and we are taking advantage
+        // of that assumption here.  If we ever introduce more and allow for this backpointer
+        // to change then we'll have to overhaul this method.
+        _physicsEngine = engine;
+    }
+}
+
 void CharacterController::setCollisionless(bool collisionless) {
     if (collisionless != _collisionless) {
         _collisionless = collisionless;
@@ -681,7 +842,7 @@ void CharacterController::computeNewVelocity(btScalar dt, glm::vec3& velocity) {
 }
 
 void CharacterController::updateState() {
-    if (!_dynamicsWorld) {
+    if (!_physicsEngine) {
         return;
     }
     if (_pendingFlags & PENDING_FLAG_RECOMPUTE_FLYING) {
@@ -712,7 +873,7 @@ void CharacterController::updateState() {
 
     ClosestNotMe rayCallback(_rigidBody);
     rayCallback.m_closestHitFraction = 1.0f;
-    _dynamicsWorld->rayTest(rayStart, rayEnd, rayCallback);
+    _physicsEngine->getDynamicsWorld()->rayTest(rayStart, rayEnd, rayCallback);
     bool rayHasHit = rayCallback.hasHit();
     quint64 now = usecTimestampNow();
     if (rayHasHit) {
@@ -829,6 +990,21 @@ void CharacterController::updateState() {
 }
 
 void CharacterController::preSimulation() {
+    if (needsRemoval()) {
+        removeFromWorld();
+
+        // We must remove any existing contacts for the avatar so that any new contacts will have
+        // valid data.  MyAvatar's RigidBody is the ONLY one in the simulation that does not yet
+        // have a MotionState so we pass nullptr to removeContacts().
+        if (_physicsEngine) {
+            _physicsEngine->removeContacts(nullptr);
+        }
+    }
+    updateShapeIfNecessary();
+    if (needsAddition()) {
+        addToWorld();
+    }
+
     if (_rigidBody) {
         // slam body transform and remember velocity
         _rigidBody->setWorldTransform(btTransform(btTransform(_rotation, _position)));

libraries/physics/src/CharacterController.h

@@ -20,11 +20,11 @@
 
 #include <GLMHelpers.h>
 #include <NumericalConstants.h>
-#include <PhysicsCollisionGroups.h>
 
+#include "AvatarConstants.h"
 #include "BulletUtil.h"
 #include "CharacterGhostObject.h"
-#include "AvatarConstants.h" 
+#include "PhysicsEngine.h"
 
 const uint32_t PENDING_FLAG_ADD_TO_SIMULATION = 1U << 0;
 const uint32_t PENDING_FLAG_REMOVE_FROM_SIMULATION = 1U << 1;
@@ -37,6 +37,9 @@ const uint32_t PENDING_FLAG_REMOVE_DETAILED_FROM_SIMULATION = 1U << 7;
 
 const float DEFAULT_MIN_FLOOR_NORMAL_DOT_UP = cosf(PI / 3.0f);
 
+const uint32_t NUM_FRAMES_FOR_STUCK_TRANSITION = 6; // mainloop frames
+const uint32_t NUM_FRAMES_FOR_SAFE_LANDING_RETRY = 40; // mainloop frames
+
 class btRigidBody;
 class btCollisionWorld;
 class btDynamicsWorld;
@@ -53,7 +56,8 @@ public:
     virtual ~CharacterController();
     bool needsRemoval() const;
     bool needsAddition() const;
-    virtual void setDynamicsWorld(btDynamicsWorld* world);
+    virtual void addToWorld();
+    void removeFromWorld();
     btCollisionObject* getCollisionObject() { return _rigidBody; }
 
     void setGravity(float gravity);
@@ -120,7 +124,8 @@ public:
 
     void setLocalBoundingBox(const glm::vec3& minCorner, const glm::vec3& scale);
 
-    bool isEnabledAndReady() const { return _dynamicsWorld; }
+    void setPhysicsEngine(const PhysicsEnginePointer& engine);
+    bool isEnabledAndReady() const { return (bool)_physicsEngine; }
     bool isStuck() const { return _isStuck; }
 
     void setCollisionless(bool collisionless);
@@ -187,7 +192,6 @@ protected:
     // data for walking up steps
     btVector3 _stepPoint { 0.0f, 0.0f, 0.0f };
     btVector3 _stepNormal { 0.0f, 0.0f, 0.0f };
-    bool _steppingUp { false };
     btScalar _stepHeight { 0.0f };
     btScalar _minStepHeight { 0.0f };
     btScalar _maxStepHeight { 0.0f };
@@ -197,6 +201,7 @@ protected:
     btScalar _radius { 0.0f };
 
     btScalar _floorDistance;
+    bool _steppingUp { false };
     bool _stepUpEnabled { true };
     bool _hasSupport;
 
@@ -213,11 +218,14 @@ protected:
     bool _isStuck { false };
     bool _isSeated { false };
 
-    btDynamicsWorld* _dynamicsWorld { nullptr };
+    PhysicsEnginePointer _physicsEngine { nullptr };
     btRigidBody* _rigidBody { nullptr };
     uint32_t _pendingFlags { 0 };
     uint32_t _previousFlags { 0 };
+    uint32_t _stuckTransitionCount { 0 };
+    uint32_t _numStuckFrames { 0 };
 
+    bool _inWorld { false };
     bool _zoneFlyingAllowed { true };
     bool _comfortFlyingAllowed { true };
     bool _hoverWhenUnsupported{ true };

libraries/physics/src/PhysicsEngine.cpp

@@ -27,31 +27,6 @@
 #include "ThreadSafeDynamicsWorld.h"
 #include "PhysicsLogging.h"
 
-static bool flipNormalsMyAvatarVsBackfacingTriangles(btManifoldPoint& cp,
-        const btCollisionObjectWrapper* colObj0Wrap, int partId0, int index0,
-        const btCollisionObjectWrapper* colObj1Wrap, int partId1, int index1) {
-    // This callback is designed to help MyAvatar escape entrapment inside mesh geometry.
-    // It is only activated when MyAvatar is flying because it can cause problems when MyAvatar
-    // is walking along the ground.
-    // When active it applies to ALL contact points, however we only expect it to "do interesting
-    // stuff on MyAvatar's physics.
-    // Note: we're taking advantage of the fact: MyAvatar's collisionObject always shows up as colObj0
-    // because it is added to physics first.
-    if (colObj1Wrap->getCollisionShape()->getShapeType() == TRIANGLE_SHAPE_PROXYTYPE) {
-        auto triShape = static_cast<const btTriangleShape*>(colObj1Wrap->getCollisionShape());
-        const btVector3* v = triShape->m_vertices1;
-        btVector3 faceNormal = colObj1Wrap->getWorldTransform().getBasis() * btCross(v[1] - v[0], v[2] - v[0]);
-        float nDotF = btDot(faceNormal, cp.m_normalWorldOnB);
-        if (nDotF <= 0.0f && faceNormal.length2() > EPSILON) {
-            faceNormal.normalize();
-            // flip the contact normal to be aligned with the face normal
-            cp.m_normalWorldOnB += -2.0f * nDotF * faceNormal;
-        }
-    }
-    // return value is currently ignored but to be future-proof: return false when not modifying friction
-    return false;
-}
-
 // a list of sub-callbacks
 std::vector<PhysicsEngine::ContactAddedCallback> _contactAddedCallbacks;
 
@@ -63,11 +38,15 @@ bool globalContactAddedCallback(btManifoldPoint& cp,
     // call each callback
     for (auto cb : _contactAddedCallbacks) {
         if (cb(cp, colObj0Wrap, partId0, index0, colObj1Wrap, partId1, index1)) {
-            // a return value of 'true' indicates the contact has been disabled
+            // Not a Bullet convention, but one we are using for sub-callbacks:
+            // a return value of 'true' indicates the contact has been "disabled"
             // in which case there is no need to process other callbacks
-            return true;
+            break;
         }
     }
+    // by Bullet convention for its gContactAddedCallback feature:
+    // the return value is currently ignored but to be future-proof:
+    // return true when friction has been modified
     return false;
 }
 
@@ -77,9 +56,7 @@ PhysicsEngine::PhysicsEngine(const glm::vec3& offset) :
 }
 
 PhysicsEngine::~PhysicsEngine() {
-    if (_myAvatarController) {
+    _myAvatarController = nullptr;
-        _myAvatarController->setDynamicsWorld(nullptr);
-    }
     delete _collisionConfig;
     delete _collisionDispatcher;
     delete _broadphaseFilter;
@@ -361,27 +338,6 @@ void PhysicsEngine::stepSimulation() {
     _clock.reset();
     float timeStep = btMin(dt, MAX_TIMESTEP);
 
-    if (_myAvatarController) {
-        DETAILED_PROFILE_RANGE(simulation_physics, "avatarController");
-        BT_PROFILE("avatarController");
-        // TODO: move this stuff outside and in front of stepSimulation, because
-        // the updateShapeIfNecessary() call needs info from MyAvatar and should
-        // be done on the main thread during the pre-simulation stuff
-        if (_myAvatarController->needsRemoval()) {
-            _myAvatarController->setDynamicsWorld(nullptr);
-
-            // We must remove any existing contacts for the avatar so that any new contacts will have
-            // valid data.  MyAvatar's RigidBody is the ONLY one in the simulation that does not yet
-            // have a MotionState so we pass nullptr to removeContacts().
-            removeContacts(nullptr);
-        }
-        _myAvatarController->updateShapeIfNecessary();
-        if (_myAvatarController->needsAddition()) {
-            _myAvatarController->setDynamicsWorld(_dynamicsWorld);
-        }
-        _myAvatarController->preSimulation();
-    }
-
     auto onSubStep = [this]() {
         this->updateContactMap();
         this->doOwnershipInfectionForConstraints();
@@ -390,15 +346,11 @@ void PhysicsEngine::stepSimulation() {
     int numSubsteps = _dynamicsWorld->stepSimulationWithSubstepCallback(timeStep, PHYSICS_ENGINE_MAX_NUM_SUBSTEPS,
                                                                         PHYSICS_ENGINE_FIXED_SUBSTEP, onSubStep);
     if (numSubsteps > 0) {
-        BT_PROFILE("postSimulation");
-        if (_myAvatarController) {
-            _myAvatarController->postSimulation();
-        }
         _hasOutgoingChanges = true;
-    }
+        if (_physicsDebugDraw->getDebugMode()) {
-
+            BT_PROFILE("debugDrawWorld");
-    if (_physicsDebugDraw->getDebugMode()) {
+            _dynamicsWorld->debugDrawWorld();
-        _dynamicsWorld->debugDrawWorld();
+        }
     }
 }
 
@@ -760,15 +712,8 @@ void PhysicsEngine::bumpAndPruneContacts(ObjectMotionState* motionState) {
 }
 
 void PhysicsEngine::setCharacterController(CharacterController* character) {
-    if (_myAvatarController != character) {
+    assert(!_myAvatarCharacterController);
-        if (_myAvatarController) {
+    _myAvatarController = character;
-            // remove the character from the DynamicsWorld immediately
-            _myAvatarController->setDynamicsWorld(nullptr);
-            _myAvatarController = nullptr;
-        }
-        // the character will be added to the DynamicsWorld later
-        _myAvatarController = character;
-    }
 }
 
 EntityDynamicPointer PhysicsEngine::getDynamicByID(const QUuid& dynamicID) const {
@@ -898,16 +843,6 @@ void PhysicsEngine::setShowBulletConstraintLimits(bool value) {
     }
 }
 
-void PhysicsEngine::enableGlobalContactAddedCallback(bool enabled) {
-	if (enabled) {
-        // register contact filter to help MyAvatar pass through backfacing triangles
-        addContactAddedCallback(flipNormalsMyAvatarVsBackfacingTriangles);
-	} else {
-        // deregister contact filter
-        removeContactAddedCallback(flipNormalsMyAvatarVsBackfacingTriangles);
-    }
-}
-
 void PhysicsEngine::addContactAddedCallback(PhysicsEngine::ContactAddedCallback newCb) {
     for (auto cb : _contactAddedCallbacks) {
         if (cb == newCb) {

libraries/physics/src/PhysicsEngine.h

@@ -154,10 +154,12 @@ public:
     // See PhysicsCollisionGroups.h for mask flags.
     std::vector<ContactTestResult> contactTest(uint16_t mask, const ShapeInfo& regionShapeInfo, const Transform& regionTransform, uint16_t group = USER_COLLISION_GROUP_DYNAMIC, float threshold = 0.0f) const;
 
-    void enableGlobalContactAddedCallback(bool enabled);
     void addContactAddedCallback(ContactAddedCallback cb);
     void removeContactAddedCallback(ContactAddedCallback cb);
 
+    btDiscreteDynamicsWorld* getDynamicsWorld() const { return _dynamicsWorld; }
+    void removeContacts(ObjectMotionState* motionState);
+
 private:
     QList<EntityDynamicPointer> removeDynamicsForBody(btRigidBody* body);
     void addObjectToDynamicsWorld(ObjectMotionState* motionState);
@@ -165,8 +167,6 @@ private:
     /// \brief bump any objects that touch this one, then remove contact info
     void bumpAndPruneContacts(ObjectMotionState* motionState);
 
-    void removeContacts(ObjectMotionState* motionState);
-
     void doOwnershipInfection(const btCollisionObject* objectA, const btCollisionObject* objectB);
 
     btClock _clock;