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

README.md

@@ -24,6 +24,7 @@ This game is aimed to be a puzzle game in which the player sets different laws f
   - [ ] Creation
   - [ ] Interaction with entities
   - [ ] Parsing of rules and creation of subsequent decorators
+  - [ ] Raytrace in order to find best goal for pathfinding ?
  - [ ] Graphics
   - [x] Scene rendering
   - [ ] UI
@@ -34,3 +35,4 @@ This game is aimed to be a puzzle game in which the player sets different laws f
   - [ ] Rule application
   - [ ] Saving state through serialization ?
  - [ ] UML
+ - [ ] Unit tests

resources/test_level.xml

@@ -1,7 +1,7 @@
 <?xml version = "1.0"?>
-<Level w="10" h="10" textureId="0">
+<Level w="20" h="20" textureId="0">
     <Entity x="0" y="0" type="Citizen"/>
     <Entity x="0" y="1" type="Citizen"/>
     <Entity x="10" y="10" type="Significant" textureId="1"/>
-    <Entity x="1" y="1" w="8" h="8" type="House" textureId="2"/>
+    <Entity x="1" y="1" w="5" h="5" type="House" textureId="2"/>
 </Level>

src/Entity.cpp

@@ -19,9 +19,10 @@ Entity::Entity(int x, int y, EntityType type, sf::Texture* texture, int width, i
     shape.setPosition((x+0.5*width)*pro_maat::pixelsPerUnit,(y+0.5*width)*pro_maat::pixelsPerUnit);
     shape.setTexture(texture);
 
-    currentState = State::Idle;
+    // FIXME : Testing purposes
+    currentState = State::Moving;
     nextState = State::Idle;
-    target = pro_maat::GridPos(x,y);
+    target = pro_maat::GridPos(x+10,y);
     nextTarget = target;
 }
 
@@ -36,7 +37,6 @@ Entity::Entity(const pugi::xml_node& entityNode, sf::Texture* texture)
 void Entity::move(Orientation orientation)
 {
     // TODO : Add speed ?
-    shape.setRotation(static_cast<float>(orientation));
 
     sf::Vector2f movementVector(0,0);
     switch (orientation)
@@ -53,8 +53,11 @@ void Entity::move(Orientation orientation)
         case Orientation::West:
             movementVector.x = -pro_maat::pixelsPerUnit;
             break;
+        case Orientation::None:
+            return;
     }
 
+    shape.setRotation(static_cast<float>(orientation));
     shape.setPosition(shape.getPosition()+movementVector);
 }
 
@@ -74,19 +77,22 @@ const State Entity::getState() const
 const pro_maat::GridPos Entity::getPosition() const
 {
     // Safe : size is a multiple of pro_maat::pixelsPerUnit
-    uint8_t x = (shape.getPosition().x-0.5*shape.getSize().x)/pro_maat::pixelsPerUnit;
+    pro_maat::GridUnit x = shape.getPosition().x/pro_maat::pixelsPerUnit;
-    uint8_t y = (shape.getPosition().y-0.5*shape.getSize().y)/pro_maat::pixelsPerUnit;
+    pro_maat::GridUnit y = shape.getPosition().y/pro_maat::pixelsPerUnit;
     return pro_maat::GridPos(x,y);
 }
 
 const std::vector<pro_maat::GridPos> Entity::getOccupiedSquares() const
 {
     std::vector<pro_maat::GridPos> occupiedSquares;
+
     // Safe : size is a multiple of pro_maat::pixelsPerUnit
-    uint8_t w = shape.getSize().x/pro_maat::pixelsPerUnit;
+    pro_maat::GridUnit w = shape.getSize().x/pro_maat::pixelsPerUnit;
-    uint8_t h = shape.getSize().y/pro_maat::pixelsPerUnit;
+    pro_maat::GridUnit h = shape.getSize().y/pro_maat::pixelsPerUnit;
-    uint8_t x = shape.getPosition().x/pro_maat::pixelsPerUnit - 0.5*w;
+    pro_maat::GridUnit x = shape.getPosition().x/pro_maat::pixelsPerUnit - 0.5*w;
-    uint8_t y = shape.getPosition().y/pro_maat::pixelsPerUnit - 0.5*h;
+    pro_maat::GridUnit y = shape.getPosition().y/pro_maat::pixelsPerUnit - 0.5*h;
+
+    occupiedSquares.reserve(w*h);
 
     for(int i = 0;i<w;i++)
     {

src/Entity.h

@@ -34,6 +34,7 @@ enum class Orientation
     East = 90,
     South = 180,
     West = 270,
+    None,
 };
 
 
@@ -47,13 +48,15 @@ public:
     void move(Orientation orientation);
     void update();
 
-    const sf::RectangleShape& getShape() const;
     const State getState() const;
+
+    const sf::RectangleShape& getShape() const;
+    /// Returns the grid coordinates at the center of the entity
     const pro_maat::GridPos getPosition() const;
+
     // Don't like it : iterates over every square every tick
     const std::vector<pro_maat::GridPos> getOccupiedSquares() const;
 
-
     /// Position of the target of the current action on the map
     pro_maat::GridPos target;
 private:
@@ -62,6 +65,7 @@ private:
     EntityType type;
 
     // As it contains position, size and orientation, we do not need anything more
+    // TODO : Maybe we need something more : lots of computations
     sf::RectangleShape shape;
 
     State currentState;

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
+    std::set<pro_maat::GridPos> openNodes{start};
-    return Orientation::East;
+    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;
 }

