1046 lines · 44.2 KB · C++
RawDownload
////  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.

Keyboard shortcuts

General

Show keyboard shortcuts
?
Focus the current search
/

Issues and pull requests

Open filters
f

Issues, pull requests and Inbox

Select next item
j↓
Select previous item
k↑
Open selected item
Enter
Dismiss filters or clear selection
Esc

Repository code

Go to file
t