////  CharacterControllerInterface.cpp//  libraries/physcis/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 "PhysicsCollisionGroups.h"#include "ObjectMotionState.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) {        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() {   _halfHeight = 1.0f;     _enabled = false;     _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) {        if (_dynamicsWorld) {            if (_rigidBody) {                _dynamicsWorld->removeRigidBody(_rigidBody);                _dynamicsWorld->removeAction(this);            }            _dynamicsWorld = nullptr;        }        if (world && _rigidBody) {            _dynamicsWorld = world;            _pendingFlags &= ~PENDING_FLAG_JUMP;            // Before adding the RigidBody to the world we must save its oldGravity to the side            // because adding an object to the world will overwrite it with the default gravity.            btVector3 oldGravity = _rigidBody->getGravity();            _dynamicsWorld->addRigidBody(_rigidBody, BULLET_COLLISION_GROUP_MY_AVATAR, BULLET_COLLISION_MASK_MY_AVATAR);            _dynamicsWorld->addAction(this);            // restore gravity settings            _rigidBody->setGravity(oldGravity);        }    }    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;    }} static const float COS_PI_OVER_THREE = cosf(PI / 3.0f); bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) const {    int numManifolds = collisionWorld->getDispatcher()->getNumManifolds();    for (int i = 0; i < numManifolds; i++) {        btPersistentManifold* contactManifold = collisionWorld->getDispatcher()->getManifoldByIndexInternal(i);        const btCollisionObject* obA = static_cast<const btCollisionObject*>(contactManifold->getBody0());        const btCollisionObject* obB = static_cast<const btCollisionObject*>(contactManifold->getBody1());        if (obA == _rigidBody || obB == _rigidBody) {            int numContacts = contactManifold->getNumContacts();            for (int j = 0; j < numContacts; j++) {                btManifoldPoint& pt = contactManifold->getContactPoint(j);                 // check to see if contact point is touching the bottom sphere of the capsule.                // and the contact normal is not slanted too much.                float contactPointY = (obA == _rigidBody) ? pt.m_localPointA.getY() : pt.m_localPointB.getY();                btVector3 normal = (obA == _rigidBody) ? pt.m_normalWorldOnB : -pt.m_normalWorldOnB;                if (contactPointY < -_halfHeight && normal.dot(_currentUp) > COS_PI_OVER_THREE) {                    return true;                }            }        }    }    return false;} void CharacterController::preStep(btCollisionWorld* collisionWorld) {    // trace a ray straight down to see if we're standing on the ground    const btTransform& xform = _rigidBody->getWorldTransform();     // rayStart is at center of bottom sphere    btVector3 rayStart = xform.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;    }     _hasSupport = checkForSupport(collisionWorld);} const btScalar MIN_TARGET_SPEED = 0.001f;const btScalar MIN_TARGET_SPEED_SQUARED = MIN_TARGET_SPEED * MIN_TARGET_SPEED; void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) {    btVector3 velocity = _rigidBody->getLinearVelocity() - _parentVelocity;    computeNewVelocity(dt, velocity);    _rigidBody->setLinearVelocity(velocity + _parentVelocity);     // Dynamicaly compute a follow velocity to move this body toward the _followDesiredBodyTransform.    // Rather than add this velocity to velocity the RigidBody, we explicitly teleport the RigidBody towards its goal.    // This mirrors the computation done in MyAvatar::FollowHelper::postPhysicsUpdate().     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;} 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        if (desiredState == State::Hover && _state != State::Hover) {            // hover enter            if (_rigidBody) {                _rigidBody->setGravity(btVector3(0.0f, 0.0f, 0.0f));            }        } else if (_state == State::Hover && desiredState != State::Hover) {            // hover exit            if (_rigidBody) {                _rigidBody->setGravity(DEFAULT_CHARACTER_GRAVITY * _currentUp);            }        }        _state = desiredState;    }} void CharacterController::setLocalBoundingBox(const glm::vec3& corner, const glm::vec3& scale) {    _boxScale = scale;     float x = _boxScale.x;    float z = _boxScale.z;    float radius = 0.5f * sqrtf(0.5f * (x * x + z * z));    float halfHeight = 0.5f * _boxScale.y - radius;    float MIN_HALF_HEIGHT = 0.1f;    if (halfHeight < MIN_HALF_HEIGHT) {        halfHeight = MIN_HALF_HEIGHT;    }     // compare dimensions    float radiusDelta = glm::abs(radius - _radius);    float heightDelta = glm::abs(halfHeight - _halfHeight);    if (radiusDelta < FLT_EPSILON && heightDelta < FLT_EPSILON) {        // shape hasn't changed --> nothing to do    } else {        if (_dynamicsWorld) {            // must REMOVE from world prior to shape update            _pendingFlags |= PENDING_FLAG_REMOVE_FROM_SIMULATION;        }        _pendingFlags |= PENDING_FLAG_UPDATE_SHAPE;        // only need to ADD back when we happen to be enabled        if (_enabled) {            _pendingFlags |= PENDING_FLAG_ADD_TO_SIMULATION;        }    }     // it's ok to change offset immediately -- there are no thread safety issues here    _shapeLocalOffset = corner + 0.5f * _boxScale;} void CharacterController::setEnabled(bool enabled) {    if (enabled != _enabled) {        if (enabled) {            // Don't bother clearing REMOVE bit since it might be paired with an UPDATE_SHAPE bit.            // Setting the ADD bit here works for all cases so we don't even bother checking other bits.            _pendingFlags |= PENDING_FLAG_ADD_TO_SIMULATION;        } else {            if (_dynamicsWorld) {                _pendingFlags |= PENDING_FLAG_REMOVE_FROM_SIMULATION;            }            _pendingFlags &= ~ PENDING_FLAG_ADD_TO_SIMULATION;        }        SET_STATE(State::Hover, "setEnabled");        _enabled = enabled;    }} void CharacterController::updateUpAxis(const glm::quat& rotation) {    btVector3 oldUp = _currentUp;    _currentUp = quatRotate(glmToBullet(rotation), LOCAL_UP_AXIS);    if (_state != State::Hover) {        const btScalar MIN_UP_ERROR = 0.01f;        if (oldUp.distance(_currentUp) > MIN_UP_ERROR) {            _rigidBody->setGravity(DEFAULT_CHARACTER_GRAVITY * _currentUp);        }    }} void CharacterController::setPositionAndOrientation(        const glm::vec3& position,        const glm::quat& orientation) {    // TODO: update gravity if up has changed    updateUpAxis(orientation);     btQuaternion bodyOrientation = glmToBullet(orientation);    btVector3 bodyPosition = glmToBullet(position + orientation * _shapeLocalOffset);    _characterBodyTransform = btTransform(bodyOrientation, bodyPosition);} void CharacterController::getPositionAndOrientation(glm::vec3& position, glm::quat& rotation) const {    if (_enabled && _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();} void CharacterController::addMotor(const glm::vec3& velocity, const glm::quat& rotation, float horizTimescale, float vertTimescale) {    _motors.push_back(CharacterController::CharacterMotor(velocity, rotation, horizTimescale, vertTimescale));} void CharacterController::applyMotor(int index, btScalar dt, btVector3& worldVelocity, std::vector<btVector3>& velocities, std::vector<btScalar>& weights) {    assert(index < (int)(_motors.size()));    CharacterController::CharacterMotor& motor = _motors[index];    if (motor.hTimescale >= MAX_CHARACTER_MOTOR_TIMESCALE && motor.vTimescale >= MAX_CHARACTER_MOTOR_TIMESCALE) {        // nothing to do        return;    }     // rotate into motor-frame    btVector3 axis = motor.rotation.getAxis();    btScalar angle = motor.rotation.getAngle();    btVector3 velocity = worldVelocity.rotate(axis, -angle);     if (_state == State::Hover || motor.hTimescale == motor.vTimescale) {        // modify velocity        btScalar tau = dt / motor.hTimescale;        if (tau > 1.0f) {            tau = 1.0f;        }        velocity += (motor.velocity - velocity) * tau;         // rotate back into world-frame        velocity = velocity.rotate(axis, angle);         // store the velocity and weight        velocities.push_back(velocity);        weights.push_back(tau);    } else {        // compute local UP        btVector3 up = _currentUp.rotate(axis, -angle);         // split velocity into horizontal and vertical components        btVector3 vVelocity = velocity.dot(up) * up;        btVector3 hVelocity = velocity - vVelocity;        btVector3 vTargetVelocity = motor.velocity.dot(up) * up;        btVector3 hTargetVelocity = motor.velocity - vTargetVelocity;         // modify each component separately        btScalar maxTau = 0.0f;        if (motor.hTimescale < MAX_CHARACTER_MOTOR_TIMESCALE) {            btScalar tau = dt / motor.hTimescale;            if (tau > 1.0f) {                tau = 1.0f;            }            maxTau = tau;            hVelocity += (hTargetVelocity - hVelocity) * tau;        }        if (motor.vTimescale < MAX_CHARACTER_MOTOR_TIMESCALE) {            btScalar tau = dt / motor.vTimescale;            if (tau > 1.0f) {                tau = 1.0f;            }            if (tau > maxTau) {                maxTau = tau;            }            vVelocity += (vTargetVelocity - vVelocity) * tau;        }         // add components back together and rotate into world-frame        velocity = (hVelocity + vVelocity).rotate(axis, angle);         // store velocity and weights        velocities.push_back(velocity);        weights.push_back(maxTau);    }} void CharacterController::computeNewVelocity(btScalar dt, btVector3& velocity) {    if (velocity.length2() < MIN_TARGET_SPEED_SQUARED) {        velocity = btVector3(0.0f, 0.0f, 0.0f);    }     // measure velocity changes and their weights    std::vector<btVector3> velocities;    velocities.reserve(_motors.size());    std::vector<btScalar> weights;    weights.reserve(_motors.size());    for (int i = 0; i < (int)_motors.size(); ++i) {        applyMotor(i, dt, velocity, velocities, weights);    }    assert(velocities.size() == weights.size());     // blend velocity changes according to relative weights

Lines 1–500 of 659. Load more