├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cmake └── Findbenchmark.cmake ├── data ├── maze_1000.pgm ├── maze_1000_smooth.pgm ├── maze_100_smooth.pgm ├── maze_250.pgm ├── maze_500.pgm └── maze_large.pgm ├── gridastar-config.cmake ├── map_out_big.png ├── src ├── AStar2.cpp └── AStar2.h └── test ├── benchmark.cpp └── util.h /.gitignore: -------------------------------------------------------------------------------- 1 | perf.data 2 | massif.* 3 | build-* 4 | 5 | # Prerequisites 6 | *.d 7 | 8 | # Compiled Object files 9 | *.slo 10 | *.lo 11 | *.o 12 | *.obj 13 | 14 | # Precompiled Headers 15 | *.gch 16 | *.pch 17 | 18 | # Compiled Dynamic libraries 19 | *.so 20 | *.dylib 21 | *.dll 22 | 23 | # Fortran module files 24 | *.mod 25 | *.smod 26 | 27 | # Compiled Static libraries 28 | *.lai 29 | *.la 30 | *.a 31 | *.lib 32 | 33 | # Executables 34 | *.exe 35 | *.out 36 | *.app 37 | /CMakeLists.txt.user 38 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required (VERSION 2.8.11) 2 | project (gridastar) 3 | 4 | list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake") 5 | 6 | set(CMAKE_CXX_STANDARD 11) 7 | 8 | include_directories(src) 9 | 10 | add_library(gridastar STATIC 11 | src/AStar2.cpp 12 | src/AStar2.h 13 | ) 14 | 15 | find_package(benchmark) 16 | 17 | if(benchmark_FOUND) 18 | 19 | message("Google Benchmark found") 20 | include_directories(${benchmark_INCLUDE_DIR}) 21 | 22 | add_executable(benchmark-astar 23 | test/util.h 24 | test/benchmark.cpp 25 | ) 26 | target_link_libraries(benchmark-astar 27 | gridastar 28 | ${benchmark_LIBRARIES} 29 | pthread) 30 | else() 31 | message("Google Benchmark NOT found. It can be downloaded from https://github.com/google/benchmark") 32 | endif() 33 | 34 | install(TARGETS gridastar 35 | RUNTIME DESTINATION bin COMPONENT bin 36 | ARCHIVE DESTINATION lib COMPONENT lib ) 37 | 38 | install(FILES src/AStar2.h DESTINATION include/gridastar ) 39 | install(FILES gridastar-config.cmake README.md LICENSE DESTINATION share/gridastar ) 40 | 41 | set(CPACK_GENERATOR DEB) 42 | set(CPACK_PACKAGE_NAME "gridastar") 43 | set(CPACK_PACKAGE_VENDOR "Eurecat") 44 | set(CPACK_DEBIAN_PACKAGE_MAINTAINER "Eurecat") 45 | set(CPACK_PACKAGE_VERSION_MAJOR ${PROJECT_VERSION_MAJOR}) 46 | set(CPACK_PACKAGE_VERSION_MINOR ${PROJECT_VERSION_MINOR}) 47 | set(CPACK_PACKAGE_VERSION_PATCH ${PROJECT_VERSION_PATCH}) 48 | 49 | include(CPack) 50 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # A* for 2D grids 2 | 3 | This is an implementation of the A* path planning algorithm, specifically tailored for 4 | 2D grids. 5 | 6 | It is inspired by other two open source implementations: 7 | 8 | - [hjweide/a-star](https://github.com/hjweide/a-star) 9 | 10 | Nevertheless, this implementation is 20% faster. 11 | 12 | You might want to take a look to [astar-algorithm-cpp](https://github.com/justinhj/astar-algorithm-cpp), 13 | but that implementation is more generic and it is not specifically optimized for 2D gridmaps. 14 | 15 | To improve speed, the library uses a fairly large amount of RAM to perform many 16 | operations in __O(1)__. The number of memory allocations is reduced to the very minimum. 17 | 18 | It requires __at least__ 8 bytes for each cell in the grid, in other words, 19 | more than 8 Mb of memory for a 1000x1000 gridmap. 20 | 21 | ## How fast is it? 22 | 23 | Even if a fair amount of work was spent (mostly for fun) to optimize this software, it is still an A* 24 | algorithm, that will be outperformed by other, more advanced, algorithms. 25 | 26 | If you are looking for state-of-the-art path finders, you might be interested to www.movingai.com 27 | 28 | This particularly implementation is efficient but also easy to read and understand. 29 | 30 | 31 | ## Usage 32 | 33 | You must pass the image using the method __setWorldData()__. 34 | 35 | Note that the the image data must be row-major and monochromatic. 36 | 37 | A value of 0 (black) represents an obstacle, whilst 255 (white) 38 | is a free cell. 39 | 40 | ```c++ 41 | 42 | // You don't need to use this image parser, you can use your own. 43 | Image image; 44 | image.readFromPGM("./data/maze_big.pgm"); 45 | 46 | AStar::PathFinder generator; 47 | 48 | generator.setWorldData( image.width(), 49 | image.height(), 50 | image.data() ); 51 | 52 | AStar::Coord2D startPos (image.width()/2, 0); 53 | AStar::Coord2D targetPos(image.width()/2, image.height()/2 -1); 54 | 55 | auto path = generator.findPath(startPos, targetPos); 56 | 57 | ``` 58 | 59 | In the following example, the algorithm required about 200 milliseconds and 25 Mb 60 | of RAM to find the solution in a 1586x1586 maze. 61 | 62 | The pink pixels represent the cells of the grid visited by the algorithm. 63 | 64 | ![Large map](./map_out_big.png) 65 | 66 | 67 | # License 68 | 69 | Copyright 2018-2019 Eurecat 70 | 71 | Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file 72 | except in compliance with the License. You may obtain a copy of the License at 73 | 74 | [http://www.apache.org/licenses/LICENSE-2.0](http://www.apache.org/licenses/LICENSE-2.0) 75 | 76 | Unless required by applicable law or agreed to in writing, software distributed under the 77 | License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, 78 | either express or implied. See the License for the specific language governing permissions 79 | and limitations under the License. 80 | 81 | # References 82 | 83 | [Introduction to A*](https://www.redblobgames.com/pathfinding/a-star/introduction.html) 84 | 85 | -------------------------------------------------------------------------------- /cmake/Findbenchmark.cmake: -------------------------------------------------------------------------------- 1 | # Findbenchmark.cmake 2 | # - Try to find benchmark 3 | # 4 | # The following variables are optionally searched for defaults 5 | # benchmark_ROOT_DIR: Base directory where all benchmark components are found 6 | # 7 | # Once done this will define 8 | # benchmark_FOUND - System has benchmark 9 | # benchmark_INCLUDE_DIRS - The benchmark include directories 10 | # benchmark_LIBRARIES - The libraries needed to use benchmark 11 | 12 | set(benchmark_ROOT_DIR "" CACHE PATH "Folder containing benchmark") 13 | 14 | find_path(benchmark_INCLUDE_DIR "benchmark/benchmark.h" 15 | PATHS ${benchmark_ROOT_DIR} 16 | PATH_SUFFIXES include 17 | NO_DEFAULT_PATH) 18 | find_path(benchmark_INCLUDE_DIR "benchmark/benchmark.h") 19 | 20 | find_library(benchmark_LIBRARY NAMES "benchmark" 21 | PATHS ${benchmark_ROOT_DIR} 22 | PATH_SUFFIXES lib lib64 23 | NO_DEFAULT_PATH) 24 | find_library(benchmark_LIBRARY NAMES "benchmark") 25 | 26 | include(FindPackageHandleStandardArgs) 27 | # handle the QUIETLY and REQUIRED arguments and set benchmark_FOUND to TRUE 28 | # if all listed variables are TRUE 29 | find_package_handle_standard_args(benchmark FOUND_VAR benchmark_FOUND 30 | REQUIRED_VARS benchmark_LIBRARY 31 | benchmark_INCLUDE_DIR) 32 | 33 | if(benchmark_FOUND) 34 | set(benchmark_LIBRARIES ${benchmark_LIBRARY}) 35 | set(benchmark_INCLUDE_DIRS ${benchmark_INCLUDE_DIR}) 36 | endif() 37 | 38 | mark_as_advanced(benchmark_INCLUDE_DIR benchmark_LIBRARY) 39 | -------------------------------------------------------------------------------- /data/maze_1000.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Eurecat/astar-gridmap-2d/c4dc187cb98450dd039d53b70e46dcef59dfeedc/data/maze_1000.pgm -------------------------------------------------------------------------------- /data/maze_1000_smooth.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Eurecat/astar-gridmap-2d/c4dc187cb98450dd039d53b70e46dcef59dfeedc/data/maze_1000_smooth.pgm -------------------------------------------------------------------------------- /data/maze_100_smooth.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Eurecat/astar-gridmap-2d/c4dc187cb98450dd039d53b70e46dcef59dfeedc/data/maze_100_smooth.pgm -------------------------------------------------------------------------------- /data/maze_250.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Eurecat/astar-gridmap-2d/c4dc187cb98450dd039d53b70e46dcef59dfeedc/data/maze_250.pgm -------------------------------------------------------------------------------- /data/maze_500.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Eurecat/astar-gridmap-2d/c4dc187cb98450dd039d53b70e46dcef59dfeedc/data/maze_500.pgm -------------------------------------------------------------------------------- /data/maze_large.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Eurecat/astar-gridmap-2d/c4dc187cb98450dd039d53b70e46dcef59dfeedc/data/maze_large.pgm -------------------------------------------------------------------------------- /gridastar-config.cmake: -------------------------------------------------------------------------------- 1 | set(gridastar_FOUND true) 2 | set(gridastar_INCLUDE_DIR include) 3 | set(gridastar_LIBS gridastar) 4 | -------------------------------------------------------------------------------- /map_out_big.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Eurecat/astar-gridmap-2d/c4dc187cb98450dd039d53b70e46dcef59dfeedc/map_out_big.png -------------------------------------------------------------------------------- /src/AStar2.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2018 Eurecat 3 | 4 | Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file 5 | except in compliance with the License. You may obtain a copy of the License at 6 | 7 | http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | Unless required by applicable law or agreed to in writing, software distributed under the 10 | License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, 11 | either express or implied. See the License for the specific language governing permissions 12 | and limitations under the License. 13 | */ 14 | 15 | #include "AStar2.h" 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | using namespace std::placeholders; 27 | 28 | namespace AStar{ 29 | 30 | PathFinder::PathFinder() 31 | { 32 | 33 | _obstacle_threshold = 0; 34 | setHeuristic(OCTOGONAL); 35 | 36 | _directions = {{ 37 | { -1, -1 }, { 0, -1 }, { 1, -1 }, //0 - 2 38 | { -1, 0 }, { 1, 0 }, //3 - 4 39 | { -1, 1 }, { 0, 1 }, { 1, 1 } //5 - 7 40 | }}; 41 | 42 | _direction_cost = {{ 43 | 14, 10, 14, 44 | 10, 10, 45 | 14, 10, 14 46 | }}; 47 | 48 | _direction_next = { 49 | {0,1,3}, {0,1,2}, {1,2,4}, 50 | {0,3,5}, {2,4,7}, 51 | {3,5,6}, {5,6,7}, {4,6,7}, 52 | {0,1,2,3,4,5,6,7} 53 | }; 54 | } 55 | 56 | PathFinder::~PathFinder() 57 | { 58 | } 59 | 60 | 61 | void PathFinder::setWorldData(unsigned width, unsigned height, const uint8_t *data, size_t bytes_per_line, uint8_t color_threshold) 62 | { 63 | if( width >= std::numeric_limits::max() || 64 | height >= std::numeric_limits::max() ) 65 | { 66 | throw std::invalid_argument("Either width or height exceed the maximum size allowed (32768) "); 67 | } 68 | 69 | _world_width = width; 70 | _world_height = height; 71 | _world_data = data; 72 | _bytes_per_line = bytes_per_line; 73 | if (_bytes_per_line==0) _bytes_per_line=width; 74 | _obstacle_threshold = color_threshold; 75 | } 76 | 77 | void PathFinder::setCustomHeuristic(HeuristicFunction heuristic) 78 | { 79 | _heuristic_func = heuristic; 80 | _heuristic_type = CUSTOM; 81 | } 82 | 83 | void PathFinder::setHeuristic(Heuristic heuristic) 84 | { 85 | if( heuristic == CUSTOM && !_heuristic_func) 86 | { 87 | throw std::runtime_error("Use method setCustomHeuristic instead" ); 88 | } 89 | _heuristic_type = heuristic; 90 | } 91 | 92 | 93 | void PathFinder::clear() 94 | { 95 | _open_set = decltype(_open_set)(); // priority_queue does not have "clear" 96 | _gridmap.resize(_world_width*_world_height); 97 | std::fill(_gridmap.begin(), _gridmap.end(), Cell()); 98 | } 99 | 100 | 101 | CoordinateList PathFinder::findPath(Coord2D startPos, Coord2D goalPos) 102 | { 103 | if( detectCollision( startPos ) ) 104 | { 105 | return {}; 106 | } 107 | 108 | clear(); 109 | 110 | auto toIndex = [this](const Coord2D& pos) -> int 111 | { return static_cast(_world_width*pos.y + pos.x); }; 112 | 113 | const int startIndex = toIndex(startPos); 114 | 115 | _open_set.push( {0, startPos, 8 } ); 116 | _gridmap[startIndex].cost_G = 0; 117 | 118 | bool solution_found = false; 119 | 120 | int offset_grid[8]; 121 | for (int i = 0; i < 8; ++i) 122 | { 123 | offset_grid[i] = toIndex(_directions[i]); 124 | } 125 | 126 | while (! _open_set.empty() ) 127 | { 128 | Coord2D currentCoord = _open_set.top().coord; 129 | uint8_t prev = _open_set.top().prev_dir; 130 | _open_set.pop(); 131 | 132 | if (currentCoord == goalPos) { 133 | solution_found = true; 134 | break; 135 | } 136 | int currentIndex = toIndex(currentCoord); 137 | Cell& currentCell = _gridmap[ currentIndex ]; 138 | currentCell.already_visited = true; 139 | 140 | for (int i: _direction_next[prev]) 141 | { 142 | const Coord2D newCoordinates = currentCoord + _directions[i]; 143 | 144 | if (detectCollision( newCoordinates )) { 145 | continue; 146 | } 147 | 148 | const size_t newIndex = currentIndex + offset_grid[i]; 149 | Cell& newCell = _gridmap[newIndex]; 150 | 151 | if ( newCell.already_visited ) { 152 | continue; 153 | } 154 | 155 | // Code temporary removed. 156 | //float factor = 1.0f + static_cast(EMPTY - newCell.world) / 50.0f; 157 | //float new_cost = currentCell.cost_G + (_direction_cost[i] * factor); 158 | 159 | const uint32_t new_cost = currentCell.cost_G + _direction_cost[i]; 160 | 161 | if( new_cost < newCell.cost_G) 162 | { 163 | int H = 0; 164 | switch (_heuristic_type) { 165 | case MANHATTAN: 166 | H = HeuristicImpl::manhattan(newCoordinates, goalPos); 167 | break; 168 | case EUCLIDEAN: 169 | H = HeuristicImpl::euclidean(newCoordinates, goalPos); 170 | break; 171 | case OCTOGONAL: 172 | H = HeuristicImpl::octagonal(newCoordinates, goalPos); 173 | break; 174 | default: 175 | H = _heuristic_func(newCoordinates, goalPos); 176 | break; 177 | } 178 | _open_set.push( { new_cost + H, newCoordinates, static_cast(i)} ); 179 | newCell.cost_G = new_cost; 180 | newCell.path_parent = i; 181 | } 182 | } 183 | } 184 | 185 | CoordinateList path; 186 | if( solution_found ) 187 | { 188 | Coord2D coord = goalPos; 189 | while (coord != startPos) 190 | { 191 | path.push_back( coord ); 192 | coord = coord - _directions[cell(coord).path_parent]; 193 | } 194 | path.push_back(startPos); 195 | } 196 | else 197 | { 198 | std::cout << "Solution not found\n" << 199 | " open set size= " << _open_set.size() << std::endl; 200 | } 201 | 202 | return path; 203 | } 204 | 205 | void PathFinder::exportPPM(const char *filename, CoordinateList* path) 206 | { 207 | if (_world_data==nullptr) 208 | return; 209 | 210 | std::ofstream outfile(filename, std::ios_base::out | std::ios_base::binary); 211 | 212 | char header[100]; 213 | sprintf(header, "P6\n# Done by Davide\n%d %d\n255\n", _world_width, _world_height ); 214 | outfile.write(header, strlen(header)); 215 | 216 | std::vector image( _world_width * _world_height * 3); 217 | 218 | int line_size = _world_width * 3; 219 | 220 | auto toIndex = [line_size](int x, int y) { return y*line_size + (x*3); }; 221 | 222 | for (uint32_t y=0; y<_world_height; y++) 223 | { 224 | for (uint32_t x=0; x<_world_width; x++) 225 | { 226 | uint8_t world_value = _world_data[y*_bytes_per_line+x]; 227 | 228 | if( world_value <= _obstacle_threshold ) 229 | { 230 | uint8_t color[] = {0,0,0}; 231 | std::memcpy( &image[ toIndex(x,y) ], color, 3 ); 232 | } 233 | else if( _gridmap[ y*_world_width + x ].already_visited ) 234 | { 235 | uint8_t color[] = {255,222,222}; 236 | std::memcpy( &image[ toIndex(x,y) ], color, 3 ); 237 | } 238 | else{ 239 | uint8_t color[] = {255,255,255}; 240 | std::memcpy( &image[ toIndex(x,y) ], color, 3 ); 241 | } 242 | } 243 | } 244 | 245 | if( path ) 246 | { 247 | for (const auto& point: *path) 248 | { 249 | uint8_t color[] = {50,50,250}; 250 | std::memcpy( &image[ toIndex(point.x, point.y) ], color, 3 ); 251 | } 252 | } 253 | 254 | outfile.write( reinterpret_cast(image.data()), image.size() ); 255 | outfile.close(); 256 | } 257 | 258 | inline bool PathFinder::detectCollision(const Coord2D& coordinates) 259 | { 260 | if (coordinates.x < 0 || coordinates.x >= _world_width || 261 | coordinates.y < 0 || coordinates.y >= _world_height ) return true; 262 | 263 | return _world_data[coordinates.y*_bytes_per_line + coordinates.x] <= _obstacle_threshold; 264 | } 265 | 266 | 267 | inline uint32_t HeuristicImpl::manhattan(const Coord2D& source, const Coord2D& target) 268 | { 269 | auto delta = Coord2D( (source.x - target.x), (source.y - target.y) ); 270 | return static_cast(10 * ( std::abs(delta.x) + std::abs(delta.y))); 271 | } 272 | 273 | inline uint32_t HeuristicImpl::euclidean(const Coord2D& source, const Coord2D& target) 274 | { 275 | auto delta = Coord2D( (source.x - target.x), (source.y - target.y) ); 276 | return static_cast(10 * std::sqrt(std::pow(delta.x, 2) + std::pow(delta.y, 2))); 277 | } 278 | 279 | inline uint32_t HeuristicImpl::octagonal(const Coord2D& source, const Coord2D& target) 280 | { 281 | auto delta = Coord2D( std::abs(source.x - target.x), std::abs(source.y - target.y) ); 282 | return 10 * (delta.x + delta.y) + (-6) * std::min(delta.x, delta.y); 283 | } 284 | 285 | } 286 | -------------------------------------------------------------------------------- /src/AStar2.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2018 Eurecat 3 | 4 | Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file 5 | except in compliance with the License. You may obtain a copy of the License at 6 | 7 | http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | Unless required by applicable law or agreed to in writing, software distributed under the 10 | License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, 11 | either express or implied. See the License for the specific language governing permissions 12 | and limitations under the License. 13 | */ 14 | 15 | #ifndef _ASTAR2_Taffete_eurecat_ 16 | #define _ASTAR2_Taffete_eurecat_ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | namespace AStar 27 | { 28 | 29 | struct Coord2D 30 | { 31 | int16_t x, y; 32 | Coord2D(): x(-1), y(-1) {} 33 | Coord2D(int x_, int y_): x(x_), y(y_) {} 34 | 35 | bool operator != (const Coord2D& other) const { 36 | return !(*this == other); 37 | } 38 | 39 | bool operator == (const Coord2D& other) const { 40 | return (x == other.x && y == other.y); 41 | } 42 | 43 | Coord2D operator + ( const Coord2D& other) const { 44 | return{ x + other.x, y + other.y }; 45 | } 46 | 47 | Coord2D operator - (const Coord2D& other) const { 48 | return{ x - other.x, y - other.y }; 49 | } 50 | }; 51 | 52 | 53 | using HeuristicFunction = std::function; 54 | using CoordinateList = std::vector; 55 | 56 | struct OpenNode 57 | { 58 | uint32_t score; 59 | Coord2D coord; 60 | uint8_t prev_dir; // previous direction 61 | }; 62 | 63 | // Note: we want the priority_queue to be ordered from smaller to larger 64 | inline bool operator<(const OpenNode& a, const OpenNode& b) 65 | { 66 | return a.score > b.score; 67 | } 68 | 69 | class PathFinder 70 | { 71 | 72 | public: 73 | PathFinder(); 74 | ~PathFinder(); 75 | 76 | enum Heuristic 77 | { 78 | MANHATTAN, // common heursistic 79 | EUCLIDEAN, // expensive 80 | OCTOGONAL, // default 81 | CUSTOM // automatically set by setCustomHeuristic() 82 | }; 83 | 84 | /// 85 | /** 86 | * @brief setWorldData to be called at least once before findPath 87 | * 88 | * @param width width of the image 89 | * @param height height of the image 90 | * @param data Row-major ordered map, where an obstacle is represented as a pixel with value 0 (black) 91 | * @param bytes_per_line Number of bytes per line (for padded data, eg. images). Default means image is not padded 92 | * @param color_threshold threshold used to detect if a grey pixel is an obstacle 93 | */ 94 | void setWorldData(unsigned width, unsigned height, const uint8_t *data, size_t bytes_per_line=0, uint8_t color_threshold = 20); 95 | 96 | void setHeuristic(Heuristic heuristic); 97 | 98 | void setCustomHeuristic(HeuristicFunction heuristic_); 99 | 100 | /// Function that performs the actual A* computation. 101 | CoordinateList findPath(Coord2D source_, Coord2D target_); 102 | 103 | 104 | /// Export the resulting solution in a visual way. Useful for debugging. 105 | void exportPPM(const char* filename, CoordinateList* path); 106 | 107 | struct Cell{ 108 | bool already_visited; 109 | uint8_t path_parent; 110 | uint32_t cost_G; 111 | 112 | Cell(): already_visited(false), cost_G(std::numeric_limits< decltype(cost_G)>::max()) {} 113 | }; 114 | 115 | const Cell& cell(const Coord2D& coordinates_) const 116 | { 117 | return _gridmap[coordinates_.y*_world_width + coordinates_.x]; 118 | } 119 | 120 | Cell& cell(const Coord2D& coordinates_) 121 | { 122 | return _gridmap[coordinates_.y*_world_width + coordinates_.x]; 123 | } 124 | 125 | private: 126 | 127 | Heuristic _heuristic_type; 128 | HeuristicFunction _heuristic_func; 129 | uint8_t _obstacle_threshold=0; 130 | uint32_t _world_width=0; 131 | uint32_t _world_height=0; 132 | const uint8_t* _world_data=nullptr; 133 | size_t _bytes_per_line=0; 134 | 135 | std::array _directions; 136 | std::array _direction_cost; 137 | std::vector> _direction_next; 138 | 139 | std::priority_queue> _open_set; 140 | 141 | bool detectCollision(const Coord2D& coordinates); 142 | 143 | std::vector _gridmap; 144 | 145 | void clear(); 146 | }; 147 | 148 | class HeuristicImpl 149 | { 150 | public: 151 | static uint32_t manhattan(const Coord2D& source_, const Coord2D& target_); 152 | static uint32_t euclidean(const Coord2D& source_, const Coord2D& target_); 153 | static uint32_t octagonal(const Coord2D& source_, const Coord2D& target_); 154 | }; 155 | 156 | 157 | } 158 | 159 | #endif 160 | -------------------------------------------------------------------------------- /test/benchmark.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "util.h" 5 | #include "AStar2.h" 6 | 7 | 8 | static void BM_AStar_Smooth_1000(benchmark::State& state) 9 | { 10 | AStar::PathFinder generator; 11 | Image image; 12 | image.readFromPGM("./data/maze_1000_smooth.pgm"); 13 | generator.setWorldData( image.width(), image.height(), image.data() ); 14 | 15 | AStar::CoordinateList result; 16 | for (auto _ : state) 17 | { 18 | result = generator.findPath( 19 | { image.width()/2, 0 }, 20 | { image.width()/2, image.height() -1 } ); 21 | } 22 | generator.exportPPM("map_out_smooth.ppm", &result ); 23 | } 24 | 25 | 26 | static void BM_AStar_Big(benchmark::State& state) 27 | { 28 | AStar::PathFinder generator; 29 | Image image; 30 | image.readFromPGM("./data/maze_large.pgm"); 31 | generator.setWorldData( image.width(), image.height(), image.data() ); 32 | 33 | AStar::CoordinateList result; 34 | for (auto _ : state) 35 | { 36 | result = generator.findPath( 37 | { image.width()/2, 0 }, 38 | { image.width()/2, image.height() -1 } ); 39 | } 40 | generator.exportPPM("map_out_large.ppm", &result ); 41 | } 42 | 43 | static void BM_AStar_Small(benchmark::State& state) 44 | { 45 | AStar::PathFinder generator; 46 | Image image; 47 | image.readFromPGM("./data/maze_250.pgm"); 48 | generator.setWorldData( image.width(), image.height(), image.data() ); 49 | 50 | AStar::CoordinateList result; 51 | for (auto _ : state) 52 | { 53 | result = generator.findPath( 54 | { image.width()/2, 0 }, 55 | { image.width()/2, image.height()/2 } ); 56 | } 57 | generator.exportPPM("map_out_small.ppm", &result ); 58 | } 59 | 60 | 61 | BENCHMARK(BM_AStar_Big); 62 | BENCHMARK(BM_AStar_Smooth_1000); 63 | BENCHMARK(BM_AStar_Small); 64 | 65 | BENCHMARK_MAIN(); 66 | 67 | //int main() 68 | //{ 69 | // AStar::PathFinder generator; 70 | // Image image; 71 | // image.readFromPGM("./data/maze_large.pgm"); 72 | // generator.setWorldData( image.width(), image.height(), image.data() ); 73 | 74 | // AStar::CoordinateList result; 75 | 76 | // result = generator.findPath( 77 | // { 1, 1 }, 78 | // { image.width()-1, image.height() -3 } ); 79 | 80 | // generator.exportPPM("map_out_large.ppm", &result ); 81 | // return 0; 82 | //} 83 | 84 | -------------------------------------------------------------------------------- /test/util.h: -------------------------------------------------------------------------------- 1 | #ifndef UTIL_H 2 | #define UTIL_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | class Image{ 12 | uint16_t _width, _height; 13 | std::vector _data; 14 | public: 15 | 16 | uint16_t width() const { return _width; } 17 | 18 | uint16_t height() const { return _height; } 19 | 20 | const uint8_t& at(int x, int y) const 21 | { 22 | return _data[y*_width + x]; 23 | } 24 | 25 | uint8_t& at(int x, int y) 26 | { 27 | return _data[y*_width + x]; 28 | } 29 | 30 | const uint8_t* data() const 31 | { 32 | return _data.data(); 33 | } 34 | 35 | bool readFromPGM(const char* filename) 36 | { 37 | std::ifstream infile(filename, std::ios_base::in | std::ios_base::binary); 38 | std::stringstream ss; 39 | std::string inputLine = ""; 40 | 41 | // First line : version 42 | std::getline(infile,inputLine); 43 | if(inputLine.compare("P5") != 0) 44 | { 45 | std::cerr << "PGM Version error "<< inputLine << std::endl; 46 | exit(1); 47 | } 48 | 49 | // Second line : comment 50 | std::getline(infile,inputLine); 51 | //std::cout << "Comment : " << inputLine << std::endl; 52 | 53 | std::getline(infile,inputLine); 54 | ss << inputLine; 55 | ss >> _width >> _height; 56 | 57 | uint8_t max_value = 0; 58 | std::getline(infile,inputLine); 59 | max_value = stoi(inputLine); 60 | 61 | // std::cout << image->width << " width and " << image->height << " height " 62 | // << (int)max_value << " max value " << std::endl; 63 | 64 | const int img_size = _width * _height; 65 | _data.resize( img_size ); 66 | 67 | infile.read( reinterpret_cast(_data.data()), img_size); 68 | infile.close(); 69 | 70 | return true; 71 | } 72 | 73 | void writeToPGM(const char* filename) 74 | { 75 | std::ofstream outfile(filename, std::ios_base::out | std::ios_base::binary); 76 | 77 | char header[100]; 78 | sprintf(header, "P5\n# Done by Davide\n%d %d\n255\n", _width, _height ); 79 | 80 | outfile.write(header, strlen(header)); 81 | outfile.write( reinterpret_cast( _data.data()), _data.size() ); 82 | outfile.close(); 83 | } 84 | 85 | }; 86 | 87 | 88 | 89 | 90 | #endif // UTIL_H 91 | --------------------------------------------------------------------------------