Merge pull request #1158 from logzero/move3

[RFC] Movement solver experiments

apps/openmw/mwphysics/physicssystem.cpp

@@ -55,29 +55,46 @@ namespace MWPhysics
     static const float sMaxSlope = 49.0f;
     static const float sStepSizeUp = 34.0f;
     static const float sStepSizeDown = 62.0f;
+    static const float sMinStep = 10.f;
 
     // Arbitrary number. To prevent infinite loops. They shouldn't happen but it's good to be prepared.
     static const int sMaxIterations = 8;
 
-    // FIXME: move to a separate file
+    static bool isActor(const btCollisionObject *obj)
-    class MovementSolver
+    {
+        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);
+    }
+
+    static bool canStepDown(const ActorTracer &stepper)
+    {
+        return stepper.mHitObject && isWalkableSlope(stepper.mPlaneNormal) && !isActor(stepper.mHitObject);
+    }
+
+    class Stepper
     {
     private:
-        static float getSlope(osg::Vec3f normal)
+        const btCollisionWorld *mColWorld;
-        {
+        const btCollisionObject *mColObj;
-            normal.normalize();
-            return osg::RadiansToDegrees(std::acos(normal * osg::Vec3f(0.f, 0.f, 1.f)));
-        }
 
-        enum StepMoveResult
+        ActorTracer mTracer, mUpStepper, mDownStepper;
-        {
+        bool mHaveMoved;
-            Result_Blocked, // unable to move over obstacle
-            Result_MaxSlope, // unable to end movement on this slope
-            Result_Success
-        };
 
-        static StepMoveResult stepMove(const btCollisionObject *colobj, osg::Vec3f &position,
+    public:
-                             const osg::Vec3f &toMove, float &remainingTime, const btCollisionWorld* collisionWorld)
+        Stepper(const btCollisionWorld *colWorld, const btCollisionObject *colObj)
+            : mColWorld(colWorld)
+            , mColObj(colObj)
+            , mHaveMoved(true)
+        {}
+
+        bool step(osg::Vec3f &position, const osg::Vec3f &toMove, float &remainingTime)
         {
             /*
              * Slide up an incline or set of stairs.  Should be called only after a
@@ -123,12 +140,14 @@ namespace MWPhysics
              *          +--+                         +--------
              *    ==============================================
              */
-            ActorTracer tracer, stepper;
+            if (mHaveMoved)
-
+            {
-            stepper.doTrace(colobj, position, position+osg::Vec3f(0.0f,0.0f,sStepSizeUp), collisionWorld);
+                mHaveMoved = false;
-            if(stepper.mFraction < std::numeric_limits<float>::epsilon())
+                mUpStepper.doTrace(mColObj, position, position+osg::Vec3f(0.0f,0.0f,sStepSizeUp), mColWorld);
-                return Result_Blocked; // didn't even move the smallest representable amount
+                if(mUpStepper.mFraction < std::numeric_limits<float>::epsilon())
-                              // (TODO: shouldn't this be larger? Why bother with such a small amount?)
+                    return false; // didn't even move the smallest representable amount
+                                  // (TODO: shouldn't this be larger? Why bother with such a small amount?)
+            }
 
             /*
              * Try moving from the elevated position using tracer.
@@ -143,9 +162,10 @@ namespace MWPhysics
              *          +--+
              *    ==============================================
              */
-            tracer.doTrace(colobj, stepper.mEndPos, stepper.mEndPos + toMove, collisionWorld);
+            osg::Vec3f tracerPos = mUpStepper.mEndPos;
-            if(tracer.mFraction < std::numeric_limits<float>::epsilon())
+            mTracer.doTrace(mColObj, tracerPos, tracerPos + toMove, mColWorld);
-                return Result_Blocked; // didn't even move the smallest representable amount
+            if(mTracer.mFraction < std::numeric_limits<float>::epsilon())
+                return false; // didn't even move the smallest representable amount
 
             /*
              * Try moving back down sStepSizeDown using stepper.
@@ -162,26 +182,40 @@ namespace MWPhysics
              *          +--+            +--+
              *    ==============================================
              */
-            stepper.doTrace(colobj, tracer.mEndPos, tracer.mEndPos-osg::Vec3f(0.0f,0.0f,sStepSizeDown), collisionWorld);
+            mDownStepper.doTrace(mColObj, mTracer.mEndPos, mTracer.mEndPos-osg::Vec3f(0.0f,0.0f,sStepSizeDown), mColWorld);
-            if (getSlope(stepper.mPlaneNormal) > sMaxSlope)
+            if (!canStepDown(mDownStepper))
-                return Result_MaxSlope;
+            {
-            if(stepper.mFraction < 1.0f)
+                // Try again with increased step length
+                if (mTracer.mFraction < 1.0f || toMove.length2() > sMinStep*sMinStep)
+                    return false;
+
+                osg::Vec3f direction = toMove;
+                direction.normalize();
+                mTracer.doTrace(mColObj, tracerPos, tracerPos + direction*sMinStep, mColWorld);
+                if (mTracer.mFraction < 0.001f)
+                    return false;
+
+                mDownStepper.doTrace(mColObj, mTracer.mEndPos, mTracer.mEndPos-osg::Vec3f(0.0f,0.0f,sStepSizeDown), mColWorld);
+                if (!canStepDown(mDownStepper))
+                    return false;
+            }
+            if (mDownStepper.mFraction < 1.0f)
             {
-                // don't allow stepping up other actors
-                if (stepper.mHitObject->getBroadphaseHandle()->m_collisionFilterGroup == CollisionType_Actor)
-                    return Result_Blocked;
                 // only step down onto semi-horizontal surfaces. don't step down onto the side of a house or a wall.
                 // TODO: stepper.mPlaneNormal does not appear to be reliable - needs more testing
                 // NOTE: caller's variables 'position' & 'remainingTime' are modified here
-                position = stepper.mEndPos;
+                position = mDownStepper.mEndPos;
-                remainingTime *= (1.0f-tracer.mFraction); // remaining time is proportional to remaining distance
+                remainingTime *= (1.0f-mTracer.mFraction); // remaining time is proportional to remaining distance
-                return Result_Success;
+                mHaveMoved = true;
+                return true;
             }
-
+            return false;
-            return Result_Blocked;
         }
+    };
 