src/Level.h

@@ -25,7 +25,7 @@ public:
     void runStep();
 
 private:
-    const sf::Vector2i size;
+    const pro_maat::GridPos size;
 
     std::vector<Entity> entities;
 
@@ -37,7 +37,15 @@ private:
 
     std::vector<pro_maat::GridPos> occupiedSquares;
 
-    Orientation findPath(pro_maat::GridPos start, pro_maat::GridPos end, int sign);
+    /// If sign is +1, finds a path between start and goal positions.
+    /// If sign is -1, finds a path away from the goal position, from the starting position.
+    /// THIS DOES NOT HANDLE ENTITIES BIGGER THAN ONE SQUARE
+    ///
+    /// \param start Starting position
+    /// \param goal  Goal position
+    /// \param sign  +1 or -1, defines if we go towards or away from the goal
+    /// \return      Orientation of the next move if successful, Orientation::None otherwise.
+    Orientation findPath(pro_maat::GridPos start, pro_maat::GridPos goal, int sign);
 };
 
 

src/Utils.cpp

@@ -37,10 +37,33 @@ namespace pro_maat
         }
     }
 
-    float manhattanDistance(pro_maat::GridPos leftHandSide, pro_maat::GridPos rightHandSide)
+    float manhattanDistance(const GridPos& leftHandSide, const GridPos& rightHandSide)
     {
         // The *0.01 helps with breaking ties and minimizing exploration
         // As per http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html#breaking-ties
-        return (std::abs(rightHandSide.first-leftHandSide.first)+std::abs(rightHandSide.second-leftHandSide.second))*1.01;
+        return (std::abs(rightHandSide.first-leftHandSide.first)+
+                std::abs(rightHandSide.second-leftHandSide.second))*1.01;
+    }
+
+    const std::vector<pro_maat::GridPos> getNeighbours(const pro_maat::GridPos& origin, const GridPos& gridSize)
+    {
+        std::vector<pro_maat::GridPos> neighbours{};
+        neighbours.reserve(4);
+
+        for(GridUnit i = std::max(origin.first-1,0);i<=std::min(static_cast<GridUnit>(origin.first+1),gridSize.first);i++)
+        {
+            // Do not add the original position
+            if(i == origin.first) continue;
+
+            neighbours.emplace_back(i,origin.second);
+        }
+        for(GridUnit i = std::max(origin.second-1,0);i<=std::min(static_cast<GridUnit>(origin.second+1),gridSize.second);i++)
+        {
+            if(i == origin.second) continue;
+
+            neighbours.emplace_back(origin.first,i);
+        }
+
+        return std::move(neighbours);
     }
 }

src/Utils.h

@@ -12,10 +12,12 @@
 
 namespace pro_maat
 {
-// Used with positions on the map grid
+// Used as the base of all grid based computations
-using GridPos = std::pair<uint8_t,uint8_t>;
+using GridUnit = int16_t;
+// Used when dealing with the grid representation of the map
+using GridPos = std::pair<GridUnit,GridUnit>;
 
-static constexpr uint8_t pixelsPerUnit = 50;
+static constexpr uint8_t pixelsPerUnit = 30;
 static constexpr char levelFolder[] = "resources/";
 static constexpr char textureFolder[] = "resources/";
 static constexpr char fontFolder[] = "resources/";
@@ -23,8 +25,9 @@ static constexpr char fontFolder[] = "resources/";
 void errorWindow(const std::string& error);
 
 // Good heuristic on 4-way grids
-float manhattanDistance(pro_maat::GridPos leftHandSide, pro_maat::GridPos rightHandSide);
+float manhattanDistance(const GridPos& leftHandSide, const GridPos& rightHandSide);
-
+/// Gets the 4 cardinal neighbours on the grid with edge checking
+const std::vector<pro_maat::GridPos> getNeighbours(const GridPos& origin, const GridPos& gridSize);
 }
 
 #endif //PROJECT_MAAT_UTILS_H