Prevent NPC suicides off silt the strider platform in Seyda Neen. Added some comments as well. There may be opportunities for some optimization but left that out for now.

apps/openmw/mwmechanics/pathfinding.cpp

@@ -57,97 +57,6 @@ namespace
         return closestIndex;
     }
 
-    /*std::list<ESM::Pathgrid::Point> reconstructPath(const std::vector<MWMechanics::PathFinder::Node>& graph,const ESM::Pathgrid* pathgrid, int lastNode,float xCell, float yCell)
-    {
-        std::list<ESM::Pathgrid::Point> path;
-        while(graph[lastNode].parent != -1)
-        {
-            //std::cout << "not empty" << xCell;
-            ESM::Pathgrid::Point pt = pathgrid->mPoints[lastNode];
-            pt.mX += xCell;
-            pt.mY += yCell;
-            path.push_front(pt);
-            lastNode = graph[lastNode].parent;
-        }
-        return path;
-    }*/
-
-
-
-    /*std::list<ESM::Pathgrid::Point> buildPath2(const ESM::Pathgrid* pathgrid,int start,int goal,float xCell = 0, float yCell = 0)
-    {
-        std::vector<Node> graph;
-        for(unsigned int i = 0; i < pathgrid->mPoints.size(); i++)
-        {
-            Node node;
-            node.label = i;
-            node.parent = -1;
-            graph.push_back(node);
-        }
-        for(unsigned int i = 0; i < pathgrid->mEdges.size(); i++)
-        {
-            Edge edge;
-            edge.destination = pathgrid->mEdges[i].mV1;
-            edge.cost = distance(pathgrid->mPoints[pathgrid->mEdges[i].mV0],pathgrid->mPoints[pathgrid->mEdges[i].mV1]);
-            graph[pathgrid->mEdges[i].mV0].edges.push_back(edge);
-            edge.destination = pathgrid->mEdges[i].mV0;
-            graph[pathgrid->mEdges[i].mV1].edges.push_back(edge);
-        }
-
-        std::vector<float> g_score(pathgrid->mPoints.size(),-1.);
-        std::vector<float> f_score(pathgrid->mPoints.size(),-1.);
-
-        g_score[start] = 0;
-        f_score[start] = distance(pathgrid->mPoints[start],pathgrid->mPoints[goal]);
-
-        std::list<int> openset;
-        std::list<int> closedset;
-        openset.push_back(start);
-
-        int current = -1;
-
-        while(!openset.empty())
-        {
-            current = openset.front();
-            openset.pop_front();
-
-            if(current == goal) break;
-
-            closedset.push_back(current);
-
-            for(int j = 0;j<graph[current].edges.size();j++)
-            {
-                //int next = graph[current].edges[j].destination
-                if(std::find(closedset.begin(),closedset.end(),graph[current].edges[j].destination) == closedset.end())
-                {
-                    int dest = graph[current].edges[j].destination;
-                    float tentative_g = g_score[current] + graph[current].edges[j].cost;
-                    bool isInOpenSet = std::find(openset.begin(),openset.end(),dest) != openset.end();
-                    if(!isInOpenSet
-                        || tentative_g < g_score[dest] )
-                    {
-                        graph[dest].parent = current;
-                        g_score[dest] = tentative_g;
-                        f_score[dest] = tentative_g + distance(pathgrid->mPoints[dest],pathgrid->mPoints[goal]);
-                        if(!isInOpenSet)
-                        {
-                            std::list<int>::iterator it = openset.begin();
-                            for(it = openset.begin();it!= openset.end();it++)
-                            {
-                                if(g_score[*it]>g_score[dest])
-                                    break;
-                            }
-                            openset.insert(it,dest);
-                        }
-                    }
-                }
-            }
-
-        }
-        return reconstructPath(graph,pathgrid,current,xCell,yCell);
-
-    }*/
-
 }
 
 namespace MWMechanics
@@ -166,37 +75,83 @@ namespace MWMechanics
         mIsPathConstructed = false;
     }
 