-
+    class MovementSolver
+    {
+    private:
         ///Project a vector u on another vector v
         static inline osg::Vec3f project(const osg::Vec3f& u, const osg::Vec3f &v)
         {
@@ -229,14 +263,14 @@ namespace MWPhysics
                 collisionWorld->rayTest(from, to, resultCallback1);
 
                 if (resultCallback1.hasHit() &&
-                        ( (toOsg(resultCallback1.m_hitPointWorld) - tracer.mEndPos).length() > 35
+                        ( (toOsg(resultCallback1.m_hitPointWorld) - tracer.mEndPos).length2() > 35*35
-                        || getSlope(tracer.mPlaneNormal) > sMaxSlope))
+                        || !isWalkableSlope(tracer.mPlaneNormal)))
                 {
-                    actor->setOnGround(getSlope(toOsg(resultCallback1.m_hitNormalWorld)) <= sMaxSlope);
+                    actor->setOnGround(isWalkableSlope(resultCallback1.m_hitNormalWorld));
                     return toOsg(resultCallback1.m_hitPointWorld) + osg::Vec3f(0.f, 0.f, 1.f);
                 }
 
-                actor->setOnGround(getSlope(tracer.mPlaneNormal) <= sMaxSlope);
+                actor->setOnGround(isWalkableSlope(tracer.mPlaneNormal));
 
                 return tracer.mEndPos;
             }
@@ -312,8 +346,8 @@ namespace MWPhysics
                 velocity *= 1.f-(fStromWalkMult * (angleDegrees/180.f));
             }
 
+            Stepper stepper(collisionWorld, colobj);
             osg::Vec3f origVelocity = velocity;
