////  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 <NumericalConstants.h> #include "ObjectMotionState.h"#include "PhysicsHelpers.h"#include "PhysicsLogging.h" const btVector3 LOCAL_UP_AXIS(0.0f, 1.0f, 0.0f);const float JUMP_SPEED = 3.5f;const float MAX_FALL_HEIGHT = 20.0f; #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 = MAX_FALL_HEIGHT;     _targetVelocity.setValue(0.0f, 0.0f, 0.0f);    _followDesiredBodyTransform.setIdentity();    _followTimeRemaining = 0.0f;    _jumpSpeed = JUMP_SPEED;    _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 ((_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);} void CharacterController::setDynamicsWorld(btDynamicsWorld* world) {    if (_dynamicsWorld != world) {        // remove from old world        if (_dynamicsWorld) {            if (_rigidBody) {                _dynamicsWorld->removeRigidBody(_rigidBody);                _dynamicsWorld->removeAction(this);            }            _dynamicsWorld = nullptr;        }        int16_t collisionGroup = computeCollisionGroup();        if (_rigidBody) {            updateMassProperties();        }        if (world && _rigidBody) {            // add to new world            _dynamicsWorld = world;            _pendingFlags &= ~PENDING_FLAG_JUMP;            _dynamicsWorld->addRigidBody(_rigidBody, collisionGroup, BULLET_COLLISION_MASK_MY_AVATAR);            _dynamicsWorld->addAction(this);            // restore gravity settings because adding an object to the world overwrites its gravity setting            _rigidBody->setGravity(_gravity * _currentUp);            btCollisionShape* shape = _rigidBody->getCollisionShape();            assert(shape && shape->getShapeType() == CONVEX_HULL_SHAPE_PROXYTYPE);            _ghost.setCharacterShape(static_cast<btConvexHullShape*>(shape));        }        _ghost.setCollisionGroupAndMask(collisionGroup, BULLET_COLLISION_MASK_MY_AVATAR & (~ collisionGroup));        _ghost.setCollisionWorld(_dynamicsWorld);        _ghost.setRadiusAndHalfHeight(_radius, _halfHeight);        if (_rigidBody) {            _ghost.setWorldTransform(_rigidBody->getWorldTransform());        }    }    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;        } else {            _pendingFlags &= ~PENDING_FLAG_ADD_TO_SIMULATION;        }    } else {        _pendingFlags &= ~PENDING_FLAG_REMOVE_FROM_SIMULATION;    }} bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) {    bool pushing = _targetVelocity.length2() > FLT_EPSILON;     btDispatcher* dispatcher = collisionWorld->getDispatcher();    int numManifolds = dispatcher->getNumManifolds();    bool hasFloor = false;     btTransform rotation = _rigidBody->getWorldTransform();    rotation.setOrigin(btVector3(0.0f, 0.0f, 0.0f)); // clear translation part     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;            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);                if (hitHeight < _maxStepHeight && normal.dot(_currentUp) > _minFloorNormalDotUp) {                    hasFloor = true;                    if (!pushing) {                        // we're not pushing against anything so we can early exit                        // (all we need to know is that there is a floor)                        break;                    }                }                if (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;                    } else if (hitHeight > highestStep && normal.dot(_targetVelocity) < 0.0f ) {                        highestStep = hitHeight;                        stepContactIndex = j;                        hasFloor = true;                    }                }            }            if (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 (hasFloor && !(pushing && _stepUpEnabled)) {                // early exit since all we need to know is that we're on a floor                break;            }        }    }    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;         btQuaternion startRot = bodyTransform.getRotation();        glm::vec2 currentFacing = getFacingDir2D(bulletToGLM(startRot));        glm::vec2 currentRight(currentFacing.y, -currentFacing.x);        glm::vec2 desiredFacing = getFacingDir2D(bulletToGLM(_followDesiredBodyTransform.getRotation()));        float deltaAngle = acosf(glm::clamp(glm::dot(currentFacing, desiredFacing), -1.0f, 1.0f));        float angularSpeed = deltaAngle / _followTimeRemaining;        float sign = copysignf(1.0f, glm::dot(desiredFacing, currentRight));        btQuaternion angularDisplacement = btQuaternion(btVector3(0.0f, 1.0f, 0.0f), sign * angularSpeed * dt);        btQuaternion endRot = angularDisplacement * startRot;         // in order to accumulate displacement of avatar position, we need to take _shapeLocalOffset into account.        btVector3 shapeLocalOffset = glmToBullet(_shapeLocalOffset);        btVector3 swingDisplacement = rotateVector(endRot, -shapeLocalOffset) - rotateVector(startRot, -shapeLocalOffset);         _followLinearDisplacement = linearDisplacement + swingDisplacement + _followLinearDisplacement;        _followAngularDisplacement = angularDisplacement * _followAngularDisplacement;         _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 * _gravity * 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() {    _pendingFlags |= PENDING_FLAG_JUMP;} bool CharacterController::onGround() const {    const btScalar FLOOR_PROXIMITY_THRESHOLD = 0.3f * _radius;    return _floorDistance < FLOOR_PROXIMITY_THRESHOLD || _hasSupport;} #ifdef DEBUG_STATE_CHANGEstatic const char* stateToStr(CharacterController::State state) {    switch (state) {    case CharacterController::State::Ground:        return "Ground";    case CharacterController::State::Takeoff:        return "Takeoff";    case CharacterController::State::InAir:        return "InAir";    case CharacterController::State::Hover:        return "Hover";    default:        return "Unknown";    }}#endif // #ifdef DEBUG_STATE_CHANGE #ifdef DEBUG_STATE_CHANGEvoid CharacterController::setState(State desiredState, const char* reason) {#elsevoid CharacterController::setState(State desiredState) {#endif    if (!_flyingAllowed && desiredState == State::Hover) {        desiredState = State::InAir;    }     if (desiredState != _state) {#ifdef DEBUG_STATE_CHANGE        qCDebug(physics) << "CharacterController::setState" << stateToStr(desiredState) << "from" << stateToStr(_state) << "," << reason;#endif        _state = desiredState;        updateGravity();    }} void CharacterController::updateGravity() {    int16_t collisionGroup = computeCollisionGroup();    if (_state == State::Hover || collisionGroup == BULLET_COLLISION_GROUP_COLLISIONLESS) {        _gravity = 0.0f;    } else {        const float DEFAULT_CHARACTER_GRAVITY = -5.0f;        _gravity = DEFAULT_CHARACTER_GRAVITY;    }    if (_rigidBody) {        _rigidBody->setGravity(_gravity * _currentUp);    }} void CharacterController::setLocalBoundingBox(const glm::vec3& minCorner, const glm::vec3& scale) {    float x = scale.x;    float z = scale.z;    float radius = 0.5f * sqrtf(0.5f * (x * x + z * z));    float halfHeight = 0.5f * scale.y - radius;    float MIN_HALF_HEIGHT = 0.1f;    if (halfHeight < MIN_HALF_HEIGHT) {        halfHeight = MIN_HALF_HEIGHT;    }     // compare dimensions    if (glm::abs(radius - _radius) > FLT_EPSILON || glm::abs(halfHeight - _halfHeight) > FLT_EPSILON) {        _radius = radius;        _halfHeight = halfHeight;        const btScalar DEFAULT_MIN_STEP_HEIGHT_FACTOR = 0.005f;        const btScalar DEFAULT_MAX_STEP_HEIGHT_FACTOR = 0.65f;        _minStepHeight = DEFAULT_MIN_STEP_HEIGHT_FACTOR * (_halfHeight + _radius);        _maxStepHeight = DEFAULT_MAX_STEP_HEIGHT_FACTOR * (_halfHeight + _radius);         if (_dynamicsWorld) {            // must REMOVE from world prior to shape update            _pendingFlags |= PENDING_FLAG_REMOVE_FROM_SIMULATION;        }        _pendingFlags |= PENDING_FLAG_UPDATE_SHAPE;        _pendingFlags |= PENDING_FLAG_ADD_TO_SIMULATION;    }     // it's ok to change offset immediately -- there are no thread safety issues here    _shapeLocalOffset = minCorner + 0.5f * scale;} void CharacterController::setCollisionless(bool collisionless) {    if (collisionless != _collisionless) {        _collisionless = collisionless;        _pendingFlags |= PENDING_FLAG_UPDATE_COLLISION_GROUP;    }} int16_t CharacterController::computeCollisionGroup() const {    if (_collisionless) {        return _collisionlessAllowed ? BULLET_COLLISION_GROUP_COLLISIONLESS : BULLET_COLLISION_GROUP_MY_AVATAR;    } else {        return BULLET_COLLISION_GROUP_MY_AVATAR;    }} void CharacterController::handleChangedCollisionGroup() {    if (_pendingFlags & PENDING_FLAG_UPDATE_COLLISION_GROUP) {        // ATM the easiest way to update collision groups is to remove/re-add the RigidBody        if (_dynamicsWorld) {            _dynamicsWorld->removeRigidBody(_rigidBody);            int16_t collisionGroup = computeCollisionGroup();            _dynamicsWorld->addRigidBody(_rigidBody, collisionGroup, BULLET_COLLISION_MASK_MY_AVATAR);        }        _pendingFlags &= ~PENDING_FLAG_UPDATE_COLLISION_GROUP;        updateGravity();    }} void CharacterController::updateUpAxis(const glm::quat& rotation) {    _currentUp = quatRotate(glmToBullet(rotation), LOCAL_UP_AXIS);    if (_state != State::Hover && _rigidBody) {        _rigidBody->setGravity(_gravity * _currentUp);    }} void CharacterController::setPositionAndOrientation(        const glm::vec3& position,        const glm::quat& orientation) {    updateUpAxis(orientation);    _rotation = glmToBullet(orientation);    _position = glmToBullet(position + orientation * _shapeLocalOffset);} void CharacterController::getPositionAndOrientation(glm::vec3& position, glm::quat& rotation) const {    if (_rigidBody) {        const btTransform& avatarTransform = _rigidBody->getWorldTransform();        rotation = bulletToGLM(avatarTransform.getRotation());        position = bulletToGLM(avatarTransform.getOrigin()) - rotation * _shapeLocalOffset;    }} void CharacterController::setParentVelocity(const glm::vec3& velocity) {    _parentVelocity = glmToBullet(velocity);} void CharacterController::setFollowParameters(const glm::mat4& desiredWorldBodyMatrix, float timeRemaining) {    _followTimeRemaining = timeRemaining;    _followDesiredBodyTransform = glmToBullet(desiredWorldBodyMatrix) * btTransform(btQuaternion::getIdentity(), glmToBullet(_shapeLocalOffset));} glm::vec3 CharacterController::getFollowLinearDisplacement() const {    return bulletToGLM(_followLinearDisplacement);} glm::quat CharacterController::getFollowAngularDisplacement() const {    return bulletToGLM(_followAngularDisplacement);} glm::vec3 CharacterController::getFollowVelocity() const {    if (_followTime > 0.0f) {        return bulletToGLM(_followLinearDisplacement) / _followTime;    } else {        return glm::vec3();    }} glm::vec3 CharacterController::getLinearVelocity() const {    glm::vec3 velocity(0.0f);    if (_rigidBody) {        velocity = bulletToGLM(_rigidBody->getLinearVelocity());    }    return velocity;} glm::vec3 CharacterController::getVelocityChange() const {    if (_rigidBody) {        return bulletToGLM(_velocityChange);    }    return glm::vec3(0.0f);} void CharacterController::clearMotors() {    _motors.clear();

Lines 1–500 of 805. Load more