//// CharacterControllerInterface.cpp// libraries/physics/src//// Created by Andrew Meadows 2015.10.21// Copyright 2015 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 "CharacterController.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)#else#define SET_STATE(desiredState, reason) setState(desiredState)#endif // helper class for simple ray-traces from characterclass ClosestNotMe : public btCollisionWorld::ClosestRayResultCallback {public: ClosestNotMe(btRigidBody* me) : btCollisionWorld::ClosestRayResultCallback(btVector3(0.0f, 0.0f, 0.0f), btVector3(0.0f, 0.0f, 0.0f)) { _me = me; // the RayResultCallback's group and mask must match MY_AVATAR m_collisionFilterGroup = BULLET_COLLISION_GROUP_MY_AVATAR; m_collisionFilterMask = BULLET_COLLISION_MASK_MY_AVATAR; } virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult,bool normalInWorldSpace) override { if (rayResult.m_collisionObject == _me) { return 1.0f; } return ClosestRayResultCallback::addSingleResult(rayResult, normalInWorldSpace); }protected: btRigidBody* _me;}; CharacterController::CharacterMotor::CharacterMotor(const glm::vec3& vel, const glm::quat& rot, float horizTimescale, float vertTimescale) { velocity = glmToBullet(vel); rotation = glmToBullet(rot); hTimescale = horizTimescale; if (hTimescale < MIN_CHARACTER_MOTOR_TIMESCALE) { hTimescale = MIN_CHARACTER_MOTOR_TIMESCALE; } vTimescale = vertTimescale; if (vTimescale < 0.0f) { vTimescale = hTimescale; } else if (vTimescale < MIN_CHARACTER_MOTOR_TIMESCALE) { vTimescale = MIN_CHARACTER_MOTOR_TIMESCALE; }} CharacterController::CharacterController() { _floorDistance = _scaleFactor * DEFAULT_AVATAR_FALL_HEIGHT; _targetVelocity.setValue(0.0f, 0.0f, 0.0f); _followDesiredBodyTransform.setIdentity(); _followTimeRemaining = 0.0f; _state = State::Hover; _isPushingUp = false; _rayHitStartTime = 0; _takeoffToInAirStartTime = 0; _jumpButtonDownStartTime = 0; _jumpButtonDownCount = 0; _followTime = 0.0f; _followLinearDisplacement = btVector3(0, 0, 0); _followAngularDisplacement = btQuaternion::getIdentity(); _hasSupport = false; _pendingFlags = PENDING_FLAG_UPDATE_SHAPE;} CharacterController::~CharacterController() { if (_rigidBody) { btCollisionShape* shape = _rigidBody->getCollisionShape(); if (shape) { delete shape; } delete _rigidBody; _rigidBody = nullptr; }} bool CharacterController::needsRemoval() const { return (_physicsEngine && (_pendingFlags & PENDING_FLAG_REMOVE_FROM_SIMULATION) == PENDING_FLAG_REMOVE_FROM_SIMULATION);} bool CharacterController::needsAddition() const { return (_physicsEngine && (_pendingFlags & PENDING_FLAG_ADD_TO_SIMULATION) == PENDING_FLAG_ADD_TO_SIMULATION);} void CharacterController::removeFromWorld() { if (_inWorld) { if (_rigidBody) { _physicsEngine->getDynamicsWorld()->removeRigidBody(_rigidBody); _physicsEngine->getDynamicsWorld()->removeAction(this); } _inWorld = false; } _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_ADD_TO_SIMULATION; }} bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) { bool pushing = _targetVelocity.length2() > FLT_EPSILON; btDispatcher* dispatcher = collisionWorld->getDispatcher(); int numManifolds = dispatcher->getNumManifolds(); bool hasFloor = 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" btManifoldPoint& contact = contactManifold->getContactPoint(j); 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); 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 (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 stepValid = false; } else if (hitHeight > highestStep && normal.dot(_targetVelocity) < 0.0f ) { highestStep = hitHeight; stepContactIndex = j; hasFloor = true; } } } 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 _stepNormal = characterIsFirst ? contact.m_normalWorldOnB : -contact.m_normalWorldOnB; // points toward character _stepHeight = highestStep; _stepPoint = rotation * pointOnCharacter; // rotate into world-frame } } } // 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;} void CharacterController::updateAction(btCollisionWorld* collisionWorld, btScalar deltaTime) { preStep(collisionWorld); playerStep(collisionWorld, deltaTime);} void CharacterController::preStep(btCollisionWorld* collisionWorld) { // trace a ray straight down to see if we're standing on the ground const btTransform& transform = _rigidBody->getWorldTransform(); // rayStart is at center of bottom sphere btVector3 rayStart = transform.getOrigin() - _halfHeight * _currentUp; // rayEnd is some short distance outside bottom sphere const btScalar FLOOR_PROXIMITY_THRESHOLD = 0.3f * _radius; btScalar rayLength = _radius + FLOOR_PROXIMITY_THRESHOLD; btVector3 rayEnd = rayStart - rayLength * _currentUp; // scan down for nearby floor ClosestNotMe rayCallback(_rigidBody); rayCallback.m_closestHitFraction = 1.0f; collisionWorld->rayTest(rayStart, rayEnd, rayCallback); if (rayCallback.hasHit()) { _floorDistance = rayLength * rayCallback.m_closestHitFraction - _radius; }} const btScalar MIN_TARGET_SPEED = 0.001f;const btScalar MIN_TARGET_SPEED_SQUARED = MIN_TARGET_SPEED * MIN_TARGET_SPEED; void CharacterController::playerStep(btCollisionWorld* collisionWorld, btScalar dt) { _stepHeight = _minStepHeight; // clears memory of last step obstacle _hasSupport = checkForSupport(collisionWorld); btVector3 velocity = _rigidBody->getLinearVelocity() - _parentVelocity; computeNewVelocity(dt, velocity); const float MINIMUM_TIME_REMAINING = 0.005f; const float MAX_DISPLACEMENT = 0.5f * _radius; _followTimeRemaining -= dt; if (_followTimeRemaining >= MINIMUM_TIME_REMAINING) { btTransform bodyTransform = _rigidBody->getWorldTransform(); btVector3 startPos = bodyTransform.getOrigin(); btVector3 deltaPos = _followDesiredBodyTransform.getOrigin() - startPos; btVector3 vel = deltaPos / _followTimeRemaining; btVector3 linearDisplacement = clampLength(vel * dt, MAX_DISPLACEMENT); // clamp displacement to prevent tunneling. btVector3 endPos = startPos + linearDisplacement; // resolve the simple linearDisplacement _followLinearDisplacement += linearDisplacement; // now for the rotational part... btQuaternion startRot = bodyTransform.getRotation(); btQuaternion desiredRot = _followDesiredBodyTransform.getRotation(); // startRot as default rotation btQuaternion endRot = startRot; // the dot product between two quaternions is equal to +/- cos(angle/2) // where 'angle' is that of the rotation between them float qDot = desiredRot.dot(startRot); // when the abs() value of the dot product is approximately 1.0 // then the two rotations are effectively adjacent const float MIN_DOT_PRODUCT_OF_ADJACENT_QUATERNIONS = 0.99999f; // corresponds to approx 0.5 degrees if (fabsf(qDot) < MIN_DOT_PRODUCT_OF_ADJACENT_QUATERNIONS) { if (qDot < 0.0f) { // the quaternions are actually on opposite hyperhemispheres // so we move one to agree with the other and negate qDot desiredRot = -desiredRot; qDot = -qDot; } btQuaternion deltaRot = desiredRot * startRot.inverse(); // the axis is the imaginary part, but scaled by sin(angle/2) btVector3 axis(deltaRot.getX(), deltaRot.getY(), deltaRot.getZ()); axis /= sqrtf(1.0f - qDot * qDot); // compute the angle we will resolve for this dt, but don't overshoot float angle = 2.0f * acosf(qDot); if (dt < _followTimeRemaining) { angle *= dt / _followTimeRemaining; } // accumulate rotation deltaRot = btQuaternion(axis, angle); _followAngularDisplacement = (deltaRot * _followAngularDisplacement).normalize(); // in order to accumulate displacement of avatar position, we need to take _shapeLocalOffset into account. btVector3 shapeLocalOffset = glmToBullet(_shapeLocalOffset); endRot = deltaRot * startRot; btVector3 swingDisplacement = rotateVector(endRot, -shapeLocalOffset) - rotateVector(startRot, -shapeLocalOffset); _followLinearDisplacement += swingDisplacement; } _rigidBody->setWorldTransform(btTransform(endRot, endPos)); } _followTime += dt; if (_steppingUp) { float horizontalTargetSpeed = (_targetVelocity - _targetVelocity.dot(_currentUp) * _currentUp).length(); if (horizontalTargetSpeed > FLT_EPSILON) { // compute a stepUpSpeed that will reach the top of the step in the time it would take // to move over the _stepPoint at target speed float horizontalDistance = (_stepPoint - _stepPoint.dot(_currentUp) * _currentUp).length(); float timeToStep = horizontalDistance / horizontalTargetSpeed; float stepUpSpeed = _stepHeight / timeToStep; // magically clamp stepUpSpeed to a fraction of horizontalTargetSpeed // to prevent the avatar from moving unreasonably fast according to human eye const float MAX_STEP_UP_SPEED = 0.65f * horizontalTargetSpeed; if (stepUpSpeed > MAX_STEP_UP_SPEED) { stepUpSpeed = MAX_STEP_UP_SPEED; } // add minimum velocity to counteract gravity's displacement during one step // Note: the 0.5 factor comes from the fact that we really want the // average velocity contribution from gravity during the step stepUpSpeed -= 0.5f * _currentGravity * timeToStep; // remember: _gravity is negative scalar btScalar vDotUp = velocity.dot(_currentUp); if (vDotUp < stepUpSpeed) { // character doesn't have enough upward velocity to cover the step so we help using a "sky hook"Lines 1–500 of 1046. Load more
// // CharacterControllerInterface.cpp // libraries/physics/src // // Created by Andrew Meadows 2015.10.21 // Copyright 2015 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 "CharacterController.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) #else #define SET_STATE(desiredState, reason) setState(desiredState) #endif // helper class for simple ray-traces from character class ClosestNotMe : public btCollisionWorld::ClosestRayResultCallback { public: ClosestNotMe(btRigidBody* me) : btCollisionWorld::ClosestRayResultCallback(btVector3(0.0f, 0.0f, 0.0f), btVector3(0.0f, 0.0f, 0.0f)) { _me = me; // the RayResultCallback's group and mask must match MY_AVATAR m_collisionFilterGroup = BULLET_COLLISION_GROUP_MY_AVATAR; m_collisionFilterMask = BULLET_COLLISION_MASK_MY_AVATAR; } virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult,bool normalInWorldSpace) override { if (rayResult.m_collisionObject == _me) { return 1.0f; } return ClosestRayResultCallback::addSingleResult(rayResult, normalInWorldSpace); } protected: btRigidBody* _me; }; CharacterController::CharacterMotor::CharacterMotor(const glm::vec3& vel, const glm::quat& rot, float horizTimescale, float vertTimescale) { velocity = glmToBullet(vel); rotation = glmToBullet(rot); hTimescale = horizTimescale; if (hTimescale < MIN_CHARACTER_MOTOR_TIMESCALE) { hTimescale = MIN_CHARACTER_MOTOR_TIMESCALE; } vTimescale = vertTimescale; if (vTimescale < 0.0f) { vTimescale = hTimescale; } else if (vTimescale < MIN_CHARACTER_MOTOR_TIMESCALE) { vTimescale = MIN_CHARACTER_MOTOR_TIMESCALE; } } CharacterController::CharacterController() { _floorDistance = _scaleFactor * DEFAULT_AVATAR_FALL_HEIGHT; _targetVelocity.setValue(0.0f, 0.0f, 0.0f); _followDesiredBodyTransform.setIdentity(); _followTimeRemaining = 0.0f; _state = State::Hover; _isPushingUp = false; _rayHitStartTime = 0; _takeoffToInAirStartTime = 0; _jumpButtonDownStartTime = 0; _jumpButtonDownCount = 0; _followTime = 0.0f; _followLinearDisplacement = btVector3(0, 0, 0); _followAngularDisplacement = btQuaternion::getIdentity(); _hasSupport = false; _pendingFlags = PENDING_FLAG_UPDATE_SHAPE; } CharacterController::~CharacterController() { if (_rigidBody) { btCollisionShape* shape = _rigidBody->getCollisionShape(); if (shape) { delete shape; } delete _rigidBody; _rigidBody = nullptr; } } bool CharacterController::needsRemoval() const { return (_physicsEngine && (_pendingFlags & PENDING_FLAG_REMOVE_FROM_SIMULATION) == PENDING_FLAG_REMOVE_FROM_SIMULATION); } bool CharacterController::needsAddition() const { return (_physicsEngine && (_pendingFlags & PENDING_FLAG_ADD_TO_SIMULATION) == PENDING_FLAG_ADD_TO_SIMULATION); } void CharacterController::removeFromWorld() { if (_inWorld) { if (_rigidBody) { _physicsEngine->getDynamicsWorld()->removeRigidBody(_rigidBody); _physicsEngine->getDynamicsWorld()->removeAction(this); } _inWorld = false; } _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_ADD_TO_SIMULATION; } } bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) { bool pushing = _targetVelocity.length2() > FLT_EPSILON; btDispatcher* dispatcher = collisionWorld->getDispatcher(); int numManifolds = dispatcher->getNumManifolds(); bool hasFloor = 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" btManifoldPoint& contact = contactManifold->getContactPoint(j); 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); 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 (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 stepValid = false; } else if (hitHeight > highestStep && normal.dot(_targetVelocity) < 0.0f ) { highestStep = hitHeight; stepContactIndex = j; hasFloor = true; } } } 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 _stepNormal = characterIsFirst ? contact.m_normalWorldOnB : -contact.m_normalWorldOnB; // points toward character _stepHeight = highestStep; _stepPoint = rotation * pointOnCharacter; // rotate into world-frame } } } // 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; } void CharacterController::updateAction(btCollisionWorld* collisionWorld, btScalar deltaTime) { preStep(collisionWorld); playerStep(collisionWorld, deltaTime); } void CharacterController::preStep(btCollisionWorld* collisionWorld) { // trace a ray straight down to see if we're standing on the ground const btTransform& transform = _rigidBody->getWorldTransform(); // rayStart is at center of bottom sphere btVector3 rayStart = transform.getOrigin() - _halfHeight * _currentUp; // rayEnd is some short distance outside bottom sphere const btScalar FLOOR_PROXIMITY_THRESHOLD = 0.3f * _radius; btScalar rayLength = _radius + FLOOR_PROXIMITY_THRESHOLD; btVector3 rayEnd = rayStart - rayLength * _currentUp; // scan down for nearby floor ClosestNotMe rayCallback(_rigidBody); rayCallback.m_closestHitFraction = 1.0f; collisionWorld->rayTest(rayStart, rayEnd, rayCallback); if (rayCallback.hasHit()) { _floorDistance = rayLength * rayCallback.m_closestHitFraction - _radius; } } const btScalar MIN_TARGET_SPEED = 0.001f; const btScalar MIN_TARGET_SPEED_SQUARED = MIN_TARGET_SPEED * MIN_TARGET_SPEED; void CharacterController::playerStep(btCollisionWorld* collisionWorld, btScalar dt) { _stepHeight = _minStepHeight; // clears memory of last step obstacle _hasSupport = checkForSupport(collisionWorld); btVector3 velocity = _rigidBody->getLinearVelocity() - _parentVelocity; computeNewVelocity(dt, velocity); const float MINIMUM_TIME_REMAINING = 0.005f; const float MAX_DISPLACEMENT = 0.5f * _radius; _followTimeRemaining -= dt; if (_followTimeRemaining >= MINIMUM_TIME_REMAINING) { btTransform bodyTransform = _rigidBody->getWorldTransform(); btVector3 startPos = bodyTransform.getOrigin(); btVector3 deltaPos = _followDesiredBodyTransform.getOrigin() - startPos; btVector3 vel = deltaPos / _followTimeRemaining; btVector3 linearDisplacement = clampLength(vel * dt, MAX_DISPLACEMENT); // clamp displacement to prevent tunneling. btVector3 endPos = startPos + linearDisplacement; // resolve the simple linearDisplacement _followLinearDisplacement += linearDisplacement; // now for the rotational part... btQuaternion startRot = bodyTransform.getRotation(); btQuaternion desiredRot = _followDesiredBodyTransform.getRotation(); // startRot as default rotation btQuaternion endRot = startRot; // the dot product between two quaternions is equal to +/- cos(angle/2) // where 'angle' is that of the rotation between them float qDot = desiredRot.dot(startRot); // when the abs() value of the dot product is approximately 1.0 // then the two rotations are effectively adjacent const float MIN_DOT_PRODUCT_OF_ADJACENT_QUATERNIONS = 0.99999f; // corresponds to approx 0.5 degrees if (fabsf(qDot) < MIN_DOT_PRODUCT_OF_ADJACENT_QUATERNIONS) { if (qDot < 0.0f) { // the quaternions are actually on opposite hyperhemispheres // so we move one to agree with the other and negate qDot desiredRot = -desiredRot; qDot = -qDot; } btQuaternion deltaRot = desiredRot * startRot.inverse(); // the axis is the imaginary part, but scaled by sin(angle/2) btVector3 axis(deltaRot.getX(), deltaRot.getY(), deltaRot.getZ()); axis /= sqrtf(1.0f - qDot * qDot); // compute the angle we will resolve for this dt, but don't overshoot float angle = 2.0f * acosf(qDot); if (dt < _followTimeRemaining) { angle *= dt / _followTimeRemaining; } // accumulate rotation deltaRot = btQuaternion(axis, angle); _followAngularDisplacement = (deltaRot * _followAngularDisplacement).normalize(); // in order to accumulate displacement of avatar position, we need to take _shapeLocalOffset into account. btVector3 shapeLocalOffset = glmToBullet(_shapeLocalOffset); endRot = deltaRot * startRot; btVector3 swingDisplacement = rotateVector(endRot, -shapeLocalOffset) - rotateVector(startRot, -shapeLocalOffset); _followLinearDisplacement += swingDisplacement; } _rigidBody->setWorldTransform(btTransform(endRot, endPos)); } _followTime += dt; if (_steppingUp) { float horizontalTargetSpeed = (_targetVelocity - _targetVelocity.dot(_currentUp) * _currentUp).length(); if (horizontalTargetSpeed > FLT_EPSILON) { // compute a stepUpSpeed that will reach the top of the step in the time it would take // to move over the _stepPoint at target speed float horizontalDistance = (_stepPoint - _stepPoint.dot(_currentUp) * _currentUp).length(); float timeToStep = horizontalDistance / horizontalTargetSpeed; float stepUpSpeed = _stepHeight / timeToStep; // magically clamp stepUpSpeed to a fraction of horizontalTargetSpeed // to prevent the avatar from moving unreasonably fast according to human eye const float MAX_STEP_UP_SPEED = 0.65f * horizontalTargetSpeed; if (stepUpSpeed > MAX_STEP_UP_SPEED) { stepUpSpeed = MAX_STEP_UP_SPEED; } // add minimum velocity to counteract gravity's displacement during one step // Note: the 0.5 factor comes from the fact that we really want the // average velocity contribution from gravity during the step stepUpSpeed -= 0.5f * _currentGravity * timeToStep; // remember: _gravity is negative scalar btScalar vDotUp = velocity.dot(_currentUp); if (vDotUp < stepUpSpeed) { // character doesn't have enough upward velocity to cover the step so we help using a "sky hook"