Use osg::Vec3f to store path nodes in Pathfinder

apps/openmw/mwmechanics/aiactivate.cpp

@@ -36,7 +36,7 @@ namespace MWMechanics
         return true;   //Target doesn't exist
 
         //Set the target destination for the actor
-        ESM::Pathgrid::Point dest = target.getRefData().getPosition().pos;
+        const auto dest = target.getRefData().getPosition().asVec3();
 
         if (pathTo(actor, dest, duration, MWBase::Environment::get().getWorld()->getMaxActivationDistance())) //Stop when you get in activation range
         {

apps/openmw/mwmechanics/aicast.cpp

@@ -37,7 +37,7 @@ bool MWMechanics::AiCast::execute(const MWWorld::Ptr& actor, MWMechanics::Charac
         if (!target)
             return true;
 
-        if (!mManual && !pathTo(actor, target.getRefData().getPosition().pos, duration, mDistance))
+        if (!mManual && !pathTo(actor, target.getRefData().getPosition().asVec3(), duration, mDistance))
         {
             return false;
         }

apps/openmw/mwmechanics/aicombat.cpp

@@ -124,7 +124,7 @@ namespace MWMechanics
                 float targetReachedTolerance = 0.0f;
                 if (storage.mLOS)
                     targetReachedTolerance = storage.mAttackRange;
-                bool is_target_reached = pathTo(actor, target.getRefData().getPosition().pos, duration, targetReachedTolerance);
+                const bool is_target_reached = pathTo(actor, target.getRefData().getPosition().asVec3(), duration, targetReachedTolerance);
                 if (is_target_reached) storage.mReadyToAttack = true;
             }
 
@@ -355,7 +355,7 @@ namespace MWMechanics
 
                     float dist = (actor.getRefData().getPosition().asVec3() - target.getRefData().getPosition().asVec3()).length();
                     if ((dist > fFleeDistance && !storage.mLOS)
-                            || pathTo(actor, storage.mFleeDest, duration))
+                            || pathTo(actor, PathFinder::MakeOsgVec3(storage.mFleeDest), duration))
                     {
                         state = AiCombatStorage::FleeState_Idle;
                     }

apps/openmw/mwmechanics/aiescort.cpp

@@ -100,7 +100,7 @@ namespace MWMechanics
             point.mAutogenerated = 0;
             point.mConnectionNum = 0;
             point.mUnknown = 0;
