////  MyCharacterController.h//  interface/src/avatar////  Created by AndrewMeadows 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 "MyCharacterController.h" #include <BulletUtil.h>#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" #include "MyAvatar.h"#include "DetailedMotionState.h" // TODO: make avatars stand on steep slope// TODO: make avatars not snag on low ceilings  void MyCharacterController::RayShotgunResult::reset() {    hitFraction = 1.0f;    walkable = true;} MyCharacterController::MyCharacterController(std::shared_ptr<MyAvatar> avatar,                                             const FollowTimePerType& followTimeRemainingPerType) :    CharacterController(followTimeRemainingPerType) {     assert(avatar);    _avatar = avatar;    updateShapeIfNecessary();} MyCharacterController::~MyCharacterController() {} void MyCharacterController::addToWorld() {    CharacterController::addToWorld();    if (_rigidBody) {        initRayShotgun(_physicsEngine->getDynamicsWorld());    }} void MyCharacterController::updateShapeIfNecessary() {    if (_pendingFlags & PENDING_FLAG_UPDATE_SHAPE) {        _pendingFlags &= ~PENDING_FLAG_UPDATE_SHAPE;        if (_radius > 0.0f) {            // create RigidBody if it doesn't exist            if (!_rigidBody) {                btCollisionShape* shape = computeShape();                btScalar mass = 1.0f;                btVector3 inertia(1.0f, 1.0f, 1.0f);                _rigidBody = new btRigidBody(mass, nullptr, shape, inertia);            } else {                btCollisionShape* shape = _rigidBody->getCollisionShape();                if (shape) {                    delete shape;                }                shape = computeShape();                _rigidBody->setCollisionShape(shape);            }            updateMassProperties();             _rigidBody->setSleepingThresholds(0.0f, 0.0f);            _rigidBody->setAngularFactor(0.0f);            _rigidBody->setWorldTransform(btTransform(glmToBullet(_avatar->getWorldOrientation()),                                                      glmToBullet(_avatar->getWorldPosition())));            _rigidBody->setDamping(0.0f, 0.0f);            if (_state == State::Hover) {                _rigidBody->setGravity(btVector3(0.0f, 0.0f, 0.0f));            } else {                _rigidBody->setGravity(_gravity * _currentUp);            }            _rigidBody->setCollisionFlags(_rigidBody->getCollisionFlags() &                    ~(btCollisionObject::CF_KINEMATIC_OBJECT | btCollisionObject::CF_STATIC_OBJECT));        } else {            // TODO: handle this failure case        }    }} bool MyCharacterController::testRayShotgun(const glm::vec3& position, const glm::vec3& step, RayShotgunResult& result) {    btVector3 rayDirection = glmToBullet(step);    btScalar stepLength = rayDirection.length();    if (stepLength < FLT_EPSILON) {        return false;    }    rayDirection /= stepLength;     // get _ghost ready for ray traces    btTransform transform = _rigidBody->getWorldTransform();    btVector3 newPosition = glmToBullet(position);    transform.setOrigin(newPosition);    _ghost.setWorldTransform(transform);    btMatrix3x3 rotation = transform.getBasis();    _ghost.refreshOverlappingPairCache();     CharacterRayResult rayResult(&_ghost);    CharacterRayResult closestRayResult(&_ghost);    btVector3 rayStart;    btVector3 rayEnd;     // compute rotation that will orient local ray start points to face step direction    btVector3 forward = rotation * btVector3(0.0f, 0.0f, -1.0f);    btVector3 adjustedDirection = rayDirection - rayDirection.dot(_currentUp) * _currentUp;    btVector3 axis = forward.cross(adjustedDirection);    btScalar lengthAxis = axis.length();    if (lengthAxis > FLT_EPSILON) {        // we're walking sideways        btScalar cosAngle = lengthAxis / adjustedDirection.length();        if (cosAngle < 1.0f) {            btScalar angle = acosf(cosAngle);            if (rayDirection.dot(forward) < 0.0f) {                angle = PI - angle;            }            axis /= lengthAxis;            rotation = btMatrix3x3(btQuaternion(axis, angle)) * rotation;        }    } else if (rayDirection.dot(forward) < 0.0f) {        // we're walking backwards        rotation = btMatrix3x3(btQuaternion(_currentUp, PI)) * rotation;    }     // scan the top    // NOTE: if we scan an extra distance forward we can detect flat surfaces that are too steep to walk on.    // The approximate extra distance can be derived with trigonometry.    //    //   minimumForward = [ (maxStepHeight + radius / cosTheta - radius) * (cosTheta / sinTheta) - radius ]    //    // where: theta = max angle between floor normal and vertical    //    // if stepLength is not long enough we can add the difference.    //    btScalar cosTheta = _minFloorNormalDotUp;    btScalar sinTheta = sqrtf(1.0f - cosTheta * cosTheta);    const btScalar MIN_FORWARD_SLOP = 0.12f; // HACK: not sure why this is necessary to detect steepest walkable slope    btScalar forwardSlop = (_maxStepHeight + _radius / cosTheta - _radius) * (cosTheta / sinTheta) - (_radius + stepLength) + MIN_FORWARD_SLOP;    if (forwardSlop < 0.0f) {        // BIG step, no slop necessary        forwardSlop = 0.0f;    }     const btScalar backSlop = 0.04f;    for (int32_t i = 0; i < _topPoints.size(); ++i) {        rayStart = newPosition + rotation * _topPoints[i] - backSlop * rayDirection;        rayEnd = rayStart + (backSlop + stepLength + forwardSlop) * rayDirection;        if (_ghost.rayTest(rayStart, rayEnd, rayResult)) {            if (rayResult.m_closestHitFraction < closestRayResult.m_closestHitFraction) {                closestRayResult = rayResult;            }            if (result.walkable) {                if (rayResult.m_hitNormalWorld.dot(_currentUp) < _minFloorNormalDotUp) {                    result.walkable = false;                    // the top scan wasn't walkable so don't bother scanning the bottom                    // remove both forwardSlop and backSlop                    result.hitFraction = glm::min(1.0f, (closestRayResult.m_closestHitFraction * (backSlop + stepLength + forwardSlop) - backSlop) / stepLength);                    return result.hitFraction < 1.0f;                }            }        }    }    if (_state == State::Hover) {        // scan the bottom just like the top        for (int32_t i = 0; i < _bottomPoints.size(); ++i) {            rayStart = newPosition + rotation * _bottomPoints[i] - backSlop * rayDirection;            rayEnd = rayStart + (backSlop + stepLength + forwardSlop) * rayDirection;            if (_ghost.rayTest(rayStart, rayEnd, rayResult)) {                if (rayResult.m_closestHitFraction < closestRayResult.m_closestHitFraction) {                    closestRayResult = rayResult;                }                if (result.walkable) {                    if (rayResult.m_hitNormalWorld.dot(_currentUp) < _minFloorNormalDotUp) {                        result.walkable = false;                        // the bottom scan wasn't walkable                        // remove both forwardSlop and backSlop                        result.hitFraction = glm::min(1.0f, (closestRayResult.m_closestHitFraction * (backSlop + stepLength + forwardSlop) - backSlop) / stepLength);                        return result.hitFraction < 1.0f;                    }                }            }        }    } else {        // scan the bottom looking for nearest step point        // remove forwardSlop        result.hitFraction = (closestRayResult.m_closestHitFraction * (backSlop + stepLength + forwardSlop)) / (backSlop + stepLength);         for (int32_t i = 0; i < _bottomPoints.size(); ++i) {            rayStart = newPosition + rotation * _bottomPoints[i] - backSlop * rayDirection;            rayEnd = rayStart + (backSlop + stepLength) * rayDirection;            if (_ghost.rayTest(rayStart, rayEnd, rayResult)) {                if (rayResult.m_closestHitFraction < closestRayResult.m_closestHitFraction) {                    closestRayResult = rayResult;                }            }        }        // remove backSlop        // NOTE: backSlop removal can produce a NEGATIVE hitFraction!        // which means the shape is actually in interpenetration        result.hitFraction = ((closestRayResult.m_closestHitFraction * (backSlop + stepLength)) - backSlop) / stepLength;    }    return result.hitFraction < 1.0f;} int32_t MyCharacterController::computeCollisionMask() const {    int32_t collisionMask = BULLET_COLLISION_MASK_MY_AVATAR;    if (_collisionless && _collisionlessAllowed) {        collisionMask = BULLET_COLLISION_MASK_COLLISIONLESS;    } else if (!_collideWithOtherAvatars) {        collisionMask &= ~BULLET_COLLISION_GROUP_OTHER_AVATAR;    }    return collisionMask;} void MyCharacterController::handleChangedCollisionMask() {    if (_pendingFlags & PENDING_FLAG_UPDATE_COLLISION_MASK) {        // ATM the easiest way to update collision groups/masks is to remove/re-add the RigidBody        // but we don't do it here.  Instead we set some flags to remind us to do it later.        _pendingFlags |= (PENDING_FLAG_REMOVE_FROM_SIMULATION | PENDING_FLAG_ADD_TO_SIMULATION);        _pendingFlags &= ~PENDING_FLAG_UPDATE_COLLISION_MASK;        updateCurrentGravity();    }} bool MyCharacterController::needsSafeLandingSupport() const {    return _isStuck && _numStuckSubsteps >= NUM_SUBSTEPS_FOR_SAFE_LANDING_RETRY;} btConvexHullShape* MyCharacterController::computeShape() const {    // HACK: the avatar collides using convex hull with a collision margin equal to    // the old capsule radius.  Two points define a capsule and additional points are    // spread out at chest level to produce a slight taper toward the feet.  This    // makes the avatar more likely to collide with vertical walls at a higher point    // and thus less likely to produce a single-point collision manifold below the    // _maxStepHeight when walking into against vertical surfaces --> fixes a bug    // where the "walk up steps" feature would allow the avatar to walk up vertical    // walls.    const int32_t NUM_POINTS = 6;    btVector3 points[NUM_POINTS];    btVector3 xAxis = btVector3(1.0f, 0.0f, 0.0f);    btVector3 yAxis = btVector3(0.0f, 1.0f, 0.0f);    btVector3 zAxis = btVector3(0.0f, 0.0f, 1.0f);    points[0] = _halfHeight * yAxis;    points[1] = -_halfHeight * yAxis;    points[2] = (0.75f * _halfHeight) * yAxis - (0.1f * _radius) * zAxis;    points[3] = (0.75f * _halfHeight) * yAxis + (0.1f * _radius) * zAxis;    points[4] = (0.75f * _halfHeight) * yAxis - (0.1f * _radius) * xAxis;    points[5] = (0.75f * _halfHeight) * yAxis + (0.1f * _radius) * xAxis;    btConvexHullShape* shape = new btConvexHullShape(reinterpret_cast<btScalar*>(points), NUM_POINTS);    shape->setMargin(_radius);    return shape;} void MyCharacterController::initRayShotgun(const btCollisionWorld* world) {    // In order to trace rays out from the avatar's shape surface we need to know where the start points are in    // the local-frame.  Since the avatar shape is somewhat irregular computing these points by hand is a hassle    // so instead we ray-trace backwards to the avatar to find them.    //    // We trace back a regular grid (see below) of points against the shape and keep any that hit.    //        ___    //     + / + \ +    //      |+   +|    //     +|  +  | +    //      |+   +|    //     +|  +  | +    //      |+   +|    //     + \ + /  +    //        ---    // The shotgun will send rays out from these same points to see if the avatar's shape can proceed through space.     // helper class for simple ray-traces against character    class MeOnlyResultCallback : public btCollisionWorld::ClosestRayResultCallback {    public:        MeOnlyResultCallback (btRigidBody* me) : btCollisionWorld::ClosestRayResultCallback(btVector3(0.0f, 0.0f, 0.0f), btVector3(0.0f, 0.0f, 0.0f)) {            _me = me;            m_collisionFilterGroup = BULLET_COLLISION_GROUP_DYNAMIC;            m_collisionFilterMask = BULLET_COLLISION_MASK_DYNAMIC;        }        virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult,bool normalInWorldSpace) override {            if (rayResult.m_collisionObject != _me) {                return 1.0f;            }            return ClosestRayResultCallback::addSingleResult(rayResult, normalInWorldSpace);        }        btRigidBody* _me;    };     const btScalar fullHalfHeight = _radius + _halfHeight;    const btScalar divisionLine = -fullHalfHeight + _maxStepHeight; // line between top and bottom    const btScalar topHeight = fullHalfHeight - divisionLine;    const btScalar slop = 0.02f;     const int32_t NUM_ROWS = 5; // must be odd number > 1    const int32_t NUM_COLUMNS = 5; // must be odd number > 1    btVector3 reach = (2.0f * _radius) * btVector3(0.0f, 0.0f, 1.0f);     { // top points        _topPoints.clear();        _topPoints.reserve(NUM_ROWS * NUM_COLUMNS);        btScalar stepY = (topHeight - slop) / (btScalar)(NUM_ROWS - 1);        btScalar stepX = 2.0f * (_radius - slop) / (btScalar)(NUM_COLUMNS - 1);         btTransform transform = _rigidBody->getWorldTransform();        btVector3 position = transform.getOrigin();        btMatrix3x3 rotation = transform.getBasis();         for (int32_t i = 0; i < NUM_ROWS; ++i) {            int32_t maxJ = NUM_COLUMNS;            btScalar offsetX = -(btScalar)((NUM_COLUMNS - 1) / 2) * stepX;            if (i % 2 == 1) {                // odd rows have one less point and start a halfStep closer                maxJ -= 1;                offsetX += 0.5f * stepX;            }            for (int32_t j = 0; j < maxJ; ++j) {                btVector3 localRayEnd(offsetX + (btScalar)(j) * stepX, divisionLine + (btScalar)(i) * stepY, 0.0f);                btVector3 localRayStart = localRayEnd - reach;                MeOnlyResultCallback result(_rigidBody);                world->rayTest(position + rotation * localRayStart, position + rotation * localRayEnd, result);                if (result.m_closestHitFraction < 1.0f) {                    _topPoints.push_back(localRayStart + result.m_closestHitFraction * reach);                }            }        }    }     { // bottom points        _bottomPoints.clear();        _bottomPoints.reserve(NUM_ROWS * NUM_COLUMNS);         btScalar steepestStepHitHeight = (_radius + 0.04f) * (1.0f - DEFAULT_MIN_FLOOR_NORMAL_DOT_UP);        btScalar stepY = (_maxStepHeight - slop - steepestStepHitHeight) / (btScalar)(NUM_ROWS - 1);        btScalar stepX = 2.0f * (_radius - slop) / (btScalar)(NUM_COLUMNS - 1);         btTransform transform = _rigidBody->getWorldTransform();        btVector3 position = transform.getOrigin();        btMatrix3x3 rotation = transform.getBasis();         for (int32_t i = 0; i < NUM_ROWS; ++i) {            int32_t maxJ = NUM_COLUMNS;            btScalar offsetX = -(btScalar)((NUM_COLUMNS - 1) / 2) * stepX;            if (i % 2 == 1) {                // odd rows have one less point and start a halfStep closer                maxJ -= 1;                offsetX += 0.5f * stepX;            }            for (int32_t j = 0; j < maxJ; ++j) {                btVector3 localRayEnd(offsetX + (btScalar)(j) * stepX, (divisionLine - slop) - (btScalar)(i) * stepY, 0.0f);                btVector3 localRayStart = localRayEnd - reach;                MeOnlyResultCallback result(_rigidBody);                world->rayTest(position + rotation * localRayStart, position + rotation * localRayEnd, result);                if (result.m_closestHitFraction < 1.0f) {                    _bottomPoints.push_back(localRayStart + result.m_closestHitFraction * reach);                }            }        }    }} void MyCharacterController::updateMassProperties() {    assert(_rigidBody);    // the inertia tensor of a capsule with Y-axis of symmetry, radius R and cylinder height H is:    // Ix = density * (volumeCylinder * (H^2 / 12 + R^2 / 4) + volumeSphere * (2R^2 / 5 + H^2 / 2 + 3HR / 8))    // Iy = density * (volumeCylinder * (R^2 / 2) + volumeSphere * (2R^2 / 5)    btScalar r2 = _radius * _radius;    btScalar h2 = 4.0f * _halfHeight * _halfHeight;    btScalar volumeSphere = 4.0f * PI * r2 * _radius / 3.0f;    btScalar volumeCylinder = TWO_PI * r2 * 2.0f * _halfHeight;    btScalar cylinderXZ = volumeCylinder * (h2 / 12.0f + r2 / 4.0f);    btScalar capsXZ = volumeSphere * (2.0f * r2 / 5.0f + h2 / 2.0f + 6.0f * _halfHeight * _radius / 8.0f);    btScalar inertiaXZ = _density * (cylinderXZ + capsXZ);    btScalar inertiaY = _density * ((volumeCylinder * r2 / 2.0f) + volumeSphere * (2.0f * r2 / 5.0f));    btVector3 inertia(inertiaXZ, inertiaY, inertiaXZ);     btScalar mass = _density * (volumeCylinder + volumeSphere);     _rigidBody->setMassProps(mass, inertia);} const btCollisionShape* MyCharacterController::createDetailedCollisionShapeForJoint(int32_t jointIndex) {    ShapeInfo shapeInfo;    _avatar->computeDetailedShapeInfo(shapeInfo, jointIndex);    if (shapeInfo.getType() != SHAPE_TYPE_NONE) {        const btCollisionShape* shape = ObjectMotionState::getShapeManager()->getShape(shapeInfo);        return shape;    }    return nullptr;} DetailedMotionState* MyCharacterController::createDetailedMotionStateForJoint(int32_t jointIndex) {    const btCollisionShape* shape = createDetailedCollisionShapeForJoint(jointIndex);    if (shape) {        DetailedMotionState* motionState = new DetailedMotionState(_avatar, shape, jointIndex);        motionState->setMass(_avatar->computeMass());        return motionState;    }    return nullptr;} void MyCharacterController::clearDetailedMotionStates() {    // we don't actually clear the MotionStates here    // instead we twiddle some flags as a signal of what to do later    _pendingFlags |= PENDING_FLAG_REMOVE_DETAILED_FROM_SIMULATION;     // We make sure we don't add them again    _pendingFlags &= ~PENDING_FLAG_ADD_DETAILED_TO_SIMULATION;} void MyCharacterController::buildPhysicsTransaction(PhysicsEngine::Transaction& transaction) {    for (auto& detailedMotionState : _detailedMotionStates) {        detailedMotionState->forceActive();    }    if (_pendingFlags & PENDING_FLAG_REMOVE_DETAILED_FROM_SIMULATION) {        _pendingFlags &= ~PENDING_FLAG_REMOVE_DETAILED_FROM_SIMULATION;        for (auto& detailedMotionState : _detailedMotionStates) {            transaction.objectsToRemove.push_back(detailedMotionState);        }        // NOTE: the DetailedMotionStates are deleted after being added to PhysicsEngine::Transaction::_objectsToRemove        // See AvatarManager::handleProcessedPhysicsTransaction()        _detailedMotionStates.clear();    }    if (_pendingFlags & PENDING_FLAG_ADD_DETAILED_TO_SIMULATION) {        _pendingFlags &= ~PENDING_FLAG_ADD_DETAILED_TO_SIMULATION;        for (int32_t i = 0; i < _avatar->getJointCount(); i++) {            auto dMotionState = createDetailedMotionStateForJoint(i);            if (dMotionState) {                _detailedMotionStates.push_back(dMotionState);                transaction.objectsToAdd.push_back(dMotionState);            }        }    }} class DetailedRayResultCallback : public btCollisionWorld::AllHitsRayResultCallback {public:    DetailedRayResultCallback()        : btCollisionWorld::AllHitsRayResultCallback(btVector3(0.0f, 0.0f, 0.0f), btVector3(0.0f, 0.0f, 0.0f)) {        // the RayResultCallback's group and mask must match MY_AVATAR        m_collisionFilterGroup = BULLET_COLLISION_GROUP_DETAILED_RAY;        m_collisionFilterMask = BULLET_COLLISION_MASK_DETAILED_RAY;    }     virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult, bool normalInWorldSpace) override {        return AllHitsRayResultCallback::addSingleResult(rayResult, normalInWorldSpace);    }}; std::vector<MyCharacterController::RayAvatarResult> MyCharacterController::rayTest(const btVector3& origin, const btVector3& direction,                                                                                   const btScalar& length, const QVector<uint>& jointsToExclude) const {    std::vector<RayAvatarResult> foundAvatars;    if (_physicsEngine) {        btVector3 end = origin + length * direction;        DetailedRayResultCallback rayCallback = DetailedRayResultCallback();        rayCallback.m_flags |= btTriangleRaycastCallback::kF_KeepUnflippedNormal;        rayCallback.m_flags |= btTriangleRaycastCallback::kF_UseSubSimplexConvexCastRaytest;        _physicsEngine->getDynamicsWorld()->rayTest(origin, end, rayCallback);        if (rayCallback.m_hitFractions.size() > 0) {            foundAvatars.reserve(rayCallback.m_hitFractions.size());            for (int32_t i = 0; i < rayCallback.m_hitFractions.size(); i++) {                auto object = rayCallback.m_collisionObjects[i];                ObjectMotionState* motionState = static_cast<ObjectMotionState*>(object->getUserPointer());                if (motionState && motionState->getType() == MOTIONSTATE_TYPE_DETAILED) {                    DetailedMotionState* detailedMotionState = dynamic_cast<DetailedMotionState*>(motionState);                    MyCharacterController::RayAvatarResult result;                    result._intersect = true;                    result._intersectWithAvatar = detailedMotionState->getAvatarID();                    result._intersectionPoint = bulletToGLM(rayCallback.m_hitPointWorld[i]);                    result._intersectionNormal = bulletToGLM(rayCallback.m_hitNormalWorld[i]);                    result._distance = length * rayCallback.m_hitFractions[i];                    result._intersectWithJoint = detailedMotionState->getJointIndex();                    result._isBound = detailedMotionState->getIsBound(result._boundJoints);                    btVector3 center;                    btScalar radius;                    detailedMotionState->getShape()->getBoundingSphere(center, radius);                    result._maxDistance = (float)radius;                    foundAvatars.push_back(result);                }            }            std::sort(foundAvatars.begin(), foundAvatars.end(), [](const RayAvatarResult& resultA, const RayAvatarResult& resultB) {                return resultA._distance < resultB._distance;            });        }    }    return foundAvatars;}