+    /*
+     * NOTE: based on buildPath2(), please check git history if interested
+     *
+     * Populate mGraph with the cost of each allowed edge (measured in distance ^2)
+     * Any existing data in mGraph is wiped clean first.  The node's parent is
+     * set with initial value of -1. The parent values are populated by aStarSearch().
+     * mGSore and mFScore are also resized.
+     *
+     *
+     * mGraph[f].edges[n].destination = t
+     *
+     *   f = point index of location "from"
+     *   t = point index of location "to"
+     *   n = index of edges from point f
+     *
+     *
+     * Example: (note from p(0) to p(2) not allowed)
+     *
+     *   mGraph[0].edges[0].destination = 1
+     *            .edges[1].destination = 3
+     *
+     *   mGraph[1].edges[0].destination = 0
+     *            .edges[1].destination = 2
+     *            .edges[2].destination = 3
+     *
+     *   mGraph[2].edges[0].destination = 1
+     *
+     *   (etc, etc)
+     *
+     *
+     *        low
+     *        cost
+     *   p(0) <---> p(1) <------------> p(2)
+     *    ^          ^
+     *    |          |
+     *    |          +-----> p(3)
+     *    +---------------->
+     *      high cost
+     */
     void PathFinder::buildPathgridGraph(const ESM::Pathgrid* pathGrid)
     {
         mGraph.clear();
-        mGScore.resize(pathGrid->mPoints.size(),-1);
+        // resize lists
-        mFScore.resize(pathGrid->mPoints.size(),-1);
+        mGScore.resize(pathGrid->mPoints.size(), -1);
+        mFScore.resize(pathGrid->mPoints.size(), -1);
         Node defaultNode;
         defaultNode.label = -1;
         defaultNode.parent = -1;
         mGraph.resize(pathGrid->mPoints.size(),defaultNode);
+        // initialise mGraph
         for(unsigned int i = 0; i < pathGrid->mPoints.size(); i++)
         {
             Node node;
             node.label = i;
             node.parent = -1;
-            mGraph[i] = node;
+            mGraph[i] = node; // TODO: old code used push_back(node), check if any difference
         }
+        // store the costs (measured in distance ^2) of each edge, in both directions
         for(unsigned int i = 0; i < pathGrid->mEdges.size(); i++)
         {
             Edge edge;
+            edge.cost = distance(pathGrid->mPoints[pathGrid->mEdges[i].mV0],
+                                 pathGrid->mPoints[pathGrid->mEdges[i].mV1]);
+            // forward path of the edge
             edge.destination = pathGrid->mEdges[i].mV1;
-            edge.cost = distance(pathGrid->mPoints[pathGrid->mEdges[i].mV0],pathGrid->mPoints[pathGrid->mEdges[i].mV1]);
             mGraph[pathGrid->mEdges[i].mV0].edges.push_back(edge);
-            edge.destination = pathGrid->mEdges[i].mV0;
+            // reverse path of the edge
-            mGraph[pathGrid->mEdges[i].mV1].edges.push_back(edge);
+            // NOTE: These are redundant, ESM already contains the required reverse paths
+            //edge.destination = pathGrid->mEdges[i].mV0;
+            //mGraph[pathGrid->mEdges[i].mV1].edges.push_back(edge);
         }
         mIsGraphConstructed = true;
     }
 
     void PathFinder::cleanUpAStar()
     {
-        for(int i=0;i<static_cast<int> (mGraph.size());i++)
+        for(int i = 0; i < static_cast<int> (mGraph.size()); i++)
         {
             mGraph[i].parent = -1;
             mGScore[i] = -1;
@@ -204,6 +159,25 @@ namespace MWMechanics
         }
     }
 
