A* implementation

Working except for end of path endless loop (0,0 point appearing in paths for no reason?)

Commit mainly because the HDD is on the verge of dying

src/Level.cpp

@@ -7,7 +7,7 @@
 
 Level::Level(const pugi::xml_document& xmlDoc, const TextureStore& textureStore)
     : textures(textureStore),
-      size(xmlDoc.child("Level").attribute("width").as_int(),xmlDoc.child("Level").attribute("width").as_int())
+      size(xmlDoc.child("Level").attribute("w").as_int(),xmlDoc.child("Level").attribute("h").as_int())
 {
     pugi::xml_node levelNode = xmlDoc.child("Level");
     for(const pugi::xml_node& child : levelNode.children())
@@ -17,7 +17,7 @@ Level::Level(const pugi::xml_document& xmlDoc, const TextureStore& textureStore)
             entities.emplace_back(child,textures.at(child.attribute("textureId").as_int(0)).get());
 
             // Initialize the occupied squares vector with the new entity's squares
-            std::vector<std::pair<uint8_t,uint8_t>> entitySquares = entities.rbegin()->getOccupiedSquares();
+            std::vector<pro_maat::GridPos> entitySquares = entities.rbegin()->getOccupiedSquares();
             std::move(entitySquares.begin(),entitySquares.end(),std::back_inserter(occupiedSquares));
         }
     }
@@ -62,8 +62,14 @@ void Level::runStep()
             case State::Idle:break;
         }
 
+        std::vector<pro_maat::GridPos> entitySquares = entity.getOccupiedSquares();
+
+        // FIXME : Very heavy memory usage and a lot of duplicates, slows down occupiedSquares.find() calls too.
+        // Copy the squares to the currently occupied squares : prevents moving into an entity that just moved
+        occupiedSquares.insert(occupiedSquares.end(),entitySquares.begin(),entitySquares.end());
+        std::sort(occupiedSquares.begin(),occupiedSquares.end());
+
         // Moves the occupied squares from the entity to the new occupied squares vector
-        std::vector<std::pair<uint8_t,uint8_t>> entitySquares = entity.getOccupiedSquares();
         std::move(entitySquares.begin(),entitySquares.end(),std::back_inserter(newOccupiedSquares));
     }
 
@@ -72,8 +78,108 @@ void Level::runStep()
     std::sort(occupiedSquares.begin(),occupiedSquares.end());
 }
 
-Orientation Level::findPath(pro_maat::GridPos start, pro_maat::GridPos end, int sign)
+Orientation Level::findPath(pro_maat::GridPos start, pro_maat::GridPos goal, int sign)
 {
-    // TODO : A* which returns the next move
-    return Orientation::East;
+    std::set<pro_maat::GridPos> openNodes{start};
+    std::set<pro_maat::GridPos> closedNodes{};
+
+    std::map<pro_maat::GridPos,float> estimatedCosts{{start,pro_maat::manhattanDistance(start,goal)*sign}};
+    std::map<pro_maat::GridPos,float> pathCosts{{start,0}};
+    std::map<pro_maat::GridPos,pro_maat::GridPos> paths{};
+
+
+    const std::vector<pro_maat::GridPos> goalNeighbours = pro_maat::getNeighbours(goal,size);
+    // Save the iterators : vector is const and .begin() and .end() might get called a lot
+    auto goalNeighboursBeginIterator = goalNeighbours.begin();
+    auto goalNeighboursEndIterator = goalNeighbours.end();
+
+
+    // FIXME : Find an efficient way to get rid of openNodes.find calls
+    // Lambda checking if the current element is also in the open nodes set
+    auto compWithOpen = [&openNodes](const std::pair<pro_maat::GridPos,float>& leftHandSide,
+                                     const std::pair<pro_maat::GridPos,float>& rightHandSide){
+        if(openNodes.find(leftHandSide.first) == openNodes.end())
+        {
+            return (false);
+        }
+        else if(openNodes.find(rightHandSide.first) == openNodes.end())
+        {
+            return (true);
+        }
+        else
+        {
+            return (leftHandSide.second < rightHandSide.second);
+        }
+    };
+
+
+    while(!openNodes.empty())
+    {
+        // Expand from the open node with the smallest estimated cost
+        pro_maat::GridPos currentNode = std::min_element(estimatedCosts.begin(),estimatedCosts.end(),compWithOpen)->first;
+
+        if(std::find(goalNeighboursBeginIterator,goalNeighboursEndIterator,currentNode) != goalNeighboursEndIterator)
+        {
+            // Trace back to the start
+            pro_maat::GridPos& previousNode = paths[currentNode];
+            for(;paths[previousNode]!=start;previousNode = paths[previousNode])
+            {}
+
+            pro_maat::GridUnit dx = previousNode.first - start.first;
+            pro_maat::GridUnit dy = previousNode.second - start.second;
+
+            if(dx < 0)
+            {
+                return(Orientation::West);
+            }
+            else if(dx > 0)
+            {
+                return(Orientation::East);
+            }
+            else if(dy < 0)
+            {
+                return(Orientation::North);
+            }
+            else if(dy > 0 )
+            {
+                return(Orientation::South);
+            }
+            else
+            {
+                return(Orientation::None);
+            }
+        }
+
+        openNodes.erase(currentNode);
+        closedNodes.insert(currentNode);
+
+        for(pro_maat::GridPos neighbour : pro_maat::getNeighbours(currentNode,size))
+        {
+            // Checks if node has been closed or is an obstacle
+            if(std::find(occupiedSquares.begin(),occupiedSquares.end(),neighbour) != occupiedSquares.end() ||
+               closedNodes.find(neighbour) != closedNodes.end())
+            {
+                continue;
+            }
+
+            // As the neighbours are adjacent squares, distance from them is always +-1
+            float newPathCost = pathCosts[currentNode] + sign;
+
+            if(openNodes.find(neighbour) == openNodes.end())
+            {
+                openNodes.insert(neighbour);
+            }
+            else if(newPathCost >= pathCosts[neighbour]) // If the node is open but this path is longer, ignore it
+            {
+                continue;
+            }
+
+            pathCosts.insert_or_assign(neighbour,newPathCost);
+            estimatedCosts.insert_or_assign(neighbour,newPathCost + pro_maat::manhattanDistance(neighbour,goal));
+            auto returnPathIt = paths.insert_or_assign(neighbour,currentNode);
+        }
+    }
+
+    // If we did not find a path, do not move
+    return Orientation::None;
 }