openmw-tes3mp/apps/openmw/mwphysics/movementsolver.cpp

#include "movementsolver.hpp"

#include <BulletCollision/CollisionDispatch/btCollisionObject.h>
#include <BulletCollision/CollisionDispatch/btCollisionWorld.h>
#include <BulletCollision/CollisionShapes/btCollisionShape.h>

#include <components/esm/loadgmst.hpp>
#include <components/misc/convert.hpp>

#include "../mwbase/world.hpp"
#include "../mwbase/environment.hpp"

#include "../mwworld/class.hpp"
#include "../mwworld/esmstore.hpp"
#include "../mwworld/refdata.hpp"

#ifdef USE_OPENXR
#include "../mwvr/vrsession.hpp"
#include "../mwvr/vrcamera.hpp"
#include "../mwvr/vrenvironment.hpp"
#include "../mwrender/renderingmanager.hpp"
#include "../mwworld/player.hpp"
#endif
#include "../mwmechanics/actorutil.hpp"

#include "actor.hpp"
#include "collisiontype.hpp"
#include "constants.hpp"
#include "physicssystem.hpp"
#include "stepper.hpp"
#include "trace.h"

namespace MWPhysics
{
    static bool isActor(const btCollisionObject *obj)
    {
        assert(obj);
        return obj->getBroadphaseHandle()->m_collisionFilterGroup == CollisionType_Actor;
    }

    template <class Vec3>
    static bool isWalkableSlope(const Vec3 &normal)
    {
        static const float sMaxSlopeCos = std::cos(osg::DegreesToRadians(sMaxSlope));
        return (normal.z() > sMaxSlopeCos);
    }

    osg::Vec3f MovementSolver::traceDown(const MWWorld::Ptr &ptr, const osg::Vec3f& position, Actor* actor, btCollisionWorld* collisionWorld, float maxHeight)
    {
        osg::Vec3f offset = actor->getCollisionObjectPosition() - ptr.getRefData().getPosition().asVec3();

        ActorTracer tracer;
        tracer.findGround(actor, position + offset, position + offset - osg::Vec3f(0,0,maxHeight), collisionWorld);
        if (tracer.mFraction >= 1.0f)
        {
            actor->setOnGround(false);
            return position;
        }

        actor->setOnGround(true);

        // Check if we actually found a valid spawn point (use an infinitely thin ray this time).
        // Required for some broken door destinations in Morrowind.esm, where the spawn point
        // intersects with other geometry if the actor's base is taken into account
        btVector3 from = Misc::Convert::toBullet(position);
        btVector3 to = from - btVector3(0,0,maxHeight);

        btCollisionWorld::ClosestRayResultCallback resultCallback1(from, to);
        resultCallback1.m_collisionFilterGroup = 0xff;
        resultCallback1.m_collisionFilterMask = CollisionType_World|CollisionType_HeightMap;

        collisionWorld->rayTest(from, to, resultCallback1);

        if (resultCallback1.hasHit() && ((Misc::Convert::toOsg(resultCallback1.m_hitPointWorld) - tracer.mEndPos + offset).length2() > 35*35
            || !isWalkableSlope(tracer.mPlaneNormal)))
        {
            actor->setOnSlope(!isWalkableSlope(resultCallback1.m_hitNormalWorld));
            return Misc::Convert::toOsg(resultCallback1.m_hitPointWorld) + osg::Vec3f(0.f, 0.f, sGroundOffset);
        }

        actor->setOnSlope(!isWalkableSlope(tracer.mPlaneNormal));

        return tracer.mEndPos-offset + osg::Vec3f(0.f, 0.f, sGroundOffset);
    }