+    /*
+     * NOTE: based on buildPath2(), please check git history if interested
+     *
+     * Find the shortest path to the target goal using a well known algorithm.
+     * Uses mGraph which has pre-computed costs for allowed edges.  It is assumed
+     * that mGraph is already constructed.  The caller, i.e.  buildPath(), needs
+     * to ensure this.
+     *
+     * Returns path (a list of pathgrid point indexes) which may be empty.
+     *
+     * openset - point indexes to be traversed, lowest cost at the front
+     * closedset - point indexes already traversed
+     *
+     * mGScore - past accumulated costs vector indexed by point index
+     * mFScore - future estimated costs vector indexed by point index
+     *   these are resized by buildPathgridGraph()
+     *
+     * The heuristics used is distance^2 from current position to the final goal.
+     */
     std::list<ESM::Pathgrid::Point> PathFinder::aStarSearch(const ESM::Pathgrid* pathGrid,int start,int goal,float xCell, float yCell)
     {
         cleanUpAStar();
@@ -218,18 +192,19 @@ namespace MWMechanics
 
         while(!openset.empty())
         {
-            current = openset.front();
+            current = openset.front(); // front has the lowest cost
             openset.pop_front();
 
             if(current == goal) break;
 
             closedset.push_back(current);
 
-            for(int j = 0;j<static_cast<int> (mGraph[current].edges.size());j++)
+            // check all edges for the "current" point index
+            for(int j = 0; j < static_cast<int> (mGraph[current].edges.size()); j++)
             {
-                //int next = mGraph[current].edges[j].destination
                 if(std::find(closedset.begin(),closedset.end(),mGraph[current].edges[j].destination) == closedset.end())
                 {
+                    // not in closedset - i.e. have not traversed this edge destination
                     int dest = mGraph[current].edges[j].destination;
                     float tentative_g = mGScore[current] + mGraph[current].edges[j].cost;
                     bool isInOpenSet = std::find(openset.begin(),openset.end(),dest) != openset.end();
@@ -241,20 +216,22 @@ namespace MWMechanics
                         mFScore[dest] = tentative_g + distance(pathGrid->mPoints[dest],pathGrid->mPoints[goal]);
                         if(!isInOpenSet)
                         {
+                            // add this edge to openset, lowest cost goes to the front
+                            // TODO: if this causes performance problems a hash table may help (apparently)
                             std::list<int>::iterator it = openset.begin();
                             for(it = openset.begin();it!= openset.end();it++)
                             {
-                                if(mGScore[*it]>mGScore[dest])
+                                if(mGScore[*it] > mGScore[dest])
                                     break;
                             }
-                            openset.insert(it,dest);
+                            openset.insert(it, dest);
                         }
                     }
+                } // if in closedset, i.e. traversed this edge already, try the next edge
             }
         }
 
-        }
+        // reconstruct path to return
-
         std::list<ESM::Pathgrid::Point> path;
         while(mGraph[current].parent != -1)
         {
@@ -266,6 +243,9 @@ namespace MWMechanics
             current = mGraph[current].parent;
         }
 
+        // TODO: Is this a bug?  If path is empty the destination is inserted.
+        //       Commented out pending further testing.
+#if 0
         if(path.empty())
         {
             ESM::Pathgrid::Point pt = pathGrid->mPoints[goal];
@@ -273,10 +253,19 @@ namespace MWMechanics
             pt.mY += yCell;
             path.push_front(pt);
         }
-
+#endif
         return path;
     }
 
+    /*
+     * NOTE: This method may fail to find a path.  The caller must check the result before using it.
+     * If there is no path the AI routies need to implement some other heuristics to reach the target.
+     *
+     * Updates mPath using aStarSearch().
+     *   mPathConstructed is set true if successful, false if not
+     *
+     * May update mGraph by calling buildPathgridGraph() if it isn't constructed yet.
+     */
     void PathFinder::buildPath(const ESM::Pathgrid::Point &startPoint, const ESM::Pathgrid::Point &endPoint,
                                const MWWorld::CellStore* cell, bool allowShortcuts)
     {
@@ -310,11 +299,10 @@ namespace MWMechanics
             {
                 if(!mIsGraphConstructed) buildPathgridGraph(pathGrid);
 
-                mPath = aStarSearch(pathGrid,startNode,endNode,xCell,yCell);//findPath(startNode, endNode, mGraph);
+                mPath = aStarSearch(pathGrid,startNode,endNode,xCell,yCell);
 
                 if(!mPath.empty())
                 {
-                    mPath.push_back(endPoint);
                     mIsPathConstructed = true;
                 }
             }
@@ -331,8 +319,8 @@ namespace MWMechanics
 
     float PathFinder::getZAngleToNext(float x, float y) const
     {
-        // This should never happen (programmers should have an if statement checking mIsPathConstructed that prevents this call
+        // This should never happen (programmers should have an if statement checking
-        // if otherwise).
+        // mIsPathConstructed that prevents this call if otherwise).
         if(mPath.empty())
             return 0.;