├── README.md ├── main.cpp ├── pathfinding.hpp ├── tileadaptor.hpp └── utility.hpp /README.md: -------------------------------------------------------------------------------- 1 | # Lazy Theta* with optimization pathfinding 2 | 3 | Files: 4 | 5 | - pathfinding.hpp 6 | - The main file, the only one you need 7 | - Contain the algoritm 8 | - tileadaptor.hpp 9 | - To use the pathfinding class you'll need an adaptor, this is an exemple adaptor for tile grid 10 | - utility.hpp 11 | - Dummy vector class and distance function used by tileadaptor 12 | - main.cpp 13 | - Console based demo 14 | 15 | ``` 16 | ###################################################################### 17 | #S # # # 18 | # # # # 19 | # # # # 20 | # # # 2 3# 21 | # # # ################## # 22 | # # # # # 23 | # # # #4 # 24 | # # # # #### 25 | # # # # # 26 | # # # # # 27 | # # # # # 28 | # # # # # 29 | # # # # # 30 | # 0 # # # 31 | # # # # 32 | # 1 #5 # 33 | # # # 34 | # # E# 35 | ###################################################################### 36 | # = walls 37 | S = start 38 | E = end 39 | number = path nodes 40 | ``` 41 | 42 | Implementation of the algorithm described here: http://aigamedev.com/open/tutorial/lazy-theta-star/ 43 | 44 | At first I could find any code a part from this and honestly I had no idea of what line 37-38 means 45 | ![pseudo cat](http://aigamedev.com/wp-content/blogs.dir/5/files/2013/07/fig53-full.png) 46 | 47 | After some amount of research I found this page http://idm-lab.org/project-o.html which provided me with some code. 48 | I rewrote most of it because the code style dind't suite me and to take advantage of C++14. 49 | 50 | I also made it more modular so that it can be used for grid, hexagonTile, polygon map, anything where you need to find a path between to point. 51 | 52 | Also added the possibility to use weighted h-value as described in the first link, it make the search faster but may give slitghly longer path. 53 | -------------------------------------------------------------------------------- /main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "tileadaptor.hpp" 4 | 5 | int main() 6 | { 7 | constexpr int mapSizeX = 70; 8 | constexpr int mapSizeY = 20; 9 | 10 | //Normaly would have used a std::vector of size x*y but just for this test it's 11 | //Gonna be way easier and readeable to do it this way 12 | std::array, mapSizeX> map; 13 | 14 | Vectori startPoint = {1, 1}; 15 | Vectori endPoint = {mapSizeX - 2, mapSizeY - 2}; 16 | 17 | auto makeWall = [&map](const Vectori& pos, const Vectori& size) 18 | { 19 | for(int x = 0; x < size.x; x++) 20 | { 21 | for(int y = 0; y < size.y; y++) 22 | { 23 | map[pos.x + x][pos.y + y] = '#'; 24 | } 25 | } 26 | }; 27 | 28 | //Instantiating our path adaptor 29 | //passing the map size and a lambda that return false if the tile is a wall 30 | TileAdaptor adaptor({mapSizeX, mapSizeY}, [&map](const Vectori& vec){return map[vec.x][vec.y] != '#';}); 31 | //This is a bit of an exageration here for the weight, but it did make my performance test go from 8s to 2s 32 | Pathfinder pathfinder(adaptor, 100.f /*weight*/); 33 | 34 | //set everythings to space 35 | for(auto& cs : map) 36 | for(auto& c : cs) 37 | c = ' '; 38 | 39 | //borders 40 | makeWall({0, 0}, {mapSizeX, 1}); 41 | makeWall({0, 0}, {1, mapSizeY}); 42 | makeWall({0, mapSizeY - 1}, {mapSizeX, 1}); 43 | makeWall({mapSizeX - 1, 0}, {1, mapSizeY}); 44 | 45 | //walls 46 | makeWall({5, 0}, {1, mapSizeY - 6}); 47 | makeWall({mapSizeX - 6, 5}, {1, mapSizeY - 6}); 48 | 49 | makeWall({mapSizeX - 6, 5}, {4, 1}); 50 | makeWall({mapSizeX - 4, 8}, {4, 1}); 51 | 52 | makeWall({20, 0}, {1, mapSizeY - 4}); 53 | makeWall({mapSizeX - 20, 5}, {14, 1}); 54 | 55 | //start and end point 56 | map[startPoint.x][startPoint.y] = 'S'; 57 | map[endPoint.x][endPoint.y] = 'E'; 58 | 59 | //The map was edited so we need to regenerate teh neighbors 60 | pathfinder.generateNodes(); 61 | 62 | //doing the search 63 | //merly to show the point of how it work 64 | //as it would have been way easier to simply transform the vector to id and pass it to search 65 | auto nodePath = pathfinder.search(adaptor.posToId(startPoint), adaptor.posToId(endPoint)); 66 | 67 | //Convert Ids onto map position 68 | std::vector path; 69 | path.reserve(nodePath.size()); 70 | 71 | for(const auto id : nodePath) 72 | path.push_back(adaptor.idToPos(id)); 73 | 74 | 75 | //There is also a serach function that do the conversion between your data type 76 | //And the id, it take lambdas to convert between the two 77 | //Here's an exemple of how it would be called for this code 78 | // auto path = pathfinder.search(startPoint, endPoint, 79 | // { 80 | // [&adaptor](const auto id) 81 | // { 82 | // return adaptor.idToPos(id); 83 | // } 84 | // }, 85 | // { 86 | // [&adaptor](const auto& data) 87 | // { 88 | // return adaptor.posToId(data); 89 | // } 90 | // }); 91 | 92 | 93 | //If we found a path we just want to remove the first and last node 94 | //Because it will be at our start and end position 95 | if(path.size()) 96 | { 97 | path.pop_back(); 98 | path.erase(path.begin()); 99 | } 100 | 101 | //nodes 102 | int x = 0; 103 | for(const auto& node : path) 104 | map[node.x][node.y] = '0' + x++; 105 | 106 | //draw map 107 | for(int y = 0; y < map[0].size(); y++) 108 | { 109 | for(int x = 0; x < map.size(); x++) 110 | std::cout << map[x][y]; 111 | 112 | std::cout << std::endl; 113 | } 114 | 115 | std::cout << "# = walls" << std::endl; 116 | std::cout << "S = start" << std::endl; 117 | std::cout << "E = end" << std::endl; 118 | std::cout << "number = path nodes" << std::endl; 119 | 120 | return 0; 121 | } 122 | -------------------------------------------------------------------------------- /pathfinding.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | class Pathfinder 9 | { 10 | public: 11 | 12 | using NodeId = uint32_t; 13 | using Cost = float; 14 | 15 | //The pathfinder is a general algorithm that can be used for mutliple purpose 16 | //So it use adaptor 17 | class PathfinderAdaptor 18 | { 19 | public: 20 | friend Pathfinder; 21 | 22 | virtual size_t getNodeCount() const = 0; 23 | virtual Cost distance(const NodeId n1, const NodeId n2) const = 0; 24 | virtual bool lineOfSight(const NodeId n1, const NodeId n2) const = 0; 25 | virtual std::vector> getNodeNeighbors(const NodeId id) const = 0; 26 | }; 27 | 28 | static constexpr const float EPSILON = 0.00001f; 29 | static constexpr Cost INFINITE_COST = std::numeric_limits::max(); 30 | 31 | enum ListType {NO_LIST, OPEN_LIST, CLOSED_LIST}; 32 | 33 | struct Node 34 | { 35 | std::vector> neighbors; 36 | uint32_t searchIndex = 0; //The last search at which it has bean generated, bassicacly used to decide if the node need to be reseted before we use it 37 | NodeId parent; // Parent of the node. 38 | Cost g; // Initialized to infinity when generated. 39 | Cost h; // Initialized to the heuristic distance to the goal when generated. 40 | ListType list; // Initially NO_LIST, can be changed to OPEN_LIST or CLOSED_LIST. 41 | }; 42 | 43 | struct HeapElement 44 | { 45 | NodeId id; 46 | Cost g; // Used for tie-breaking 47 | Cost f; // Main key 48 | 49 | //inverted so that the smaller is at the end of the vector 50 | bool operator<(const HeapElement& rhs) const 51 | { 52 | if(abs(f - rhs.f) < EPSILON) 53 | // return g > rhs.g; 54 | return g < rhs.g; 55 | 56 | // return f < rhs.f; 57 | return f > rhs.f; 58 | } 59 | }; 60 | 61 | 62 | Pathfinder(PathfinderAdaptor& adaptor, Cost weight = 1.0f) : adaptor(adaptor), weight(weight) 63 | { 64 | generateNodes(); 65 | } 66 | 67 | template 68 | std::vector search(const DataType& start, const DataType& end, std::function idToData, std::function dataToId) 69 | { 70 | const auto path = search(dataToId(start), dataToId(end)); 71 | std::vector finalPath; 72 | finalPath.reserve(path.size()); 73 | 74 | for(const auto id : path) 75 | finalPath.push_back(idToData(id)); 76 | 77 | return finalPath; 78 | } 79 | 80 | std::vector search(const NodeId startId, const NodeId endId) 81 | { 82 | openList.clear(); 83 | 84 | currentSearch++; 85 | 86 | generateState(startId, endId); 87 | generateState(endId, endId); 88 | 89 | nodes[startId].g = 0; 90 | nodes[startId].parent = startId; 91 | 92 | addToOpen(startId); 93 | 94 | while(!openList.empty() && nodes[endId].g > getMin().f + EPSILON) 95 | { 96 | NodeId currId = getMin().id; 97 | popMin(); 98 | 99 | // Lazy Theta* assumes that there is always line-of-sight from the parent of an expanded state to a successor state. 100 | // When expanding a state, check if this is true. 101 | if(!adaptor.lineOfSight(nodes[currId].parent, currId)) 102 | { 103 | // Since the previous parent is invalid, set g-value to infinity. 104 | nodes[currId].g = INFINITE_COST; 105 | 106 | // Go over potential parents and update its parent to the parent that yields the lowest g-value for s. 107 | for(const auto neighbordInfo : nodes[currId].neighbors) 108 | { 109 | auto newParent = neighbordInfo.first; 110 | 111 | generateState(newParent, endId); 112 | if(nodes[newParent].list == CLOSED_LIST) 113 | { 114 | Cost newG = nodes[newParent].g + neighbordInfo.second; 115 | if(newG < nodes[currId].g) 116 | { 117 | nodes[currId].g = newG; 118 | nodes[currId].parent = newParent; 119 | } 120 | } 121 | } 122 | } 123 | 124 | for(const auto neighborInfo : nodes[currId].neighbors) 125 | { 126 | auto neighborId = neighborInfo.first; 127 | 128 | generateState(neighborId, endId); 129 | 130 | NodeId newParent = nodes[currId].parent; 131 | 132 | if(nodes[neighborId].list != CLOSED_LIST) 133 | { 134 | Cost newG = nodes[newParent].g + adaptor.distance(newParent, neighborId); 135 | 136 | if(newG + EPSILON < nodes[neighborId].g) 137 | { 138 | nodes[neighborId].g = newG; 139 | nodes[neighborId].parent = newParent; 140 | addToOpen(neighborId); 141 | } 142 | } 143 | } 144 | } 145 | 146 | std::vector path; 147 | 148 | if(nodes[endId].g < INFINITE_COST) 149 | { 150 | // ValidateParent(endId, endId); 151 | NodeId curr = endId; 152 | while(curr != startId) 153 | { 154 | path.push_back(curr); 155 | curr = nodes[curr].parent; 156 | } 157 | 158 | path.push_back(curr); 159 | std::reverse(path.begin(), path.end()); 160 | } 161 | 162 | return path; 163 | } 164 | 165 | void generateNodes() 166 | { 167 | nodes.clear(); 168 | nodes.resize(adaptor.getNodeCount()); 169 | 170 | NodeId current = 0; 171 | for(auto& node : nodes) 172 | node.neighbors = adaptor.getNodeNeighbors(current++); 173 | } 174 | 175 | private: 176 | 177 | std::vector nodes; 178 | std::vector openList; 179 | 180 | PathfinderAdaptor& adaptor; 181 | 182 | const Cost weight; 183 | 184 | uint32_t currentSearch = 0; 185 | 186 | void generateState(NodeId s, NodeId goal) 187 | { 188 | if(nodes[s].searchIndex != currentSearch) 189 | { 190 | nodes[s].searchIndex = currentSearch; 191 | nodes[s].h = adaptor.distance(s, goal) * weight; 192 | nodes[s].g = INFINITE_COST; 193 | nodes[s].list = NO_LIST; 194 | } 195 | } 196 | 197 | void addToOpen(NodeId id) 198 | { 199 | // If it is already in the open list, remove it and do a sorted insert 200 | if (nodes[id].list == OPEN_LIST) 201 | { 202 | auto index = std::find_if(openList.begin(), openList.end(), [&](const auto& heap){return heap.id == id;}); 203 | auto id = index->id; 204 | openList.erase(index); 205 | insert_sorted(openList, {id, nodes[id].g, nodes[id].g + nodes[id].h}); 206 | } 207 | // Otherwise, add it to the open list 208 | else 209 | { 210 | nodes[id].list = OPEN_LIST; 211 | insert_sorted(openList, {id, nodes[id].g, nodes[id].g + nodes[id].h}); 212 | } 213 | 214 | } 215 | 216 | const HeapElement getMin() const 217 | { 218 | return openList.back(); 219 | } 220 | 221 | void popMin() 222 | { 223 | nodes[openList.back().id].list = CLOSED_LIST; 224 | openList.pop_back(); 225 | 226 | } 227 | 228 | template< typename T > 229 | typename std::vector::iterator 230 | insert_sorted( std::vector & vec, T const& item ) 231 | { 232 | return vec.insert 233 | ( 234 | std::upper_bound( vec.begin(), vec.end(), item ), 235 | item 236 | ); 237 | } 238 | 239 | template< typename T, typename Pred > 240 | typename std::vector::iterator 241 | insert_sorted( std::vector & vec, T const& item, Pred pred ) 242 | { 243 | return vec.insert 244 | ( 245 | std::upper_bound( vec.begin(), vec.end(), item, pred ), 246 | item 247 | ); 248 | } 249 | }; 250 | 251 | -------------------------------------------------------------------------------- /tileadaptor.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "utility.hpp" 4 | #include "pathfinding.hpp" 5 | 6 | //The pathfinder is a general algorithm that can be used for mutliple purpose 7 | //So it use adaptor 8 | //This adaptor is for tile grid 9 | class TileAdaptor: public Pathfinder::PathfinderAdaptor 10 | { 11 | public: 12 | 13 | using NodeId = Pathfinder::NodeId; 14 | using Cost = Pathfinder::Cost; 15 | 16 | TileAdaptor(const Vectori& mapSize, const std::function& mIsTraversable) : mMapSize(mapSize), mIsTraversable(mIsTraversable) 17 | { 18 | 19 | } 20 | 21 | virtual size_t getNodeCount() const override 22 | { 23 | return mMapSize.x * mMapSize.y; 24 | } 25 | 26 | //return the distance between two node 27 | virtual Cost distance(const NodeId n1, const NodeId n2) const override 28 | { 29 | return dist((Vectorf)idToPos(n1), (Vectorf)idToPos(n2)); 30 | } 31 | 32 | //Return true if there is a direct path between n1 and n2 33 | //Totally not stole this code and did some heavy rewrite 34 | //The original code was way worse, trust me 35 | virtual bool lineOfSight(const NodeId n1, const NodeId n2) const override 36 | { 37 | // This line of sight check uses only integer values. First it checks whether the movement along the x or the y axis is longer and moves along the longer 38 | // one cell by cell. dx and dy specify how many cells to move in each direction. Suppose dx is longer and we are moving along the x axis. For each 39 | // cell we pass in the x direction, we increase variable f by dy, which is initially 0. When f >= dx, we move along the y axis and set f -= dx. This way, 40 | // after dx movements along the x axis, we also move dy moves along the y axis. 41 | 42 | Vectori l1 = idToPos(n1); 43 | Vectori l2 = idToPos(n2); 44 | 45 | Vectori diff = l2 - l1; 46 | 47 | int f = 0; 48 | Vectori dir; // Direction of movement. Value can be either 1 or -1. 49 | 50 | // The x and y locations correspond to nodes, not cells. We might need to check different surrounding cells depending on the direction we do the 51 | // line of sight check. The following values are used to determine which cell to check to see if it is unblocked. 52 | Vectori offset; 53 | 54 | if(diff.y < 0) 55 | { 56 | diff.y = -diff.y; 57 | dir.y = -1; 58 | offset.y = 0; // Cell is to the North 59 | } 60 | else 61 | { 62 | dir.y = 1; 63 | offset.y = 1; // Cell is to the South 64 | } 65 | 66 | if(diff.x < 0) 67 | { 68 | diff.x = -diff.x; 69 | dir.x = -1; 70 | offset.x = 0; // Cell is to the West 71 | } 72 | else 73 | { 74 | dir.x = 1; 75 | offset.x = 1; // Cell is to the East 76 | } 77 | 78 | if(diff.x >= diff.y) 79 | { // Move along the x axis and increment/decrement y when f >= diff.x. 80 | while(l1.x != l2.x) 81 | { 82 | f += diff.y; 83 | if(f >= diff.x) 84 | { // We are changing rows, we might need to check two cells this iteration. 85 | if (!mIsTraversable(l1 + offset)) 86 | return false; 87 | 88 | l1.y += dir.y; 89 | f -= diff.x; 90 | } 91 | 92 | if(f != 0 && !mIsTraversable(l1 + offset)) 93 | return false; 94 | 95 | // If we are moving along a horizontal line, either the north or the south cell should be unblocked. 96 | if (diff.y == 0 && !mIsTraversable({l1.x + offset.x, l1.y}) && !mIsTraversable({l1.x + offset.x, l1.y + 1})) 97 | return false; 98 | 99 | l1.x += dir.x; 100 | } 101 | } 102 | else 103 | { //if (diff.x < diff.y). Move along the y axis and increment/decrement x when f >= diff.y. 104 | while (l1.y != l2.y) 105 | { 106 | f += diff.x; 107 | if(f >= diff.y) 108 | { 109 | if(!mIsTraversable(l1 + offset)) 110 | return false; 111 | 112 | l1.x += dir.x; 113 | f -= diff.y; 114 | } 115 | 116 | if(f != 0 && !mIsTraversable(l1 + offset)) 117 | return false; 118 | 119 | if (diff.x == 0 && !mIsTraversable({l1.x, l1.y + offset.y}) && !mIsTraversable({l1.x + 1, l1.y + offset.y})) 120 | return false; 121 | 122 | l1.y += dir.y; 123 | } 124 | } 125 | 126 | return true; 127 | } 128 | 129 | //return a vector of all the neighbors ids and the cost to travel to them 130 | //In this adaptor we only need to check the four tileneibors and the cost is always 1 131 | virtual std::vector> getNodeNeighbors(const NodeId id) const override 132 | { 133 | auto pos = idToPos(id); 134 | 135 | const Pathfinder::Cost cost = 1; 136 | 137 | std::vector> neighbors; 138 | 139 | //check if we are not on most left if not check if the tile to the left is traversable 140 | //if so then add it to the neighbor list with its cost(1 for all neighbors) 141 | if(pos.x != 0 && mIsTraversable({pos.x - 1, pos.y})) 142 | neighbors.push_back({posToId({pos.x - 1, pos.y}), cost}); 143 | 144 | if(pos.y != 0 && mIsTraversable({pos.x, pos.y - 1})) 145 | neighbors.push_back({posToId({pos.x, pos.y - 1}), cost}); 146 | 147 | if(pos.x != mMapSize.x - 1 && mIsTraversable({pos.x + 1, pos.y})) 148 | neighbors.push_back({posToId({pos.x + 1, pos.y}), cost}); 149 | 150 | if(pos.y != mMapSize.y - 1 && mIsTraversable({pos.x, pos.y + 1})) 151 | neighbors.push_back({posToId({pos.x, pos.y + 1}), cost}); 152 | 153 | return neighbors; 154 | } 155 | 156 | //custom function used to map tile to id 157 | Pathfinder::NodeId posToId(const Vectori& pos) const 158 | { 159 | return pos.y * mMapSize.x + pos.x; 160 | } 161 | 162 | //custom function used to map id to tile 163 | Vectori idToPos(const Pathfinder::NodeId id) const 164 | { 165 | return {id % mMapSize.x, id / mMapSize.x}; 166 | } 167 | 168 | private: 169 | const Vectori mMapSize; 170 | 171 | //Function used to know if one tile of the map is travesable 172 | //The adaptor could also store a reference or a pointer to the map directly 173 | //But I find this way make the adaptor way more reusable 174 | const std::function mIsTraversable; 175 | }; 176 | -------------------------------------------------------------------------------- /utility.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | template 8 | struct Vector 9 | { 10 | T x; 11 | T y; 12 | 13 | Vector() = default; 14 | Vector(const Vector&) = default; 15 | Vector(const T x, const T y) 16 | { 17 | this->x = x; 18 | this->y = y; 19 | } 20 | 21 | template 22 | Vector(const Vector& other) 23 | { 24 | x = other.x; 25 | y = other.y; 26 | } 27 | 28 | Vector operator-(const Vector& other) const 29 | { 30 | return {x - other.x, y - other.y}; 31 | } 32 | 33 | Vector operator+(const Vector& other) const 34 | { 35 | return {x + other.x, y + other.y}; 36 | } 37 | }; 38 | 39 | using Vectori = Vector; 40 | using Vectorf = Vector; 41 | 42 | float dist(Vectorf v1, Vectorf v2) 43 | { 44 | Vectorf v(v1.x-v2.x, v1.y-v2.y); 45 | return std::sqrt(v.x*v.x + v.y*v.y); 46 | } 47 | --------------------------------------------------------------------------------