From 6d7565a56857cdba4960e8197926a3faec91bada Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 31 Jul 2019 16:07:44 -0700 Subject: [PATCH 01/13] CharacterController don't fight MyaAvatar::safeLanding() --- interface/src/avatar/MyAvatar.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 0ff2e055b7..8896621e3d 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -2733,7 +2733,7 @@ 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.isStuck()) { _characterController.getPositionAndOrientation(position, orientation); } else { position = getWorldPosition(); @@ -2746,7 +2746,7 @@ void MyAvatar::harvestResultsFromPhysicsSimulation(float deltaTime) { setWorldVelocity(_characterController.getLinearVelocity() + _characterController.getFollowVelocity()); if (_characterController.isStuck()) { _physicsSafetyPending = true; - _goToPosition = getWorldPosition(); + _goToPosition = position; } } else { setWorldVelocity(getWorldVelocity() + _characterController.getFollowVelocity()); From c8c704eb5749a66d2085384bd912830924baff68 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 5 Aug 2019 14:22:01 -0700 Subject: [PATCH 02/13] revert last change --- interface/src/avatar/MyAvatar.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 8896621e3d..55c57d04ae 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -2733,7 +2733,7 @@ void MyAvatar::nextAttitude(glm::vec3 position, glm::quat orientation) { void MyAvatar::harvestResultsFromPhysicsSimulation(float deltaTime) { glm::vec3 position; glm::quat orientation; - if (_characterController.isEnabledAndReady() && !_characterController.isStuck()) { + if (_characterController.isEnabledAndReady()) { _characterController.getPositionAndOrientation(position, orientation); } else { position = getWorldPosition(); From 9382fb87454a6b6a183195a66c7d8ca6a4717d3e Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 5 Aug 2019 14:22:33 -0700 Subject: [PATCH 03/13] more versatile contactAddedCallback support --- libraries/physics/src/PhysicsEngine.cpp | 55 ++++++++++++++++++++++++- libraries/physics/src/PhysicsEngine.h | 6 +++ 2 files changed, 60 insertions(+), 1 deletion(-) diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp index 655edd4e13..668e12d966 100644 --- a/libraries/physics/src/PhysicsEngine.cpp +++ b/libraries/physics/src/PhysicsEngine.cpp @@ -30,6 +30,13 @@ 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; @@ -45,6 +52,25 @@ static bool flipNormalsMyAvatarVsBackfacingTriangles(btManifoldPoint& cp, return false; } +// a list of sub-callbacks +std::vector _contactAddedCallbacks; + +// a callback that calls each sub-callback in the list +// if one returns 'true' --> break and return +bool globalContactAddedCallback(btManifoldPoint& cp, + const btCollisionObjectWrapper* colObj0Wrap, int partId0, int index0, + const btCollisionObjectWrapper* colObj1Wrap, int partId1, int index1) { + // 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 + // in which case there is no need to process other callbacks + return true; + } + } + return false; +} + PhysicsEngine::PhysicsEngine(const glm::vec3& offset) : _originOffset(offset), _myAvatarController(nullptr) { @@ -875,9 +901,36 @@ void PhysicsEngine::setShowBulletConstraintLimits(bool value) { void PhysicsEngine::enableGlobalContactAddedCallback(bool enabled) { if (enabled) { // register contact filter to help MyAvatar pass through backfacing triangles - gContactAddedCallback = flipNormalsMyAvatarVsBackfacingTriangles; + addContactAddedCallback(flipNormalsMyAvatarVsBackfacingTriangles); } else { // deregister contact filter + removeContactAddedCallback(flipNormalsMyAvatarVsBackfacingTriangles); + } +} + +void PhysicsEngine::addContactAddedCallback(PhysicsEngine::ContactAddedCallback newCb) { + for (auto cb : _contactAddedCallbacks) { + if (cb == newCb) { + // newCb is already in the list + return; + } + } + _contactAddedCallbacks.push_back(newCb); + gContactAddedCallback = globalContactAddedCallback; +} + +void PhysicsEngine::removeContactAddedCallback(PhysicsEngine::ContactAddedCallback cb) { + uint32_t numCallbacks = _contactAddedCallbacks.size(); + for (uint32_t i = 0; i < numCallbacks; ++i) { + if (_contactAddedCallbacks[i] == cb) { + // found it --> remove it + _contactAddedCallbacks[i] = _contactAddedCallbacks[numCallbacks - 1]; + _contactAddedCallbacks.pop_back(); + numCallbacks--; + break; + } + } + if (numCallbacks == 0) { gContactAddedCallback = nullptr; } } diff --git a/libraries/physics/src/PhysicsEngine.h b/libraries/physics/src/PhysicsEngine.h index d11b52f1af..4f665c6663 100644 --- a/libraries/physics/src/PhysicsEngine.h +++ b/libraries/physics/src/PhysicsEngine.h @@ -74,6 +74,10 @@ using CollisionEvents = std::vector; class PhysicsEngine { public: + using ContactAddedCallback = bool (*)(btManifoldPoint& cp, + const btCollisionObjectWrapper* colObj0Wrap, int partId0, int index0, + const btCollisionObjectWrapper* colObj1Wrap, int partId1, int index1); + class Transaction { public: void clear() { @@ -151,6 +155,8 @@ public: 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); private: QList removeDynamicsForBody(btRigidBody* body); From 32400a6baf417c080ca780b0d6cdef27c8e597cd Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 8 Aug 2019 14:42:33 -0700 Subject: [PATCH 04/13] 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; From 027a9d34d4b848c2b448791a7269f59c21c29af0 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 9 Aug 2019 10:17:13 -0700 Subject: [PATCH 05/13] fix warning on windows about signed conversion of unsigned int --- libraries/physics/src/PhysicsEngine.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp index 7ae8da0c7b..94787c0b12 100644 --- a/libraries/physics/src/PhysicsEngine.cpp +++ b/libraries/physics/src/PhysicsEngine.cpp @@ -855,8 +855,8 @@ void PhysicsEngine::addContactAddedCallback(PhysicsEngine::ContactAddedCallback } void PhysicsEngine::removeContactAddedCallback(PhysicsEngine::ContactAddedCallback cb) { - uint32_t numCallbacks = _contactAddedCallbacks.size(); - for (uint32_t i = 0; i < numCallbacks; ++i) { + int32_t numCallbacks = _contactAddedCallbacks.size(); + for (int32_t i = 0; i < numCallbacks; ++i) { if (_contactAddedCallbacks[i] == cb) { // found it --> remove it _contactAddedCallbacks[i] = _contactAddedCallbacks[numCallbacks - 1]; From 7cf0899d59dcd0b84d764d4ff16e94b0de363ba1 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 12 Aug 2019 10:46:11 -0700 Subject: [PATCH 06/13] more correct safeLanding trigger --- interface/src/avatar/MyAvatar.cpp | 3 ++- .../src/avatar/MyCharacterController.cpp | 2 +- libraries/physics/src/CharacterController.cpp | 23 +++++++++++++++---- libraries/physics/src/CharacterController.h | 8 ++++--- 4 files changed, 26 insertions(+), 10 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 2f7faec198..af49aa1e3c 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -2740,7 +2740,8 @@ void MyAvatar::harvestResultsFromPhysicsSimulation(float deltaTime) { position = getWorldPosition(); orientation = getWorldOrientation(); if (_characterController.needsSafeLandingSupport() && !_goToPending) { - _goToPending = true; + _characterController.resetStuckCounter(); + _physicsSafetyPending = true; _goToSafe = true; _goToPosition = position; } diff --git a/interface/src/avatar/MyCharacterController.cpp b/interface/src/avatar/MyCharacterController.cpp index ae9d7d39ef..3a25721528 100755 --- a/interface/src/avatar/MyCharacterController.cpp +++ b/interface/src/avatar/MyCharacterController.cpp @@ -224,7 +224,7 @@ void MyCharacterController::handleChangedCollisionMask() { } bool MyCharacterController::needsSafeLandingSupport() const { - return _isStuck && 0 == (_numStuckFrames % NUM_FRAMES_FOR_SAFE_LANDING_RETRY); + return _isStuck && _numStuckSubsteps >= NUM_SUBSTEPS_FOR_SAFE_LANDING_RETRY; } btConvexHullShape* MyCharacterController::computeShape() const { diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 137f753921..dd0e2ee3e1 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -348,16 +348,19 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) { // 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); + probablyStuck = probablyStuck + || deepestDistance < 2.0f * STUCK_PENETRATION + || strongestImpulse > 2.0f * STUCK_IMPULSE + || (deepestDistance < STUCK_PENETRATION && strongestImpulse > STUCK_IMPULSE); if (_isStuck != probablyStuck) { ++_stuckTransitionCount; - if (_stuckTransitionCount == NUM_FRAMES_FOR_STUCK_TRANSITION) { + if (_stuckTransitionCount > NUM_SUBSTEPS_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; + // start _numStuckSubsteps at NUM_SUBSTEPS_FOR_SAFE_LANDING_RETRY so SafeLanding tries to help immediately + _numStuckSubsteps = NUM_SUBSTEPS_FOR_SAFE_LANDING_RETRY; _stuckTransitionCount = 0; if (_isStuck) { _physicsEngine->addContactAddedCallback(flipBackfaceTriangleNormals); @@ -369,7 +372,7 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) { } else { _stuckTransitionCount = 0; if (_isStuck) { - ++_numStuckFrames; + ++_numStuckSubsteps; _flippedThisFrame = false; } } @@ -795,6 +798,8 @@ void CharacterController::applyMotor(int index, btScalar dt, btVector3& worldVel } void CharacterController::computeNewVelocity(btScalar dt, btVector3& velocity) { + btVector3 currentVelocity = velocity; + if (velocity.length2() < MIN_TARGET_SPEED_SQUARED) { velocity = btVector3(0.0f, 0.0f, 0.0f); } @@ -833,6 +838,14 @@ void CharacterController::computeNewVelocity(btScalar dt, btVector3& velocity) { // Note the differences between these two variables: // _targetVelocity = ideal final velocity according to input // velocity = real final velocity after motors are applied to current velocity + + bool gettingStuck = !_isStuck && _stuckTransitionCount > 1 && _state == State::Hover; + if (gettingStuck && velocity.length2() > currentVelocity.length2()) { + // we are probably trying to fly fast into a mesh obstacle + // which is causing us to tickle the "stuck" detection code + // so we average our new velocity with currentVeocity to prevent a "safe landing" response + velocity = 0.5f * (velocity + currentVelocity); + } } void CharacterController::computeNewVelocity(btScalar dt, glm::vec3& velocity) { diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h index 88b709668c..d736487d20 100755 --- a/libraries/physics/src/CharacterController.h +++ b/libraries/physics/src/CharacterController.h @@ -37,8 +37,8 @@ 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 +const uint32_t NUM_SUBSTEPS_FOR_STUCK_TRANSITION = 6; // physics substeps +const uint32_t NUM_SUBSTEPS_FOR_SAFE_LANDING_RETRY = 40; // physics substeps class btRigidBody; class btCollisionWorld; @@ -144,6 +144,8 @@ public: void setSeated(bool isSeated) { _isSeated = isSeated; } bool getSeated() { return _isSeated; } + void resetStuckCounter() { _numStuckSubsteps = 0; } + protected: #ifdef DEBUG_STATE_CHANGE void setState(State state, const char* reason); @@ -223,7 +225,7 @@ protected: uint32_t _pendingFlags { 0 }; uint32_t _previousFlags { 0 }; uint32_t _stuckTransitionCount { 0 }; - uint32_t _numStuckFrames { 0 }; + uint32_t _numStuckSubsteps { 0 }; bool _inWorld { false }; bool _zoneFlyingAllowed { true }; From 113d132d8e1ef9f59cd10de76fe9422da43256e3 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 12 Aug 2019 17:28:54 -0700 Subject: [PATCH 07/13] use pairwise collision filtering to help unstuck MyAvatar --- libraries/physics/src/CharacterController.cpp | 86 ++++++++++++++++--- .../src/TemporaryPairwiseCollisionFilter.cpp | 36 ++++++++ .../src/TemporaryPairwiseCollisionFilter.h | 35 ++++++++ 3 files changed, 143 insertions(+), 14 deletions(-) create mode 100755 libraries/physics/src/TemporaryPairwiseCollisionFilter.cpp create mode 100755 libraries/physics/src/TemporaryPairwiseCollisionFilter.h diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index dd0e2ee3e1..15f9e88aff 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -18,13 +18,58 @@ #include "ObjectMotionState.h" #include "PhysicsHelpers.h" #include "PhysicsLogging.h" +#include "TemporaryPairwiseCollisionFilter.h" + +const float STUCK_PENETRATION = -0.05f; // always negative into the object. +const float STUCK_IMPULSE = 500.0f; + const btVector3 LOCAL_UP_AXIS(0.0f, 1.0f, 0.0f); -static bool _flippedThisFrame = false; +static bool _appliedStuckRecoveryStrategy = false; + +static TemporaryPairwiseCollisionFilter _pairwiseFilter; + +// Note: applyPairwiseFilter is registered as a sub-callback to Bullet's gContactAddedCallback feature +// when we detect MyAvatar is "stuck". It will disable new ManifoldPoints between MyAvatar and mesh objects with +// which it has deep penetration, and will continue disabling new contact until new contacts stop happening +// (no overlap). If MyAvatar is not trying to move its velocity is defaulted to "up", to help it escape overlap. +bool applyPairwiseFilter(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 (_pairwiseFilter.isFiltered(other)) { + _pairwiseFilter.incrementEntry(other); + // disable contact point by setting distance too large and normal to zero + cp.setDistance(1.0e6f); + cp.m_normalWorldOnB.setValue(0.0f, 0.0f, 0.0f); + _appliedStuckRecoveryStrategy = true; + return false; + } + + if (other->getCollisionShape()->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) { + if ( cp.getDistance() < 2.0f * STUCK_PENETRATION || + cp.getAppliedImpulse() > 2.0f * STUCK_IMPULSE || + (cp.getDistance() < STUCK_PENETRATION && cp.getAppliedImpulse() > STUCK_IMPULSE)) { + _pairwiseFilter.incrementEntry(other); + // disable contact point by setting distance too large and normal to zero + cp.setDistance(1.0e6f); + cp.m_normalWorldOnB.setValue(0.0f, 0.0f, 0.0f); + _appliedStuckRecoveryStrategy = true; + } + } + return 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. +// +// KEEP THIS: flipBackfaceTriangleNormals is NOT USED, but KEEP THIS implemenation in case we want to use it. bool flipBackfaceTriangleNormals(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper* colObj1Wrap, int partId1, int index1) { @@ -111,12 +156,13 @@ bool flipBackfaceTriangleNormals(btManifoldPoint& cp, if (cp.m_normalWorldOnB.dot(up) <= 0.0f) { nDotF = btDot(faceNormal, cp.m_normalWorldOnB); cp.m_normalWorldOnB -= 2.0f * nDotF * faceNormal; - _flippedThisFrame = true; + _appliedStuckRecoveryStrategy = 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 + // So we set '_appliedStuckRecoveryStrategy = true' here and use it later to stay in "stuck" state + // until flipping stops } } @@ -140,7 +186,7 @@ bool flipBackfaceTriangleNormals(btManifoldPoint& cp, // 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) @@ -287,7 +333,7 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) { btDispatcher* dispatcher = collisionWorld->getDispatcher(); int numManifolds = dispatcher->getNumManifolds(); bool hasFloor = false; - bool probablyStuck = _isStuck && _flippedThisFrame; + bool probablyStuck = _isStuck && _appliedStuckRecoveryStrategy; btTransform rotation = _rigidBody->getWorldTransform(); rotation.setOrigin(btVector3(0.0f, 0.0f, 0.0f)); // clear translation part @@ -346,8 +392,6 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) { } // 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 < 2.0f * STUCK_PENETRATION || strongestImpulse > 2.0f * STUCK_IMPULSE @@ -356,24 +400,27 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) { if (_isStuck != probablyStuck) { ++_stuckTransitionCount; if (_stuckTransitionCount > NUM_SUBSTEPS_FOR_STUCK_TRANSITION) { - // we've been in this "probablyStuck" state for several consecutive frames + // we've been in this "probablyStuck" state for several consecutive substeps // --> make it official by changing state _isStuck = probablyStuck; - // start _numStuckSubsteps at NUM_SUBSTEPS_FOR_SAFE_LANDING_RETRY so SafeLanding tries to help immediately + // init _numStuckSubsteps at NUM_SUBSTEPS_FOR_SAFE_LANDING_RETRY so SafeLanding tries to help immediately _numStuckSubsteps = NUM_SUBSTEPS_FOR_SAFE_LANDING_RETRY; _stuckTransitionCount = 0; if (_isStuck) { - _physicsEngine->addContactAddedCallback(flipBackfaceTriangleNormals); + _physicsEngine->addContactAddedCallback(applyPairwiseFilter); + _pairwiseFilter.incrementStepCount(); } else { - _physicsEngine->removeContactAddedCallback(flipBackfaceTriangleNormals); - _flippedThisFrame = false; + _physicsEngine->removeContactAddedCallback(applyPairwiseFilter); + _appliedStuckRecoveryStrategy = false; + _pairwiseFilter.clearAllEntries(); } + updateCurrentGravity(); } } else { _stuckTransitionCount = 0; if (_isStuck) { ++_numStuckSubsteps; - _flippedThisFrame = false; + _appliedStuckRecoveryStrategy = false; } } return hasFloor; @@ -549,7 +596,7 @@ static const char* stateToStr(CharacterController::State state) { void CharacterController::updateCurrentGravity() { int32_t collisionMask = computeCollisionMask(); - if (_state == State::Hover || collisionMask == BULLET_COLLISION_MASK_COLLISIONLESS) { + if (_state == State::Hover || collisionMask == BULLET_COLLISION_MASK_COLLISIONLESS || _isStuck) { _currentGravity = 0.0f; } else { _currentGravity = _gravity; @@ -831,6 +878,12 @@ void CharacterController::computeNewVelocity(btScalar dt, btVector3& velocity) { if (velocity.length2() < MIN_TARGET_SPEED_SQUARED) { velocity = btVector3(0.0f, 0.0f, 0.0f); } + if (_isStuck && _targetVelocity.length2() < MIN_TARGET_SPEED_SQUARED) { + // we're stuck, but not trying to move --> move UP by default + // in the hopes we'll get unstuck + const float STUCK_EXTRACTION_SPEED = 1.0f; + velocity = STUCK_EXTRACTION_SPEED * _currentUp; + } // 'thrust' is applied at the very end _targetVelocity += dt * _linearAcceleration; @@ -1032,6 +1085,11 @@ void CharacterController::preSimulation() { _followTime = 0.0f; _followLinearDisplacement = btVector3(0, 0, 0); _followAngularDisplacement = btQuaternion::getIdentity(); + + if (_isStuck) { + _pairwiseFilter.expireOldEntries(); + _pairwiseFilter.incrementStepCount(); + } } void CharacterController::postSimulation() { diff --git a/libraries/physics/src/TemporaryPairwiseCollisionFilter.cpp b/libraries/physics/src/TemporaryPairwiseCollisionFilter.cpp new file mode 100755 index 0000000000..5f2e46542f --- /dev/null +++ b/libraries/physics/src/TemporaryPairwiseCollisionFilter.cpp @@ -0,0 +1,36 @@ +// +// TemporaryPairwiseCollisionFilter.cpp +// libraries/physics/src +// +// Created by Andrew Meadows 2019.08.12 +// Copyright 2018 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#include "TemporaryPairwiseCollisionFilter.h" + +bool TemporaryPairwiseCollisionFilter::isFiltered(const btCollisionObject* object) const { + return _filteredContacts.find(object) != _filteredContacts.end(); +} + +void TemporaryPairwiseCollisionFilter::incrementEntry(const btCollisionObject* object) { + LastContactMap::iterator itr = _filteredContacts.find(object); + if (itr == _filteredContacts.end()) { + _filteredContacts.emplace(std::make_pair(object, _stepCount)); + } else { + itr->second = _stepCount; + } +} + +void TemporaryPairwiseCollisionFilter::expireOldEntries() { + LastContactMap::iterator itr = _filteredContacts.begin(); + while (itr != _filteredContacts.end()) { + if (itr->second < _stepCount) { + itr = _filteredContacts.erase(itr); + } else { + ++itr; + } + } +} diff --git a/libraries/physics/src/TemporaryPairwiseCollisionFilter.h b/libraries/physics/src/TemporaryPairwiseCollisionFilter.h new file mode 100755 index 0000000000..4c3f52ba1b --- /dev/null +++ b/libraries/physics/src/TemporaryPairwiseCollisionFilter.h @@ -0,0 +1,35 @@ +// +// TemporaryPairwiseCollisionFilter.h +// libraries/physics/src +// +// Created by Andrew Meadows 2019.08.12 +// Copyright 2018 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#ifndef hifi_TemporaryPairwiseCollisionFilter_h +#define hifi_TemporaryPairwiseCollisionFilter_h + +#include +#include + +class TemporaryPairwiseCollisionFilter { +public: + using LastContactMap = std::unordered_map; + + TemporaryPairwiseCollisionFilter() { } + + bool isFiltered(const btCollisionObject* object) const; + void incrementEntry(const btCollisionObject* object); + void expireOldEntries(); + void clearAllEntries() { _filteredContacts.clear(); _stepCount = 0; } + void incrementStepCount() { ++_stepCount; } + +protected: + LastContactMap _filteredContacts; + uint32_t _stepCount { 0 }; +}; + +#endif // hifi_TemporaryPairwiseCollisionFilter_h From 42d2d4bbe5be980de9882e2bbff5c92d2e4c21c9 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 13 Aug 2019 09:24:06 -0700 Subject: [PATCH 08/13] fix warning in VisualStudio --- libraries/physics/src/PhysicsEngine.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp index 94787c0b12..2dd1140e45 100644 --- a/libraries/physics/src/PhysicsEngine.cpp +++ b/libraries/physics/src/PhysicsEngine.cpp @@ -855,7 +855,7 @@ void PhysicsEngine::addContactAddedCallback(PhysicsEngine::ContactAddedCallback } void PhysicsEngine::removeContactAddedCallback(PhysicsEngine::ContactAddedCallback cb) { - int32_t numCallbacks = _contactAddedCallbacks.size(); + int32_t numCallbacks = (int32_t)(_contactAddedCallbacks.size()); for (int32_t i = 0; i < numCallbacks; ++i) { if (_contactAddedCallbacks[i] == cb) { // found it --> remove it From dba7cadcae93b83b34a836127bf4234c119374dd Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 16 Aug 2019 14:57:38 -0700 Subject: [PATCH 09/13] remove cruft --- libraries/physics/src/CharacterController.cpp | 123 ------------------ 1 file changed, 123 deletions(-) diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 15f9e88aff..8e96d256b7 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -65,129 +65,6 @@ bool applyPairwiseFilter(btManifoldPoint& cp, return 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. -// -// KEEP THIS: flipBackfaceTriangleNormals is NOT USED, but KEEP THIS implemenation in case we want to use it. -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; - _appliedStuckRecoveryStrategy = 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 '_appliedStuckRecoveryStrategy = 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) #else From 434af8bdfb0ec717f829779ea1a1a402849430c8 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 16 Aug 2019 15:30:20 -0700 Subject: [PATCH 10/13] remove support for sub-contactAddecCallbacks from PhysicsEngine --- libraries/physics/src/CharacterController.cpp | 6 ++- libraries/physics/src/PhysicsEngine.cpp | 53 ++----------------- libraries/physics/src/PhysicsEngine.h | 3 +- 3 files changed, 10 insertions(+), 52 deletions(-) diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 8e96d256b7..e488a4da98 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -284,10 +284,12 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) { _numStuckSubsteps = NUM_SUBSTEPS_FOR_SAFE_LANDING_RETRY; _stuckTransitionCount = 0; if (_isStuck) { - _physicsEngine->addContactAddedCallback(applyPairwiseFilter); + // enable pairwise filter + _physicsEngine->setContactAddedCallback(applyPairwiseFilter); _pairwiseFilter.incrementStepCount(); } else { - _physicsEngine->removeContactAddedCallback(applyPairwiseFilter); + // disable pairwise filter + _physicsEngine->setContactAddedCallback(nullptr); _appliedStuckRecoveryStrategy = false; _pairwiseFilter.clearAllEntries(); } diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp index 2dd1140e45..c6f9aa84bc 100644 --- a/libraries/physics/src/PhysicsEngine.cpp +++ b/libraries/physics/src/PhysicsEngine.cpp @@ -27,29 +27,6 @@ #include "ThreadSafeDynamicsWorld.h" #include "PhysicsLogging.h" -// a list of sub-callbacks -std::vector _contactAddedCallbacks; - -// a callback that calls each sub-callback in the list -// if one returns 'true' --> break and return -bool globalContactAddedCallback(btManifoldPoint& cp, - const btCollisionObjectWrapper* colObj0Wrap, int partId0, int index0, - const btCollisionObjectWrapper* colObj1Wrap, int partId1, int index1) { - // call each callback - for (auto cb : _contactAddedCallbacks) { - if (cb(cp, colObj0Wrap, partId0, index0, colObj1Wrap, partId1, index1)) { - // 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 - 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; -} - PhysicsEngine::PhysicsEngine(const glm::vec3& offset) : _originOffset(offset), _myAvatarController(nullptr) { @@ -843,31 +820,11 @@ void PhysicsEngine::setShowBulletConstraintLimits(bool value) { } } -void PhysicsEngine::addContactAddedCallback(PhysicsEngine::ContactAddedCallback newCb) { - for (auto cb : _contactAddedCallbacks) { - if (cb == newCb) { - // newCb is already in the list - return; - } - } - _contactAddedCallbacks.push_back(newCb); - gContactAddedCallback = globalContactAddedCallback; -} - -void PhysicsEngine::removeContactAddedCallback(PhysicsEngine::ContactAddedCallback cb) { - int32_t numCallbacks = (int32_t)(_contactAddedCallbacks.size()); - for (int32_t i = 0; i < numCallbacks; ++i) { - if (_contactAddedCallbacks[i] == cb) { - // found it --> remove it - _contactAddedCallbacks[i] = _contactAddedCallbacks[numCallbacks - 1]; - _contactAddedCallbacks.pop_back(); - numCallbacks--; - break; - } - } - if (numCallbacks == 0) { - gContactAddedCallback = nullptr; - } +void PhysicsEngine::setContactAddedCallback(PhysicsEngine::ContactAddedCallback newCb) { + // gContactAddedCallback is a special feature hook in Bullet + // if non-null AND one of the colliding objects has btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK flag set + // then it is called whenever a new candidate contact point is created + gContactAddedCallback = newCb; } struct AllContactsCallback : public btCollisionWorld::ContactResultCallback { diff --git a/libraries/physics/src/PhysicsEngine.h b/libraries/physics/src/PhysicsEngine.h index bbca20c301..e0164a33f7 100644 --- a/libraries/physics/src/PhysicsEngine.h +++ b/libraries/physics/src/PhysicsEngine.h @@ -154,8 +154,7 @@ 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 addContactAddedCallback(ContactAddedCallback cb); - void removeContactAddedCallback(ContactAddedCallback cb); + void setContactAddedCallback(ContactAddedCallback cb); btDiscreteDynamicsWorld* getDynamicsWorld() const { return _dynamicsWorld; } void removeContacts(ObjectMotionState* motionState); From 4f0c2c5ee67042ca22ea259c82ecf10912f3705b Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 16 Aug 2019 15:36:09 -0700 Subject: [PATCH 11/13] remove incorrect assert --- libraries/physics/src/PhysicsEngine.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp index c6f9aa84bc..2fac9b0cc6 100644 --- a/libraries/physics/src/PhysicsEngine.cpp +++ b/libraries/physics/src/PhysicsEngine.cpp @@ -689,7 +689,6 @@ void PhysicsEngine::bumpAndPruneContacts(ObjectMotionState* motionState) { } void PhysicsEngine::setCharacterController(CharacterController* character) { - assert(!_myAvatarCharacterController); _myAvatarController = character; } From 7084908df3eeb01870ad3b1b1eae616e966fda43 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 20 Aug 2019 15:36:43 -0700 Subject: [PATCH 12/13] add isStuck logging, add assert because singleton --- libraries/physics/src/CharacterController.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index e488a4da98..e102388822 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -105,6 +105,8 @@ CharacterController::CharacterMotor::CharacterMotor(const glm::vec3& vel, const } } +static uint32_t _numCharacterControllers { 0 }; + CharacterController::CharacterController() { _floorDistance = _scaleFactor * DEFAULT_AVATAR_FALL_HEIGHT; @@ -123,6 +125,11 @@ CharacterController::CharacterController() { _hasSupport = false; _pendingFlags = PENDING_FLAG_UPDATE_SHAPE; + + // ATM CharacterController is a singleton. When we want more we'll have to + // overhaul the applyPairwiseFilter() logic to handle multiple instances. + ++_numCharacterControllers; + assert(numCharacterControllers == 1); } CharacterController::~CharacterController() { @@ -279,6 +286,7 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) { if (_stuckTransitionCount > NUM_SUBSTEPS_FOR_STUCK_TRANSITION) { // we've been in this "probablyStuck" state for several consecutive substeps // --> make it official by changing state + qCDebug(physics) << "CharacterController::_isStuck :" << _isStuck << "-->" << probablyStuck; _isStuck = probablyStuck; // init _numStuckSubsteps at NUM_SUBSTEPS_FOR_SAFE_LANDING_RETRY so SafeLanding tries to help immediately _numStuckSubsteps = NUM_SUBSTEPS_FOR_SAFE_LANDING_RETRY; From 54c69001b916cb0b4c60bb12b28c60681b5e94b1 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 20 Aug 2019 15:37:23 -0700 Subject: [PATCH 13/13] clarify safeLanding retry rate --- libraries/physics/src/CharacterController.h | 3 ++- libraries/shared/src/PhysicsHelpers.h | 5 +++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h index d736487d20..77a75c7bbd 100755 --- a/libraries/physics/src/CharacterController.h +++ b/libraries/physics/src/CharacterController.h @@ -25,6 +25,7 @@ #include "BulletUtil.h" #include "CharacterGhostObject.h" #include "PhysicsEngine.h" +#include "PhysicsHelpers.h" const uint32_t PENDING_FLAG_ADD_TO_SIMULATION = 1U << 0; const uint32_t PENDING_FLAG_REMOVE_FROM_SIMULATION = 1U << 1; @@ -38,7 +39,7 @@ 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_SUBSTEPS_FOR_STUCK_TRANSITION = 6; // physics substeps -const uint32_t NUM_SUBSTEPS_FOR_SAFE_LANDING_RETRY = 40; // physics substeps +const uint32_t NUM_SUBSTEPS_FOR_SAFE_LANDING_RETRY = NUM_SUBSTEPS_PER_SECOND / 2; // retry every half second class btRigidBody; class btCollisionWorld; diff --git a/libraries/shared/src/PhysicsHelpers.h b/libraries/shared/src/PhysicsHelpers.h index 332100f03b..2e32865fd0 100644 --- a/libraries/shared/src/PhysicsHelpers.h +++ b/libraries/shared/src/PhysicsHelpers.h @@ -19,8 +19,9 @@ // TODO: move everything in here to the physics library after the physics/entities library // dependency order is swapped. -const int PHYSICS_ENGINE_MAX_NUM_SUBSTEPS = 6; // Bullet will start to "lose time" at 10 FPS. -const float PHYSICS_ENGINE_FIXED_SUBSTEP = 1.0f / 90.0f; +const int32_t PHYSICS_ENGINE_MAX_NUM_SUBSTEPS = 6; // Bullet will start to "lose time" at 10 FPS. +const uint32_t NUM_SUBSTEPS_PER_SECOND = 90; +const float PHYSICS_ENGINE_FIXED_SUBSTEP = 1.0f / (float)NUM_SUBSTEPS_PER_SECOND; const float DYNAMIC_LINEAR_SPEED_THRESHOLD = 0.05f; // 5 cm/sec const float DYNAMIC_ANGULAR_SPEED_THRESHOLD = 0.087266f; // ~5 deg/sec