Merge pull request #14795 from AndrewMeadows/myAvatar-vs-backfacing-triangles-2

case 17773: reduce likelihood MyAvatar will get stuck in mesh geometry when flying around

interface/src/Application.cpp

@@ -6329,6 +6329,8 @@ void Application::update(float deltaTime) {
                 _physicsEngine->processTransaction(transaction);
                 myAvatar->getCharacterController()->handleProcessedPhysicsTransaction(transaction);
                 myAvatar->prepareForPhysicsSimulation();
+                _physicsEngine->enableGlobalContactAddedCallback(myAvatar->isFlying());
+
                 _physicsEngine->forEachDynamic([&](EntityDynamicPointer dynamic) {
                     dynamic->prepareForPhysicsSimulation();
                 });

libraries/physics/src/CharacterController.cpp

@@ -124,6 +124,11 @@ void CharacterController::setDynamicsWorld(btDynamicsWorld* world) {
             _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));
@@ -455,6 +460,12 @@ void CharacterController::setLocalBoundingBox(const glm::vec3& minCorner, const
 
     // it's ok to change offset immediately -- there are no thread safety issues here
     _shapeLocalOffset = minCorner + 0.5f * scale;
+
+    if (_rigidBody) {
+        // update CCD with new _radius
+        _rigidBody->setCcdSweptSphereRadius(_radius);
+        _rigidBody->setCcdMotionThreshold(_radius);
+    }
 }
 
 void CharacterController::setCollisionless(bool collisionless) {

libraries/physics/src/PhysicsEngine.cpp

@@ -27,6 +27,23 @@
 #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) {
+    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.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;
+}
 
 PhysicsEngine::PhysicsEngine(const glm::vec3& offset) :
         _originOffset(offset),
@@ -904,6 +921,16 @@ void PhysicsEngine::setShowBulletConstraintLimits(bool value) {
     }
 }
 
+void PhysicsEngine::enableGlobalContactAddedCallback(bool enabled) {
+	if (enabled) {
+        // register contact filter to help MyAvatar pass through backfacing triangles
+        gContactAddedCallback = flipNormalsMyAvatarVsBackfacingTriangles;
+	} else {
+        // deregister contact filter
+        gContactAddedCallback = nullptr;
+    }
+}
+
 struct AllContactsCallback : public btCollisionWorld::ContactResultCallback {
     AllContactsCallback(int32_t mask, int32_t group, const ShapeInfo& shapeInfo, const Transform& transform, btCollisionObject* myAvatarCollisionObject, float threshold) :
         btCollisionWorld::ContactResultCallback(),

libraries/physics/src/PhysicsEngine.h

@@ -148,6 +148,8 @@ 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);
+
 private:
     QList<EntityDynamicPointer> removeDynamicsForBody(btRigidBody* body);
     void addObjectToDynamicsWorld(ObjectMotionState* motionState);