    void MovementSolver::move(ActorFrameData& actor, float time, const btCollisionWorld* collisionWorld,
                                           WorldFrameData& worldData)
    {
        auto* physicActor = actor.mActorRaw;
        auto ptr = actor.mPtr;
        ESM::Position refpos = actor.mRefpos;
        
        // Early-out for totally static creatures
        // (Not sure if gravity should still apply?)
        if (!ptr.getClass().isMobile(ptr))
            return;

        const bool isPlayer = (ptr == MWMechanics::getPlayer());
        auto* world = MWBase::Environment::get().getWorld();

        // In VR, player should move according to current direction of
        // a selected limb, rather than current orientation of camera.
#ifdef USE_OPENXR
        // Regarding this and the duplicate movement solver later in this method:
        // As my two edits in this code are obviously hacks, I could use feedback on how i could implement
        // VR movement mechanics as not-a-hack. This hack, for instance, does not trigger movement animations
        // and will obviously be a poor fit for a future merge with tes3mp.

        // The exact mechanics are: 
        // 1. When moving with the controller, the player moves in the direction he is currently pointing his left controller.
        // 2. The game should seek to eliminate all distance between the player character and the player's position within VR,
        //    without teleporting the player or ignoring collisions.

        // I assume (1.) is easily solved, i just haven't taken the effort to study openmw's code enough.
        // But 2. is not so obvious. I guess it's doable if i compute the direction between current position and the player's
        // position in the VR stage, and just let it catch up at the character's own move speed, but it still needs to reach the position as exactly as possible.

        if (isPlayer)
        {
            auto* session = MWVR::Environment::get().getSession();
            if (session)
            {
                float pitch = 0.f;
                float yaw = 0.f;
                session->movementAngles(yaw, pitch);
                refpos.rot[0] += pitch;
                refpos.rot[2] += yaw;
            }
        }
#endif

        // Reset per-frame data
        physicActor->setWalkingOnWater(false);
        // Anything to collide with?
        if(!physicActor->getCollisionMode())
        {
            actor.mPosition += (osg::Quat(refpos.rot[0], osg::Vec3f(-1, 0, 0)) *
                                osg::Quat(refpos.rot[2], osg::Vec3f(0, 0, -1))
                                ) * actor.mMovement * time;
            return;
        }

        const btCollisionObject *colobj = physicActor->getCollisionObject();
        osg::Vec3f halfExtents = physicActor->getHalfExtents();

        // NOTE: here we don't account for the collision box translation (i.e. physicActor->getPosition() - refpos.pos).
        // That means the collision shape used for moving this actor is in a different spot than the collision shape
        // other actors are using to collide against this actor.
        // While this is strictly speaking wrong, it's needed for MW compatibility.
        actor.mPosition.z() += halfExtents.z();

        static const float fSwimHeightScale = world->getStore().get<ESM::GameSetting>().find("fSwimHeightScale")->mValue.getFloat();
        float swimlevel = actor.mWaterlevel + halfExtents.z() - (physicActor->getRenderingHalfExtents().z() * 2 * fSwimHeightScale);

        ActorTracer tracer;

        osg::Vec3f inertia = physicActor->getInertialForce();
        osg::Vec3f velocity;

        if (actor.mPosition.z() < swimlevel || actor.mFlying)
        {
            velocity = (osg::Quat(refpos.rot[0], osg::Vec3f(-1, 0, 0)) * osg::Quat(refpos.rot[2], osg::Vec3f(0, 0, -1))) * actor.mMovement;
        }
        else
        {
            velocity = (osg::Quat(refpos.rot[2], osg::Vec3f(0, 0, -1))) * actor.mMovement;

            if ((velocity.z() > 0.f && physicActor->getOnGround() && !physicActor->getOnSlope())
            || (velocity.z() > 0.f && velocity.z() + inertia.z() <= -velocity.z() && physicActor->getOnSlope()))
                inertia = velocity;
            else if (!physicActor->getOnGround() || physicActor->getOnSlope())
                velocity = velocity + inertia;
        }

        // dead actors underwater will float to the surface, if the CharacterController tells us to do so
        if (actor.mMovement.z() > 0 && actor.mIsDead && actor.mPosition.z() < swimlevel)
            velocity = osg::Vec3f(0,0,1) * 25;

        if (actor.mWantJump)
            actor.mDidJump = true;

        // Now that we have the effective movement vector, apply wind forces to it
        if (worldData.mIsInStorm)
        {
            osg::Vec3f stormDirection = worldData.mStormDirection;
            float angleDegrees = osg::RadiansToDegrees(std::acos(stormDirection * velocity / (stormDirection.length() * velocity.length())));
            static const float fStromWalkMult = world->getStore().get<ESM::GameSetting>().find("fStromWalkMult")->mValue.getFloat();
            velocity *= 1.f-(fStromWalkMult * (angleDegrees/180.f));
        }

        Stepper stepper(collisionWorld, colobj);
        osg::Vec3f origVelocity = velocity;
        osg::Vec3f newPosition = actor.mPosition;

#ifdef USE_OPENXR
        // Catch the player character up to the real world position of the player.
        // TODO: Hack.
        if (isPlayer && !world->getPlayer().isDisabled())
        {

            auto* inputManager = reinterpret_cast<MWVR::VRCamera*>(MWBase::Environment::get().getWorld()->getRenderingManager().getCamera());

            osg::Vec3 headOffset = inputManager->headOffset();
            osg::Vec3 trackingOffset = headOffset;
            // Player's tracking height should not affect character position
            trackingOffset.z() = 0;

            float remainingTime = time;
            float remainder = 1.f;

            for (int iterations = 0; iterations < sMaxIterations && remainingTime > 0.01f && remainder > 0.01; ++iterations)
            {
                osg::Vec3 toMove = trackingOffset * remainder;
                osg::Vec3 nextpos = newPosition + toMove;

                if ((newPosition - nextpos).length2() > 0.0001)
                {
                    // trace to where character would go if there were no obstructions
                    tracer.doTrace(colobj, newPosition, nextpos, collisionWorld);

                    // check for obstructions
                    if (tracer.mFraction >= 1.0f)
                    {
                        newPosition = tracer.mEndPos; // ok to move, so set newPosition
                        remainder = 0.f;
                        break;
                    }
                }
                else
                {
                    // The current position and next position are nearly the same, so just exit.
                    // Note: Bullet can trigger an assert in debug modes if the positions
                    // are the same, since that causes it to attempt to normalize a zero
                    // length vector (which can also happen with nearly identical vectors, since
                    // precision can be lost due to any math Bullet does internally). Since we
                    // aren't performing any collision detection, we want to reject the next
                    // position, so that we don't slowly move inside another object.
                    remainder = 0.f;
                    break;
                }
                // We are touching something.
                if (tracer.mFraction < 1E-9f)
                {
                    // Try to separate by backing off slighly to unstuck the solver
                    osg::Vec3f backOff = (newPosition - tracer.mHitPoint) * 1E-2f;
                    newPosition += backOff;
                }

                // We hit something. Check if we can step up.
                float hitHeight = tracer.mHitPoint.z() - tracer.mEndPos.z() + halfExtents.z();
                osg::Vec3f oldPosition = newPosition;
                bool result = false;
                if (hitHeight < sStepSizeUp && !isActor(tracer.mHitObject))
                {
                    // Try to step up onto it.
                    // NOTE: stepMove does not allow stepping over, modifies newPosition if successful
                    result = stepper.step(newPosition, toMove, remainingTime);
                    remainder = remainingTime / time;
                }
            }

            // Try not to lose any tracking
            osg::Vec3 moved = newPosition - actor.mPosition;
            headOffset.x() -= moved.x();
            headOffset.y() -= moved.y();
            inputManager->setHeadOffset(headOffset);
        }
#endif

        /*
         * A loop to find newPosition using tracer, if successful different from the starting position.
         * nextpos is the local variable used to find potential newPosition, using velocity and remainingTime
         * The initial velocity was set earlier (see above).
        */
        float remainingTime = time;
        for (int iterations = 0; iterations < sMaxIterations && remainingTime > 0.01f; ++iterations)
        {
            osg::Vec3f nextpos = newPosition + velocity * remainingTime;

            // If not able to fly, don't allow to swim up into the air
            if(!actor.mFlying && nextpos.z() > swimlevel && newPosition.z() < swimlevel)
            {
                const osg::Vec3f down(0,0,-1);
                velocity = slide(velocity, down);
                // NOTE: remainingTime is unchanged before the loop continues
                continue; // velocity updated, calculate nextpos again
            }

            if((newPosition - nextpos).length2() > 0.0001)
            {
                // trace to where character would go if there were no obstructions
                tracer.doTrace(colobj, newPosition, nextpos, collisionWorld);

                // check for obstructions
                if(tracer.mFraction >= 1.0f)
                {
                    newPosition = tracer.mEndPos; // ok to move, so set newPosition
                    break;
                }
            }
            else
            {
                // The current position and next position are nearly the same, so just exit.
                // Note: Bullet can trigger an assert in debug modes if the positions
                // are the same, since that causes it to attempt to normalize a zero
                // length vector (which can also happen with nearly identical vectors, since
                // precision can be lost due to any math Bullet does internally). Since we
                // aren't performing any collision detection, we want to reject the next
                // position, so that we don't slowly move inside another object.
                break;
            }

            // We are touching something.
            if (tracer.mFraction < 1E-9f)
            {
                // Try to separate by backing off slighly to unstuck the solver
                osg::Vec3f backOff = (newPosition - tracer.mHitPoint) * 1E-2f;
                newPosition += backOff;
            }

            // We hit something. Check if we can step up.
            float hitHeight = tracer.mHitPoint.z() - tracer.mEndPos.z() + halfExtents.z();
            osg::Vec3f oldPosition = newPosition;
            bool result = false;
            if (hitHeight < sStepSizeUp && !isActor(tracer.mHitObject))
            {
                // Try to step up onto it.
                // NOTE: stepMove does not allow stepping over, modifies newPosition if successful
                result = stepper.step(newPosition, velocity*remainingTime, remainingTime);
            }
            if (result)
            {
                // don't let pure water creatures move out of water after stepMove
                if (ptr.getClass().isPureWaterCreature(ptr) && newPosition.z() + halfExtents.z() > actor.mWaterlevel)
                    newPosition = oldPosition;
            }
            else
            {
                // Can't move this way, try to find another spot along the plane
                osg::Vec3f newVelocity = slide(velocity, tracer.mPlaneNormal);

                // Do not allow sliding upward if there is gravity.
                // Stepping will have taken care of that.
                if(!(newPosition.z() < swimlevel || actor.mFlying))
                    newVelocity.z() = std::min(newVelocity.z(), 0.0f);

                if ((newVelocity-velocity).length2() < 0.01)
                    break;
                if ((newVelocity * origVelocity) <= 0.f)
                    break; // ^ dot product

                velocity = newVelocity;
            }
        }

        bool isOnGround = false;
        bool isOnSlope = false;
        if (!(inertia.z() > 0.f) && !(newPosition.z() < swimlevel))
        {
            osg::Vec3f from = newPosition;
            osg::Vec3f to = newPosition - (physicActor->getOnGround() ? osg::Vec3f(0,0,sStepSizeDown + 2*sGroundOffset) : osg::Vec3f(0,0,2*sGroundOffset));
            tracer.doTrace(colobj, from, to, collisionWorld);
            if(tracer.mFraction < 1.0f && !isActor(tracer.mHitObject))
            {
                const btCollisionObject* standingOn = tracer.mHitObject;
                PtrHolder* ptrHolder = static_cast<PtrHolder*>(standingOn->getUserPointer());
                if (ptrHolder)
                    actor.mStandingOn = ptrHolder->getPtr();

                if (standingOn->getBroadphaseHandle()->m_collisionFilterGroup == CollisionType_Water)
                    physicActor->setWalkingOnWater(true);
                if (!actor.mFlying)
                    newPosition.z() = tracer.mEndPos.z() + sGroundOffset;

                isOnGround = true;

                isOnSlope = !isWalkableSlope(tracer.mPlaneNormal);
            }
            else
            {
                // standing on actors is not allowed (see above).
                // in addition to that, apply a sliding effect away from the center of the actor,
                // so that we do not stay suspended in air indefinitely.
                if (tracer.mFraction < 1.0f && isActor(tracer.mHitObject))
                {
                    if (osg::Vec3f(velocity.x(), velocity.y(), 0).length2() < 100.f*100.f)
                    {
                        btVector3 aabbMin, aabbMax;
                        tracer.mHitObject->getCollisionShape()->getAabb(tracer.mHitObject->getWorldTransform(), aabbMin, aabbMax);
                        btVector3 center = (aabbMin + aabbMax) / 2.f;
                        inertia = osg::Vec3f(actor.mPosition.x() - center.x(), actor.mPosition.y() - center.y(), 0);
                        inertia.normalize();
                        inertia *= 100;
                    }
                }

                isOnGround = false;
            }
        }

        if((isOnGround && !isOnSlope) || newPosition.z() < swimlevel || actor.mFlying)
            physicActor->setInertialForce(osg::Vec3f(0.f, 0.f, 0.f));
        else
        {
            inertia.z() -= time * Constants::GravityConst * Constants::UnitsPerMeter;
            if (inertia.z() < 0)
                inertia.z() *= actor.mSlowFall;
            if (actor.mSlowFall < 1.f) {
                inertia.x() *= actor.mSlowFall;
                inertia.y() *= actor.mSlowFall;
            }
            physicActor->setInertialForce(inertia);
        }
        physicActor->setOnGround(isOnGround);
        physicActor->setOnSlope(isOnSlope);

        newPosition.z() -= halfExtents.z(); // remove what was added at the beginning
        actor.mPosition = newPosition;
    }
}