improved isStuck detection and MyAvatar::safeLanding() trigger

This commit is contained in:
Andrew Meadows 2019-08-08 14:42:33 -07:00
parent 9382fb8745
commit 32400a6baf
8 changed files with 313 additions and 191 deletions

View file

@ -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);
}

View file

@ -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 {

View file

@ -36,10 +36,10 @@ MyCharacterController::MyCharacterController(std::shared_ptr<MyAvatar> 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::RayAvatarResult> MyCharacterController::rayTest(const btVector3& origin, const btVector3& direction,
const btScalar& length, const QVector<uint>& jointsToExclude) const {
std::vector<RayAvatarResult> foundAvatars;
if (_dynamicsWorld) {
if (_physicsEngine) {
btVector3 end = origin + length * direction;
DetailedRayResultCallback rayCallback = DetailedRayResultCallback();
rayCallback.m_flags |= btTriangleRaycastCallback::kF_KeepUnflippedNormal;
rayCallback.m_flags |= btTriangleRaycastCallback::kF_UseSubSimplexConvexCastRaytest;
_dynamicsWorld->rayTest(origin, end, rayCallback);
_physicsEngine->getDynamicsWorld()->rayTest(origin, end, rayCallback);
if (rayCallback.m_hitFractions.size() > 0) {
foundAvatars.reserve(rayCallback.m_hitFractions.size());
for (int32_t i = 0; i < rayCallback.m_hitFractions.size(); i++) {

View file

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

View file

@ -11,14 +11,136 @@
#include "CharacterController.h"
#include <NumericalConstants.h>
#include <AvatarConstants.h>
#include <NumericalConstants.h>
#include <PhysicsCollisionGroups.h>
#include "ObjectMotionState.h"
#include "PhysicsHelpers.h"
#include "PhysicsLogging.h"
const btVector3 LOCAL_UP_AXIS(0.0f, 1.0f, 0.0f);
static bool _flippedThisFrame = false;
// Note: flipBackfaceTriangleNormals is registered as a sub-callback to Bullet's gContactAddedCallback feature
// when we detect MyAvatar is "stuck". It will reverse the triangles on the backface of mesh shapes, unless it thinks
// the old normal would be better at extracting MyAvatar out along its UP direction.
bool flipBackfaceTriangleNormals(btManifoldPoint& cp,
const btCollisionObjectWrapper* colObj0Wrap, int partId0, int index0,
const btCollisionObjectWrapper* colObj1Wrap, int partId1, int index1) {
static int32_t numCalls = 0;
++numCalls;
// This callback is ONLY called on objects with btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK flag
// and the flagged object will always be sorted to Obj0. Hence the "other" is always Obj1.
const btCollisionObject* other = colObj1Wrap->m_collisionObject;
if (other->getCollisionShape()->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) {
// access the meshInterface
auto meshShape = static_cast<const btBvhTriangleMeshShape*>(other->getCollisionShape());
const btStridingMeshInterface* meshInterface = meshShape->getMeshInterface();
// figure out about how to navigate meshInterface
const uint8_t* vertexBase;
int32_t numverts;
PHY_ScalarType vertexType;
int32_t vertexStride;
const uint8_t* indexBase;
int32_t indexStride;
int32_t numFaces;
PHY_ScalarType indicesType;
int32_t subPart = colObj1Wrap->m_partId;
// NOTE: all arguments are being passed by reference except the bases (passed by pointer) and subPart (true input)
meshInterface->getLockedReadOnlyVertexIndexBase(&vertexBase, numverts, vertexType, vertexStride, &indexBase, indexStride, numFaces, indicesType, subPart);
// fetch the triangle vertices
int32_t triangleIndex = colObj1Wrap->m_index;
assert(vertexType == PHY_FLOAT); // all mesh vertex data is float...
// ...but indicesType can vary
btVector3 triangleVertex[3];
switch (indicesType) {
case PHY_INTEGER: {
uint32_t* triangleIndices = (uint32_t*)(indexBase + triangleIndex * indexStride);
float* triangleBase;
triangleBase = (float*)(vertexBase + triangleIndices[0] * vertexStride);
triangleVertex[0].setValue(triangleBase[0], triangleBase[1], triangleBase[2]);
triangleBase = (float*)(vertexBase + triangleIndices[1] * vertexStride);
triangleVertex[1].setValue(triangleBase[0], triangleBase[1], triangleBase[2]);
triangleBase = (float*)(vertexBase + triangleIndices[2] * vertexStride);
triangleVertex[2].setValue(triangleBase[0], triangleBase[1], triangleBase[2]);
}
break;
case PHY_SHORT: {
uint16_t* triangleIndices = (uint16_t*)(indexBase + triangleIndex * indexStride);
float* triangleBase;
triangleBase = (float*)(vertexBase + triangleIndices[0] * vertexStride);
triangleVertex[0].setValue(triangleBase[0], triangleBase[1], triangleBase[2]);
triangleBase = (float*)(vertexBase + triangleIndices[1] * vertexStride);
triangleVertex[1].setValue(triangleBase[0], triangleBase[1], triangleBase[2]);
triangleBase = (float*)(vertexBase + triangleIndices[2] * vertexStride);
triangleVertex[2].setValue(triangleBase[0], triangleBase[1], triangleBase[2]);
}
break;
case PHY_UCHAR: {
uint8_t* triangleIndices = (uint8_t*)(indexBase + triangleIndex * indexStride);
float* triangleBase;
triangleBase = (float*)(vertexBase + triangleIndices[0] * vertexStride);
triangleVertex[0].setValue(triangleBase[0], triangleBase[1], triangleBase[2]);
triangleBase = (float*)(vertexBase + triangleIndices[1] * vertexStride);
triangleVertex[1].setValue(triangleBase[0], triangleBase[1], triangleBase[2]);
triangleBase = (float*)(vertexBase + triangleIndices[2] * vertexStride);
triangleVertex[2].setValue(triangleBase[0], triangleBase[1], triangleBase[2]);
}
break;
default:
return false;
}
// compute faceNormal
btVector3 meshScaling = meshInterface->getScaling();
triangleVertex[0] *= meshScaling;
triangleVertex[1] *= meshScaling;
triangleVertex[2] *= meshScaling;
btVector3 faceNormal = other->getWorldTransform().getBasis() * btCross(triangleVertex[1] - triangleVertex[0], triangleVertex[2] - triangleVertex[0]);
float nDotF = btDot(faceNormal, cp.m_normalWorldOnB);
if (nDotF <= 0.0f && faceNormal.length2() > EPSILON) {
faceNormal.normalize();
// flip the contact normal to be aligned with the face normal...
// ...but only if old normal does NOT point along obj0's UP
// (because we're "stuck" and UP is the likely path out)
btVector3 up = colObj0Wrap->m_collisionObject->getWorldTransform().getBasis() * LOCAL_UP_AXIS;
if (cp.m_normalWorldOnB.dot(up) <= 0.0f) {
nDotF = btDot(faceNormal, cp.m_normalWorldOnB);
cp.m_normalWorldOnB -= 2.0f * nDotF * faceNormal;
_flippedThisFrame = true;
}
// Note: if we're flipping normals it means the "Are we stuck?" logic is concluding "Yes, we are".
// But when we flip the normals it typically causes the ContactManifold to discard the modified ManifoldPoint
// which in turn causes the "Are we stuck?" logic to incorrectly conclude "No, we are not".
// So we set '_flippedThisFrame = true' here and use it later to stay in "stuck" state until flipping stops
}
}
// KEEP THIS: in case we add support for concave shapes which delegate to temporary btTriangleShapes (such as btHeightfieldTerrainShape)
//else if (other->getCollisionShape()->getShapeType() == TRIANGLE_SHAPE_PROXYTYPE) {
// auto triShape = static_cast<const btTriangleShape*>(other->getCollisionShape());
// const btVector3* v = triShape->m_vertices1;
// btVector3 faceNormal = other->getWorldTransform().getBasis() * btCross(v[1] - v[0], v[2] - v[0]);
// float nDotF = btDot(faceNormal, cp.m_normalWorldOnB);
// if (nDotF <= 0.0f && faceNormal.length2() > EPSILON) {
// faceNormal.normalize();
// // flip the contact normal to be aligned with the face normal
// cp.m_normalWorldOnB += -2.0f * nDotF * faceNormal;
// }
//}
// KEEP THIS
// NOTE: this ManifoldPoint is a candidate and hasn't been accepted yet into the final ContactManifold yet.
// So when we modify its parameters we can convince Bullet to discard it.
//
// by our own convention:
// return true when this ManifoldPoint has been modified in a way that would disable it
return false;
};
#ifdef DEBUG_STATE_CHANGE
#define SET_STATE(desiredState, reason) setState(desiredState, reason)
@ -92,64 +214,70 @@ CharacterController::~CharacterController() {
}
bool CharacterController::needsRemoval() const {
return ((_pendingFlags & PENDING_FLAG_REMOVE_FROM_SIMULATION) == PENDING_FLAG_REMOVE_FROM_SIMULATION);
return (_physicsEngine && (_pendingFlags & PENDING_FLAG_REMOVE_FROM_SIMULATION) == PENDING_FLAG_REMOVE_FROM_SIMULATION);
}
bool CharacterController::needsAddition() const {
return ((_pendingFlags & PENDING_FLAG_ADD_TO_SIMULATION) == PENDING_FLAG_ADD_TO_SIMULATION);
return (_physicsEngine && (_pendingFlags & PENDING_FLAG_ADD_TO_SIMULATION) == PENDING_FLAG_ADD_TO_SIMULATION);
}
void CharacterController::setDynamicsWorld(btDynamicsWorld* world) {
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<btConvexHullShape*>(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<btConvexHullShape*>(shape));
_ghost.setCollisionGroupAndMask(collisionGroup, collisionMask & (~ collisionGroup));
_ghost.setCollisionWorld(world);
_ghost.setRadiusAndHalfHeight(_radius, _halfHeight);
if (_rigidBody) {
_ghost.setWorldTransform(_rigidBody->getWorldTransform());
}
if (_pendingFlags & PENDING_FLAG_UPDATE_SHAPE) {
// shouldn't fall in here, but if we do make sure both ADD and REMOVE bits are still set
_pendingFlags |= PENDING_FLAG_ADD_TO_SIMULATION | PENDING_FLAG_REMOVE_FROM_SIMULATION |
PENDING_FLAG_ADD_DETAILED_TO_SIMULATION | PENDING_FLAG_REMOVE_DETAILED_FROM_SIMULATION;
} else {
_pendingFlags &= ~PENDING_FLAG_REMOVE_FROM_SIMULATION;
_pendingFlags &= ~PENDING_FLAG_ADD_TO_SIMULATION;
}
}
@ -159,17 +287,21 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) {
btDispatcher* dispatcher = collisionWorld->getDispatcher();
int numManifolds = dispatcher->getNumManifolds();
bool hasFloor = false;
bool isStuck = false;
bool probablyStuck = _isStuck && _flippedThisFrame;
btTransform rotation = _rigidBody->getWorldTransform();
rotation.setOrigin(btVector3(0.0f, 0.0f, 0.0f)); // clear translation part
float deepestDistance = 0.0f;
float strongestImpulse = 0.0f;
for (int i = 0; i < numManifolds; i++) {
btPersistentManifold* contactManifold = dispatcher->getManifoldByIndexInternal(i);
if (_rigidBody == contactManifold->getBody1() || _rigidBody == contactManifold->getBody0()) {
bool characterIsFirst = _rigidBody == contactManifold->getBody0();
int numContacts = contactManifold->getNumContacts();
int stepContactIndex = -1;
bool stepValid = true;
float highestStep = _minStepHeight;
for (int j = 0; j < numContacts; j++) {
// check for "floor"
@ -177,28 +309,24 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) {
btVector3 pointOnCharacter = characterIsFirst ? contact.m_localPointA : contact.m_localPointB; // object-local-frame
btVector3 normal = characterIsFirst ? contact.m_normalWorldOnB : -contact.m_normalWorldOnB; // points toward character
btScalar hitHeight = _halfHeight + _radius + pointOnCharacter.dot(_currentUp);
// If there's non-trivial penetration with a big impulse for several steps, we're probably stuck.
// Note it here in the controller, and let MyAvatar figure out what to do about it.
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)));

View file

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

View file

@ -27,31 +27,6 @@
#include "ThreadSafeDynamicsWorld.h"
#include "PhysicsLogging.h"
static bool flipNormalsMyAvatarVsBackfacingTriangles(btManifoldPoint& cp,
const btCollisionObjectWrapper* colObj0Wrap, int partId0, int index0,
const btCollisionObjectWrapper* colObj1Wrap, int partId1, int index1) {
// This callback is designed to help MyAvatar escape entrapment inside mesh geometry.
// It is only activated when MyAvatar is flying because it can cause problems when MyAvatar
// is walking along the ground.
// When active it applies to ALL contact points, however we only expect it to "do interesting
// stuff on MyAvatar's physics.
// Note: we're taking advantage of the fact: MyAvatar's collisionObject always shows up as colObj0
// because it is added to physics first.
if (colObj1Wrap->getCollisionShape()->getShapeType() == TRIANGLE_SHAPE_PROXYTYPE) {
auto triShape = static_cast<const btTriangleShape*>(colObj1Wrap->getCollisionShape());
const btVector3* v = triShape->m_vertices1;
btVector3 faceNormal = colObj1Wrap->getWorldTransform().getBasis() * btCross(v[1] - v[0], v[2] - v[0]);
float nDotF = btDot(faceNormal, cp.m_normalWorldOnB);
if (nDotF <= 0.0f && faceNormal.length2() > EPSILON) {
faceNormal.normalize();
// flip the contact normal to be aligned with the face normal
cp.m_normalWorldOnB += -2.0f * nDotF * faceNormal;
}
}
// return value is currently ignored but to be future-proof: return false when not modifying friction
return false;
}
// a list of sub-callbacks
std::vector<PhysicsEngine::ContactAddedCallback> _contactAddedCallbacks;
@ -63,11 +38,15 @@ bool globalContactAddedCallback(btManifoldPoint& cp,
// call each callback
for (auto cb : _contactAddedCallbacks) {
if (cb(cp, colObj0Wrap, partId0, index0, colObj1Wrap, partId1, index1)) {
// a return value of 'true' indicates the contact has been disabled
// Not a Bullet convention, but one we are using for sub-callbacks:
// a return value of 'true' indicates the contact has been "disabled"
// in which case there is no need to process other callbacks
return true;
break;
}
}
// by Bullet convention for its gContactAddedCallback feature:
// the return value is currently ignored but to be future-proof:
// return true when friction has been modified
return false;
}
@ -77,9 +56,7 @@ PhysicsEngine::PhysicsEngine(const glm::vec3& offset) :
}
PhysicsEngine::~PhysicsEngine() {
if (_myAvatarController) {
_myAvatarController->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) {

View file

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