-            if (pathTo(actor,point,duration)) //Returns true on path complete
+            if (pathTo(actor, osg::Vec3f(mX, mY, mZ), duration)) //Returns true on path complete
             {
                 mRemainingDuration = mDuration;
                 return true;

apps/openmw/mwmechanics/aifollow.cpp

@@ -163,7 +163,7 @@ bool AiFollow::execute (const MWWorld::Ptr& actor, CharacterController& characte
     }
 
     //Set the target destination from the actor
-    ESM::Pathgrid::Point dest = target.getRefData().getPosition().pos;
+    const auto dest = target.getRefData().getPosition().asVec3();
 
     short baseFollowDistance = followDistance;
     short threshold = 30; // to avoid constant switching between moving/stopping
@@ -196,7 +196,7 @@ bool AiFollow::execute (const MWWorld::Ptr& actor, CharacterController& characte
     if (storage.mMoving)
     {
         //Check if you're far away
-        float dist = distance(dest, pos.pos[0], pos.pos[1], pos.pos[2]);
+        float dist = distance(dest, pos.asVec3());
 
         if (dist > 450)
             actor.getClass().getCreatureStats(actor).setMovementFlag(MWMechanics::CreatureStats::Flag_Run, true); //Make NPC run

apps/openmw/mwmechanics/aipackage.cpp

@@ -90,13 +90,13 @@ void MWMechanics::AiPackage::reset()
     mTimer = AI_REACTION_TIME + 1.0f;
     mIsShortcutting = false;
     mShortcutProhibited = false;
-    mShortcutFailPos = ESM::Pathgrid::Point();
+    mShortcutFailPos = osg::Vec3f();
 
     mPathFinder.clearPath();
     mObstacleCheck.clear();
 }
 
-bool MWMechanics::AiPackage::pathTo(const MWWorld::Ptr& actor, const ESM::Pathgrid::Point& dest, float duration, float destTolerance)
+bool MWMechanics::AiPackage::pathTo(const MWWorld::Ptr& actor, const osg::Vec3f& dest, float duration, float destTolerance)
 {
     mTimer += duration; //Update timer
 
@@ -113,7 +113,7 @@ bool MWMechanics::AiPackage::pathTo(const MWWorld::Ptr& actor, const ESM::Pathgr
     }
 
     // handle path building and shortcutting
-    const ESM::Pathgrid::Point start = pos.pos;
+    const osg::Vec3f start = pos.asVec3();
 
     const float distToTarget = distance(start, dest);
     const bool isDestReached = (distToTarget <= destTolerance);
@@ -148,7 +148,7 @@ bool MWMechanics::AiPackage::pathTo(const MWWorld::Ptr& actor, const ESM::Pathgr
                 if (destInLOS && mPathFinder.getPath().size() > 1)
                 {
                     // get point just before dest
-                    std::list<ESM::Pathgrid::Point>::const_iterator pPointBeforeDest = mPathFinder.getPath().end();
+                    auto pPointBeforeDest = mPathFinder.getPath().end();
                     --pPointBeforeDest;
                     --pPointBeforeDest;
 
@@ -163,7 +163,7 @@ bool MWMechanics::AiPackage::pathTo(const MWWorld::Ptr& actor, const ESM::Pathgr
 
             if (!mPathFinder.getPath().empty()) //Path has points in it
             {
-                ESM::Pathgrid::Point lastPos = mPathFinder.getPath().back(); //Get the end of the proposed path
+                const auto& lastPos = mPathFinder.getPath().back(); //Get the end of the proposed path
 
                 if(distance(dest, lastPos) > 100) //End of the path is far from the destination
                     mPathFinder.addPointToPath(dest); //Adds the final destination to the path, to try to get to where you want to go
@@ -266,14 +266,15 @@ const MWMechanics::PathgridGraph& MWMechanics::AiPackage::getPathGridGraph(const
     return *cache[id].get();
 }
 
-bool MWMechanics::AiPackage::shortcutPath(const ESM::Pathgrid::Point& startPoint, const ESM::Pathgrid::Point& endPoint, const MWWorld::Ptr& actor, bool *destInLOS, bool isPathClear)
+bool MWMechanics::AiPackage::shortcutPath(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint,
+        const MWWorld::Ptr& actor, bool *destInLOS, bool isPathClear)
 {
-    if (!mShortcutProhibited || (PathFinder::MakeOsgVec3(mShortcutFailPos) - PathFinder::MakeOsgVec3(startPoint)).length() >= PATHFIND_SHORTCUT_RETRY_DIST)
+    if (!mShortcutProhibited || (mShortcutFailPos - startPoint).length() >= PATHFIND_SHORTCUT_RETRY_DIST)
     {
         // check if target is clearly visible
         isPathClear = !MWBase::Environment::get().getWorld()->castRay(
-            static_cast<float>(startPoint.mX), static_cast<float>(startPoint.mY), static_cast<float>(startPoint.mZ),
+            startPoint.x(), startPoint.y(), startPoint.z(),
-            static_cast<float>(endPoint.mX), static_cast<float>(endPoint.mY), static_cast<float>(endPoint.mZ));
+            endPoint.x(), endPoint.y(), endPoint.z());
 
         if (destInLOS != nullptr) *destInLOS = isPathClear;
 
@@ -294,21 +295,21 @@ bool MWMechanics::AiPackage::shortcutPath(const ESM::Pathgrid::Point& startPoint
     return false;
 }
 
-bool MWMechanics::AiPackage::checkWayIsClearForActor(const ESM::Pathgrid::Point& startPoint, const ESM::Pathgrid::Point& endPoint, const MWWorld::Ptr& actor)
+bool MWMechanics::AiPackage::checkWayIsClearForActor(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint, const MWWorld::Ptr& actor)
 {
-    bool actorCanMoveByZ = (actor.getClass().canSwim(actor) && MWBase::Environment::get().getWorld()->isSwimming(actor))
+    const bool actorCanMoveByZ = (actor.getClass().canSwim(actor) && MWBase::Environment::get().getWorld()->isSwimming(actor))
         || MWBase::Environment::get().getWorld()->isFlying(actor);
 
     if (actorCanMoveByZ)
         return true;
 
-    float actorSpeed = actor.getClass().getSpeed(actor);
+    const float actorSpeed = actor.getClass().getSpeed(actor);
-    float maxAvoidDist = AI_REACTION_TIME * actorSpeed + actorSpeed / MAX_VEL_ANGULAR_RADIANS * 2; // *2 - for reliability
+    const float maxAvoidDist = AI_REACTION_TIME * actorSpeed + actorSpeed / MAX_VEL_ANGULAR_RADIANS * 2; // *2 - for reliability
-    osg::Vec3f::value_type distToTarget = osg::Vec3f(static_cast<float>(endPoint.mX), static_cast<float>(endPoint.mY), 0).length();
+    const float distToTarget = osg::Vec2f(endPoint.x(), endPoint.y()).length();
 
-    float offsetXY = distToTarget > maxAvoidDist*1.5? maxAvoidDist : maxAvoidDist/2;
+    const float offsetXY = distToTarget > maxAvoidDist*1.5? maxAvoidDist : maxAvoidDist/2;
 
-    bool isClear = checkWayIsClear(PathFinder::MakeOsgVec3(startPoint), PathFinder::MakeOsgVec3(endPoint), offsetXY);
+    const bool isClear = checkWayIsClear(startPoint, endPoint, offsetXY);
 
     // update shortcut prohibit state
     if (isClear)
@@ -316,12 +317,12 @@ bool MWMechanics::AiPackage::checkWayIsClearForActor(const ESM::Pathgrid::Point&
         if (mShortcutProhibited)
         {
             mShortcutProhibited = false;
-            mShortcutFailPos = ESM::Pathgrid::Point();
+            mShortcutFailPos = osg::Vec3f();
         }
     }
     if (!isClear)
     {
-        if (mShortcutFailPos.mX == 0 && mShortcutFailPos.mY == 0 && mShortcutFailPos.mZ == 0)
+        if (mShortcutFailPos == osg::Vec3f())
         {
             mShortcutProhibited = true;
             mShortcutFailPos = startPoint;
@@ -331,9 +332,11 @@ bool MWMechanics::AiPackage::checkWayIsClearForActor(const ESM::Pathgrid::Point&
     return isClear;
 }
 
-bool MWMechanics::AiPackage::doesPathNeedRecalc(const ESM::Pathgrid::Point& newDest, const MWWorld::CellStore* currentCell)
+bool MWMechanics::AiPackage::doesPathNeedRecalc(const osg::Vec3f& newDest, const MWWorld::CellStore* currentCell)
 {
-    return mPathFinder.getPath().empty() || (distance(mPathFinder.getPath().back(), newDest) > 10) || mPathFinder.getPathCell() != currentCell;
+    return mPathFinder.getPath().empty()
+        || (distance(mPathFinder.getPath().back(), newDest) > 10)
+        || mPathFinder.getPathCell() != currentCell;
 }
 
 bool MWMechanics::AiPackage::isTargetMagicallyHidden(const MWWorld::Ptr& target)
@@ -367,7 +370,7 @@ bool MWMechanics::AiPackage::isNearInactiveCell(const ESM::Position& actorPos)
     }
 }
 
-bool MWMechanics::AiPackage::isReachableRotatingOnTheRun(const MWWorld::Ptr& actor, const ESM::Pathgrid::Point& dest)
+bool MWMechanics::AiPackage::isReachableRotatingOnTheRun(const MWWorld::Ptr& actor, const osg::Vec3f& dest)
 {
     // get actor's shortest radius for moving in circle
     float speed = actor.getClass().getSpeed(actor);
@@ -383,13 +386,12 @@ bool MWMechanics::AiPackage::isReachableRotatingOnTheRun(const MWWorld::Ptr& act
     radiusDir *= radius;
 
     // pick up the nearest center candidate
-    osg::Vec3f dest_ = PathFinder::MakeOsgVec3(dest);
     osg::Vec3f pos = actor.getRefData().getPosition().asVec3();
     osg::Vec3f center1 = pos - radiusDir;
     osg::Vec3f center2 = pos + radiusDir;
-    osg::Vec3f center = (center1 - dest_).length2() < (center2 - dest_).length2() ? center1 : center2;
+    osg::Vec3f center = (center1 - dest).length2() < (center2 - dest).length2() ? center1 : center2;
 
-    float distToDest = (center - dest_).length();
+    float distToDest = (center - dest).length();
 
     // if pathpoint is reachable for the actor rotating on the run:
     // no points of actor's circle should be farther from the center than destination point

apps/openmw/mwmechanics/aipackage.hpp

@@ -105,23 +105,24 @@ namespace MWMechanics
             bool isTargetMagicallyHidden(const MWWorld::Ptr& target);
 
             /// Return if actor's rotation speed is sufficient to rotate to the destination pathpoint on the run. Otherwise actor should rotate while standing.
-            static bool isReachableRotatingOnTheRun(const MWWorld::Ptr& actor, const ESM::Pathgrid::Point& dest);
+            static bool isReachableRotatingOnTheRun(const MWWorld::Ptr& actor, const osg::Vec3f& dest);
 
         protected:
             /// Handles path building and shortcutting with obstacles avoiding
             /** \return If the actor has arrived at his destination **/
-            bool pathTo(const MWWorld::Ptr& actor, const ESM::Pathgrid::Point& dest, float duration, float destTolerance = 0.0f);
+            bool pathTo(const MWWorld::Ptr& actor, const osg::Vec3f& dest, float duration, float destTolerance = 0.0f);
 
             /// Check if there aren't any obstacles along the path to make shortcut possible
             /// If a shortcut is possible then path will be cleared and filled with the destination point.
             /// \param destInLOS If not nullptr function will return ray cast check result
             /// \return If can shortcut the path
-            bool shortcutPath(const ESM::Pathgrid::Point& startPoint, const ESM::Pathgrid::Point& endPoint, const MWWorld::Ptr& actor, bool *destInLOS, bool isPathClear);
+            bool shortcutPath(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint, const MWWorld::Ptr& actor,
+                              bool *destInLOS, bool isPathClear);
 
             /// Check if the way to the destination is clear, taking into account actor speed
-            bool checkWayIsClearForActor(const ESM::Pathgrid::Point& startPoint, const ESM::Pathgrid::Point& endPoint, const MWWorld::Ptr& actor);
+            bool checkWayIsClearForActor(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint, const MWWorld::Ptr& actor);
 
-            bool doesPathNeedRecalc(const ESM::Pathgrid::Point& newDest, const MWWorld::CellStore* currentCell);
+            bool doesPathNeedRecalc(const osg::Vec3f& newDest, const MWWorld::CellStore* currentCell);
 
             void evadeObstacles(const MWWorld::Ptr& actor, float duration, const ESM::Position& pos);
             void openDoors(const MWWorld::Ptr& actor);
@@ -143,7 +144,7 @@ namespace MWMechanics
 
             bool mIsShortcutting;   // if shortcutting at the moment
             bool mShortcutProhibited; // shortcutting may be prohibited after unsuccessful attempt
-            ESM::Pathgrid::Point mShortcutFailPos; // position of last shortcut fail
+            osg::Vec3f mShortcutFailPos; // position of last shortcut fail
 
         private:
             bool isNearInactiveCell(const ESM::Position& actorPos);

apps/openmw/mwmechanics/aipursue.cpp

@@ -49,13 +49,13 @@ bool AiPursue::execute (const MWWorld::Ptr& actor, CharacterController& characte
     actor.getClass().getCreatureStats(actor).setDrawState(DrawState_Nothing);
 
     //Set the target desition from the actor
-    ESM::Pathgrid::Point dest = target.getRefData().getPosition().pos;
+    const auto dest = target.getRefData().getPosition().asVec3();
     ESM::Position aPos = actor.getRefData().getPosition();
 
     float pathTolerance = 100.0;
 
     if (pathTo(actor, dest, duration, pathTolerance) &&
-        std::abs(dest.mZ - aPos.pos[2]) < pathTolerance)      // check the true distance in case the target is far away in Z-direction
+        std::abs(dest.z() - aPos.pos[2]) < pathTolerance)      // check the true distance in case the target is far away in Z-direction
     {
         target.getClass().activate(target,actor).get()->execute(actor); //Arrest player when reached
         return true;

apps/openmw/mwmechanics/aitravel.cpp

@@ -56,8 +56,7 @@ namespace MWMechanics
         // Unfortunately, with vanilla assets destination is sometimes blocked by other actor.
         // If we got close to target, check for actors nearby. If they are, finish AI package.
         int destinationTolerance = 64;
-        ESM::Pathgrid::Point dest(static_cast<int>(mX), static_cast<int>(mY), static_cast<int>(mZ));
+        if (distance(pos.asVec3(), osg::Vec3f(mX, mY, mZ)) <= destinationTolerance)
-        if (distance(pos.pos, dest) <= destinationTolerance)
         {
             std::vector<MWWorld::Ptr> targetActors;
             std::pair<MWWorld::Ptr, osg::Vec3f> result = MWBase::Environment::get().getWorld()->getHitContact(actor, destinationTolerance, targetActors);
@@ -69,7 +68,7 @@ namespace MWMechanics
             }
         }
 
-        if (pathTo(actor, dest, duration))
+        if (pathTo(actor, osg::Vec3f(mX, mY, mZ), duration))
         {
             actor.getClass().getMovementSettings(actor).mPosition[1] = 0;
             return true;

apps/openmw/mwmechanics/aiwander.cpp

@@ -151,10 +151,8 @@ namespace MWMechanics
         // rebuild a path to it
         if (!mPathFinder.isPathConstructed() && mHasDestination)
         {
-            ESM::Pathgrid::Point dest(PathFinder::MakePathgridPoint(mDestination));
+            mPathFinder.buildSyncedPath(pos.asVec3(), mDestination, actor.getCell(),
-            ESM::Pathgrid::Point start(PathFinder::MakePathgridPoint(pos));
+                getPathGridGraph(actor.getCell()));
-
-            mPathFinder.buildSyncedPath(start, dest, actor.getCell(), getPathGridGraph(actor.getCell()));
 
             if (mPathFinder.isPathConstructed())
                 storage.setState(AiWanderStorage::Wander_Walking);
@@ -292,11 +290,9 @@ namespace MWMechanics
      * Commands actor to walk to a random location near original spawn location.
      */
     void AiWander::wanderNearStart(const MWWorld::Ptr &actor, AiWanderStorage &storage, int wanderDistance) {
-        const ESM::Pathgrid::Point currentPosition = actor.getRefData().getPosition().pos;
+        const auto currentPosition = actor.getRefData().getPosition().asVec3();
-        const osg::Vec3f currentPositionVec3f = osg::Vec3f(currentPosition.mX, currentPosition.mY, currentPosition.mZ);
 
         std::size_t attempts = 10; // If a unit can't wander out of water, don't want to hang here
-        ESM::Pathgrid::Point destinationPosition;
         bool isWaterCreature = actor.getClass().isPureWaterCreature(actor);
         do {
             // Determine a random location within radius of original position
@@ -305,13 +301,15 @@ namespace MWMechanics
             const float destinationX = mInitialActorPosition.x() + wanderRadius * std::cos(randomDirection);
             const float destinationY = mInitialActorPosition.y() + wanderRadius * std::sin(randomDirection);
             const float destinationZ = mInitialActorPosition.z();
-            destinationPosition = ESM::Pathgrid::Point(destinationX, destinationY, destinationZ);
+            const osg::Vec3f destinationPosition(destinationX, destinationY, destinationZ);
             mDestination = osg::Vec3f(destinationX, destinationY, destinationZ);
 
             // Check if land creature will walk onto water or if water creature will swim onto land
             if ((!isWaterCreature && !destinationIsAtWater(actor, mDestination)) ||
-                (isWaterCreature && !destinationThroughGround(currentPositionVec3f, mDestination))) {
+                (isWaterCreature && !destinationThroughGround(currentPosition, mDestination)))
-                mPathFinder.buildSyncedPath(currentPosition, destinationPosition, actor.getCell(), getPathGridGraph(actor.getCell()));
+            {
+                mPathFinder.buildSyncedPath(currentPosition, destinationPosition, actor.getCell(),
+                    getPathGridGraph(actor.getCell()));
                 mPathFinder.addPointToPath(destinationPosition);
 
                 if (mPathFinder.isPathConstructed())
@@ -417,7 +415,7 @@ namespace MWMechanics
         float duration, AiWanderStorage& storage, ESM::Position& pos)
     {
         // Is there no destination or are we there yet?
-        if ((!mPathFinder.isPathConstructed()) || pathTo(actor, ESM::Pathgrid::Point(mPathFinder.getPath().back()), duration, DESTINATION_TOLERANCE))
+        if ((!mPathFinder.isPathConstructed()) || pathTo(actor, osg::Vec3f(mPathFinder.getPath().back()), duration, DESTINATION_TOLERANCE))
         {
             stopWalking(actor, storage);
             storage.setState(AiWanderStorage::Wander_ChooseAction);
@@ -592,14 +590,15 @@ namespace MWMechanics
         ToWorldCoordinates(dest, storage.mCell->getCell());
 
         // actor position is already in world coordinates
-        ESM::Pathgrid::Point start(PathFinder::MakePathgridPoint(actorPos));
+        const auto start = actorPos.asVec3();
 
         // don't take shortcuts for wandering
-        mPathFinder.buildSyncedPath(start, dest, actor.getCell(), getPathGridGraph(actor.getCell()));
+        const auto destVec3f = PathFinder::MakeOsgVec3(dest);
+        mPathFinder.buildSyncedPath(start, destVec3f, actor.getCell(), getPathGridGraph(actor.getCell()));
 
         if (mPathFinder.isPathConstructed())
         {
-            mDestination = osg::Vec3f(dest.mX, dest.mY, dest.mZ);
+            mDestination = destVec3f;
             mHasDestination = true;
             // Remove this node as an option and add back the previously used node (stops NPC from picking the same node):
             ESM::Pathgrid::Point temp = storage.mAllowedNodes[randNode];
@@ -631,15 +630,15 @@ namespace MWMechanics
         // Every now and then check whether one of the doors is opened. (maybe
         // at the end of playing idle?) If the door is opened then re-calculate
         // allowed nodes starting from the spawn point.
-        std::list<ESM::Pathgrid::Point> paths = pathfinder.getPath();
+        auto paths = pathfinder.getPath();
         while(paths.size() >= 2)
         {
-            ESM::Pathgrid::Point pt = paths.back();
+            const auto pt = paths.back();
             for(unsigned int j = 0; j < nodes.size(); j++)
             {
                 // FIXME: doesn't handle a door with the same X/Y
                 //        coordinates but with a different Z
-                if(nodes[j].mX == pt.mX && nodes[j].mY == pt.mY)
+                if (std::abs(nodes[j].mX - pt.x()) <= 0.5 && std::abs(nodes[j].mY - pt.y()) <= 0.5)
                 {
                     nodes.erase(nodes.begin() + j);
                     break;

apps/openmw/mwmechanics/coordinateconverter.cpp

@@ -33,11 +33,12 @@ namespace MWMechanics
         point.y() -= static_cast<float>(mCellY);
     }
 
-    osg::Vec3f CoordinateConverter::toLocalVec3(const ESM::Pathgrid::Point& point)
+    osg::Vec3f CoordinateConverter::toLocalVec3(const osg::Vec3f& point)
     {
         return osg::Vec3f(
-            static_cast<float>(point.mX - mCellX),
+            point.x() - static_cast<float>(mCellX),
-            static_cast<float>(point.mY - mCellY),
+            point.y() - static_cast<float>(mCellY),
-            static_cast<float>(point.mZ));
+            point.z()
+        );
     }
 }

apps/openmw/mwmechanics/coordinateconverter.hpp

@@ -26,7 +26,7 @@ namespace MWMechanics
             /// in-place conversion from world to local
             void toLocal(osg::Vec3f& point);
 
-            osg::Vec3f toLocalVec3(const ESM::Pathgrid::Point& point);
+            osg::Vec3f toLocalVec3(const osg::Vec3f& point);
 
         private:
             int mCellX;

apps/openmw/mwmechanics/pathfinding.cpp

@@ -55,56 +55,16 @@ namespace
             (closestReachableIndex, closestReachableIndex == closestIndex);
     }
 
+    float sqrDistanceIgnoreZ(const osg::Vec3f& point, float x, float y)
+    {
+        x -= point.x();
+        y -= point.y();
+        return (x * x + y * y);
+    }
 }
 
 namespace MWMechanics
 {
-    float sqrDistanceIgnoreZ(const ESM::Pathgrid::Point& point, float x, float y)
-    {
-        x -= point.mX;
-        y -= point.mY;
-        return (x * x + y * y);
-    }
-
-    float distance(const ESM::Pathgrid::Point& point, float x, float y, float z)
-    {
-        x -= point.mX;
-        y -= point.mY;
-        z -= point.mZ;
-        return sqrt(x * x + y * y + z * z);
-    }
-
-    float distance(const ESM::Pathgrid::Point& a, const ESM::Pathgrid::Point& b)
-    {
-        float x = static_cast<float>(a.mX - b.mX);
-        float y = static_cast<float>(a.mY - b.mY);
-        float z = static_cast<float>(a.mZ - b.mZ);
-        return sqrt(x * x + y * y + z * z);
-    }
-
-    float getZAngleToDir(const osg::Vec3f& dir)
-    {
-        return std::atan2(dir.x(), dir.y());
-    }
-
-    float getXAngleToDir(const osg::Vec3f& dir)
-    {
-        float dirLen = dir.length();
-        return (dirLen != 0) ? -std::asin(dir.z() / dirLen) : 0;
-    }
-
-    float getZAngleToPoint(const ESM::Pathgrid::Point &origin, const ESM::Pathgrid::Point &dest)
-    {
-        osg::Vec3f dir = PathFinder::MakeOsgVec3(dest) - PathFinder::MakeOsgVec3(origin);
-        return getZAngleToDir(dir);
-    }
-
-    float getXAngleToPoint(const ESM::Pathgrid::Point &origin, const ESM::Pathgrid::Point &dest)
-    {
-        osg::Vec3f dir = PathFinder::MakeOsgVec3(dest) - PathFinder::MakeOsgVec3(origin);
-        return getXAngleToDir(dir);
-    }
-
     bool checkWayIsClear(const osg::Vec3f& from, const osg::Vec3f& to, float offsetXY)
     {
         osg::Vec3f dir = to - from;
@@ -167,8 +127,7 @@ namespace MWMechanics
      *    j = @.x in local coordinates (i.e. within the cell)
      *    k = @.x in world coordinates
      */
-    void PathFinder::buildPath(const ESM::Pathgrid::Point &startPoint,
+    void PathFinder::buildPath(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint,
-                               const ESM::Pathgrid::Point &endPoint,
                                const MWWorld::CellStore* cell, const PathgridGraph& pathgridGraph)
     {
         mPath.clear();
@@ -225,17 +184,17 @@ namespace MWMechanics
         {
             ESM::Pathgrid::Point temp(mPathgrid->mPoints[startNode]);
             converter.toWorld(temp);
-            mPath.push_back(temp);
+            mPath.push_back(MakeOsgVec3(temp));
         }
         else
         {
-            mPath = pathgridGraph.aStarSearch(startNode, endNode.first);
+            auto path = pathgridGraph.aStarSearch(startNode, endNode.first);
 
             // If nearest path node is in opposite direction from second, remove it from path.
             // Especially useful for wandering actors, if the nearest node is blocked for some reason.
-            if (mPath.size() > 1)
+            if (path.size() > 1)
             {
-                ESM::Pathgrid::Point secondNode = *(++mPath.begin());
+                ESM::Pathgrid::Point secondNode = *(++path.begin());
                 osg::Vec3f firstNodeVec3f = MakeOsgVec3(mPathgrid->mPoints[startNode]);
                 osg::Vec3f secondNodeVec3f = MakeOsgVec3(secondNode);
                 osg::Vec3f toSecondNodeVec3f = secondNodeVec3f - firstNodeVec3f;
@@ -247,17 +206,19 @@ namespace MWMechanics
                     // Add Z offset since path node can overlap with other objects.
                     // Also ignore doors in raytesting.
                     bool isPathClear = !MWBase::Environment::get().getWorld()->castRay(
-                        startPoint.mX, startPoint.mY, startPoint.mZ+16, temp.mX, temp.mY, temp.mZ+16, true);
+                        startPoint.x(), startPoint.y(), startPoint.z() + 16, temp.mX, temp.mY, temp.mZ + 16, true);
                     if (isPathClear)
-                        mPath.pop_front();
+                        path.pop_front();
                 }
             }
 
             // convert supplied path to world coordinates
-            for (std::list<ESM::Pathgrid::Point>::iterator iter(mPath.begin()); iter != mPath.end(); ++iter)
+            std::transform(path.begin(), path.end(), std::back_inserter(mPath),
-            {
+                [&] (ESM::Pathgrid::Point& point)
-                converter.toWorld(*iter);
+                {
-            }
+                    converter.toWorld(point);
+                    return MakeOsgVec3(point);
+                });
         }
 
         // If endNode found is NOT the closest PathGrid point to the endPoint,
@@ -283,9 +244,9 @@ namespace MWMechanics
         if(mPath.empty())
             return 0.;
 
-        const ESM::Pathgrid::Point &nextPoint = *mPath.begin();
+        const auto& nextPoint = mPath.front();
-        float directionX = nextPoint.mX - x;
+        const float directionX = nextPoint.x() - x;
-        float directionY = nextPoint.mY - y;
+        const float directionY = nextPoint.y() - y;
 
         return std::atan2(directionX, directionY);
     }
@@ -297,8 +258,7 @@ namespace MWMechanics
         if(mPath.empty())
             return 0.;
 
-        const ESM::Pathgrid::Point &nextPoint = *mPath.begin();
+        const osg::Vec3f dir = mPath.front() - osg::Vec3f(x, y, z);
-        osg::Vec3f dir = MakeOsgVec3(nextPoint) - osg::Vec3f(x,y,z);
 
         return getXAngleToDir(dir);
     }
@@ -308,8 +268,7 @@ namespace MWMechanics
         if(mPath.empty())
             return true;
 
-        const ESM::Pathgrid::Point& nextPoint = *mPath.begin();
+        if (sqrDistanceIgnoreZ(mPath.front(), x, y) < tolerance*tolerance)
-        if (sqrDistanceIgnoreZ(nextPoint, x, y) < tolerance*tolerance)
         {
             mPath.pop_front();
             if(mPath.empty())
@@ -322,8 +281,7 @@ namespace MWMechanics
     }
 
     // see header for the rationale
-    void PathFinder::buildSyncedPath(const ESM::Pathgrid::Point &startPoint,
+    void PathFinder::buildSyncedPath(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint,
-        const ESM::Pathgrid::Point &endPoint,
         const MWWorld::CellStore* cell, const MWMechanics::PathgridGraph& pathgridGraph)
     {
         if (mPath.size() < 2)
@@ -334,16 +292,14 @@ namespace MWMechanics
         }
         else
         {
-            const ESM::Pathgrid::Point oldStart(*getPath().begin());
+            const auto oldStart = getPath().front();
             buildPath(startPoint, endPoint, cell, pathgridGraph);
             if (mPath.size() >= 2)
             {
                 // if 2nd waypoint of new path == 1st waypoint of old,
                 // delete 1st waypoint of new path.
-                std::list<ESM::Pathgrid::Point>::iterator iter = ++mPath.begin();
+                const auto iter = ++mPath.begin();
-                if (iter->mX == oldStart.mX
+                if (*iter == oldStart)
-                    && iter->mY == oldStart.mY
-                    && iter->mZ == oldStart.mZ)
                 {
                     mPath.pop_front();
                 }

apps/openmw/mwmechanics/pathfinding.hpp

@@ -16,12 +16,31 @@ namespace MWMechanics
 {
     class PathgridGraph;
 
-    float distance(const ESM::Pathgrid::Point& point, float x, float y, float);
+    inline float distance(const osg::Vec3f& lhs, const osg::Vec3f& rhs)
-    float distance(const ESM::Pathgrid::Point& a, const ESM::Pathgrid::Point& b);
+    {
-    float getZAngleToDir(const osg::Vec3f& dir);
+        return (lhs - rhs).length();
-    float getXAngleToDir(const osg::Vec3f& dir);
+    }
-    float getZAngleToPoint(const ESM::Pathgrid::Point &origin, const ESM::Pathgrid::Point &dest);
+
-    float getXAngleToPoint(const ESM::Pathgrid::Point &origin, const ESM::Pathgrid::Point &dest);
+    inline float getZAngleToDir(const osg::Vec3f& dir)
+    {
+        return std::atan2(dir.x(), dir.y());
+    }
+
+    inline float getXAngleToDir(const osg::Vec3f& dir)
+    {
+        float dirLen = dir.length();
+        return (dirLen != 0) ? -std::asin(dir.z() / dirLen) : 0;
+    }
+
+    inline float getZAngleToPoint(const osg::Vec3f& origin, const osg::Vec3f& dest)
+    {
+        return getZAngleToDir(dest - origin);
+    }
+
+    inline float getXAngleToPoint(const osg::Vec3f& origin, const osg::Vec3f& dest)
+    {
+        return getXAngleToDir(dest - origin);
+    }
 
      const float PATHFIND_Z_REACH = 50.0f;
     //static const float sMaxSlope = 49.0f; // duplicate as in physicssystem
@@ -55,7 +74,7 @@ namespace MWMechanics
 
             void clearPath();
 
-            void buildPath(const ESM::Pathgrid::Point &startPoint, const ESM::Pathgrid::Point &endPoint,
+            void buildPath(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint,
                            const MWWorld::CellStore* cell, const PathgridGraph& pathgridGraph);
 
             bool checkPathCompleted(float x, float y, float tolerance = PathTolerance);
@@ -76,7 +95,7 @@ namespace MWMechanics
                 return mPath.size();
             }
 
-            const std::list<ESM::Pathgrid::Point>& getPath() const
+            const std::list<osg::Vec3f>& getPath() const
             {
                 return mPath;
             }
@@ -90,10 +109,10 @@ namespace MWMechanics
                 makes the 2nd point of the new path == the 1st point of old path.
                 Which results in NPC "running in a circle" back to the just passed waypoint.
              */
-            void buildSyncedPath(const ESM::Pathgrid::Point &startPoint, const ESM::Pathgrid::Point &endPoint,
+            void buildSyncedPath(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint,
                 const MWWorld::CellStore* cell, const PathgridGraph& pathgridGraph);
 
-            void addPointToPath(const ESM::Pathgrid::Point &point)
+            void addPointToPath(const osg::Vec3f& point)
             {
                 mPath.push_back(point);
             }
@@ -153,7 +172,7 @@ namespace MWMechanics
             }
 
         private:
-            std::list<ESM::Pathgrid::Point> mPath;
+            std::list<osg::Vec3f> mPath;
 
             const ESM::Pathgrid *mPathgrid;
             const MWWorld::CellStore* mCell;