From 32400a6baf417c080ca780b0d6cdef27c8e597cd Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 8 Aug 2019 14:42:33 -0700 Subject: [PATCH] improved isStuck detection and MyAvatar::safeLanding() trigger --- interface/src/Application.cpp | 9 +- interface/src/avatar/MyAvatar.cpp | 21 +- .../src/avatar/MyCharacterController.cpp | 25 +- interface/src/avatar/MyCharacterController.h | 6 +- libraries/physics/src/CharacterController.cpp | 326 ++++++++++++++---- libraries/physics/src/CharacterController.h | 20 +- libraries/physics/src/PhysicsEngine.cpp | 91 +---- libraries/physics/src/PhysicsEngine.h | 6 +- 8 files changed, 313 insertions(+), 191 deletions(-) diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index 2a310df7f5..090cdd52b6 100644 --- a/interface/src/Application.cpp +++ b/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); } diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 55c57d04ae..2f7faec198 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/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 { diff --git a/interface/src/avatar/MyCharacterController.cpp b/interface/src/avatar/MyCharacterController.cpp index 2f59b70592..ae9d7d39ef 100755 --- a/interface/src/avatar/MyCharacterController.cpp +++ b/interface/src/avatar/MyCharacterController.cpp @@ -36,10 +36,10 @@ MyCharacterController::MyCharacterController(std::shared_ptr avatar) { MyCharacterController::~MyCharacterController() { } -void MyCharacterController::setDynamicsWorld(btDynamicsWorld* world) { - CharacterController::setDynamicsWorld(world); - if (world && _rigidBody) { - initRayShotgun(world); +void MyCharacterController::addToWorld() { + CharacterController::addToWorld(); + if (_rigidBody) { + 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) { - _dynamicsWorld->removeRigidBody(_rigidBody); - int32_t collisionMask = computeCollisionMask(); - _dynamicsWorld->addRigidBody(_rigidBody, BULLET_COLLISION_GROUP_MY_AVATAR, collisionMask); - } + // but we don't do it here. Instead we set some flags to remind us to do it later. + _pendingFlags |= (PENDING_FLAG_REMOVE_FROM_SIMULATION | PENDING_FLAG_ADD_TO_SIMULATION); _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::rayTest(const btVector3& origin, const btVector3& direction, const btScalar& length, const QVector& jointsToExclude) const { std::vector 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++) { diff --git a/interface/src/avatar/MyCharacterController.h b/interface/src/avatar/MyCharacterController.h index 7ddcf94f67..eefcc92637 100644 --- a/interface/src/avatar/MyCharacterController.h +++ b/interface/src/avatar/MyCharacterController.h @@ -26,7 +26,7 @@ public: explicit MyCharacterController(std::shared_ptr 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 _detailedMotionStates; + bool _collideWithOtherAvatars { true }; }; #endif // hifi_MyCharacterController_h diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index e36f97b425..137f753921 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -11,14 +11,136 @@ #include "CharacterController.h" -#include #include +#include +#include #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(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(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) { - if (_dynamicsWorld != world) { - // 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; +void CharacterController::removeFromWorld() { + if (_inWorld) { if (_rigidBody) { - updateMassProperties(); - } - 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(shape)); - } - _ghost.setCollisionGroupAndMask(collisionGroup, collisionMask & (~ collisionGroup)); - _ghost.setCollisionWorld(_dynamicsWorld); - _ghost.setRadiusAndHalfHeight(_radius, _halfHeight); - if (_rigidBody) { - _ghost.setWorldTransform(_rigidBody->getWorldTransform()); + _physicsEngine->getDynamicsWorld()->removeRigidBody(_rigidBody); + _physicsEngine->getDynamicsWorld()->removeAction(this); } + _inWorld = false; } - if (_dynamicsWorld) { - 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_ADD_TO_SIMULATION; - } + _pendingFlags &= ~PENDING_FLAG_REMOVE_FROM_SIMULATION; +} + +void CharacterController::addToWorld() { + if (!_rigidBody) { + return; + } + 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(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. - const float STUCK_PENETRATION = -0.05f; // always negative into the object. - const float STUCK_IMPULSE = 500.0f; - const int STUCK_LIFETIME = 3; - if ((contact.getDistance() < STUCK_PENETRATION) && (contact.getAppliedImpulse() > STUCK_IMPULSE) && (contact.getLifeTime() > STUCK_LIFETIME)) { - isStuck = true; // latch on + + float distance = contact.getDistance(); + if (distance < deepestDistance) { + deepestDistance = distance; } + 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; - break; + stepValid = false; } 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))); diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h index 8c65310ff1..88b709668c 100755 --- a/libraries/physics/src/CharacterController.h +++ b/libraries/physics/src/CharacterController.h @@ -20,11 +20,11 @@ #include #include -#include +#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 }; diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp index 668e12d966..7ae8da0c7b 100644 --- a/libraries/physics/src/PhysicsEngine.cpp +++ b/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(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 _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->setDynamicsWorld(nullptr); - } + _myAvatarController = 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()) { - _dynamicsWorld->debugDrawWorld(); + if (_physicsDebugDraw->getDebugMode()) { + BT_PROFILE("debugDrawWorld"); + _dynamicsWorld->debugDrawWorld(); + } } } @@ -760,15 +712,8 @@ void PhysicsEngine::bumpAndPruneContacts(ObjectMotionState* motionState) { } void PhysicsEngine::setCharacterController(CharacterController* character) { - if (_myAvatarController != character) { - if (_myAvatarController) { - // remove the character from the DynamicsWorld immediately - _myAvatarController->setDynamicsWorld(nullptr); - _myAvatarController = nullptr; - } - // the character will be added to the DynamicsWorld later - _myAvatarController = character; - } + assert(!_myAvatarCharacterController); + _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) { diff --git a/libraries/physics/src/PhysicsEngine.h b/libraries/physics/src/PhysicsEngine.h index 4f665c6663..bbca20c301 100644 --- a/libraries/physics/src/PhysicsEngine.h +++ b/libraries/physics/src/PhysicsEngine.h @@ -154,10 +154,12 @@ public: // See PhysicsCollisionGroups.h for mask flags. std::vector 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 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;