-
             osg::Vec3f newPosition = position;
             /*
              * A loop to find newPosition using tracer, if successful different from the starting position.
@@ -332,10 +366,7 @@ namespace MWPhysics
                    newPosition.z() <= swimlevel)
                 {
                     const osg::Vec3f down(0,0,-1);
-                    float movelen = velocity.normalize();
+                    velocity = slide(velocity, down);
-                    osg::Vec3f reflectdir = reflect(velocity, down);
-                    reflectdir.normalize();
-                    velocity = slide(reflectdir, down)*movelen;
                     // NOTE: remainingTime is unchanged before the loop continues
                     continue; // velocity updated, calculate nextpos again
                 }
@@ -364,19 +395,25 @@ namespace MWPhysics
                     break;
                 }
 
+                // We are touching something.
+                if (tracer.mFraction < 1E-9f)
+                {
+                    // Try to separate by backing off slighly to unstuck the solver
+                    const osg::Vec3f backOff = (newPosition - tracer.mHitPoint) * 1E-3f;
+                    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;
-                // We hit something. Try to step up onto it. (NOTE: stepMove does not allow stepping over)
+                bool result = false;
-                // NOTE: stepMove modifies newPosition if successful
+                if (hitHeight < sStepSizeUp && !isActor(tracer.mHitObject))
-                const float minStep = 10.f;
-                StepMoveResult result = stepMove(colobj, newPosition, velocity*remainingTime, remainingTime, collisionWorld);
-                if (result == Result_MaxSlope && (velocity*remainingTime).length() < minStep) // to make sure the maximum stepping distance isn't framerate-dependent or movement-speed dependent
                 {
-                    osg::Vec3f normalizedVelocity = velocity;
+                    // Try to step up onto it.
-                    normalizedVelocity.normalize();
+                    // NOTE: stepMove does not allow stepping over, modifies newPosition if successful
-                    result = stepMove(colobj, newPosition, normalizedVelocity*minStep, remainingTime, collisionWorld);
+                    result = stepper.step(newPosition, velocity*remainingTime, remainingTime);
                 }
-                if(result == Result_Success)
+                if (result)
                 {
                     // don't let pure water creatures move out of water after stepMove
                     if (ptr.getClass().isPureWaterCreature(ptr)
@@ -386,23 +423,19 @@ namespace MWPhysics
                 else
                 {
                     // Can't move this way, try to find another spot along the plane
-                    osg::Vec3f direction = velocity;
+                    osg::Vec3f newVelocity = slide(velocity, tracer.mPlaneNormal);
-                    float movelen = direction.normalize();
+
-                    osg::Vec3f reflectdir = reflect(velocity, tracer.mPlaneNormal);
+                    // Do not allow sliding upward if there is gravity.
-                    reflectdir.normalize();
+                    // Stepping will have taken care of that.
+                    if(!(newPosition.z() < swimlevel || isFlying))
+                        newVelocity.z() = std::min(newVelocity.z(), 0.0f);
 
-                    osg::Vec3f newVelocity = slide(reflectdir, tracer.mPlaneNormal)*movelen;
                     if ((newVelocity-velocity).length2() < 0.01)
                         break;
-                    if ((velocity * origVelocity) <= 0.f)
+                    if ((newVelocity * origVelocity) <= 0.f)
                         break; // ^ dot product
 
                     velocity = newVelocity;
-
-                    // Do not allow sliding upward if there is gravity. Stepping will have taken
-                    // care of that.
-                    if(!(newPosition.z() < swimlevel || isFlying))
-                        velocity.z() = std::min(velocity.z(), 0.0f);
                 }
             }
 
@@ -413,7 +446,7 @@ namespace MWPhysics
                 osg::Vec3f to = newPosition - (physicActor->getOnGround() ?
                              osg::Vec3f(0,0,sStepSizeDown+2.f) : osg::Vec3f(0,0,2.f));
                 tracer.doTrace(colobj, from, to, collisionWorld);
-                if(tracer.mFraction < 1.0f && getSlope(tracer.mPlaneNormal) <= sMaxSlope
+                if(tracer.mFraction < 1.0f && isWalkableSlope(tracer.mPlaneNormal)
                         && tracer.mHitObject->getBroadphaseHandle()->m_collisionFilterGroup != CollisionType_Actor)
                 {
                     const btCollisionObject* standingOn = tracer.mHitObject;

apps/openmw/mwphysics/trace.cpp

@@ -78,6 +78,7 @@ void ActorTracer::doTrace(const btCollisionObject *actor, const osg::Vec3f& star
         mFraction = newTraceCallback.m_closestHitFraction;
         mPlaneNormal = osg::Vec3f(tracehitnormal.x(), tracehitnormal.y(), tracehitnormal.z());
         mEndPos = (end-start)*mFraction + start;
+        mHitPoint = toOsg(newTraceCallback.m_hitPointWorld);
         mHitObject = newTraceCallback.m_hitCollisionObject;
     }
     else
@@ -85,6 +86,7 @@ void ActorTracer::doTrace(const btCollisionObject *actor, const osg::Vec3f& star
         mEndPos = end;
         mPlaneNormal = osg::Vec3f(0.0f, 0.0f, 1.0f);
         mFraction = 1.0f;
+        mHitPoint = end;
         mHitObject = NULL;
     }
 }

apps/openmw/mwphysics/trace.h

@@ -15,6 +15,7 @@ namespace MWPhysics
     {
         osg::Vec3f mEndPos;
         osg::Vec3f mPlaneNormal;
+        osg::Vec3f mHitPoint;
         const btCollisionObject* mHitObject;
 
         float mFraction;