////  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"#include "TemporaryPairwiseCollisionFilter.h" const float STUCK_PENETRATION = -0.05f; // always negative into the object.const float STUCK_IMPULSE = 500.0f;  const btVector3 LOCAL_UP_AXIS(0.0f, 1.0f, 0.0f);static bool _appliedStuckRecoveryStrategy = false; static TemporaryPairwiseCollisionFilter _pairwiseFilter; // Note: applyPairwiseFilter is registered as a sub-callback to Bullet's gContactAddedCallback feature// when we detect MyAvatar is "stuck".  It will disable new ManifoldPoints between MyAvatar and mesh objects with// which it has deep penetration, and will continue disabling new contact until new contacts stop happening// (no overlap).  If MyAvatar is not trying to move its velocity is defaulted to "up", to help it escape overlap.bool applyPairwiseFilter(btManifoldPoint& cp,        const btCollisionObjectWrapper* colObj0Wrap, int partId0, int index0,        const btCollisionObjectWrapper* colObj1Wrap, int partId1, int index1) {     // This callback is ONLY called on objects with btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK flag    // and the flagged object will always be sorted to Obj0.  Hence the "other" is always Obj1.    const btCollisionObject* other = colObj1Wrap->m_collisionObject;     if (_pairwiseFilter.isFiltered(other)) {        _pairwiseFilter.incrementEntry(other);        // disable contact point by setting distance too large and normal to zero        cp.setDistance(1.0e6f);        cp.m_normalWorldOnB.setValue(0.0f, 0.0f, 0.0f);        _appliedStuckRecoveryStrategy = true;        return false;    }     if (other->getCollisionShape()->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) {        if ( cp.getDistance() < 2.0f * STUCK_PENETRATION ||                cp.getAppliedImpulse() > 2.0f * STUCK_IMPULSE ||                (cp.getDistance() < STUCK_PENETRATION && cp.getAppliedImpulse() > STUCK_IMPULSE)) {            _pairwiseFilter.incrementEntry(other);            // disable contact point by setting distance too large and normal to zero            cp.setDistance(1.0e6f);            cp.m_normalWorldOnB.setValue(0.0f, 0.0f, 0.0f);            _appliedStuckRecoveryStrategy = true;        }    }    return false;} #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;    }} static uint32_t _numCharacterControllers { 0 }; CharacterController::CharacterController(const FollowTimePerType& followTimeRemainingPerType) :    _followTimeRemainingPerType(followTimeRemainingPerType) {    _floorDistance = _scaleFactor * DEFAULT_AVATAR_FALL_HEIGHT;     _targetVelocity.setValue(0.0f, 0.0f, 0.0f);    _followDesiredBodyTransform.setIdentity();    _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;     // ATM CharacterController is a singleton.  When we want more we'll have to    // overhaul the applyPairwiseFilter() logic to handle multiple instances.    ++_numCharacterControllers;    assert(_numCharacterControllers == 1);} CharacterController::~CharacterController() {    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(_halfHeight);    _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 && _appliedStuckRecoveryStrategy;     btTransform rotation = _rigidBody->getWorldTransform();    rotation.setOrigin(btVector3(0.0f, 0.0f, 0.0f)); // clear translation part     float deepestDistance = 0.0f;    float strongestImpulse = 0.0f;     _netCollisionImpulse = btVector3(0.0f, 0.0f, 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();                _netCollisionImpulse += impulse * normal;                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.    probablyStuck = probablyStuck        || deepestDistance < 2.0f * STUCK_PENETRATION        || strongestImpulse > 2.0f * STUCK_IMPULSE        || (deepestDistance < STUCK_PENETRATION && strongestImpulse > STUCK_IMPULSE);     if (_isStuck != probablyStuck) {        ++_stuckTransitionCount;        if (_stuckTransitionCount > NUM_SUBSTEPS_FOR_STUCK_TRANSITION) {            // we've been in this "probablyStuck" state for several consecutive substeps            // --> make it official by changing state            qCDebug(physics) << "CharacterController::_isStuck :" << _isStuck << "-->" << probablyStuck;            _isStuck = probablyStuck;            // init _numStuckSubsteps at NUM_SUBSTEPS_FOR_SAFE_LANDING_RETRY so SafeLanding tries to help immediately            _numStuckSubsteps = NUM_SUBSTEPS_FOR_SAFE_LANDING_RETRY;            _stuckTransitionCount = 0;            if (_isStuck) {                // enable pairwise filter                _physicsEngine->setContactAddedCallback(applyPairwiseFilter);                _pairwiseFilter.incrementStepCount();            } else {                // disable pairwise filter                _physicsEngine->setContactAddedCallback(nullptr);                _appliedStuckRecoveryStrategy = false;                _pairwiseFilter.clearAllEntries();            }            updateCurrentGravity();        }    } else {        _stuckTransitionCount = 0;        if (_isStuck) {            ++_numStuckSubsteps;            _appliedStuckRecoveryStrategy = 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);     constexpr float MINIMUM_TIME_REMAINING = 0.005f;    static_assert(FOLLOW_TIME_IMMEDIATE_SNAP > MINIMUM_TIME_REMAINING, "The code below assumes this condition is true.");     bool hasFollowTimeRemaining = false;    for (float followTime : _followTimeRemainingPerType) {        if (followTime > MINIMUM_TIME_REMAINING) {            hasFollowTimeRemaining = true;            break;        }    }     if (hasFollowTimeRemaining) {        const float MAX_DISPLACEMENT = 0.5f * _radius;         btTransform bodyTransform = _rigidBody->getWorldTransform();        btVector3 startPos = bodyTransform.getOrigin();        btVector3 deltaPos = _followDesiredBodyTransform.getOrigin() - startPos;         btVector3 linearDisplacement(0.0f, 0.0f, 0.0f);        {            float horizontalTime = _followTimeRemainingPerType[static_cast<uint>(FollowType::Horizontal)];            float verticalTime = _followTimeRemainingPerType[static_cast<uint>(FollowType::Vertical)];             if (horizontalTime == FOLLOW_TIME_IMMEDIATE_SNAP) {                linearDisplacement.setX(deltaPos.x());                linearDisplacement.setZ(deltaPos.z());            } else if (horizontalTime > MINIMUM_TIME_REMAINING) {                linearDisplacement.setX((deltaPos.x() * dt) / horizontalTime);                linearDisplacement.setZ((deltaPos.z() * dt) / horizontalTime);            }             if (verticalTime == FOLLOW_TIME_IMMEDIATE_SNAP) {                linearDisplacement.setY(deltaPos.y());            } else if (verticalTime > MINIMUM_TIME_REMAINING) {                linearDisplacement.setY((deltaPos.y() * dt) / verticalTime);            }             linearDisplacement = clampLength(linearDisplacement, 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();         // startRot as default rotation        btQuaternion endRot = startRot;         float rotationTime = _followTimeRemainingPerType[static_cast<uint>(FollowType::Rotation)];        if (rotationTime > MINIMUM_TIME_REMAINING) {            btQuaternion desiredRot = _followDesiredBodyTransform.getRotation();             // 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 (rotationTime != FOLLOW_TIME_IMMEDIATE_SNAP) {                    if (dt < rotationTime) {                        angle *= dt / rotationTime;                    }                }                 // 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"                // which uses micro-teleports rather than applying real velocity                // to prevent the avatar from popping up after the step is done                btTransform transform = _rigidBody->getWorldTransform();                transform.setOrigin(transform.getOrigin() + (dt * stepUpSpeed) * _currentUp);                _rigidBody->setWorldTransform(transform);            }             // don't allow the avatar to fall downward when stepping up            // since otherwise this would tend to defeat the step-up behavior            if (vDotUp < 0.0f) {                velocity -= vDotUp * _currentUp;            }        }    }    _rigidBody->setLinearVelocity(velocity + _parentVelocity);    _ghost.setWorldTransform(_rigidBody->getWorldTransform());} void CharacterController::jump(const btVector3& dir) {    _pendingFlags |= PENDING_FLAG_JUMP;} bool CharacterController::onGround() const {

Lines 1–500 of 1125. Load more