├── .clang-format ├── .github └── workflows │ └── build.yml ├── .gitignore ├── .vscode └── c_cpp_properties.json ├── CMakeLists.txt ├── LICENSE ├── README.md ├── config └── algorithms.ini ├── dependencies ├── CMakeLists.txt ├── imgui-sfml │ └── CMakeLists.txt └── sfml │ └── CMakeLists.txt ├── figures ├── img0.png ├── img1.png └── img2.png ├── fonts └── OpenSans │ ├── LICENSE.txt │ ├── OpenSans-Bold.ttf │ ├── OpenSans-BoldItalic.ttf │ ├── OpenSans-ExtraBold.ttf │ ├── OpenSans-ExtraBoldItalic.ttf │ ├── OpenSans-Italic.ttf │ ├── OpenSans-Light.ttf │ ├── OpenSans-LightItalic.ttf │ ├── OpenSans-Medium.ttf │ ├── OpenSans-MediumItalic.ttf │ ├── OpenSans-Regular.ttf │ ├── OpenSans-SemiBold.ttf │ └── OpenSans-SemiBoldItalic.ttf ├── include ├── Game.h ├── Gui.h ├── MessageQueue.h ├── State.h └── States │ └── Algorithms │ ├── GraphBased │ ├── ASTAR │ │ └── ASTAR.h │ ├── BFS │ │ └── BFS.h │ ├── DFS │ │ └── DFS.h │ ├── DIJKSTRA │ │ └── DIJKSTRA.h │ ├── GraphBased.h │ ├── Node.h │ └── Utils.h │ └── SamplingBased │ ├── RRT │ └── RRT.h │ ├── RRT_STAR │ └── RRT_STAR.h │ ├── SamplingBased.h │ └── Utils.h └── src ├── Game.cpp ├── State.cpp ├── States └── Algorithms │ ├── GraphBased │ ├── ASTAR │ │ └── ASTAR.cpp │ ├── BFS │ │ └── BFS.cpp │ ├── DFS │ │ └── DFS.cpp │ ├── DIJKSTRA │ │ └── DIJKSTRA.cpp │ ├── GraphBased.cpp │ └── Node.cpp │ └── SamplingBased │ ├── RRT │ └── RRT.cpp │ ├── RRT_STAR │ └── RRT_STAR.cpp │ └── SamplingBased.cpp └── main.cpp /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | # BasedOnStyle: Google 4 | AccessModifierOffset: -1 5 | AlignAfterOpenBracket: Align 6 | AlignConsecutiveMacros: false 7 | AlignConsecutiveAssignments: false 8 | AlignConsecutiveDeclarations: false 9 | AlignEscapedNewlines: Left 10 | AlignOperands: true 11 | AlignTrailingComments: true 12 | AllowAllArgumentsOnNextLine: true 13 | AllowAllConstructorInitializersOnNextLine: true 14 | AllowAllParametersOfDeclarationOnNextLine: true 15 | AllowShortBlocksOnASingleLine: Never 16 | AllowShortCaseLabelsOnASingleLine: false 17 | AllowShortFunctionsOnASingleLine: All 18 | AllowShortLambdasOnASingleLine: All 19 | AllowShortIfStatementsOnASingleLine: WithoutElse 20 | AllowShortLoopsOnASingleLine: true 21 | AlwaysBreakAfterDefinitionReturnType: None 22 | AlwaysBreakAfterReturnType: None 23 | AlwaysBreakBeforeMultilineStrings: true 24 | AlwaysBreakTemplateDeclarations: Yes 25 | BinPackArguments: true 26 | BinPackParameters: true 27 | BraceWrapping: 28 | AfterCaseLabel: false 29 | AfterClass: false 30 | AfterControlStatement: false 31 | AfterEnum: false 32 | AfterFunction: false 33 | AfterNamespace: false 34 | AfterObjCDeclaration: false 35 | AfterStruct: false 36 | AfterUnion: false 37 | AfterExternBlock: false 38 | BeforeCatch: false 39 | BeforeElse: false 40 | IndentBraces: false 41 | SplitEmptyFunction: true 42 | SplitEmptyRecord: true 43 | SplitEmptyNamespace: true 44 | BreakBeforeBinaryOperators: None 45 | BreakBeforeBraces: Attach 46 | BreakBeforeInheritanceComma: false 47 | BreakInheritanceList: BeforeColon 48 | BreakBeforeTernaryOperators: true 49 | BreakConstructorInitializersBeforeComma: false 50 | BreakConstructorInitializers: BeforeColon 51 | BreakAfterJavaFieldAnnotations: false 52 | BreakStringLiterals: true 53 | ColumnLimit: 80 54 | CommentPragmas: '^ IWYU pragma:' 55 | CompactNamespaces: false 56 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 57 | ConstructorInitializerIndentWidth: 4 58 | ContinuationIndentWidth: 4 59 | Cpp11BracedListStyle: true 60 | DeriveLineEnding: true 61 | DerivePointerAlignment: true 62 | DisableFormat: false 63 | ExperimentalAutoDetectBinPacking: false 64 | FixNamespaceComments: true 65 | ForEachMacros: 66 | - foreach 67 | - Q_FOREACH 68 | - BOOST_FOREACH 69 | IncludeBlocks: Regroup 70 | IncludeCategories: 71 | - Regex: '^' 72 | Priority: 2 73 | SortPriority: 0 74 | - Regex: '^<.*\.h>' 75 | Priority: 1 76 | SortPriority: 0 77 | - Regex: '^<.*' 78 | Priority: 2 79 | SortPriority: 0 80 | - Regex: '.*' 81 | Priority: 3 82 | SortPriority: 0 83 | IncludeIsMainRegex: '([-_](test|unittest))?$' 84 | IncludeIsMainSourceRegex: '' 85 | IndentCaseLabels: true 86 | IndentGotoLabels: true 87 | IndentPPDirectives: None 88 | IndentWidth: 2 89 | IndentWrappedFunctionNames: false 90 | JavaScriptQuotes: Leave 91 | JavaScriptWrapImports: true 92 | KeepEmptyLinesAtTheStartOfBlocks: false 93 | MacroBlockBegin: '' 94 | MacroBlockEnd: '' 95 | MaxEmptyLinesToKeep: 1 96 | NamespaceIndentation: None 97 | ObjCBinPackProtocolList: Never 98 | ObjCBlockIndentWidth: 2 99 | ObjCSpaceAfterProperty: false 100 | ObjCSpaceBeforeProtocolList: true 101 | PenaltyBreakAssignment: 2 102 | PenaltyBreakBeforeFirstCallParameter: 1 103 | PenaltyBreakComment: 300 104 | PenaltyBreakFirstLessLess: 120 105 | PenaltyBreakString: 1000 106 | PenaltyBreakTemplateDeclaration: 10 107 | PenaltyExcessCharacter: 1000000 108 | PenaltyReturnTypeOnItsOwnLine: 200 109 | PointerAlignment: Left 110 | RawStringFormats: 111 | - Language: Cpp 112 | Delimiters: 113 | - cc 114 | - CC 115 | - cpp 116 | - Cpp 117 | - CPP 118 | - 'c++' 119 | - 'C++' 120 | CanonicalDelimiter: '' 121 | BasedOnStyle: google 122 | - Language: TextProto 123 | Delimiters: 124 | - pb 125 | - PB 126 | - proto 127 | - PROTO 128 | EnclosingFunctions: 129 | - EqualsProto 130 | - EquivToProto 131 | - PARSE_PARTIAL_TEXT_PROTO 132 | - PARSE_TEST_PROTO 133 | - PARSE_TEXT_PROTO 134 | - ParseTextOrDie 135 | - ParseTextProtoOrDie 136 | CanonicalDelimiter: '' 137 | BasedOnStyle: google 138 | ReflowComments: true 139 | SortIncludes: true 140 | SortUsingDeclarations: true 141 | SpaceAfterCStyleCast: false 142 | SpaceAfterLogicalNot: false 143 | SpaceAfterTemplateKeyword: true 144 | SpaceBeforeAssignmentOperators: true 145 | SpaceBeforeCpp11BracedList: false 146 | SpaceBeforeCtorInitializerColon: true 147 | SpaceBeforeInheritanceColon: true 148 | SpaceBeforeParens: ControlStatements 149 | SpaceBeforeRangeBasedForLoopColon: true 150 | SpaceInEmptyBlock: false 151 | SpaceInEmptyParentheses: false 152 | SpacesBeforeTrailingComments: 2 153 | SpacesInAngles: false 154 | SpacesInConditionalStatement: false 155 | SpacesInContainerLiterals: true 156 | SpacesInCStyleCastParentheses: false 157 | SpacesInParentheses: false 158 | SpacesInSquareBrackets: false 159 | SpaceBeforeSquareBrackets: false 160 | Standard: Auto 161 | StatementMacros: 162 | - Q_UNUSED 163 | - QT_REQUIRE_VERSION 164 | TabWidth: 8 165 | UseCRLF: false 166 | UseTab: Never 167 | ... 168 | 169 | -------------------------------------------------------------------------------- /.github/workflows/build.yml: -------------------------------------------------------------------------------- 1 | name: build 2 | 3 | on: 4 | # Trigger the workflow on push or pull request, 5 | push: 6 | pull_request: 7 | schedule: 8 | # 7am UTC, 12am PDT 9 | - cron: '0 7 * * *' 10 | # allow manually starting this workflow 11 | workflow_dispatch: 12 | 13 | jobs: 14 | Linux: 15 | runs-on: ubuntu-20.04 16 | steps: 17 | - uses: actions/checkout@v2 18 | - uses: DoozyX/clang-format-lint-action@v0.11 19 | with: 20 | source: 'src include' 21 | extensions: 'h,cpp' 22 | clangFormatVersion: 11 23 | 24 | - name: Install Dependencies 25 | run: | 26 | sudo apt-get update 27 | sudo apt-get -y install xorg-dev libboost-all-dev libudev-dev 28 | 29 | - name: Building 30 | run: | 31 | mkdir build 32 | cd build 33 | cmake .. 34 | make 35 | 36 | MacOS: 37 | runs-on: macos-latest 38 | steps: 39 | - uses: actions/checkout@v2 40 | 41 | - name: Building 42 | run: | 43 | mkdir build 44 | cd build 45 | cmake .. 46 | make 47 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .DS_Store 2 | build 3 | -------------------------------------------------------------------------------- /.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "name": "Linux", 5 | "includePath": [ 6 | "${workspaceFolder}/**", 7 | "/usr/include/**" 8 | ], 9 | "defines": [], 10 | "compilerPath": "/usr/bin/clang-10", 11 | "cStandard": "c11", 12 | "cppStandard": "c++14", 13 | "intelliSenseMode": "linux-clang-x64" 14 | } 15 | ], 16 | "version": 4 17 | } -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # minimum version needed for FetchContent and FetchContent_MakeAvailable 2 | cmake_minimum_required(VERSION 3.14) 3 | project(path_finding_visualizer) 4 | 5 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17 -pthread") 6 | 7 | include_directories(include) 8 | include_directories(include/States) 9 | include_directories(include/States/Algorithms) 10 | include_directories(include/States/Algorithms/GraphBased) 11 | include_directories(include/States/Algorithms/GraphBased/BFS) 12 | include_directories(include/States/Algorithms/GraphBased/DFS) 13 | include_directories(include/States/Algorithms/GraphBased/DIJKSTRA) 14 | include_directories(include/States/Algorithms/GraphBased/ASTAR) 15 | include_directories(include/States/Algorithms/SamplingBased) 16 | include_directories(include/States/Algorithms/SamplingBased/RRT) 17 | include_directories(include/States/Algorithms/SamplingBased/RRT_STAR) 18 | 19 | set(EXECUTABLE_NAME "main") 20 | 21 | add_executable(${EXECUTABLE_NAME} 22 | src/main.cpp 23 | src/Game.cpp 24 | src/State.cpp 25 | src/States/Algorithms/GraphBased/GraphBased.cpp 26 | src/States/Algorithms/GraphBased/Node.cpp 27 | src/States/Algorithms/GraphBased/BFS/BFS.cpp 28 | src/States/Algorithms/GraphBased/DFS/DFS.cpp 29 | src/States/Algorithms/GraphBased/DIJKSTRA/DIJKSTRA.cpp 30 | src/States/Algorithms/GraphBased/ASTAR/ASTAR.cpp 31 | src/States/Algorithms/SamplingBased/SamplingBased.cpp 32 | src/States/Algorithms/SamplingBased/RRT/RRT.cpp 33 | src/States/Algorithms/SamplingBased/RRT_STAR/RRT_STAR.cpp 34 | ) 35 | 36 | target_link_libraries( 37 | ${EXECUTABLE_NAME} 38 | PRIVATE 39 | ImGui-SFML::ImGui-SFML 40 | ) 41 | 42 | add_subdirectory(dependencies) -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Phone Thiha Kyaw 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Pathfinding Visualizer 2 | ===== 3 | ![MIT](https://img.shields.io/badge/license-MIT-blue.svg) 4 | ![CI](https://github.com/mlsdpk/path-finding-visualizer/workflows/build/badge.svg) 5 | 6 | A tool for visualizing numerous pathfinding algorithms in two dimensions. 7 | 8 | This project involves minimal implementations of the popular planning algorithms, including both graph-based and sampling-based planners. We provide an easy-to-use GUI to control the animation process and explore different planner configurations. Current implementation of the project involves four search-based planning algorithms: BFS, DFS, DIJKSTRA and A-Star and two sampling-based planners: RRT and RRT*. The project extensively uses SFML, ImGui and Modern C++ features such as smart pointers, lamda expressions along with multi-threading concepts. 9 | 10 | ![](figures/img0.png) 11 | 12 | ![panel1](figures/img1.png) ![panel2](figures/img2.png) 13 | 14 | ## Dependencies 15 | 16 | * cmake >= 3.14 17 | * All OSes: [click here for installation instructions](https://cmake.org/install/) 18 | * make >= 4.1 (Linux, Mac), 3.81 (Windows) 19 | * Linux: make is installed by default on most Linux distros 20 | * Mac: [install Xcode command line tools to get make](https://developer.apple.com/xcode/features/) 21 | * Windows: [Click here for installation instructions](http://gnuwin32.sourceforge.net/packages/make.htm) 22 | * gcc/g++ >= 5.4 23 | * Linux: gcc / g++ is installed by default on most Linux distros 24 | * Mac: same deal as make - [install Xcode command line tools](https://developer.apple.com/xcode/features/) 25 | * Windows: recommend using [MinGW](http://www.mingw.org/) 26 | 27 | ## Basic Build Instructions 28 | 29 | The project depends on [SFML](https://github.com/SFML/SFML), [Dear ImGui](https://github.com/ocornut/imgui) and [ImGui-SFML](https://github.com/eliasdaler/imgui-sfml). However, you are not required to install external dependencies by yourself. The following build instructions will manage all the necessary external dependencies for you. 30 | 31 | 1. Clone this repo. 32 | 2. Make a build directory in the top level project directory: `mkdir build && cd build` 33 | 3. Compile: `cmake .. && make` 34 | 4. Run it: `./main`. 35 | 36 | ## TODO 37 | 38 | ### Graph-based planners 39 | - [x] BFS 40 | - [x] DFS 41 | - [x] DIJKSTRA 42 | - [x] A* 43 | - [ ] Bidirectional-A* 44 | - [ ] D* 45 | - [ ] LPA* 46 | 47 | ### Sampling-based planners 48 | - [x] RRT 49 | - [ ] RRT-Connect 50 | - [x] RRT* 51 | - [ ] Informed-RRT* 52 | - [ ] FMT* 53 | - [ ] BIT* 54 | - [ ] ABIT* 55 | - [ ] AIT* 56 | 57 | ## Related Publications 58 | - [DIJKSTRA](https://ir.cwi.nl/pub/9256/9256D.pdf): A Note on Two Problems in Connexion with Graphs 59 | - [A*](https://ieeexplore.ieee.org/abstract/document/4082128?casa_token=0ltx8josfO0AAAAA:nA2z0T2qvr00C6rIhIM3Z7GhWJTQpFrYsdzpY9xc_VicZ0DZr5Q9KcclJT1215N3If6pae87MXRHHd0): A Formal Basis for the Heuristic Determination of Minimum Cost Paths 60 | - [LPA*](https://www.cs.cmu.edu/~maxim/files/aij04.pdf): Lifelong Planning A* 61 | - [RRT](https://journals.sagepub.com/doi/pdf/10.1177/02783640122067453?casa_token=fgVkbBjl93wAAAAA:xatnfEy0HmRWnZyzPcPMHoWpW2ch4WIFYY1SSVT-OjyVKidKavkiE7D3QMl3cHSpof4BlXQcSVzhbvo): Randomized kinodynamic planning 62 | - [RRT*](https://journals.sagepub.com/doi/abs/10.1177/0278364911406761): Sampling-based algorithms for optimal motion planning 63 | -------------------------------------------------------------------------------- /config/algorithms.ini: -------------------------------------------------------------------------------- 1 | BFS 2 | DFS 3 | ASTAR 4 | DIJKSTRA 5 | RRT 6 | RRT_STAR 7 | -------------------------------------------------------------------------------- /dependencies/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include(FetchContent) 2 | 3 | FetchContent_Declare( 4 | sfml 5 | GIT_REPOSITORY "https://github.com/SFML/SFML" 6 | GIT_TAG 2f11710abc5aa478503a7ff3f9e654bd2078ebab # 2.5.1 7 | ) 8 | 9 | add_subdirectory(sfml) 10 | 11 | FetchContent_Declare( 12 | imgui 13 | GIT_REPOSITORY https://github.com/ocornut/imgui 14 | # GIT_TAG 55d35d8387c15bf0cfd71861df67af8cfbda7456 15 | GIT_TAG 719d9313041b85227a3e6deb289a313819aaeab3 # latest commit of docking-branch 16 | ) 17 | 18 | FetchContent_Declare( 19 | imgui-sfml 20 | GIT_REPOSITORY https://github.com/eliasdaler/imgui-sfml 21 | GIT_TAG 82dc2033e51b8323857c3ae1cf1f458b3a933c35 22 | ) 23 | 24 | FetchContent_MakeAvailable(imgui) 25 | add_subdirectory(imgui-sfml) 26 | -------------------------------------------------------------------------------- /dependencies/imgui-sfml/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | message(STATUS "Fetching ImGui-SFML...") 2 | 3 | set(IMGUI_DIR ${imgui_SOURCE_DIR}) 4 | set(IMGUI_SFML_FIND_SFML OFF) 5 | set(IMGUI_SFML_IMGUI_DEMO ON) 6 | 7 | FetchContent_MakeAvailable(imgui-sfml) -------------------------------------------------------------------------------- /dependencies/sfml/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | message(STATUS "Fetching SFML...") 2 | 3 | # No need to build audio and network modules 4 | set(SFML_BUILD_AUDIO FALSE) 5 | set(SFML_BUILD_NETWORK FALSE) 6 | 7 | FetchContent_MakeAvailable(sfml) -------------------------------------------------------------------------------- /figures/img0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mlsdpk/path-finding-visualizer/9b3c48fa709d7b1cb743a6fe226e19c484b93148/figures/img0.png -------------------------------------------------------------------------------- /figures/img1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mlsdpk/path-finding-visualizer/9b3c48fa709d7b1cb743a6fe226e19c484b93148/figures/img1.png -------------------------------------------------------------------------------- /figures/img2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mlsdpk/path-finding-visualizer/9b3c48fa709d7b1cb743a6fe226e19c484b93148/figures/img2.png -------------------------------------------------------------------------------- /fonts/OpenSans/LICENSE.txt: -------------------------------------------------------------------------------- 1 | 2 | Apache License 3 | Version 2.0, January 2004 4 | http://www.apache.org/licenses/ 5 | 6 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 7 | 8 | 1. Definitions. 9 | 10 | "License" shall mean the terms and conditions for use, reproduction, 11 | and distribution as defined by Sections 1 through 9 of this document. 12 | 13 | "Licensor" shall mean the copyright owner or entity authorized by 14 | the copyright owner that is granting the License. 15 | 16 | "Legal Entity" shall mean the union of the acting entity and all 17 | other entities that control, are controlled by, or are under common 18 | control with that entity. For the purposes of this definition, 19 | "control" means (i) the power, direct or indirect, to cause the 20 | direction or management of such entity, whether by contract or 21 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 22 | outstanding shares, or (iii) beneficial ownership of such entity. 23 | 24 | "You" (or "Your") shall mean an individual or Legal Entity 25 | exercising permissions granted by this License. 26 | 27 | "Source" form shall mean the preferred form for making modifications, 28 | including but not limited to software source code, documentation 29 | source, and configuration files. 30 | 31 | "Object" form shall mean any form resulting from mechanical 32 | transformation or translation of a Source form, including but 33 | not limited to compiled object code, generated documentation, 34 | and conversions to other media types. 35 | 36 | "Work" shall mean the work of authorship, whether in Source or 37 | Object form, made available under the License, as indicated by a 38 | copyright notice that is included in or attached to the work 39 | (an example is provided in the Appendix below). 40 | 41 | "Derivative Works" shall mean any work, whether in Source or Object 42 | form, that is based on (or derived from) the Work and for which the 43 | editorial revisions, annotations, elaborations, or other modifications 44 | represent, as a whole, an original work of authorship. For the purposes 45 | of this License, Derivative Works shall not include works that remain 46 | separable from, or merely link (or bind by name) to the interfaces of, 47 | the Work and Derivative Works thereof. 48 | 49 | "Contribution" shall mean any work of authorship, including 50 | the original version of the Work and any modifications or additions 51 | to that Work or Derivative Works thereof, that is intentionally 52 | submitted to Licensor for inclusion in the Work by the copyright owner 53 | or by an individual or Legal Entity authorized to submit on behalf of 54 | the copyright owner. For the purposes of this definition, "submitted" 55 | means any form of electronic, verbal, or written communication sent 56 | to the Licensor or its representatives, including but not limited to 57 | communication on electronic mailing lists, source code control systems, 58 | and issue tracking systems that are managed by, or on behalf of, the 59 | Licensor for the purpose of discussing and improving the Work, but 60 | excluding communication that is conspicuously marked or otherwise 61 | designated in writing by the copyright owner as "Not a Contribution." 62 | 63 | "Contributor" shall mean Licensor and any individual or Legal Entity 64 | on behalf of whom a Contribution has been received by Licensor and 65 | subsequently incorporated within the Work. 66 | 67 | 2. Grant of Copyright License. Subject to the terms and conditions of 68 | this License, each Contributor hereby grants to You a perpetual, 69 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 70 | copyright license to reproduce, prepare Derivative Works of, 71 | publicly display, publicly perform, sublicense, and distribute the 72 | Work and such Derivative Works in Source or Object form. 73 | 74 | 3. Grant of Patent License. Subject to the terms and conditions of 75 | this License, each Contributor hereby grants to You a perpetual, 76 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 77 | (except as stated in this section) patent license to make, have made, 78 | use, offer to sell, sell, import, and otherwise transfer the Work, 79 | where such license applies only to those patent claims licensable 80 | by such Contributor that are necessarily infringed by their 81 | Contribution(s) alone or by combination of their Contribution(s) 82 | with the Work to which such Contribution(s) was submitted. If You 83 | institute patent litigation against any entity (including a 84 | cross-claim or counterclaim in a lawsuit) alleging that the Work 85 | or a Contribution incorporated within the Work constitutes direct 86 | or contributory patent infringement, then any patent licenses 87 | granted to You under this License for that Work shall terminate 88 | as of the date such litigation is filed. 89 | 90 | 4. Redistribution. You may reproduce and distribute copies of the 91 | Work or Derivative Works thereof in any medium, with or without 92 | modifications, and in Source or Object form, provided that You 93 | meet the following conditions: 94 | 95 | (a) You must give any other recipients of the Work or 96 | Derivative Works a copy of this License; and 97 | 98 | (b) You must cause any modified files to carry prominent notices 99 | stating that You changed the files; and 100 | 101 | (c) You must retain, in the Source form of any Derivative Works 102 | that You distribute, all copyright, patent, trademark, and 103 | attribution notices from the Source form of the Work, 104 | excluding those notices that do not pertain to any part of 105 | the Derivative Works; and 106 | 107 | (d) If the Work includes a "NOTICE" text file as part of its 108 | distribution, then any Derivative Works that You distribute must 109 | include a readable copy of the attribution notices contained 110 | within such NOTICE file, excluding those notices that do not 111 | pertain to any part of the Derivative Works, in at least one 112 | of the following places: within a NOTICE text file distributed 113 | as part of the Derivative Works; within the Source form or 114 | documentation, if provided along with the Derivative Works; or, 115 | within a display generated by the Derivative Works, if and 116 | wherever such third-party notices normally appear. The contents 117 | of the NOTICE file are for informational purposes only and 118 | do not modify the License. You may add Your own attribution 119 | notices within Derivative Works that You distribute, alongside 120 | or as an addendum to the NOTICE text from the Work, provided 121 | that such additional attribution notices cannot be construed 122 | as modifying the License. 123 | 124 | You may add Your own copyright statement to Your modifications and 125 | may provide additional or different license terms and conditions 126 | for use, reproduction, or distribution of Your modifications, or 127 | for any such Derivative Works as a whole, provided Your use, 128 | reproduction, and distribution of the Work otherwise complies with 129 | the conditions stated in this License. 130 | 131 | 5. Submission of Contributions. Unless You explicitly state otherwise, 132 | any Contribution intentionally submitted for inclusion in the Work 133 | by You to the Licensor shall be under the terms and conditions of 134 | this License, without any additional terms or conditions. 135 | Notwithstanding the above, nothing herein shall supersede or modify 136 | the terms of any separate license agreement you may have executed 137 | with Licensor regarding such Contributions. 138 | 139 | 6. Trademarks. This License does not grant permission to use the trade 140 | names, trademarks, service marks, or product names of the Licensor, 141 | except as required for reasonable and customary use in describing the 142 | origin of the Work and reproducing the content of the NOTICE file. 143 | 144 | 7. Disclaimer of Warranty. Unless required by applicable law or 145 | agreed to in writing, Licensor provides the Work (and each 146 | Contributor provides its Contributions) on an "AS IS" BASIS, 147 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 148 | implied, including, without limitation, any warranties or conditions 149 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 150 | PARTICULAR PURPOSE. You are solely responsible for determining the 151 | appropriateness of using or redistributing the Work and assume any 152 | risks associated with Your exercise of permissions under this License. 153 | 154 | 8. Limitation of Liability. In no event and under no legal theory, 155 | whether in tort (including negligence), contract, or otherwise, 156 | unless required by applicable law (such as deliberate and grossly 157 | negligent acts) or agreed to in writing, shall any Contributor be 158 | liable to You for damages, including any direct, indirect, special, 159 | incidental, or consequential damages of any character arising as a 160 | result of this License or out of the use or inability to use the 161 | Work (including but not limited to damages for loss of goodwill, 162 | work stoppage, computer failure or malfunction, or any and all 163 | other commercial damages or losses), even if such Contributor 164 | has been advised of the possibility of such damages. 165 | 166 | 9. Accepting Warranty or Additional Liability. While redistributing 167 | the Work or Derivative Works thereof, You may choose to offer, 168 | and charge a fee for, acceptance of support, warranty, indemnity, 169 | or other liability obligations and/or rights consistent with this 170 | License. However, in accepting such obligations, You may act only 171 | on Your own behalf and on Your sole responsibility, not on behalf 172 | of any other Contributor, and only if You agree to indemnify, 173 | defend, and hold each Contributor harmless for any liability 174 | incurred by, or claims asserted against, such Contributor by reason 175 | of your accepting any such warranty or additional liability. 176 | 177 | END OF TERMS AND CONDITIONS 178 | 179 | APPENDIX: How to apply the Apache License to your work. 180 | 181 | To apply the Apache License to your work, attach the following 182 | boilerplate notice, with the fields enclosed by brackets "[]" 183 | replaced with your own identifying information. (Don't include 184 | the brackets!) The text should be enclosed in the appropriate 185 | comment syntax for the file format. We also recommend that a 186 | file or class name and description of purpose be included on the 187 | same "printed page" as the copyright notice for easier 188 | identification within third-party archives. 189 | 190 | Copyright [yyyy] [name of copyright owner] 191 | 192 | Licensed under the Apache License, Version 2.0 (the "License"); 193 | you may not use this file except in compliance with the License. 194 | You may obtain a copy of the License at 195 | 196 | http://www.apache.org/licenses/LICENSE-2.0 197 | 198 | Unless required by applicable law or agreed to in writing, software 199 | distributed under the License is distributed on an "AS IS" BASIS, 200 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 201 | See the License for the specific language governing permissions and 202 | limitations under the License. 203 | -------------------------------------------------------------------------------- /fonts/OpenSans/OpenSans-Bold.ttf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mlsdpk/path-finding-visualizer/9b3c48fa709d7b1cb743a6fe226e19c484b93148/fonts/OpenSans/OpenSans-Bold.ttf -------------------------------------------------------------------------------- /fonts/OpenSans/OpenSans-BoldItalic.ttf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mlsdpk/path-finding-visualizer/9b3c48fa709d7b1cb743a6fe226e19c484b93148/fonts/OpenSans/OpenSans-BoldItalic.ttf -------------------------------------------------------------------------------- /fonts/OpenSans/OpenSans-ExtraBold.ttf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mlsdpk/path-finding-visualizer/9b3c48fa709d7b1cb743a6fe226e19c484b93148/fonts/OpenSans/OpenSans-ExtraBold.ttf -------------------------------------------------------------------------------- /fonts/OpenSans/OpenSans-ExtraBoldItalic.ttf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mlsdpk/path-finding-visualizer/9b3c48fa709d7b1cb743a6fe226e19c484b93148/fonts/OpenSans/OpenSans-ExtraBoldItalic.ttf -------------------------------------------------------------------------------- /fonts/OpenSans/OpenSans-Italic.ttf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mlsdpk/path-finding-visualizer/9b3c48fa709d7b1cb743a6fe226e19c484b93148/fonts/OpenSans/OpenSans-Italic.ttf -------------------------------------------------------------------------------- /fonts/OpenSans/OpenSans-Light.ttf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mlsdpk/path-finding-visualizer/9b3c48fa709d7b1cb743a6fe226e19c484b93148/fonts/OpenSans/OpenSans-Light.ttf -------------------------------------------------------------------------------- /fonts/OpenSans/OpenSans-LightItalic.ttf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mlsdpk/path-finding-visualizer/9b3c48fa709d7b1cb743a6fe226e19c484b93148/fonts/OpenSans/OpenSans-LightItalic.ttf -------------------------------------------------------------------------------- /fonts/OpenSans/OpenSans-Medium.ttf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mlsdpk/path-finding-visualizer/9b3c48fa709d7b1cb743a6fe226e19c484b93148/fonts/OpenSans/OpenSans-Medium.ttf -------------------------------------------------------------------------------- /fonts/OpenSans/OpenSans-MediumItalic.ttf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mlsdpk/path-finding-visualizer/9b3c48fa709d7b1cb743a6fe226e19c484b93148/fonts/OpenSans/OpenSans-MediumItalic.ttf -------------------------------------------------------------------------------- /fonts/OpenSans/OpenSans-Regular.ttf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mlsdpk/path-finding-visualizer/9b3c48fa709d7b1cb743a6fe226e19c484b93148/fonts/OpenSans/OpenSans-Regular.ttf -------------------------------------------------------------------------------- /fonts/OpenSans/OpenSans-SemiBold.ttf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mlsdpk/path-finding-visualizer/9b3c48fa709d7b1cb743a6fe226e19c484b93148/fonts/OpenSans/OpenSans-SemiBold.ttf -------------------------------------------------------------------------------- /fonts/OpenSans/OpenSans-SemiBoldItalic.ttf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mlsdpk/path-finding-visualizer/9b3c48fa709d7b1cb743a6fe226e19c484b93148/fonts/OpenSans/OpenSans-SemiBoldItalic.ttf -------------------------------------------------------------------------------- /include/Game.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "State.h" 13 | 14 | namespace path_finding_visualizer { 15 | 16 | static const std::vector GRAPH_BASED_PLANNERS{"BFS", "DFS", 17 | "DIJKSTRA", "A*"}; 18 | static const std::vector SAMPLING_BASED_PLANNERS{"RRT", "RRT*"}; 19 | enum GRAPH_BASED_PLANNERS_IDS { BFS, DFS, DIJKSTRA, AStar }; 20 | enum SAMPLING_BASED_PLANNERS_IDS { RRT, RRT_STAR }; 21 | 22 | class Game { 23 | public: 24 | // Constructors 25 | Game(sf::RenderWindow* window, sf::RenderTexture* render_texture); 26 | 27 | // Destructors 28 | virtual ~Game(); 29 | 30 | // Accessors 31 | const bool running() const; 32 | 33 | // Functions 34 | void pollEvents(); 35 | void updateDt(); 36 | void update(); 37 | void render(); 38 | void initGuiTheme(); 39 | void renderNewPlannerMenu(); 40 | void renderRunMenu(ImGuiIO& io); 41 | void setGraphBasedPlanner(const int id); 42 | void setSamplingBasedPlanner(const int id); 43 | void showHowToUseWindow(); 44 | void showAboutWindow(); 45 | 46 | private: 47 | sf::RenderWindow* window_; 48 | sf::RenderTexture* render_texture_; 49 | sf::Vector2f view_move_xy_; 50 | ImVec2 mouse_pos_in_canvas_; 51 | sf::Event ev_; 52 | sf::Clock dtClock_; 53 | float dt_; 54 | std::stack> states_; 55 | std::string curr_planner_; 56 | std::shared_ptr logger_panel_; 57 | bool disable_run_; 58 | bool show_how_to_use_window_{true}; 59 | bool show_about_window_{true}; 60 | bool show_control_panel_{true}; 61 | bool show_console_{true}; 62 | bool show_stats_panel_{true}; 63 | }; 64 | 65 | } // namespace path_finding_visualizer -------------------------------------------------------------------------------- /include/Gui.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace path_finding_visualizer { 7 | namespace gui { 8 | 9 | class LoggerPanel { 10 | public: 11 | LoggerPanel() { 12 | AutoScroll = true; 13 | clear(); 14 | } 15 | 16 | void clear() { 17 | Buf.clear(); 18 | LineOffsets.clear(); 19 | LineOffsets.push_back(0); 20 | } 21 | 22 | void render(const char* title) { 23 | if (!ImGui::Begin(title)) { 24 | ImGui::End(); 25 | return; 26 | } 27 | 28 | ImGui::BeginChild("scrolling", ImVec2(0, 0), false, 29 | ImGuiWindowFlags_HorizontalScrollbar); 30 | 31 | ImGui::PushStyleVar(ImGuiStyleVar_ItemSpacing, ImVec2(0, 0)); 32 | const char* buf = Buf.begin(); 33 | const char* buf_end = Buf.end(); 34 | { 35 | ImGuiListClipper clipper; 36 | clipper.Begin(LineOffsets.Size); 37 | while (clipper.Step()) { 38 | for (int line_no = clipper.DisplayStart; line_no < clipper.DisplayEnd; 39 | line_no++) { 40 | const char* line_start = buf + LineOffsets[line_no]; 41 | const char* line_end = (line_no + 1 < LineOffsets.Size) 42 | ? (buf + LineOffsets[line_no + 1] - 1) 43 | : buf_end; 44 | ImGui::TextUnformatted(line_start, line_end); 45 | } 46 | } 47 | clipper.End(); 48 | } 49 | ImGui::PopStyleVar(); 50 | 51 | if (AutoScroll && ImGui::GetScrollY() >= ImGui::GetScrollMaxY()) 52 | ImGui::SetScrollHereY(1.0f); 53 | 54 | ImGui::EndChild(); 55 | ImGui::End(); 56 | } 57 | 58 | void info(const std::string& msg) { AddLog("[INFO] %s\n", msg.c_str()); } 59 | 60 | private: 61 | ImGuiTextBuffer Buf; 62 | ImVector LineOffsets; // Index to lines offset. We maintain this with 63 | // AddLog() calls. 64 | bool AutoScroll; // Keep scrolling if already at the bottom. 65 | 66 | void AddLog(const char* fmt, ...) IM_FMTARGS(2) { 67 | int old_size = Buf.size(); 68 | va_list args; 69 | va_start(args, fmt); 70 | Buf.appendfv(fmt, args); 71 | va_end(args); 72 | for (int new_size = Buf.size(); old_size < new_size; old_size++) 73 | if (Buf[old_size] == '\n') LineOffsets.push_back(old_size + 1); 74 | } 75 | }; 76 | 77 | // Helper to display a little (?) mark which shows a tooltip when hovered. 78 | // In your own code you may want to display an actual icon if you are using a 79 | // merged icon fonts (see docs/FONTS.md) 80 | inline void HelpMarker(const char* desc) { 81 | ImGui::TextDisabled("(?)"); 82 | if (ImGui::IsItemHovered()) { 83 | ImGui::BeginTooltip(); 84 | ImGui::PushTextWrapPos(ImGui::GetFontSize() * 35.0f); 85 | ImGui::TextUnformatted(desc); 86 | ImGui::PopTextWrapPos(); 87 | ImGui::EndTooltip(); 88 | } 89 | } 90 | 91 | inline bool inputInt(const std::string& label, int* val, const int& min_val, 92 | const int& max_val, const int& step = 1, 93 | const int& step_fast = 100, 94 | const std::string& help_marker = "", 95 | ImGuiInputTextFlags flags = 0) { 96 | flags |= ImGuiInputTextFlags_EnterReturnsTrue; 97 | bool is_active = ImGui::InputInt(label.c_str(), val, step, step_fast, flags); 98 | if (!help_marker.empty()) { 99 | ImGui::SameLine(); 100 | HelpMarker(help_marker.c_str()); 101 | } 102 | if (is_active) { 103 | if (*val < min_val) 104 | *val = min_val; 105 | else if (*val > max_val) 106 | *val = max_val; 107 | } 108 | return is_active; 109 | } 110 | 111 | inline bool inputDouble(const std::string& label, double* val, 112 | const double& min_val, const double& max_val, 113 | const double& step = 0.0, const double& step_fast = 0.0, 114 | const std::string& help_marker = "", 115 | const char* format = "%.6f", 116 | ImGuiInputTextFlags flags = 0) { 117 | flags |= ImGuiInputTextFlags_EnterReturnsTrue; 118 | bool is_active = 119 | ImGui::InputDouble(label.c_str(), val, step, step_fast, format, flags); 120 | if (!help_marker.empty()) { 121 | ImGui::SameLine(); 122 | HelpMarker(help_marker.c_str()); 123 | } 124 | if (is_active) { 125 | if (*val < min_val) 126 | *val = min_val; 127 | else if (*val > max_val) 128 | *val = max_val; 129 | } 130 | return is_active; 131 | } 132 | 133 | } // namespace gui 134 | } // namespace path_finding_visualizer -------------------------------------------------------------------------------- /include/MessageQueue.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | template 9 | class MessageQueue { 10 | public: 11 | T receive() { 12 | // perform queue modification under the lock 13 | std::unique_lock uLock(mutex_); 14 | cond_.wait(uLock, [this] { 15 | return !messages_.empty(); 16 | }); // pass unique lock to condition variable 17 | 18 | // remove last vector element from queue 19 | T msg = std::move(messages_.back()); 20 | messages_.pop(); 21 | 22 | return msg; // will not be copied due to return value optimization (RVO) in 23 | // C++ 24 | }; 25 | 26 | void send(T &&msg) { 27 | // perform vector modification under the lock 28 | std::lock_guard uLock(mutex_); 29 | 30 | // add vector to queue 31 | messages_.push(std::move(msg)); 32 | cond_.notify_one(); // notify client after pushing new Vehicle into vector 33 | }; 34 | 35 | private: 36 | std::mutex mutex_; 37 | std::condition_variable cond_; 38 | std::queue messages_; 39 | }; 40 | -------------------------------------------------------------------------------- /include/State.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include "Gui.h" 15 | 16 | /* 17 | State Base Class 18 | */ 19 | 20 | namespace path_finding_visualizer { 21 | 22 | class State { 23 | private: 24 | protected: 25 | std::shared_ptr logger_panel_; 26 | sf::Vector2f mousePositionWindow_; 27 | bool is_reset_; 28 | bool is_running_; 29 | 30 | public: 31 | // Constructor 32 | State(std::shared_ptr logger_panel); 33 | 34 | // Destructor 35 | virtual ~State(); 36 | 37 | void setReset(bool is_reset) { is_reset_ = is_reset; } 38 | void setRunning(bool is_running) { is_running_ = is_running; } 39 | 40 | // Functions 41 | void updateMousePosition(const ImVec2 &mousePos); 42 | 43 | // virtual functions 44 | virtual void endState() = 0; 45 | virtual void update(const float &dt, const ImVec2 &mousePos) = 0; 46 | virtual void renderConfig() = 0; 47 | virtual void renderScene(sf::RenderTexture &render_texture) = 0; 48 | }; 49 | 50 | } // namespace path_finding_visualizer -------------------------------------------------------------------------------- /include/States/Algorithms/GraphBased/ASTAR/ASTAR.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "States/Algorithms/GraphBased/BFS/BFS.h" 6 | #include "States/Algorithms/GraphBased/Utils.h" 7 | 8 | namespace path_finding_visualizer { 9 | namespace graph_based { 10 | 11 | // custom function for returning minimum distance node 12 | // to be used in priority queue 13 | struct MinimumDistanceASTAR { 14 | // operator overloading 15 | bool operator()(const std::shared_ptr &n1, 16 | const std::shared_ptr &n2) const { 17 | return n1->getFDistance() > n2->getFDistance(); 18 | } 19 | }; 20 | 21 | class ASTAR : public BFS { 22 | public: 23 | // Constructor 24 | ASTAR(std::shared_ptr logger_panel); 25 | 26 | // Destructor 27 | virtual ~ASTAR(); 28 | 29 | // Overriden functions 30 | virtual void initAlgorithm() override; 31 | 32 | // override main update function 33 | virtual void updatePlanner(bool &solved, Node &start_node, 34 | Node &end_node) override; 35 | 36 | protected: 37 | // ASTAR related 38 | std::priority_queue, std::vector>, 39 | MinimumDistanceASTAR> 40 | frontier_; 41 | 42 | bool use_manhattan_heuristics_{true}; 43 | }; 44 | 45 | } // namespace graph_based 46 | } // namespace path_finding_visualizer -------------------------------------------------------------------------------- /include/States/Algorithms/GraphBased/BFS/BFS.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "GraphBased.h" 6 | #include "Node.h" 7 | 8 | namespace path_finding_visualizer { 9 | namespace graph_based { 10 | 11 | class BFS : public GraphBased { 12 | public: 13 | // Constructor 14 | BFS(std::shared_ptr logger_panel); 15 | 16 | // Destructor 17 | virtual ~BFS(); 18 | 19 | // override initialization Functions 20 | virtual void initAlgorithm() override; 21 | 22 | // override update functions 23 | virtual void updateNodes() override; 24 | 25 | // override render functions 26 | virtual void renderNodes(sf::RenderTexture &render_texture) override; 27 | virtual void renderParametersGui() override; 28 | 29 | // override main update function 30 | virtual void updatePlanner(bool &solved, Node &start_node, 31 | Node &end_node) override; 32 | 33 | private: 34 | // BFS related 35 | std::queue> frontier_; 36 | }; 37 | 38 | } // namespace graph_based 39 | } // namespace path_finding_visualizer -------------------------------------------------------------------------------- /include/States/Algorithms/GraphBased/DFS/DFS.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "States/Algorithms/GraphBased/BFS/BFS.h" 6 | 7 | namespace path_finding_visualizer { 8 | namespace graph_based { 9 | 10 | class DFS : public BFS { 11 | public: 12 | // Constructor 13 | DFS(std::shared_ptr logger_panel); 14 | 15 | // Destructor 16 | virtual ~DFS(); 17 | 18 | // override initialization Functions 19 | void initAlgorithm() override; 20 | 21 | // override main update function 22 | virtual void updatePlanner(bool &solved, Node &start_node, 23 | Node &end_node) override; 24 | 25 | private: 26 | // DFS related 27 | std::stack> frontier_; 28 | }; 29 | 30 | } // namespace graph_based 31 | } // namespace path_finding_visualizer -------------------------------------------------------------------------------- /include/States/Algorithms/GraphBased/DIJKSTRA/DIJKSTRA.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "States/Algorithms/GraphBased/BFS/BFS.h" 6 | #include "States/Algorithms/GraphBased/Utils.h" 7 | 8 | namespace path_finding_visualizer { 9 | namespace graph_based { 10 | 11 | // custom function for returning minimum distance node 12 | // to be used in priority queue 13 | struct MinimumDistanceDIJKSTRA { 14 | // operator overloading 15 | bool operator()(const std::shared_ptr &n1, 16 | const std::shared_ptr &n2) const { 17 | return n1->getGDistance() > n2->getGDistance(); 18 | } 19 | }; 20 | 21 | class DIJKSTRA : public BFS { 22 | public: 23 | // Constructor 24 | DIJKSTRA(std::shared_ptr logger_panel); 25 | 26 | // Destructor 27 | virtual ~DIJKSTRA(); 28 | 29 | // Overriden functions 30 | virtual void initAlgorithm() override; 31 | 32 | // override main update function 33 | virtual void updatePlanner(bool &solved, Node &start_node, 34 | Node &end_node) override; 35 | 36 | protected: 37 | // DIJKSTRA related 38 | std::priority_queue, std::vector>, 39 | MinimumDistanceDIJKSTRA> 40 | frontier_; 41 | }; 42 | 43 | } // namespace graph_based 44 | } // namespace path_finding_visualizer -------------------------------------------------------------------------------- /include/States/Algorithms/GraphBased/GraphBased.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include "Gui.h" 14 | #include "MessageQueue.h" 15 | #include "State.h" 16 | #include "States/Algorithms/GraphBased/Node.h" 17 | #include "States/Algorithms/GraphBased/Utils.h" 18 | 19 | namespace path_finding_visualizer { 20 | namespace graph_based { 21 | 22 | class GraphBased : public State { 23 | public: 24 | // Constructor 25 | GraphBased(std::shared_ptr logger_panel); 26 | 27 | // Destructor 28 | virtual ~GraphBased(); 29 | 30 | // Override Functions 31 | void endState() override; 32 | void update(const float& dt, const ImVec2& mousePos) override; 33 | void renderConfig() override; 34 | void renderScene(sf::RenderTexture& render_texture) override; 35 | 36 | // virtual functions 37 | virtual void clearObstacles(); 38 | virtual void renderGui(); 39 | // render planner specific parameters 40 | virtual void renderParametersGui() = 0; 41 | virtual void renderNodes(sf::RenderTexture& render_texture) = 0; 42 | virtual void updateNodes() = 0; 43 | virtual void initAlgorithm() = 0; 44 | // pure virtual function need to be implemented by graph-based planners 45 | virtual void updatePlanner(bool& solved, Node& node_start, 46 | Node& node_end) = 0; 47 | 48 | void solveConcurrently(std::shared_ptr nodeStart, 49 | std::shared_ptr nodeEnd, 50 | std::shared_ptr> message_queue); 51 | void updateKeyTime(const float& dt); 52 | const bool getKeyTime(); 53 | 54 | protected: 55 | // initialization Functions 56 | void initColors(); 57 | void initVariables(); 58 | void initGridMapParams(); 59 | void initNodes(bool reset = true, bool reset_neighbours_only = false); 60 | 61 | // colors 62 | sf::Color BGN_COL, FONT_COL, IDLE_COL, HOVER_COL, ACTIVE_COL, START_COL, 63 | END_COL, VISITED_COL, FRONTIER_COL, OBST_COL, PATH_COL; 64 | 65 | // key timers 66 | float keyTime_; 67 | float keyTimeMax_; 68 | 69 | // Map Variables 70 | int no_of_grid_rows_; 71 | int no_of_grid_cols_; 72 | int grid_size_; 73 | int ui_grid_size_; 74 | sf::Vector2f init_grid_xy_; 75 | // 0 = 4 connected grid, 1 = 8 connected grid 76 | int grid_connectivity_; 77 | unsigned int map_width_; 78 | unsigned int map_height_; 79 | 80 | // Algorithm related 81 | std::string algo_name_; 82 | std::vector> nodes_; 83 | std::shared_ptr nodeStart_; 84 | std::shared_ptr nodeEnd_; 85 | 86 | // MessageQueue Object 87 | std::shared_ptr> message_queue_; 88 | 89 | // logic flags 90 | bool is_initialized_; 91 | bool is_solved_; 92 | bool disable_run_; 93 | bool disable_gui_parameters_; 94 | 95 | // threads 96 | std::thread t_; 97 | bool thread_joined_; 98 | }; 99 | 100 | } // namespace graph_based 101 | } // namespace path_finding_visualizer 102 | -------------------------------------------------------------------------------- /include/States/Algorithms/GraphBased/Node.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | namespace path_finding_visualizer { 10 | namespace graph_based { 11 | 12 | class Node { 13 | public: 14 | // Constructor 15 | Node(); 16 | 17 | // Destructor 18 | ~Node(); 19 | 20 | // Functions 21 | const bool isObstacle() const; 22 | const bool isVisited() const; 23 | const bool isFrontier() const; 24 | const bool isPath() const; 25 | const bool isStart() const; 26 | const bool isGoal() const; 27 | 28 | // Accessors 29 | sf::Vector2i getPos() const; 30 | std::shared_ptr getParentNode(); 31 | const std::vector>* getNeighbours() const; 32 | const double getGDistance() const; 33 | const double getFDistance() const; 34 | 35 | // Mutators 36 | void setObstacle(bool b); 37 | void setVisited(bool b); 38 | void setFrontier(bool b); 39 | void setPath(bool b); 40 | void setStart(bool b); 41 | void setGoal(bool b); 42 | void setPosition(sf::Vector2i pos); 43 | void setNeighbours(std::shared_ptr node); 44 | void clearNeighbours(); 45 | void setParentNode(std::shared_ptr node); 46 | void setGDistance(double dist); 47 | void setFDistance(double dist); 48 | 49 | protected: 50 | // Variables 51 | bool isObstacle_; 52 | bool isVisited_; 53 | bool isFrontier_; 54 | bool isPath_; 55 | bool isStart_; 56 | bool isGoal_; 57 | sf::Vector2i pos_; 58 | std::vector> vecNeighbours_; 59 | std::shared_ptr parent_; 60 | double gDist_; 61 | double fDist_; 62 | }; 63 | 64 | } // namespace graph_based 65 | } // namespace path_finding_visualizer -------------------------------------------------------------------------------- /include/States/Algorithms/GraphBased/Utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "States/Algorithms/GraphBased/Node.h" 6 | 7 | namespace path_finding_visualizer { 8 | namespace graph_based { 9 | namespace utils { 10 | 11 | inline double distanceCost(const Node &n1, const Node &n2) { 12 | return std::sqrt( 13 | (n1.getPos().x - n2.getPos().x) * (n1.getPos().x - n2.getPos().x) + 14 | (n1.getPos().y - n2.getPos().y) * (n1.getPos().y - n2.getPos().y)); 15 | } 16 | 17 | inline double costToGoHeuristics(const Node &n1, const Node &n2, 18 | bool use_manhattan = false) { 19 | if (use_manhattan) 20 | return fabs(n1.getPos().x - n2.getPos().x) + 21 | fabs(n1.getPos().y - n2.getPos().y); 22 | 23 | return std::sqrt( 24 | (n1.getPos().x - n2.getPos().x) * (n1.getPos().x - n2.getPos().x) + 25 | (n1.getPos().y - n2.getPos().y) * (n1.getPos().y - n2.getPos().y)); 26 | } 27 | 28 | inline void addNeighbours(std::vector> &nodes, 29 | const unsigned int node_idx, const unsigned int width, 30 | const unsigned int height, 31 | bool use_eight_connectivity = false) { 32 | // TODO: Add error handling here 33 | 34 | // setup 35 | const int size = width * height; 36 | const int cell_minus_width = node_idx - width; 37 | const int cell_plus_width = node_idx + width; 38 | const int cell_mod_width = node_idx % width; 39 | const int top_right = width - 1; 40 | 41 | unsigned int top_idx, bottom_idx; 42 | 43 | // if not at top-edge and bottom-edge, we add both top and bottom neighbours 44 | if (cell_minus_width >= 0 && cell_plus_width < size) { 45 | top_idx = cell_minus_width; 46 | bottom_idx = cell_plus_width; 47 | nodes[node_idx]->setNeighbours(nodes[top_idx]); 48 | nodes[node_idx]->setNeighbours(nodes[bottom_idx]); 49 | } 50 | // otherwise, it can be at either top-edge or bottom-edge 51 | else { 52 | // if at top-edge, only add bottom neighbour 53 | if (cell_minus_width < 0) { 54 | bottom_idx = cell_plus_width; 55 | nodes[node_idx]->setNeighbours(nodes[bottom_idx]); 56 | } 57 | // otherwise, add top neighbour 58 | else { 59 | top_idx = cell_minus_width; 60 | nodes[node_idx]->setNeighbours(nodes[top_idx]); 61 | } 62 | } 63 | 64 | // if the cell is not at the right-edge of 2D grid, 65 | // we find all the right, top-right and bottom-right neighbours 66 | if (cell_mod_width != top_right) { 67 | nodes[node_idx]->setNeighbours(nodes[node_idx + 1]); // right neighbour 68 | 69 | if (use_eight_connectivity) { 70 | // now find if node is at top edge or bottom edge or otherwise 71 | if (cell_minus_width < 0) { // if at top-edge 72 | nodes[node_idx]->setNeighbours( 73 | nodes[bottom_idx + 1]); // bottom-right neighbour 74 | } 75 | // if at bottom-edge 76 | else if (cell_plus_width >= size) { 77 | nodes[node_idx]->setNeighbours( 78 | nodes[top_idx + 1]); // top-right neighbour 79 | } 80 | // otherwise, we add both 81 | else { 82 | nodes[node_idx]->setNeighbours( 83 | nodes[top_idx + 1]); // top-right neighbour 84 | nodes[node_idx]->setNeighbours( 85 | nodes[bottom_idx + 1]); // bottom-right neighbour 86 | } 87 | } 88 | } 89 | 90 | // if the cell is not at the left-edge of 2D grid, 91 | // we find all the left, top-left and bottom-left neighbours 92 | if (cell_mod_width != 0) { 93 | nodes[node_idx]->setNeighbours(nodes[node_idx - 1]); // left neighbour 94 | 95 | if (use_eight_connectivity) { 96 | // now find if node is at top edge or bottom edge or otherwise 97 | if (cell_minus_width < 0) { // if at top-edge 98 | nodes[node_idx]->setNeighbours( 99 | nodes[bottom_idx - 1]); // bottom-left neighbour 100 | } 101 | // if at bottom-edge 102 | else if (cell_plus_width >= size) { 103 | nodes[node_idx]->setNeighbours( 104 | nodes[top_idx - 1]); // top-left neighbour 105 | } 106 | // otherwise, we add both 107 | else { 108 | nodes[node_idx]->setNeighbours( 109 | nodes[top_idx - 1]); // top-right neighbour 110 | nodes[node_idx]->setNeighbours( 111 | nodes[bottom_idx - 1]); // bottom-right neighbour 112 | } 113 | } 114 | } 115 | } 116 | 117 | } // namespace utils 118 | } // namespace graph_based 119 | } // namespace path_finding_visualizer -------------------------------------------------------------------------------- /include/States/Algorithms/SamplingBased/RRT/RRT.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "States/Algorithms/SamplingBased/SamplingBased.h" 7 | 8 | namespace path_finding_visualizer { 9 | namespace sampling_based { 10 | 11 | class RRT : public SamplingBased { 12 | public: 13 | // Constructor 14 | RRT(std::shared_ptr logger_panel, const std::string &name); 15 | 16 | // Destructor 17 | virtual ~RRT(); 18 | 19 | // override initialization functions 20 | virtual void initialize() override; 21 | virtual void initPlanner() override; 22 | virtual void initParameters() override; 23 | 24 | // override render functions 25 | virtual void renderPlannerData(sf::RenderTexture &render_texture) override; 26 | virtual void renderParametersGui() override; 27 | 28 | // override main update function 29 | virtual void updatePlanner(bool &solved, Vertex &start, 30 | Vertex &goal) override; 31 | 32 | /** 33 | * @brief Randomly sample a vertex 34 | * @param v Sampled vertex 35 | */ 36 | void sample(const std::shared_ptr &v); 37 | 38 | /** 39 | * @brief Find the nearest neighbour in a tree 40 | * @param v Nearest vertex 41 | */ 42 | void nearest(const std::shared_ptr &x_rand, 43 | std::shared_ptr &x_near); 44 | 45 | /** 46 | * @brief The cost to come of a vertex (g-value) 47 | */ 48 | double cost(std::shared_ptr v); 49 | 50 | /** 51 | * @brief The euclidean distance between two vertices 52 | */ 53 | double distance(const std::shared_ptr &v1, 54 | const std::shared_ptr &v2); 55 | 56 | /** 57 | * @brief Check whether collision or not between two vertices 58 | * This function assumes from_v vertex is collision-free 59 | * @param from_v Starting vertex 60 | * @param to_v Ending vertex 61 | * @return true if there is a collision otherwise false 62 | */ 63 | bool isCollision(const std::shared_ptr &from_v, 64 | const std::shared_ptr &to_v); 65 | 66 | /** 67 | * @brief Find the new interpolated vertex from from_v vertex to to_v 68 | * vertex 69 | * @param from_v Starting vertex 70 | * @param to_v Ending vertex 71 | * @param t Interpolation distance 72 | * @param v New vertex 73 | */ 74 | void interpolate(const std::shared_ptr &from_v, 75 | const std::shared_ptr &to_v, const double t, 76 | const std::shared_ptr &v); 77 | 78 | /** 79 | * @brief Check whether a vertex lies within goal radius or not 80 | */ 81 | bool inGoalRegion(const std::shared_ptr &v); 82 | 83 | protected: 84 | /** 85 | * @brief Interpolation distance during collsion checking 86 | */ 87 | double interpolation_dist_; 88 | 89 | /** 90 | * @brief Maximum distance allowed between two vertices 91 | */ 92 | double range_; 93 | 94 | /** 95 | * @brief Distance between vertex and goal 96 | */ 97 | double goal_radius_; 98 | }; 99 | 100 | } // namespace sampling_based 101 | } // namespace path_finding_visualizer -------------------------------------------------------------------------------- /include/States/Algorithms/SamplingBased/RRT_STAR/RRT_STAR.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "States/Algorithms/SamplingBased/RRT/RRT.h" 6 | 7 | namespace path_finding_visualizer { 8 | namespace sampling_based { 9 | 10 | class RRT_STAR : public RRT { 11 | public: 12 | // Constructor 13 | RRT_STAR(std::shared_ptr logger_panel, 14 | const std::string& name); 15 | 16 | // Destructor 17 | virtual ~RRT_STAR(); 18 | 19 | virtual void renderParametersGui() override; 20 | 21 | // override initialization functions 22 | virtual void initialize() override; 23 | virtual void initPlanner() override; 24 | virtual void initParameters() override; 25 | 26 | // override algorithm function 27 | virtual void updatePlanner(bool& solved, Vertex& start, 28 | Vertex& goal) override; 29 | 30 | /** 31 | * @brief Find all the nearest neighbours inside the radius of particular 32 | * vertex provided 33 | * @param x_new Target vertex 34 | * @param X_near Vector of nearest neighbours 35 | */ 36 | void near(const std::shared_ptr& x_new, 37 | std::vector>& X_near); 38 | 39 | /** 40 | * @brief Calculate r_rrt_ based on current measure 41 | */ 42 | void updateRewiringLowerBounds(); 43 | 44 | protected: 45 | /** 46 | * @brief Find best goal parent at every n iteration 47 | */ 48 | unsigned int update_goal_every_; 49 | 50 | /** 51 | * @brief Rewiring lower bound constant 52 | */ 53 | double r_rrt_; 54 | 55 | /** 56 | * @brief Rewiring factor 57 | */ 58 | double rewire_factor_; 59 | 60 | /** 61 | * @brief Measure (i.e., n-dimensional volume) of the current state space 62 | */ 63 | double current_measure_; 64 | 65 | /** 66 | * @brief Vertices that lie within the goal radius 67 | */ 68 | std::vector> x_soln_; 69 | }; 70 | 71 | } // namespace sampling_based 72 | } // namespace path_finding_visualizer -------------------------------------------------------------------------------- /include/States/Algorithms/SamplingBased/SamplingBased.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include "MessageQueue.h" 14 | #include "State.h" 15 | #include "States/Algorithms/SamplingBased/Utils.h" 16 | 17 | namespace path_finding_visualizer { 18 | namespace sampling_based { 19 | 20 | static const sf::Color OBST_COL = sf::Color(0, 0, 0, 255); 21 | static const sf::Color START_COL = sf::Color(0, 255, 0, 255); 22 | static const sf::Color GOAL_COL = sf::Color(255, 0, 0, 255); 23 | static const sf::Color EDGE_COL = sf::Color(0, 0, 255, 255); 24 | static const sf::Color PATH_COL = sf::Color(255, 0, 255, 255); 25 | 26 | struct Vertex { 27 | double x, y; 28 | std::shared_ptr parent{nullptr}; 29 | }; 30 | 31 | class SamplingBased : public State { 32 | public: 33 | // Constructor 34 | SamplingBased(std::shared_ptr logger_panel, 35 | const std::string &name); 36 | 37 | // Destructor 38 | virtual ~SamplingBased(); 39 | 40 | // Override Functions 41 | void endState() override; 42 | void update(const float &dt, const ImVec2 &mousePos) override; 43 | void renderConfig() override; 44 | void renderScene(sf::RenderTexture &render_texture) override; 45 | 46 | void updateUserInput(); 47 | void renderMap(sf::RenderTexture &render_texture); 48 | void renderObstacles(sf::RenderTexture &render_texture); 49 | void clearObstacles(); 50 | void initMapVariables(); 51 | void initVariables(); 52 | void updateKeyTime(const float &dt); 53 | const bool getKeyTime(); 54 | 55 | virtual void renderGui(); 56 | 57 | // virtual functions 58 | // all the sampling-based planners need to override this functions 59 | 60 | // rendering function for algorithm specific 61 | virtual void renderPlannerData(sf::RenderTexture &render_texture) = 0; 62 | 63 | // render planner specific parameters 64 | virtual void renderParametersGui() = 0; 65 | 66 | // initialization function 67 | // this function runs at start of the program & when user presses RESET 68 | // buttons 69 | virtual void initialize() = 0; 70 | 71 | // planner related initialization 72 | // this function runs when user presses RUN buttons 73 | virtual void initPlanner() = 0; 74 | virtual void initParameters() = 0; 75 | 76 | // pure virtual function need to be implemented by (single-query) 77 | // sampling-based planners 78 | virtual void updatePlanner(bool &solved, Vertex &start, Vertex &goal) = 0; 79 | 80 | // main algorithm function (runs in separate thread) 81 | void solveConcurrently(std::shared_ptr start_point, 82 | std::shared_ptr goal_point, 83 | std::shared_ptr> message_queue); 84 | 85 | protected: 86 | // key timers 87 | float key_time_; 88 | float key_time_max_; 89 | 90 | // Map related 91 | sf::Vector2f init_grid_xy_; 92 | unsigned int obst_size_; 93 | int map_width_; 94 | int map_height_; 95 | std::vector> obstacles_; 96 | 97 | /** 98 | * @brief Random number generator 99 | */ 100 | std::mt19937 rn_gen_; 101 | 102 | // planner related 103 | // vertices & edges 104 | std::vector> vertices_; 105 | std::vector, std::shared_ptr>> 106 | edges_; 107 | std::shared_ptr start_vertex_; 108 | std::shared_ptr goal_vertex_; 109 | 110 | /** 111 | * @brief Maximum number of iterations to run the algorithm 112 | */ 113 | int max_iterations_; 114 | 115 | std::mutex iter_no_mutex_; 116 | unsigned int curr_iter_no_{0u}; 117 | 118 | // MessageQueue Object 119 | std::shared_ptr> message_queue_; 120 | 121 | // logic flags 122 | bool is_initialized_; 123 | bool is_solved_; 124 | bool is_stopped_; 125 | bool disable_run_; 126 | bool disable_gui_parameters_; 127 | 128 | // threads & mutex 129 | std::thread t_; 130 | bool thread_joined_; 131 | std::mutex mutex_; 132 | }; 133 | 134 | } // namespace sampling_based 135 | } // namespace path_finding_visualizer -------------------------------------------------------------------------------- /include/States/Algorithms/SamplingBased/Utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace path_finding_visualizer { 7 | namespace sampling_based { 8 | namespace utils { 9 | 10 | inline double map(double x, double in_min, double in_max, double out_min, 11 | double out_max) { 12 | return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; 13 | } 14 | 15 | inline std::vector linspace(double start, double end, int num) { 16 | std::vector linspaced; 17 | 18 | if (num == 0) { 19 | linspaced.push_back(start); 20 | return linspaced; 21 | } else if (num == 1) { 22 | linspaced.push_back(start); 23 | linspaced.push_back(end); 24 | return linspaced; 25 | } 26 | 27 | double delta = (end - start) / (num); 28 | 29 | for (int i = 0; i < num; i++) { 30 | linspaced.push_back(start + delta * i); 31 | } 32 | linspaced.push_back(end); 33 | return linspaced; 34 | } 35 | 36 | class sfPath : public sf::Drawable { 37 | public: 38 | sfPath(const sf::Vector2f& point1, const sf::Vector2f& point2, 39 | float thickness, sf::Color color) 40 | : color(color), thickness(thickness) { 41 | sf::Vector2f direction = point2 - point1; 42 | sf::Vector2f unitDirection = 43 | direction / 44 | std::sqrt(direction.x * direction.x + direction.y * direction.y); 45 | sf::Vector2f unitPerpendicular(-unitDirection.y, unitDirection.x); 46 | 47 | sf::Vector2f offset = (thickness / 2.f) * unitPerpendicular; 48 | 49 | vertices[0].position = point1 + offset; 50 | vertices[1].position = point2 + offset; 51 | vertices[2].position = point2 - offset; 52 | vertices[3].position = point1 - offset; 53 | 54 | for (int i = 0; i < 4; ++i) vertices[i].color = color; 55 | } 56 | 57 | void draw(sf::RenderTarget& target, sf::RenderStates states) const { 58 | target.draw(vertices, 4, sf::Quads); 59 | } 60 | 61 | private: 62 | sf::Vertex vertices[4]; 63 | float thickness; 64 | sf::Color color; 65 | }; 66 | 67 | } // namespace utils 68 | } // namespace sampling_based 69 | } // namespace path_finding_visualizer -------------------------------------------------------------------------------- /src/Game.cpp: -------------------------------------------------------------------------------- 1 | #include "Game.h" 2 | 3 | #include "States/Algorithms/GraphBased/ASTAR/ASTAR.h" 4 | #include "States/Algorithms/GraphBased/BFS/BFS.h" 5 | #include "States/Algorithms/GraphBased/DFS/DFS.h" 6 | #include "States/Algorithms/GraphBased/DIJKSTRA/DIJKSTRA.h" 7 | #include "States/Algorithms/SamplingBased/RRT/RRT.h" 8 | #include "States/Algorithms/SamplingBased/RRT_STAR/RRT_STAR.h" 9 | 10 | using bfs_state_type = path_finding_visualizer::graph_based::BFS; 11 | using dfs_state_type = path_finding_visualizer::graph_based::DFS; 12 | using dijkstra_state_type = path_finding_visualizer::graph_based::DIJKSTRA; 13 | using astar_state_type = path_finding_visualizer::graph_based::ASTAR; 14 | using rrt_state_type = path_finding_visualizer::sampling_based::RRT; 15 | using rrtstar_state_type = path_finding_visualizer::sampling_based::RRT_STAR; 16 | 17 | namespace path_finding_visualizer { 18 | 19 | // Constructor 20 | Game::Game(sf::RenderWindow* window, sf::RenderTexture* render_texture) 21 | : window_{window}, render_texture_{render_texture}, disable_run_{false} { 22 | logger_panel_ = std::make_shared(); 23 | curr_planner_ = GRAPH_BASED_PLANNERS[0]; 24 | // manually add BFS for now 25 | states_.push(std::make_unique(logger_panel_)); 26 | view_move_xy_.x = view_move_xy_.y = 0.f; 27 | initGuiTheme(); 28 | } 29 | 30 | // Destructor 31 | Game::~Game() {} 32 | 33 | // Accessors 34 | const bool Game::running() const { return window_->isOpen(); } 35 | 36 | // Functions 37 | void Game::pollEvents() { 38 | // Event polling 39 | while (window_->pollEvent(ev_)) { 40 | // ImGui::SFML::ProcessEvent(ev_); 41 | ImGui::SFML::ProcessEvent(ev_); 42 | switch (ev_.type) { 43 | case sf::Event::Closed: 44 | window_->close(); 45 | break; 46 | case sf::Event::KeyPressed: 47 | if (ev_.key.code == sf::Keyboard::Escape) window_->close(); 48 | break; 49 | } 50 | } 51 | } 52 | 53 | void Game::updateDt() { dt_ = dtClock_.getElapsedTime().asSeconds(); } 54 | 55 | void Game::update() { 56 | pollEvents(); 57 | updateDt(); 58 | 59 | if (!states_.empty()) { 60 | states_.top()->update(dt_, mouse_pos_in_canvas_); 61 | } else { 62 | // End the Application 63 | window_->close(); 64 | } 65 | ImGui::SFML::Update(*window_, dtClock_.restart()); 66 | } 67 | 68 | void Game::renderNewPlannerMenu() { 69 | if (ImGui::BeginMenu("New Planner")) { 70 | if (ImGui::BeginMenu("Graph-based Planners")) { 71 | for (int n = 0; n < GRAPH_BASED_PLANNERS.size(); n++) { 72 | bool selected = (GRAPH_BASED_PLANNERS[n] == curr_planner_); 73 | if (ImGui::MenuItem(GRAPH_BASED_PLANNERS[n].c_str(), nullptr, selected, 74 | !selected)) { 75 | if (!selected) { 76 | // change the planner 77 | logger_panel_->clear(); 78 | disable_run_ = false; 79 | setGraphBasedPlanner(n); 80 | } 81 | curr_planner_ = GRAPH_BASED_PLANNERS[n]; 82 | } 83 | } 84 | ImGui::EndMenu(); 85 | } 86 | 87 | if (ImGui::BeginMenu("Sampling-based Planners")) { 88 | for (int n = 0; n < SAMPLING_BASED_PLANNERS.size(); n++) { 89 | bool selected = (SAMPLING_BASED_PLANNERS[n] == curr_planner_); 90 | if (ImGui::MenuItem(SAMPLING_BASED_PLANNERS[n].c_str(), nullptr, 91 | selected, !selected)) { 92 | if (!selected) { 93 | // change the planner 94 | logger_panel_->clear(); 95 | disable_run_ = false; 96 | setSamplingBasedPlanner(n); 97 | } 98 | curr_planner_ = SAMPLING_BASED_PLANNERS[n]; 99 | } 100 | } 101 | ImGui::EndMenu(); 102 | } 103 | 104 | ImGui::EndMenu(); 105 | } 106 | } 107 | 108 | void Game::renderRunMenu(ImGuiIO& io) { 109 | if (ImGui::BeginMenu("Run")) { 110 | { 111 | if (disable_run_) ImGui::BeginDisabled(); 112 | bool clicked = ImGui::MenuItem("Start Planning"); 113 | if (disable_run_) ImGui::EndDisabled(); 114 | if (clicked) { 115 | logger_panel_->info("RUN button pressed. Planning started."); 116 | disable_run_ = true; 117 | if (!states_.empty()) { 118 | states_.top()->setRunning(true); 119 | } 120 | } 121 | } 122 | { 123 | if (!disable_run_) ImGui::BeginDisabled(); 124 | bool clicked = ImGui::MenuItem("Reset Planner Data"); 125 | if (!disable_run_) ImGui::EndDisabled(); 126 | if (clicked) { 127 | logger_panel_->info("RESET button pressed. Planning resetted."); 128 | disable_run_ = false; 129 | if (!states_.empty()) { 130 | states_.top()->setReset(true); 131 | } 132 | } 133 | } 134 | ImGui::EndMenu(); 135 | } 136 | } 137 | 138 | void Game::render() { 139 | window_->clear(); 140 | render_texture_->clear(sf::Color::White); 141 | 142 | if (!states_.empty()) { 143 | // DOCKING STUFFS 144 | static bool opt_dockspace = true; 145 | static bool opt_padding = false; 146 | static bool opt_fullscreen = true; 147 | static ImGuiDockNodeFlags dockspace_flags = ImGuiDockNodeFlags_None; 148 | 149 | // We are using the ImGuiWindowFlags_NoDocking flag to make the parent 150 | // window not dockable into, because it would be confusing to have two 151 | // docking targets within each others. 152 | ImGuiWindowFlags window_flags = 153 | ImGuiWindowFlags_MenuBar | ImGuiWindowFlags_NoDocking; 154 | if (opt_fullscreen) { 155 | const ImGuiViewport* viewport = ImGui::GetMainViewport(); 156 | ImGui::SetNextWindowPos(viewport->WorkPos); 157 | ImGui::SetNextWindowSize(viewport->WorkSize); 158 | ImGui::SetNextWindowViewport(viewport->ID); 159 | ImGui::PushStyleVar(ImGuiStyleVar_WindowRounding, 0.0f); 160 | ImGui::PushStyleVar(ImGuiStyleVar_WindowBorderSize, 0.0f); 161 | window_flags |= ImGuiWindowFlags_NoTitleBar | 162 | ImGuiWindowFlags_NoCollapse | ImGuiWindowFlags_NoResize | 163 | ImGuiWindowFlags_NoMove; 164 | window_flags |= 165 | ImGuiWindowFlags_NoBringToFrontOnFocus | ImGuiWindowFlags_NoNavFocus; 166 | } else { 167 | dockspace_flags &= ~ImGuiDockNodeFlags_PassthruCentralNode; 168 | } 169 | 170 | if (dockspace_flags & ImGuiDockNodeFlags_PassthruCentralNode) 171 | window_flags |= ImGuiWindowFlags_NoBackground; 172 | 173 | if (!opt_padding) 174 | ImGui::PushStyleVar(ImGuiStyleVar_WindowPadding, ImVec2(0.0f, 0.0f)); 175 | ImGui::Begin("DockSpace Demo", &opt_dockspace, window_flags); 176 | if (!opt_padding) ImGui::PopStyleVar(); 177 | if (opt_fullscreen) ImGui::PopStyleVar(2); 178 | 179 | // Submit the DockSpace 180 | ImGuiIO& io = ImGui::GetIO(); 181 | if (io.ConfigFlags & ImGuiConfigFlags_DockingEnable) { 182 | ImGuiID dockspace_id = ImGui::GetID("MyDockSpace"); 183 | ImGui::DockSpace(dockspace_id, ImVec2(0.0f, 0.0f), dockspace_flags); 184 | } 185 | 186 | if (ImGui::BeginMenuBar()) { 187 | if (ImGui::BeginMenu("File")) { 188 | renderNewPlannerMenu(); 189 | ImGui::Separator(); 190 | if (ImGui::MenuItem("Exit", NULL, false)) window_->close(); 191 | ImGui::EndMenu(); 192 | } 193 | if (ImGui::BeginMenu("View")) { 194 | ImGui::MenuItem("Show Control Panel", nullptr, &show_control_panel_); 195 | ImGui::MenuItem("Show Console", nullptr, &show_console_); 196 | ImGui::MenuItem("Show Stats", nullptr, &show_stats_panel_); 197 | ImGui::EndMenu(); 198 | } 199 | renderRunMenu(io); 200 | if (ImGui::BeginMenu("Help")) { 201 | ImGui::MenuItem("How To Use", nullptr, &show_how_to_use_window_); 202 | ImGui::MenuItem("About", nullptr, &show_about_window_); 203 | ImGui::EndMenu(); 204 | } 205 | ImGui::EndMenuBar(); 206 | } 207 | 208 | if (show_how_to_use_window_) { 209 | ImGui::Begin("How To Use"); 210 | ImGui::Text("USAGE GUIDE:"); 211 | ImGui::BulletText("Left-click to place/remove obstacle cells"); 212 | ImGui::BulletText("Left-SHIFT+left-click to change starting cell"); 213 | ImGui::BulletText("Left-CTRL+left-click to change end cell"); 214 | ImGui::End(); 215 | } 216 | 217 | if (show_about_window_) { 218 | ImGui::Begin("About"); 219 | ImGui::Text("Path-finding Visualizer (v1.0.0)"); 220 | ImGui::Text("Developed and maintained by Phone Thiha Kyaw."); 221 | ImGui::Text("Email: mlsdphonethk @ gmail dot com"); 222 | ImGui::Separator(); 223 | ImGui::Text("ABOUT THIS VISUALIZER:"); 224 | ImGui::Text( 225 | "This project involves minimal implementations of\nthe popular " 226 | "planning algorithms, including\nboth graph-based and " 227 | "sampling-based planners."); 228 | 229 | ImGui::Separator(); 230 | 231 | ImGui::Text("AVAILABLE PLANNERS:"); 232 | 233 | ImGui::BulletText("Graph-based Planners:"); 234 | ImGui::Indent(); 235 | ImGui::BulletText("Breadth-first search (BFS)"); 236 | ImGui::BulletText("Depth-first search (DFS)"); 237 | ImGui::BulletText("Dijkstra"); 238 | ImGui::BulletText("A*"); 239 | 240 | ImGui::Unindent(); 241 | ImGui::BulletText("Sampling-based Planners:"); 242 | ImGui::Indent(); 243 | ImGui::BulletText("Rapidly-exploring random trees (RRT)"); 244 | ImGui::BulletText("RRT*"); 245 | ImGui::Unindent(); 246 | ImGui::End(); 247 | } 248 | 249 | if (show_control_panel_) { 250 | //////////////////////////////// 251 | // Control Panel 252 | //////////////////////////////// 253 | ImGui::Begin("Control Panel"); 254 | 255 | // render planner specific configurations 256 | states_.top()->renderConfig(); 257 | 258 | ImGui::End(); // end Configurations 259 | } 260 | 261 | if (show_stats_panel_) { 262 | ImGui::Begin("Stats"); 263 | ImGui::Text("Current Planner: %s", curr_planner_.c_str()); 264 | ImGui::Spacing(); 265 | ImGui::Spacing(); 266 | ImGui::End(); 267 | } 268 | 269 | ////////////////////////////////////////////////////////////////////// 270 | // Planning Scene Panel 271 | ////////////////////////////////////////////////////////////////////// 272 | ImGui::PushStyleVar(ImGuiStyleVar_WindowPadding, ImVec2{0.f, 0.f}); 273 | ImGui::Begin("Planning Scene"); 274 | 275 | const ImVec2 planning_scene_panel_size = ImGui::GetContentRegionAvail(); 276 | render_texture_->create(static_cast(planning_scene_panel_size.x), 277 | static_cast(planning_scene_panel_size.y)); 278 | render_texture_->clear(sf::Color::White); 279 | 280 | sf::View view; 281 | view.setSize( 282 | sf::Vector2f(planning_scene_panel_size.x, planning_scene_panel_size.y)); 283 | view.setCenter( 284 | sf::Vector2f((planning_scene_panel_size.x / 2.f) + view_move_xy_.x, 285 | (planning_scene_panel_size.y / 2.f) + view_move_xy_.y)); 286 | render_texture_->setView(view); 287 | states_.top()->renderScene(*render_texture_); 288 | 289 | ImGui::ImageButton(*render_texture_, 0); 290 | 291 | const bool is_hovered = ImGui::IsItemHovered(); // Hovered 292 | const bool is_active = ImGui::IsItemActive(); // Held 293 | 294 | // move the planning scene around by dragging mouse Right-click 295 | if (is_hovered && ImGui::IsMouseDragging(ImGuiMouseButton_Right)) { 296 | view_move_xy_.x -= io.MouseDelta.x; 297 | view_move_xy_.y -= io.MouseDelta.y; 298 | } 299 | 300 | // Update the current mouse position in planning scene panel 301 | ImVec2 canvas_p0 = ImGui::GetCursorScreenPos(); 302 | const ImVec2 origin(canvas_p0.x - view_move_xy_.x, 303 | canvas_p0.y - view_move_xy_.y); 304 | mouse_pos_in_canvas_.x = io.MousePos.x - origin.x; 305 | mouse_pos_in_canvas_.y = 306 | io.MousePos.y - origin.y + planning_scene_panel_size.y; 307 | 308 | ImGui::End(); 309 | ImGui::PopStyleVar(); 310 | ////////////////////////////////////////////////////////////////////// 311 | 312 | if (show_console_) { 313 | ////////////////////////// 314 | // Console Panel 315 | ////////////////////////// 316 | ImGui::Begin("Console"); 317 | ImGui::PushStyleVar(ImGuiStyleVar_FrameRounding, 6.f); 318 | if (ImGui::Button("Clear##console_clear")) { 319 | logger_panel_->clear(); 320 | } 321 | ImGui::PopStyleVar(); 322 | ImGui::Spacing(); 323 | ImGui::Separator(); 324 | ImGui::End(); 325 | logger_panel_->render("Console"); 326 | ////////////////////////// 327 | } 328 | 329 | ImGui::End(); // dockspace end 330 | 331 | // ImGui::ShowDemoWindow(); 332 | 333 | ImGui::SFML::Render(*window_); 334 | window_->display(); 335 | render_texture_->display(); 336 | } 337 | } 338 | 339 | void Game::initGuiTheme() { 340 | // setup imgui style 341 | ImGuiIO& io = ImGui::GetIO(); 342 | io.Fonts->AddFontFromFileTTF("../fonts/OpenSans/OpenSans-Regular.ttf", 18.0f); 343 | ImGui::SFML::UpdateFontTexture(); 344 | 345 | // enable docking 346 | io.ConfigFlags |= ImGuiConfigFlags_DockingEnable; 347 | 348 | ImGuiStyle* style = &ImGui::GetStyle(); 349 | style->FramePadding = ImVec2(8.f, 4.f); 350 | 351 | // dark theme colors 352 | auto& colors = ImGui::GetStyle().Colors; 353 | colors[ImGuiCol_WindowBg] = ImVec4(0.1f, 0.105f, 0.11f, 1.0f); 354 | 355 | // headers 356 | colors[ImGuiCol_Header] = ImVec4(0.2f, 0.205f, 0.21f, 1.0f); 357 | colors[ImGuiCol_HeaderHovered] = ImVec4(0.3f, 0.305f, 0.31f, 1.0f); 358 | colors[ImGuiCol_HeaderActive] = ImVec4(0.15f, 0.1505f, 0.151f, 1.0f); 359 | 360 | // buttons 361 | colors[ImGuiCol_Button] = ImVec4(0.2f, 0.205f, 0.21f, 1.0f); 362 | colors[ImGuiCol_ButtonHovered] = ImVec4(0.3f, 0.305f, 0.31f, 1.0f); 363 | colors[ImGuiCol_ButtonActive] = ImVec4(0.15f, 0.1505f, 0.151f, 1.0f); 364 | 365 | // tabs 366 | colors[ImGuiCol_Tab] = ImVec4(0.15f, 0.1505f, 0.151f, 1.0f); 367 | colors[ImGuiCol_TabHovered] = ImVec4(0.38, 0.3805f, 0.381f, 1.0f); 368 | colors[ImGuiCol_TabActive] = ImVec4(0.28, 0.2805f, 0.281f, 1.0f); 369 | colors[ImGuiCol_TabUnfocused] = ImVec4(0.15f, 0.1505f, 0.151f, 1.0f); 370 | colors[ImGuiCol_TabUnfocusedActive] = ImVec4(0.2f, 0.205f, 0.21f, 1.0f); 371 | 372 | // frame background 373 | colors[ImGuiCol_FrameBg] = ImVec4(0.2f, 0.205f, 0.21f, 1.0f); 374 | colors[ImGuiCol_FrameBgHovered] = ImVec4(0.3f, 0.305f, 0.31f, 1.0f); 375 | colors[ImGuiCol_FrameBgActive] = ImVec4(0.15f, 0.1505f, 0.151f, 1.0f); 376 | 377 | // sider 378 | colors[ImGuiCol_SliderGrab] = colors[ImGuiCol_Text]; 379 | colors[ImGuiCol_SliderGrabActive] = colors[ImGuiCol_Text]; 380 | 381 | // progress bar 382 | colors[ImGuiCol_PlotHistogram] = ImVec4(0.3f, 0.305f, 0.31f, 1.0f); 383 | 384 | // title 385 | colors[ImGuiCol_TitleBg] = ImVec4(0.15f, 0.1505f, 0.151f, 1.0f); 386 | colors[ImGuiCol_TitleBgActive] = ImVec4(0.15f, 0.1505f, 0.151f, 1.0f); 387 | colors[ImGuiCol_TitleBgCollapsed] = ImVec4(0.15f, 0.1505f, 0.151f, 1.0f); 388 | } 389 | 390 | void Game::setGraphBasedPlanner(const int id) { 391 | switch (id) { 392 | case GRAPH_BASED_PLANNERS_IDS::BFS: 393 | // BFS 394 | states_.push(std::make_unique(logger_panel_)); 395 | break; 396 | case GRAPH_BASED_PLANNERS_IDS::DFS: 397 | // DFS 398 | states_.push(std::make_unique(logger_panel_)); 399 | break; 400 | case GRAPH_BASED_PLANNERS_IDS::DIJKSTRA: 401 | // Dijkstra 402 | states_.push(std::make_unique(logger_panel_)); 403 | break; 404 | case GRAPH_BASED_PLANNERS_IDS::AStar: 405 | // A-Star 406 | states_.push(std::make_unique(logger_panel_)); 407 | break; 408 | default: 409 | break; 410 | } 411 | } 412 | 413 | void Game::setSamplingBasedPlanner(const int id) { 414 | switch (id) { 415 | case SAMPLING_BASED_PLANNERS_IDS::RRT: 416 | // RRT 417 | states_.push(std::make_unique( 418 | logger_panel_, SAMPLING_BASED_PLANNERS[id])); 419 | break; 420 | case SAMPLING_BASED_PLANNERS_IDS::RRT_STAR: 421 | // RRTStar 422 | states_.push(std::make_unique( 423 | logger_panel_, SAMPLING_BASED_PLANNERS[id])); 424 | break; 425 | default: 426 | break; 427 | } 428 | } 429 | 430 | } // namespace path_finding_visualizer -------------------------------------------------------------------------------- /src/State.cpp: -------------------------------------------------------------------------------- 1 | #include "State.h" 2 | 3 | namespace path_finding_visualizer { 4 | 5 | State::State(std::shared_ptr logger_panel) 6 | : logger_panel_{logger_panel} {} 7 | 8 | State::~State() {} 9 | 10 | void State::updateMousePosition(const ImVec2& mousePos) { 11 | mousePositionWindow_.x = mousePos.x; 12 | mousePositionWindow_.y = mousePos.y; 13 | } 14 | 15 | } // namespace path_finding_visualizer -------------------------------------------------------------------------------- /src/States/Algorithms/GraphBased/ASTAR/ASTAR.cpp: -------------------------------------------------------------------------------- 1 | #include "ASTAR.h" 2 | 3 | namespace path_finding_visualizer { 4 | namespace graph_based { 5 | 6 | // Constructor 7 | ASTAR::ASTAR(std::shared_ptr logger_panel) 8 | : BFS(logger_panel) {} 9 | 10 | // Destructor 11 | ASTAR::~ASTAR() {} 12 | 13 | // override initAlgorithm() function 14 | void ASTAR::initAlgorithm() { 15 | // initialize ASTAR by clearing frontier and add start node 16 | while (!frontier_.empty()) { 17 | frontier_.pop(); 18 | } 19 | 20 | use_manhattan_heuristics_ = (grid_connectivity_ == 0) ? true : false; 21 | 22 | nodeStart_->setGDistance(0.0); 23 | nodeStart_->setFDistance(utils::costToGoHeuristics( 24 | *nodeStart_, *nodeEnd_, use_manhattan_heuristics_)); 25 | frontier_.push(nodeStart_); 26 | } 27 | 28 | void ASTAR::updatePlanner(bool &solved, Node &start_node, Node &end_node) { 29 | if (!frontier_.empty()) { 30 | std::shared_ptr node_current = frontier_.top(); 31 | node_current->setFrontier(false); 32 | node_current->setVisited(true); 33 | frontier_.pop(); 34 | 35 | if (node_current->isGoal()) { 36 | solved = true; 37 | } 38 | 39 | for (auto node_neighbour : *node_current->getNeighbours()) { 40 | if (node_neighbour->isVisited() || node_neighbour->isObstacle()) { 41 | continue; 42 | } 43 | 44 | double dist = node_current->getGDistance() + 45 | utils::costToGoHeuristics(*node_current, *node_neighbour, 46 | use_manhattan_heuristics_); 47 | 48 | if (dist < node_neighbour->getGDistance()) { 49 | node_neighbour->setParentNode(node_current); 50 | node_neighbour->setGDistance(dist); 51 | 52 | // f = g + h 53 | double f_dist = node_current->getGDistance() + 54 | utils::costToGoHeuristics(*node_neighbour, end_node, 55 | use_manhattan_heuristics_); 56 | node_neighbour->setFDistance(f_dist); 57 | node_neighbour->setFrontier(true); 58 | frontier_.push(node_neighbour); 59 | } 60 | } 61 | } else { 62 | solved = true; 63 | } 64 | } 65 | 66 | } // namespace graph_based 67 | } // namespace path_finding_visualizer -------------------------------------------------------------------------------- /src/States/Algorithms/GraphBased/BFS/BFS.cpp: -------------------------------------------------------------------------------- 1 | #include "States/Algorithms/GraphBased/BFS/BFS.h" 2 | 3 | namespace path_finding_visualizer { 4 | namespace graph_based { 5 | 6 | // Constructor 7 | BFS::BFS(std::shared_ptr logger_panel) 8 | : GraphBased(logger_panel) {} 9 | 10 | // Destructor 11 | BFS::~BFS() {} 12 | 13 | // override initAlgorithm() function 14 | void BFS::initAlgorithm() { 15 | // initialize BFS by clearing frontier and add start node 16 | while (!frontier_.empty()) { 17 | frontier_.pop(); 18 | } 19 | 20 | frontier_.push(nodeStart_); 21 | } 22 | 23 | // override updateNodes() function 24 | void BFS::updateNodes() { 25 | if (sf::Mouse::isButtonPressed(sf::Mouse::Left) && getKeyTime()) { 26 | int localY = ((mousePositionWindow_.x - init_grid_xy_.x) / grid_size_); 27 | int localX = ((mousePositionWindow_.y - init_grid_xy_.y) / grid_size_); 28 | 29 | if (localX >= 0 && localX < map_height_ / grid_size_) { 30 | if (localY >= 0 && localY < map_width_ / grid_size_) { 31 | // get the selected node 32 | std::shared_ptr selectedNode = 33 | nodes_[(map_width_ / grid_size_) * localX + localY]; 34 | 35 | // check the position is Obstacle free or not 36 | bool isObstacle = false; 37 | if (selectedNode->isObstacle()) { 38 | isObstacle = true; 39 | } 40 | 41 | if (!is_solved_) { 42 | if (sf::Keyboard::isKeyPressed(sf::Keyboard::LShift)) { 43 | if (!isObstacle) { 44 | if (selectedNode != nodeEnd_) { 45 | nodeStart_->setStart(false); 46 | nodeStart_ = selectedNode; 47 | nodeStart_->setStart(true); 48 | } 49 | } 50 | } else if (sf::Keyboard::isKeyPressed(sf::Keyboard::LControl)) { 51 | if (!isObstacle) { 52 | if (selectedNode != nodeStart_) { 53 | nodeEnd_->setGoal(false); 54 | nodeEnd_ = selectedNode; 55 | nodeEnd_->setGoal(true); 56 | } 57 | } 58 | } else { 59 | selectedNode->setObstacle(!isObstacle); 60 | } 61 | } else { 62 | if (sf::Keyboard::isKeyPressed(sf::Keyboard::LControl)) { 63 | if (!isObstacle) { 64 | if (selectedNode != nodeStart_) { 65 | nodeEnd_->setGoal(false); 66 | nodeEnd_ = selectedNode; 67 | nodeEnd_->setGoal(true); 68 | } 69 | } 70 | } 71 | } 72 | } 73 | } 74 | } 75 | } 76 | 77 | // override renderNodes() function 78 | void BFS::renderNodes(sf::RenderTexture &render_texture) { 79 | const auto texture_size = render_texture.getSize(); 80 | 81 | init_grid_xy_.x = (texture_size.x / 2.) - (map_width_ / 2.); 82 | init_grid_xy_.y = (texture_size.y / 2.) - (map_height_ / 2.); 83 | 84 | for (int x = 0; x < map_height_ / grid_size_; x++) { 85 | for (int y = 0; y < map_width_ / grid_size_; y++) { 86 | float size = static_cast(grid_size_); 87 | sf::RectangleShape rectangle(sf::Vector2f(size, size)); 88 | rectangle.setOutlineThickness(2.f); 89 | rectangle.setOutlineColor(BGN_COL); 90 | rectangle.setPosition(init_grid_xy_.x + y * size, 91 | init_grid_xy_.y + x * size); 92 | 93 | int nodeIndex = (map_width_ / grid_size_) * x + y; 94 | 95 | if (nodes_[nodeIndex]->isObstacle()) { 96 | rectangle.setFillColor(OBST_COL); 97 | } else if (nodes_[nodeIndex]->isPath()) { 98 | rectangle.setFillColor(PATH_COL); 99 | nodes_[nodeIndex]->setPath(false); 100 | } else if (nodes_[nodeIndex]->isFrontier()) { 101 | rectangle.setFillColor(FRONTIER_COL); 102 | } else if (nodes_[nodeIndex]->isVisited()) { 103 | rectangle.setFillColor(VISITED_COL); 104 | } else { 105 | rectangle.setFillColor(IDLE_COL); 106 | } 107 | 108 | if (nodes_[nodeIndex]->isStart()) { 109 | rectangle.setFillColor(START_COL); 110 | } else if (nodes_[nodeIndex]->isGoal()) { 111 | rectangle.setFillColor(END_COL); 112 | } 113 | render_texture.draw(rectangle); 114 | } 115 | } 116 | 117 | // visualizing path 118 | if (nodeEnd_ != nullptr) { 119 | std::shared_ptr current = nodeEnd_; 120 | while (current->getParentNode() != nullptr && !current->isStart()) { 121 | current->setPath(true); 122 | current = current->getParentNode(); 123 | } 124 | } 125 | } 126 | 127 | void BFS::renderParametersGui() {} 128 | 129 | void BFS::updatePlanner(bool &solved, Node &start_node, Node &end_node) { 130 | if (!frontier_.empty()) { 131 | std::shared_ptr node_current = frontier_.front(); 132 | node_current->setFrontier(false); 133 | frontier_.pop(); 134 | 135 | if (node_current->isGoal()) { 136 | solved = true; 137 | } 138 | 139 | for (auto node_neighbour : *node_current->getNeighbours()) { 140 | if (!node_neighbour->isVisited() && node_neighbour->isObstacle() == 0) { 141 | node_neighbour->setParentNode(node_current); 142 | node_neighbour->setVisited(true); 143 | node_neighbour->setFrontier(true); 144 | frontier_.push(node_neighbour); 145 | } 146 | } 147 | } else { 148 | solved = true; 149 | } 150 | } 151 | 152 | } // namespace graph_based 153 | } // namespace path_finding_visualizer -------------------------------------------------------------------------------- /src/States/Algorithms/GraphBased/DFS/DFS.cpp: -------------------------------------------------------------------------------- 1 | #include "DFS.h" 2 | 3 | namespace path_finding_visualizer { 4 | namespace graph_based { 5 | 6 | // Constructor 7 | DFS::DFS(std::shared_ptr logger_panel) : BFS(logger_panel) {} 8 | 9 | // Destructor 10 | DFS::~DFS() {} 11 | 12 | // override initAlgorithm() function 13 | void DFS::initAlgorithm() { 14 | // initialize DFS by clearing frontier and add start node 15 | while (!frontier_.empty()) { 16 | frontier_.pop(); 17 | } 18 | 19 | frontier_.push(nodeStart_); 20 | } 21 | 22 | void DFS::updatePlanner(bool &solved, Node &start_node, Node &end_node) { 23 | if (!frontier_.empty()) { 24 | std::shared_ptr node_current = frontier_.top(); 25 | node_current->setFrontier(false); 26 | frontier_.pop(); 27 | 28 | if (node_current->isGoal()) { 29 | solved = true; 30 | } 31 | 32 | for (auto node_neighbour : *node_current->getNeighbours()) { 33 | if (!node_neighbour->isVisited() && node_neighbour->isObstacle() == 0) { 34 | node_neighbour->setParentNode(node_current); 35 | node_neighbour->setVisited(true); 36 | node_neighbour->setFrontier(true); 37 | frontier_.push(node_neighbour); 38 | } 39 | } 40 | } else { 41 | solved = true; 42 | } 43 | } 44 | 45 | } // namespace graph_based 46 | } // namespace path_finding_visualizer -------------------------------------------------------------------------------- /src/States/Algorithms/GraphBased/DIJKSTRA/DIJKSTRA.cpp: -------------------------------------------------------------------------------- 1 | #include "DIJKSTRA.h" 2 | 3 | namespace path_finding_visualizer { 4 | namespace graph_based { 5 | 6 | // Constructor 7 | DIJKSTRA::DIJKSTRA(std::shared_ptr logger_panel) 8 | : BFS(logger_panel) {} 9 | 10 | // Destructor 11 | DIJKSTRA::~DIJKSTRA() {} 12 | 13 | // override initAlgorithm() function 14 | void DIJKSTRA::initAlgorithm() { 15 | // initialize DIJKSTRA by clearing frontier and add start node 16 | while (!frontier_.empty()) { 17 | frontier_.pop(); 18 | } 19 | nodeStart_->setGDistance(0.0); 20 | frontier_.push(nodeStart_); 21 | } 22 | 23 | void DIJKSTRA::updatePlanner(bool &solved, Node &start_node, Node &end_node) { 24 | if (!frontier_.empty()) { 25 | std::shared_ptr node_current = frontier_.top(); 26 | node_current->setFrontier(false); 27 | node_current->setVisited(true); 28 | frontier_.pop(); 29 | 30 | if (node_current->isGoal()) { 31 | solved = true; 32 | } 33 | 34 | for (auto node_neighbour : *node_current->getNeighbours()) { 35 | if (node_neighbour->isVisited() || node_neighbour->isObstacle()) { 36 | continue; 37 | } 38 | 39 | double dist = node_current->getGDistance() + 40 | utils::distanceCost(*node_current, *node_neighbour); 41 | 42 | if (dist < node_neighbour->getGDistance()) { 43 | node_neighbour->setParentNode(node_current); 44 | node_neighbour->setGDistance(dist); 45 | 46 | node_neighbour->setFrontier(true); 47 | frontier_.push(node_neighbour); 48 | } 49 | } 50 | } else { 51 | solved = true; 52 | } 53 | } 54 | 55 | } // namespace graph_based 56 | } // namespace path_finding_visualizer -------------------------------------------------------------------------------- /src/States/Algorithms/GraphBased/GraphBased.cpp: -------------------------------------------------------------------------------- 1 | #include "GraphBased.h" 2 | 3 | namespace path_finding_visualizer { 4 | namespace graph_based { 5 | 6 | // Constructor 7 | GraphBased::GraphBased(std::shared_ptr logger_panel) 8 | : State(logger_panel), keyTimeMax_{1.f}, keyTime_{0.f} { 9 | initVariables(); 10 | initNodes(); 11 | initColors(); 12 | } 13 | 14 | // Destructor 15 | GraphBased::~GraphBased() { 16 | if (!thread_joined_) { 17 | t_.join(); 18 | } 19 | } 20 | 21 | void GraphBased::initVariables() { 22 | initGridMapParams(); 23 | grid_connectivity_ = 0; 24 | 25 | message_queue_ = std::make_shared>(); 26 | 27 | is_running_ = false; 28 | is_initialized_ = false; 29 | is_reset_ = false; 30 | is_solved_ = false; 31 | thread_joined_ = true; 32 | disable_run_ = false; 33 | disable_gui_parameters_ = false; 34 | } 35 | 36 | void GraphBased::initGridMapParams() { 37 | ui_grid_size_ = 20; 38 | grid_size_ = ui_grid_size_; 39 | no_of_grid_rows_ = no_of_grid_cols_ = 10; 40 | map_width_ = no_of_grid_cols_ * grid_size_; 41 | map_height_ = no_of_grid_rows_ * grid_size_; 42 | } 43 | 44 | void GraphBased::initColors() { 45 | BGN_COL = sf::Color(246, 229, 245, 255); 46 | FONT_COL = sf::Color(78, 95, 131, 255); 47 | IDLE_COL = sf::Color(251, 244, 249, 255); 48 | HOVER_COL = sf::Color(245, 238, 243, 255); 49 | ACTIVE_COL = sf::Color(232, 232, 232); 50 | START_COL = sf::Color(67, 246, 130, 255); 51 | END_COL = sf::Color(242, 103, 101); 52 | VISITED_COL = sf::Color(198, 202, 229, 255); 53 | FRONTIER_COL = sf::Color(242, 204, 209, 255); 54 | OBST_COL = sf::Color(186, 186, 186, 255); 55 | PATH_COL = sf::Color(190, 242, 227, 255); 56 | } 57 | 58 | void GraphBased::initNodes(bool reset, bool reset_neighbours_only) { 59 | map_width_ = no_of_grid_cols_ * grid_size_; 60 | map_height_ = no_of_grid_rows_ * grid_size_; 61 | 62 | if (reset) { 63 | nodes_.clear(); 64 | 65 | for (int i = 0; i < (map_width_ / grid_size_) * (map_height_ / grid_size_); 66 | i++) { 67 | nodes_.emplace_back(std::make_shared()); 68 | } 69 | } 70 | 71 | // set all nodes to free obsts and respective positions 72 | for (int x = 0; x < map_height_ / grid_size_; x++) { 73 | for (int y = 0; y < map_width_ / grid_size_; y++) { 74 | int nodeIndex = (map_width_ / grid_size_) * x + y; 75 | if (reset) { 76 | nodes_[nodeIndex]->setPosition(sf::Vector2i(x, y)); 77 | nodes_[nodeIndex]->setObstacle(false); 78 | nodes_[nodeIndex]->setStart(false); 79 | nodes_[nodeIndex]->setGoal(false); 80 | } 81 | nodes_[nodeIndex]->setVisited(false); 82 | nodes_[nodeIndex]->setFrontier(false); 83 | nodes_[nodeIndex]->setPath(false); 84 | nodes_[nodeIndex]->setParentNode(nullptr); 85 | nodes_[nodeIndex]->setGDistance(INFINITY); 86 | nodes_[nodeIndex]->setFDistance(INFINITY); 87 | } 88 | } 89 | 90 | // add neighbours based on 4 or 8 connectivity grid 91 | if (reset || reset_neighbours_only) { 92 | for (int x = 0; x < map_height_ / grid_size_; x++) { 93 | for (int y = 0; y < map_width_ / grid_size_; y++) { 94 | int nodeIndex = (map_width_ / grid_size_) * x + y; 95 | nodes_[nodeIndex]->clearNeighbours(); 96 | utils::addNeighbours(nodes_, nodeIndex, 97 | static_cast(map_width_ / grid_size_), 98 | static_cast(map_height_ / grid_size_), 99 | [](int connectivity) { 100 | return (connectivity == 1) ? true : false; 101 | }(grid_connectivity_)); 102 | } 103 | } 104 | } 105 | 106 | if (reset) { 107 | // initialize Start and End nodes ptrs (upper left and lower right corners) 108 | nodeStart_ = nodes_[(map_width_ / grid_size_) * 0 + 0]; 109 | nodeStart_->setParentNode(nullptr); 110 | nodeStart_->setStart(true); 111 | nodeEnd_ = 112 | nodes_[(map_width_ / grid_size_) * (map_height_ / grid_size_ - 1) + 113 | (map_width_ / grid_size_ - 1)]; 114 | nodeEnd_->setGoal(true); 115 | } 116 | } 117 | 118 | void GraphBased::endState() {} 119 | 120 | /** 121 | * Getter function for Algorithm key timer. 122 | * 123 | * @param none. 124 | * @return true if keytime > keytime_max else false. 125 | */ 126 | const bool GraphBased::getKeyTime() { 127 | if (keyTime_ >= keyTimeMax_) { 128 | keyTime_ = 0.f; 129 | return true; 130 | } 131 | return false; 132 | } 133 | 134 | /** 135 | * Update the current time of key timer. 136 | * 137 | * @param dt delta time. 138 | * @return void. 139 | */ 140 | void GraphBased::updateKeyTime(const float& dt) { 141 | if (keyTime_ < keyTimeMax_) { 142 | keyTime_ += 5.f * dt; 143 | } 144 | } 145 | 146 | void GraphBased::update(const float& dt, const ImVec2& mousePos) { 147 | // from base classes 148 | updateKeyTime(dt); 149 | updateMousePosition(mousePos); 150 | 151 | if (is_reset_) { 152 | initNodes(false, false); 153 | is_running_ = false; 154 | is_initialized_ = false; 155 | is_reset_ = false; 156 | is_solved_ = false; 157 | disable_gui_parameters_ = false; 158 | 159 | message_queue_ = std::make_shared>(); 160 | } 161 | 162 | if (is_running_) { 163 | is_reset_ = false; 164 | 165 | // initialize Algorithm 166 | if (!is_initialized_) { 167 | initAlgorithm(); 168 | 169 | // create thread 170 | // solve the algorithm concurrently 171 | t_ = std::thread(&GraphBased::solveConcurrently, this, nodeStart_, 172 | nodeEnd_, message_queue_); 173 | 174 | thread_joined_ = false; 175 | is_initialized_ = true; 176 | disable_gui_parameters_ = true; 177 | } 178 | 179 | // check the algorithm is solved or not 180 | auto msg = message_queue_->receive(); 181 | // if solved 182 | if (msg) { 183 | t_.join(); 184 | thread_joined_ = true; 185 | is_running_ = false; 186 | is_solved_ = true; 187 | } 188 | } else { 189 | // only allow mouse and key inputs 190 | // if the algorithm is not running 191 | 192 | // virtual function updateNodes() 193 | updateNodes(); 194 | } 195 | } 196 | 197 | void GraphBased::clearObstacles() { 198 | for (int x = 0; x < map_height_ / grid_size_; x++) { 199 | for (int y = 0; y < map_width_ / grid_size_; y++) { 200 | int nodeIndex = (map_width_ / grid_size_) * x + y; 201 | nodes_[nodeIndex]->setObstacle(false); 202 | } 203 | } 204 | } 205 | 206 | void GraphBased::renderGui() { 207 | ImGui::PushStyleVar(ImGuiStyleVar_ItemSpacing, ImVec2(8.f, 8.f)); 208 | if (ImGui::CollapsingHeader("Edit", ImGuiTreeNodeFlags_DefaultOpen)) { 209 | if (disable_gui_parameters_) ImGui::BeginDisabled(); 210 | ImGui::Indent(8.f); 211 | 212 | ImGui::PushStyleVar(ImGuiStyleVar_FrameRounding, 6.f); 213 | ImGui::PushStyleVar(ImGuiStyleVar_ItemInnerSpacing, ImVec2(2.f, 4.f)); 214 | 215 | ImGui::Text("Gridmap:"); 216 | ImGui::PushStyleVar(ImGuiStyleVar_ItemSpacing, ImVec2(8.f, 2.f)); 217 | 218 | if (gui::inputInt("rows", &no_of_grid_rows_, 5, 1000, 1, 10, 219 | "Number of rows in the gridmap")) 220 | initNodes(true, false); 221 | 222 | if (gui::inputInt("cols", &no_of_grid_cols_, 5, 1000, 1, 10, 223 | "Number of columns in the gridmap")) 224 | initNodes(true, false); 225 | 226 | ImGui::PopStyleVar(); 227 | 228 | if (gui::inputInt("grid size", &ui_grid_size_, 10, 100, 1, 10, 229 | "The size of a grid. Set this size larger to zoom in the " 230 | "gridmap.")) { 231 | grid_size_ = ui_grid_size_; 232 | initNodes(true, false); 233 | } 234 | 235 | ImGui::Text("Random Obstacles:"); 236 | ImGui::SameLine(); 237 | gui::HelpMarker("TODO: Randomly generate obstacles in the gridmap"); 238 | static int rand_obsts_no = 10; 239 | if (ImGui::InputInt("##rand_obst_input", &rand_obsts_no, 1, 10, 240 | ImGuiInputTextFlags_EnterReturnsTrue)) { 241 | // TODO: 242 | } 243 | ImGui::SameLine(); 244 | if (ImGui::Button("Generate")) { 245 | // TODO: 246 | } 247 | 248 | if (ImGui::Button("Clear Obstacles")) { 249 | clearObstacles(); 250 | } 251 | ImGui::SameLine(); 252 | if (ImGui::Button("Restore Defaults")) { 253 | initGridMapParams(); 254 | initNodes(true, false); 255 | } 256 | 257 | ImGui::PopStyleVar(2); 258 | ImGui::Unindent(8.f); 259 | 260 | if (disable_gui_parameters_) ImGui::EndDisabled(); 261 | ImGui::Spacing(); 262 | } 263 | 264 | if (ImGui::CollapsingHeader("Configurations", 265 | ImGuiTreeNodeFlags_DefaultOpen)) { 266 | if (disable_gui_parameters_) ImGui::BeginDisabled(); 267 | 268 | ImGui::Indent(8.f); 269 | 270 | ImGui::PushStyleVar(ImGuiStyleVar_FrameRounding, 6.f); 271 | ImGui::PushStyleVar(ImGuiStyleVar_ItemInnerSpacing, ImVec2(2.f, 4.f)); 272 | ImGui::PushStyleVar(ImGuiStyleVar_ItemSpacing, ImVec2(8.f, 2.f)); 273 | 274 | // radio buttons for choosing 4 or 8 connected grids 275 | { 276 | bool a, b; 277 | a = ImGui::RadioButton("4-connected", &grid_connectivity_, 0); 278 | ImGui::SameLine(); 279 | b = ImGui::RadioButton("8-connected", &grid_connectivity_, 1); 280 | if (a || b) { 281 | initNodes(false, true); 282 | } 283 | ImGui::SameLine(); 284 | gui::HelpMarker( 285 | "4-connected and 8-connected represent the number of neighbours of a " 286 | "grid.\nPlanners relying on heuristics usually use Manhattan " 287 | "distance (L1-norm) for 4-connected grids\nand Euclidean Distance " 288 | "(L2-norm) for 8-connected grids."); 289 | } 290 | 291 | // virtual function renderParametersGui() 292 | // need to be implemented by derived class 293 | renderParametersGui(); 294 | 295 | ImGui::PopStyleVar(3); 296 | ImGui::Unindent(8.f); 297 | 298 | if (disable_gui_parameters_) ImGui::EndDisabled(); 299 | } 300 | ImGui::PopStyleVar(); 301 | } 302 | 303 | void GraphBased::renderConfig() { renderGui(); } 304 | 305 | void GraphBased::renderScene(sf::RenderTexture& render_texture) { 306 | // virtual function renderNodes() 307 | // need to be implemented by derived class 308 | renderNodes(render_texture); 309 | } 310 | 311 | void GraphBased::solveConcurrently( 312 | std::shared_ptr nodeStart, std::shared_ptr nodeEnd, 313 | std::shared_ptr> message_queue) { 314 | // copy assignment 315 | // thread-safe due to shared_ptrs 316 | std::shared_ptr s_nodeStart = nodeStart; 317 | std::shared_ptr s_nodeEnd = nodeEnd; 318 | std::shared_ptr> s_message_queue = message_queue; 319 | 320 | bool solved = false; 321 | 322 | double cycleDuration = 1.0; // duration of a single simulation cycle in ms 323 | // init stop watch 324 | auto lastUpdate = std::chrono::system_clock::now(); 325 | 326 | while (true) { 327 | // compute time difference to stop watch 328 | long timeSinceLastUpdate = 329 | std::chrono::duration_cast( 330 | std::chrono::system_clock::now() - lastUpdate) 331 | .count(); 332 | 333 | if (timeSinceLastUpdate >= cycleDuration) { 334 | updatePlanner(solved, *s_nodeStart, *s_nodeEnd); 335 | 336 | // reset stop watch for next cycle 337 | lastUpdate = std::chrono::system_clock::now(); 338 | } 339 | 340 | // sends an update method to the message queue using move semantics 341 | auto ftr = std::async(std::launch::async, &MessageQueue::send, 342 | s_message_queue, std::move(solved)); 343 | ftr.wait(); 344 | 345 | if (solved) return; 346 | 347 | // sleep at every iteration to reduce CPU usage 348 | std::this_thread::sleep_for(std::chrono::milliseconds(1)); 349 | } 350 | } 351 | 352 | } // namespace graph_based 353 | } // namespace path_finding_visualizer -------------------------------------------------------------------------------- /src/States/Algorithms/GraphBased/Node.cpp: -------------------------------------------------------------------------------- 1 | #include "Node.h" 2 | 3 | namespace path_finding_visualizer { 4 | namespace graph_based { 5 | 6 | // Constructor 7 | Node::Node() 8 | : isObstacle_{false}, 9 | isVisited_{false}, 10 | isFrontier_{false}, 11 | isPath_{false}, 12 | isStart_{false}, 13 | isGoal_{false}, 14 | parent_{nullptr}, 15 | gDist_{INFINITY}, 16 | fDist_{INFINITY} {} 17 | 18 | // Destructor 19 | Node::~Node() {} 20 | 21 | // Functions 22 | const bool Node::isObstacle() const { return isObstacle_; } 23 | 24 | const bool Node::isVisited() const { return isVisited_; } 25 | 26 | const bool Node::isFrontier() const { return isFrontier_; } 27 | 28 | const bool Node::isPath() const { return isPath_; } 29 | 30 | const bool Node::isStart() const { return isStart_; } 31 | 32 | const bool Node::isGoal() const { return isGoal_; } 33 | 34 | // Accessors 35 | sf::Vector2i Node::getPos() const { return pos_; } 36 | 37 | std::shared_ptr Node::getParentNode() { return parent_; } 38 | 39 | const std::vector>* Node::getNeighbours() const { 40 | return &vecNeighbours_; 41 | } 42 | 43 | void Node::clearNeighbours() { vecNeighbours_.clear(); } 44 | 45 | const double Node::getGDistance() const { return gDist_; } 46 | 47 | const double Node::getFDistance() const { return fDist_; } 48 | 49 | // Mutators 50 | void Node::setObstacle(bool b) { isObstacle_ = b; } 51 | 52 | void Node::setVisited(bool b) { isVisited_ = b; } 53 | 54 | void Node::setFrontier(bool b) { isFrontier_ = b; } 55 | 56 | void Node::setPath(bool b) { isPath_ = b; } 57 | 58 | void Node::setStart(bool b) { isStart_ = b; } 59 | 60 | void Node::setGoal(bool b) { isGoal_ = b; } 61 | 62 | void Node::setPosition(sf::Vector2i pos) { pos_ = pos; } 63 | 64 | void Node::setNeighbours(std::shared_ptr node) { 65 | vecNeighbours_.push_back(node); 66 | } 67 | 68 | void Node::setParentNode(std::shared_ptr node) { parent_ = node; } 69 | 70 | void Node::setGDistance(double dist) { gDist_ = dist; } 71 | 72 | void Node::setFDistance(double dist) { fDist_ = dist; } 73 | 74 | } // namespace graph_based 75 | } // namespace path_finding_visualizer 76 | -------------------------------------------------------------------------------- /src/States/Algorithms/SamplingBased/RRT/RRT.cpp: -------------------------------------------------------------------------------- 1 | #include "States/Algorithms/SamplingBased/RRT/RRT.h" 2 | 3 | namespace path_finding_visualizer { 4 | namespace sampling_based { 5 | 6 | // Constructor 7 | RRT::RRT(std::shared_ptr logger_panel, 8 | const std::string &name) 9 | : SamplingBased(logger_panel, name) { 10 | initParameters(); 11 | initialize(); 12 | } 13 | 14 | // Destructor 15 | RRT::~RRT() {} 16 | 17 | void RRT::initParameters() { 18 | // initialize default planner related params 19 | // TODO: default values should be read from file 20 | max_iterations_ = 1000; 21 | interpolation_dist_ = 0.005; 22 | range_ = 0.05; 23 | goal_radius_ = 0.1; 24 | } 25 | 26 | void RRT::initialize() { 27 | start_vertex_->x = 0.5; 28 | start_vertex_->y = 0.1; 29 | start_vertex_->parent = nullptr; 30 | 31 | goal_vertex_->x = 0.5; 32 | goal_vertex_->y = 0.9; 33 | goal_vertex_->parent = nullptr; 34 | 35 | vertices_.clear(); 36 | edges_.clear(); 37 | } 38 | 39 | // override initPlanner() function 40 | void RRT::initPlanner() { 41 | // clear all the vertices & edges 42 | vertices_.clear(); 43 | edges_.clear(); 44 | 45 | // add start vertex to vertices 46 | vertices_.emplace_back(start_vertex_); 47 | } 48 | 49 | void RRT::renderPlannerData(sf::RenderTexture &render_texture) { 50 | // render edges 51 | std::unique_lock lck(mutex_); 52 | for (const auto &edge : edges_) { 53 | double p1_y = utils::map(edge.first->y, 0.0, 1.0, init_grid_xy_.x, 54 | init_grid_xy_.x + map_width_); 55 | double p1_x = utils::map(edge.first->x, 0.0, 1.0, init_grid_xy_.y, 56 | init_grid_xy_.y + map_height_); 57 | double p2_y = utils::map(edge.second->y, 0.0, 1.0, init_grid_xy_.x, 58 | init_grid_xy_.x + map_width_); 59 | double p2_x = utils::map(edge.second->x, 0.0, 1.0, init_grid_xy_.y, 60 | init_grid_xy_.y + map_height_); 61 | 62 | sf::Vertex line[] = {sf::Vertex(sf::Vector2f(p1_y, p1_x), EDGE_COL), 63 | sf::Vertex(sf::Vector2f(p2_y, p2_x), EDGE_COL)}; 64 | 65 | render_texture.draw(line, 2, sf::Lines); 66 | } 67 | lck.unlock(); 68 | 69 | // render path if available 70 | if (goal_vertex_->parent) { 71 | std::shared_ptr current = goal_vertex_; 72 | while (current->parent && current != start_vertex_) { 73 | double p1_y = utils::map(current->y, 0.0, 1.0, init_grid_xy_.x, 74 | init_grid_xy_.x + map_width_); 75 | double p1_x = utils::map(current->x, 0.0, 1.0, init_grid_xy_.y, 76 | init_grid_xy_.y + map_height_); 77 | double p2_y = utils::map(current->parent->y, 0.0, 1.0, init_grid_xy_.x, 78 | init_grid_xy_.x + map_width_); 79 | double p2_x = utils::map(current->parent->x, 0.0, 1.0, init_grid_xy_.y, 80 | init_grid_xy_.y + map_height_); 81 | 82 | utils::sfPath path(sf::Vector2f(p1_y, p1_x), sf::Vector2f(p2_y, p2_x), 83 | 4.f, PATH_COL); 84 | render_texture.draw(path); 85 | current = current->parent; 86 | } 87 | } 88 | 89 | // render start & goal vertices 90 | sf::CircleShape start_goal_circle(obst_size_ / 2.0); 91 | start_goal_circle.setOrigin(start_goal_circle.getRadius(), 92 | start_goal_circle.getRadius()); 93 | double start_y = utils::map(start_vertex_->y, 0.0, 1.0, init_grid_xy_.x, 94 | init_grid_xy_.x + map_width_); 95 | double start_x = utils::map(start_vertex_->x, 0.0, 1.0, init_grid_xy_.y, 96 | init_grid_xy_.y + map_height_); 97 | double goal_y = utils::map(goal_vertex_->y, 0.0, 1.0, init_grid_xy_.x, 98 | init_grid_xy_.x + map_width_); 99 | double goal_x = utils::map(goal_vertex_->x, 0.0, 1.0, init_grid_xy_.y, 100 | init_grid_xy_.y + map_height_); 101 | 102 | start_goal_circle.setPosition(start_y, start_x); 103 | start_goal_circle.setFillColor(START_COL); 104 | render_texture.draw(start_goal_circle); 105 | 106 | start_goal_circle.setPosition(goal_y, goal_x); 107 | start_goal_circle.setFillColor(GOAL_COL); 108 | render_texture.draw(start_goal_circle); 109 | } 110 | 111 | void RRT::renderParametersGui() { 112 | gui::inputDouble("range", &range_, 0.01, 1000.0, 0.01, 1.0, 113 | "Maximum distance allowed between two vertices", "%.3f"); 114 | gui::inputDouble("goal_radius", &goal_radius_, 0.01, 1000.0, 0.01, 1.0, 115 | "Distance between vertex and goal to stop planning", "%.3f"); 116 | } 117 | 118 | void RRT::updatePlanner(bool &solved, Vertex &start, Vertex &goal) { 119 | std::unique_lock iter_no_lck(iter_no_mutex_); 120 | bool running = (curr_iter_no_ < max_iterations_); 121 | iter_no_lck.unlock(); 122 | 123 | if (running) { 124 | std::shared_ptr x_rand = std::make_shared(); 125 | std::shared_ptr x_nearest = std::make_shared(); 126 | std::shared_ptr x_new = std::make_shared(); 127 | 128 | sample(x_rand); 129 | nearest(x_rand, x_nearest); 130 | 131 | // find the distance between x_rand and x_nearest 132 | double d = distance(x_rand, x_nearest); 133 | 134 | // if this distance d > max_distance_, we need to find nearest state in 135 | // the direction of x_rand 136 | if (d > range_) { 137 | interpolate(x_nearest, x_rand, range_ / d, x_new); 138 | } else { 139 | x_new->x = x_rand->x; 140 | x_new->y = x_rand->y; 141 | } 142 | 143 | if (!isCollision(x_nearest, x_new)) { 144 | x_new->parent = x_nearest; 145 | 146 | std::unique_lock lck(mutex_); 147 | vertices_.emplace_back(x_new); 148 | edges_.emplace_back(x_nearest, x_new); 149 | lck.unlock(); 150 | 151 | if (inGoalRegion(x_new)) { 152 | goal.parent = std::move(x_new); 153 | solved = true; 154 | } 155 | } 156 | 157 | iter_no_lck.lock(); 158 | curr_iter_no_++; 159 | iter_no_lck.unlock(); 160 | } else { 161 | std::cout << "Iterations number reach max limit. Planning stopped." << '\n'; 162 | solved = true; 163 | } 164 | } 165 | 166 | void RRT::sample(const std::shared_ptr &v) { 167 | std::uniform_real_distribution<> dis(0, 1); 168 | v->x = dis(rn_gen_); 169 | v->y = dis(rn_gen_); 170 | } 171 | 172 | bool RRT::isCollision(const std::shared_ptr &from_v, 173 | const std::shared_ptr &to_v) { 174 | // check collison from from_v to to_v 175 | // interpolate vertices between from_v and to_v 176 | // assume from_v is collision free 177 | 178 | const double max_dist = distance(from_v, to_v); 179 | 180 | double d = interpolation_dist_; 181 | while (d < max_dist) { 182 | std::shared_ptr temp_v = std::make_shared(); 183 | interpolate(from_v, to_v, d / max_dist, temp_v); 184 | 185 | double pixel_y = utils::map(temp_v->y, 0.0, 1.0, 0.0, map_width_); 186 | double pixel_x = utils::map(temp_v->x, 0.0, 1.0, 0.0, map_height_); 187 | for (const auto &obst : obstacles_) { 188 | if (obst->getGlobalBounds().contains(sf::Vector2f(pixel_y, pixel_x))) { 189 | return true; 190 | } 191 | } 192 | d += interpolation_dist_; 193 | } 194 | 195 | // now we check the destination vertex to_v 196 | double pixel_y = utils::map(to_v->y, 0.0, 1.0, 0.0, map_width_); 197 | double pixel_x = utils::map(to_v->x, 0.0, 1.0, 0.0, map_height_); 198 | for (const auto &obst : obstacles_) { 199 | if (obst->getGlobalBounds().contains(sf::Vector2f(pixel_y, pixel_x))) { 200 | return true; 201 | } 202 | } 203 | 204 | return false; 205 | } 206 | 207 | void RRT::interpolate(const std::shared_ptr &from_v, 208 | const std::shared_ptr &to_v, const double t, 209 | const std::shared_ptr &v) { 210 | v->x = from_v->x + (to_v->x - from_v->x) * t; 211 | v->y = from_v->y + (to_v->y - from_v->y) * t; 212 | } 213 | 214 | void RRT::nearest(const std::shared_ptr &x_rand, 215 | std::shared_ptr &x_near) { 216 | double minDist = std::numeric_limits::infinity(); 217 | 218 | for (const auto &v : vertices_) { 219 | double dist = distance(v, x_rand); 220 | if (dist < minDist) { 221 | minDist = dist; 222 | x_near = v; 223 | } 224 | } 225 | } 226 | 227 | double RRT::cost(std::shared_ptr v) { 228 | std::shared_ptr curr_p = std::move(v); 229 | double cost = 0.0; 230 | while (curr_p->parent) { 231 | cost += distance(curr_p, curr_p->parent); 232 | curr_p = curr_p->parent; 233 | } 234 | return cost; 235 | } 236 | 237 | double RRT::distance(const std::shared_ptr &v1, 238 | const std::shared_ptr &v2) { 239 | return std::sqrt((v1->x - v2->x) * (v1->x - v2->x) + 240 | (v1->y - v2->y) * (v1->y - v2->y)); 241 | } 242 | 243 | bool RRT::inGoalRegion(const std::shared_ptr &v) { 244 | if (distance(v, goal_vertex_) <= goal_radius_) return true; 245 | return false; 246 | } 247 | 248 | } // namespace sampling_based 249 | } // namespace path_finding_visualizer 250 | -------------------------------------------------------------------------------- /src/States/Algorithms/SamplingBased/RRT_STAR/RRT_STAR.cpp: -------------------------------------------------------------------------------- 1 | #include "States/Algorithms/SamplingBased/RRT_STAR/RRT_STAR.h" 2 | 3 | namespace path_finding_visualizer { 4 | namespace sampling_based { 5 | 6 | // Constructor 7 | RRT_STAR::RRT_STAR(std::shared_ptr logger_panel, 8 | const std::string &name) 9 | : RRT(logger_panel, name) { 10 | initParameters(); 11 | initialize(); 12 | } 13 | 14 | // Destructor 15 | RRT_STAR::~RRT_STAR() {} 16 | 17 | void RRT_STAR::initParameters() { 18 | // initialize default planner related params 19 | // TODO: default values should be read from file 20 | max_iterations_ = 2500; 21 | interpolation_dist_ = 0.005; 22 | range_ = 0.05; 23 | goal_radius_ = 0.1; 24 | rewire_factor_ = 1.1; 25 | update_goal_every_ = 100u; 26 | } 27 | 28 | void RRT_STAR::initialize() { 29 | start_vertex_->x = 0.5; 30 | start_vertex_->y = 0.1; 31 | start_vertex_->parent = nullptr; 32 | 33 | goal_vertex_->x = 0.5; 34 | goal_vertex_->y = 0.9; 35 | goal_vertex_->parent = nullptr; 36 | 37 | vertices_.clear(); 38 | edges_.clear(); 39 | x_soln_.clear(); 40 | 41 | // update rewiring lower bounds 42 | // r_rrt > [(2*(1+1/d))*(area/unitNBallVolume)]^(1/d) 43 | r_rrt_ = rewire_factor_ * 44 | std::pow(2 * (1.0 + 1.0 / 2.0) * (1.0 / M_PI), 1.0 / 2.0); 45 | } 46 | 47 | // override initPlanner() function 48 | void RRT_STAR::initPlanner() { 49 | // clear all the vertices & edges 50 | vertices_.clear(); 51 | edges_.clear(); 52 | x_soln_.clear(); 53 | // add start point to vertices 54 | vertices_.emplace_back(start_vertex_); 55 | } 56 | 57 | void RRT_STAR::renderParametersGui() { 58 | gui::inputDouble("range", &range_, 0.01, 1000.0, 0.01, 1.0, 59 | "Maximum distance allowed between two vertices", "%.3f"); 60 | gui::inputDouble("rewire_factor", &rewire_factor_, 1.0, 2.0, 0.01, 0.1, 61 | "Rewiring factor", "%.2f"); 62 | gui::inputDouble("goal_radius", &goal_radius_, 0.01, 1000.0, 0.01, 1.0, 63 | "Distance between vertex and goal to stop planning", "%.3f"); 64 | } 65 | 66 | void RRT_STAR::updatePlanner(bool &solved, Vertex &start, Vertex &goal) { 67 | std::unique_lock iter_no_lck(iter_no_mutex_); 68 | bool running = (curr_iter_no_ < max_iterations_); 69 | iter_no_lck.unlock(); 70 | 71 | if (running) { 72 | std::shared_ptr x_rand = std::make_shared(); 73 | std::shared_ptr x_nearest = std::make_shared(); 74 | std::shared_ptr x_new = std::make_shared(); 75 | 76 | sample(x_rand); 77 | nearest(x_rand, x_nearest); 78 | 79 | // find the distance between x_rand and x_nearest 80 | double d = distance(x_rand, x_nearest); 81 | 82 | // if this distance d > delta_q, we need to find nearest state in the 83 | // direction of x_rand 84 | if (d > range_) { 85 | interpolate(x_nearest, x_rand, range_ / d, x_new); 86 | } else { 87 | x_new->x = x_rand->x; 88 | x_new->y = x_rand->y; 89 | } 90 | 91 | if (!isCollision(x_nearest, x_new)) { 92 | // find all the nearest neighbours inside radius 93 | std::vector> X_near; 94 | near(x_new, X_near); 95 | 96 | std::unique_lock lck(mutex_); 97 | vertices_.emplace_back(x_new); 98 | lck.unlock(); 99 | 100 | // choose parent 101 | std::shared_ptr x_min = x_nearest; 102 | for (const auto &x_near : X_near) { 103 | double c_new = cost(x_near) + distance(x_near, x_new); 104 | if (c_new < cost(x_min) + distance(x_min, x_new)) { 105 | if (!isCollision(x_near, x_new)) { 106 | x_min = x_near; 107 | } 108 | } 109 | } 110 | x_new->parent = x_min; 111 | 112 | lck.lock(); 113 | edges_.emplace_back(x_new->parent, x_new); 114 | lck.unlock(); 115 | 116 | // rewiring 117 | for (const auto &x_near : X_near) { 118 | double c_near = cost(x_new) + distance(x_new, x_near); 119 | if (c_near < cost(x_near)) { 120 | if (!isCollision(x_near, x_new)) { 121 | lck.lock(); 122 | edges_.erase(std::remove(edges_.begin(), edges_.end(), 123 | std::make_pair(x_near->parent, x_near)), 124 | edges_.end()); 125 | x_near->parent = x_new; 126 | edges_.emplace_back(x_new, x_near); 127 | lck.unlock(); 128 | } 129 | } 130 | } 131 | 132 | // add into x_soln if the vertex is within the goal radius 133 | if (inGoalRegion(x_new)) { 134 | x_soln_.emplace_back(x_new); 135 | } 136 | } 137 | 138 | // update the best parent for the goal vertex every n iterations 139 | iter_no_lck.lock(); 140 | bool update_solution_path = (curr_iter_no_ % update_goal_every_ == 0); 141 | iter_no_lck.unlock(); 142 | if (update_solution_path) { 143 | if (x_soln_.size() > 0) { 144 | std::shared_ptr best_goal_parent; 145 | double min_goal_parent_cost = std::numeric_limits::infinity(); 146 | 147 | for (const auto &v : x_soln_) { 148 | double c = cost(v); 149 | if (c < min_goal_parent_cost) { 150 | min_goal_parent_cost = c; 151 | best_goal_parent = v; 152 | } 153 | } 154 | goal.parent = best_goal_parent; 155 | } 156 | } 157 | iter_no_lck.lock(); 158 | curr_iter_no_++; 159 | iter_no_lck.unlock(); 160 | } else { 161 | std::cout << "Iterations number reach max limit. Planning stopped." << '\n'; 162 | solved = true; 163 | } 164 | } 165 | 166 | void RRT_STAR::near(const std::shared_ptr &x_new, 167 | std::vector> &X_near) { 168 | std::lock_guard lock(mutex_); 169 | double r = std::min( 170 | r_rrt_ * std::pow(std::log(static_cast(vertices_.size())) / 171 | (static_cast(vertices_.size())), 172 | 1.0 / 2.0), 173 | range_); 174 | 175 | for (const auto &v : vertices_) { 176 | if (distance(v, x_new) < r) { 177 | X_near.emplace_back(v); 178 | } 179 | } 180 | } 181 | 182 | } // namespace sampling_based 183 | } // namespace path_finding_visualizer -------------------------------------------------------------------------------- /src/States/Algorithms/SamplingBased/SamplingBased.cpp: -------------------------------------------------------------------------------- 1 | #include "States/Algorithms/SamplingBased/SamplingBased.h" 2 | 3 | namespace path_finding_visualizer { 4 | namespace sampling_based { 5 | 6 | // Constructor 7 | SamplingBased::SamplingBased(std::shared_ptr logger_panel, 8 | const std::string& name) 9 | : State(logger_panel), key_time_max_{1.f}, key_time_{0.f} { 10 | logger_panel_->info("Initialize " + name + " planner"); 11 | initVariables(); 12 | 13 | start_vertex_ = std::make_shared(); 14 | goal_vertex_ = std::make_shared(); 15 | 16 | std::random_device rd; 17 | rn_gen_ = std::mt19937(rd()); 18 | } 19 | 20 | // Destructor 21 | SamplingBased::~SamplingBased() { 22 | if (!thread_joined_) { 23 | std::unique_lock lck(mutex_); 24 | is_stopped_ = false; 25 | lck.unlock(); 26 | t_.join(); 27 | } 28 | } 29 | 30 | void SamplingBased::initMapVariables() { 31 | map_width_ = 700; 32 | map_height_ = 700; 33 | obst_size_ = 20; 34 | } 35 | 36 | void SamplingBased::initVariables() { 37 | initMapVariables(); 38 | 39 | message_queue_ = std::make_shared>(); 40 | 41 | is_running_ = false; 42 | is_initialized_ = false; 43 | is_reset_ = false; 44 | is_solved_ = false; 45 | is_stopped_ = false; 46 | thread_joined_ = true; 47 | disable_gui_parameters_ = false; 48 | disable_run_ = false; 49 | } 50 | 51 | void SamplingBased::endState() {} 52 | 53 | /** 54 | * Getter function for Algorithm key timer. 55 | * 56 | * @param none. 57 | * @return true if keytime > keytime_max else false. 58 | */ 59 | const bool SamplingBased::getKeyTime() { 60 | if (key_time_ >= key_time_max_) { 61 | key_time_ = 0.f; 62 | return true; 63 | } 64 | return false; 65 | } 66 | 67 | /** 68 | * Update the current time of key timer. 69 | * 70 | * @param dt delta time. 71 | * @return void. 72 | */ 73 | void SamplingBased::updateKeyTime(const float& dt) { 74 | if (key_time_ < key_time_max_) { 75 | key_time_ += 5.f * dt; 76 | } 77 | } 78 | 79 | void SamplingBased::update(const float& dt, const ImVec2& mousePos) { 80 | // from base classes 81 | updateKeyTime(dt); 82 | updateMousePosition(mousePos); 83 | 84 | if (is_reset_) { 85 | is_running_ = false; 86 | is_initialized_ = false; 87 | is_reset_ = false; 88 | is_solved_ = false; 89 | disable_gui_parameters_ = false; 90 | 91 | std::unique_lock lck(mutex_); 92 | is_stopped_ = true; 93 | lck.unlock(); 94 | 95 | if (!thread_joined_) { 96 | t_.join(); 97 | thread_joined_ = true; 98 | } 99 | 100 | message_queue_ = std::make_shared>(); 101 | 102 | initialize(); 103 | 104 | std::unique_lock iter_no_lck(iter_no_mutex_); 105 | curr_iter_no_ = 0u; 106 | } 107 | 108 | if (is_running_) { 109 | is_reset_ = false; 110 | 111 | // initialize Algorithm 112 | if (!is_initialized_) { 113 | initPlanner(); 114 | 115 | std::unique_lock lck(mutex_); 116 | is_stopped_ = false; 117 | lck.unlock(); 118 | 119 | logger_panel_->info("Planning started with " + 120 | std::to_string(max_iterations_) + " iterations."); 121 | 122 | // create thread 123 | // solve the algorithm concurrently 124 | t_ = std::thread(&SamplingBased::solveConcurrently, this, start_vertex_, 125 | goal_vertex_, message_queue_); 126 | 127 | thread_joined_ = false; 128 | is_initialized_ = true; 129 | disable_gui_parameters_ = true; 130 | } 131 | 132 | // check the algorithm is solved or not 133 | auto solved = message_queue_->receive(); 134 | // if solved 135 | if (solved) { 136 | t_.join(); 137 | thread_joined_ = true; 138 | is_running_ = false; 139 | is_solved_ = true; 140 | logger_panel_->info( 141 | "Iterations number reach max limit. Planning stopped."); 142 | } 143 | } else { 144 | // only allow mouse and key inputs 145 | // if the algorithm is not running 146 | updateUserInput(); 147 | } 148 | } 149 | 150 | void SamplingBased::updateUserInput() { 151 | if (sf::Mouse::isButtonPressed(sf::Mouse::Left) && getKeyTime()) { 152 | if (mousePositionWindow_.x > init_grid_xy_.x + obst_size_ / 2 && 153 | mousePositionWindow_.x < 154 | init_grid_xy_.x + map_width_ - obst_size_ / 2 && 155 | mousePositionWindow_.y > init_grid_xy_.y + obst_size_ / 2 && 156 | mousePositionWindow_.y < 157 | init_grid_xy_.y + map_height_ - obst_size_ / 2) { 158 | bool setObstacle = true; 159 | sf::Vector2f relative_mouse_pos = 160 | sf::Vector2f(mousePositionWindow_.x - init_grid_xy_.x, 161 | mousePositionWindow_.y - init_grid_xy_.y); 162 | 163 | for (std::size_t i = 0, e = obstacles_.size(); i != e; ++i) { 164 | if (obstacles_[i]->getGlobalBounds().contains(relative_mouse_pos)) { 165 | obstacles_.erase(obstacles_.begin() + i); 166 | setObstacle = false; 167 | break; 168 | } 169 | } 170 | 171 | if (!is_solved_) { 172 | if (sf::Keyboard::isKeyPressed(sf::Keyboard::LShift)) { 173 | if (setObstacle) { 174 | start_vertex_->y = 175 | utils::map(mousePositionWindow_.x, init_grid_xy_.x, 176 | init_grid_xy_.x + map_width_, 0.0, 1.0); 177 | start_vertex_->x = 178 | utils::map(mousePositionWindow_.y, init_grid_xy_.y, 179 | init_grid_xy_.y + map_height_, 0.0, 1.0); 180 | } 181 | } else if (sf::Keyboard::isKeyPressed(sf::Keyboard::LControl)) { 182 | if (setObstacle) { 183 | goal_vertex_->y = 184 | utils::map(mousePositionWindow_.x, init_grid_xy_.x, 185 | init_grid_xy_.x + map_width_, 0.0, 1.0); 186 | goal_vertex_->x = 187 | utils::map(mousePositionWindow_.y, init_grid_xy_.y, 188 | init_grid_xy_.y + map_height_, 0.0, 1.0); 189 | } 190 | } else { 191 | // add new obstacle 192 | if (setObstacle) { 193 | std::shared_ptr obstShape = 194 | std::make_shared( 195 | sf::Vector2f(obst_size_, obst_size_)); 196 | obstShape->setPosition( 197 | sf::Vector2f(relative_mouse_pos.x - obst_size_ / 2., 198 | relative_mouse_pos.y - obst_size_ / 2.)); 199 | obstShape->setFillColor(OBST_COL); 200 | obstacles_.emplace_back(std::move(obstShape)); 201 | } 202 | } 203 | } else { 204 | if (sf::Keyboard::isKeyPressed(sf::Keyboard::LControl)) { 205 | if (setObstacle) { 206 | goal_vertex_->y = 207 | utils::map(mousePositionWindow_.x, init_grid_xy_.x, 208 | init_grid_xy_.x + map_width_, 0.0, 1.0); 209 | goal_vertex_->x = 210 | utils::map(mousePositionWindow_.y, init_grid_xy_.y, 211 | init_grid_xy_.y + map_height_, 0.0, 1.0); 212 | 213 | // TODO: Find nearest node from goal point and set it as parent 214 | } 215 | } 216 | } 217 | } 218 | } 219 | } 220 | 221 | void SamplingBased::renderMap(sf::RenderTexture& render_texture) { 222 | sf::RectangleShape planning_map(sf::Vector2f(map_width_, map_height_)); 223 | planning_map.setFillColor(sf::Color(255, 255, 255)); 224 | planning_map.setOutlineThickness(5); 225 | planning_map.setOutlineColor(sf::Color(0, 0, 0)); 226 | planning_map.setPosition(sf::Vector2f(init_grid_xy_.x, init_grid_xy_.y)); 227 | render_texture.draw(planning_map); 228 | } 229 | 230 | void SamplingBased::renderObstacles(sf::RenderTexture& render_texture) { 231 | for (auto& shape : obstacles_) { 232 | sf::RectangleShape obst(sf::Vector2f(obst_size_, obst_size_)); 233 | obst.setPosition(sf::Vector2f(init_grid_xy_.x + shape->getPosition().x, 234 | init_grid_xy_.y + shape->getPosition().y)); 235 | obst.setFillColor(OBST_COL); 236 | render_texture.draw(obst); 237 | } 238 | } 239 | 240 | void SamplingBased::clearObstacles() { obstacles_.clear(); } 241 | 242 | void SamplingBased::renderGui() { 243 | ImGui::PushStyleVar(ImGuiStyleVar_FrameRounding, 6.f); 244 | { 245 | std::unique_lock iter_no_lck(iter_no_mutex_); 246 | const float progress = static_cast( 247 | utils::map(static_cast(curr_iter_no_), 0.0, 248 | static_cast(max_iterations_), 0.0, 1.0)); 249 | const std::string buf = 250 | std::to_string(curr_iter_no_) + "/" + std::to_string(max_iterations_); 251 | ImGui::Text("Planning Progress:"); 252 | ImGui::SameLine(); 253 | gui::HelpMarker( 254 | "Shows the current iteration number of the planning progress"); 255 | ImGui::Spacing(); 256 | ImGui::ProgressBar(progress, ImVec2(-1.0f, 0.0f), buf.c_str()); 257 | } 258 | ImGui::PopStyleVar(); 259 | ImGui::Spacing(); 260 | 261 | ImGui::PushStyleVar(ImGuiStyleVar_ItemSpacing, ImVec2(8.f, 8.f)); 262 | if (ImGui::CollapsingHeader("Edit", ImGuiTreeNodeFlags_DefaultOpen)) { 263 | if (disable_gui_parameters_) ImGui::BeginDisabled(); 264 | ImGui::Indent(8.f); 265 | 266 | ImGui::PushStyleVar(ImGuiStyleVar_FrameRounding, 6.f); 267 | ImGui::PushStyleVar(ImGuiStyleVar_ItemInnerSpacing, ImVec2(2.f, 4.f)); 268 | 269 | ImGui::Text("Map:"); 270 | ImGui::SameLine(); 271 | gui::HelpMarker( 272 | "Set width and height of the planning map.\nInternally these values " 273 | "are mapped between 0 and 1."); 274 | ImGui::PushStyleVar(ImGuiStyleVar_ItemSpacing, ImVec2(8.f, 2.f)); 275 | 276 | if (gui::inputInt("width", &map_width_, 500, 10000, 100, 1000)) { 277 | } 278 | ImGui::PopStyleVar(); 279 | if (gui::inputInt("height", &map_height_, 500, 10000, 100, 1000)) { 280 | } 281 | 282 | ImGui::Text("Random Obstacles:"); 283 | ImGui::SameLine(); 284 | gui::HelpMarker("TODO: Randomly generate obstacles in the gridmap"); 285 | static int rand_obsts_no = 10; 286 | if (ImGui::InputInt("##rand_obst_input", &rand_obsts_no, 1, 10, 287 | ImGuiInputTextFlags_EnterReturnsTrue)) { 288 | // TODO: 289 | } 290 | ImGui::SameLine(); 291 | if (ImGui::Button("Generate")) { 292 | // TODO: 293 | } 294 | 295 | if (ImGui::Button("Clear Obstacles")) { 296 | clearObstacles(); 297 | } 298 | ImGui::SameLine(); 299 | if (ImGui::Button("Restore Defaults##edit_restore")) { 300 | initMapVariables(); 301 | } 302 | 303 | ImGui::Unindent(8.f); 304 | ImGui::PopStyleVar(2); 305 | if (disable_gui_parameters_) ImGui::EndDisabled(); 306 | ImGui::Spacing(); 307 | } 308 | ImGui::PopStyleVar(); 309 | 310 | ImGui::PushStyleVar(ImGuiStyleVar_ItemSpacing, ImVec2(8.f, 8.f)); 311 | if (ImGui::CollapsingHeader("Configuration", 312 | ImGuiTreeNodeFlags_DefaultOpen)) { 313 | if (disable_gui_parameters_) ImGui::BeginDisabled(); 314 | ImGui::Indent(8.f); 315 | ImGui::PushStyleVar(ImGuiStyleVar_FrameRounding, 6.f); 316 | ImGui::PushStyleVar(ImGuiStyleVar_ItemInnerSpacing, ImVec2(2.f, 4.f)); 317 | ImGui::PushStyleVar(ImGuiStyleVar_ItemSpacing, ImVec2(8.f, 2.f)); 318 | 319 | gui::inputInt("max_iterations", &max_iterations_, 1, 100000, 1, 1000, 320 | "Maximum number of iterations to run the planner"); 321 | 322 | // virtual function renderParametersGui() 323 | // need to be implemented by derived class 324 | renderParametersGui(); 325 | 326 | ImGui::PushStyleVar(ImGuiStyleVar_ItemSpacing, ImVec2(8.f, 8.f)); 327 | ImGui::Spacing(); 328 | ImGui::PopStyleVar(); 329 | if (ImGui::Button("Restore Defaults##config_restore")) { 330 | initParameters(); 331 | logger_panel_->info( 332 | "Planner related parameters resetted to default ones."); 333 | } 334 | 335 | ImGui::Unindent(8.f); 336 | ImGui::PopStyleVar(3); 337 | if (disable_gui_parameters_) ImGui::EndDisabled(); 338 | } 339 | ImGui::PopStyleVar(); 340 | } 341 | 342 | void SamplingBased::renderConfig() { 343 | // render gui 344 | renderGui(); 345 | } 346 | 347 | void SamplingBased::renderScene(sf::RenderTexture& render_texture) { 348 | const auto texture_size = render_texture.getSize(); 349 | init_grid_xy_.x = (texture_size.x / 2.) - (map_width_ / 2.); 350 | init_grid_xy_.y = (texture_size.y / 2.) - (map_height_ / 2.); 351 | 352 | renderMap(render_texture); 353 | renderObstacles(render_texture); 354 | // virtual function renderPlannerData() 355 | // need to be implemented by derived class 356 | renderPlannerData(render_texture); 357 | } 358 | 359 | void SamplingBased::solveConcurrently( 360 | std::shared_ptr start_point, std::shared_ptr goal_point, 361 | std::shared_ptr> message_queue) { 362 | // copy assignment 363 | // thread-safe due to shared_ptrs 364 | std::shared_ptr start_vertex = start_point; 365 | std::shared_ptr goal_vertex = goal_point; 366 | std::shared_ptr> s_message_queue = message_queue; 367 | 368 | bool solved = false; 369 | 370 | double cycle_duration = 1; // duration of a single simulation cycle in ms 371 | // init stop watch 372 | auto last_update = std::chrono::system_clock::now(); 373 | 374 | while (true) { 375 | // compute time difference to stop watch 376 | long time_since_last_update = 377 | std::chrono::duration_cast( 378 | std::chrono::system_clock::now() - last_update) 379 | .count(); 380 | 381 | if (time_since_last_update >= cycle_duration) { 382 | // run the main algorithm 383 | updatePlanner(solved, *start_vertex, *goal_vertex); 384 | 385 | // reset stop watch for next cycle 386 | last_update = std::chrono::system_clock::now(); 387 | } 388 | 389 | // sends an update method to the message queue using move semantics 390 | auto ftr = std::async(std::launch::async, &MessageQueue::send, 391 | s_message_queue, std::move(solved)); 392 | ftr.wait(); 393 | 394 | if (solved) return; 395 | 396 | std::lock_guard lock(mutex_); 397 | if (is_stopped_) return; 398 | 399 | // sleep at every iteration to reduce CPU usage 400 | std::this_thread::sleep_for(std::chrono::milliseconds(1)); 401 | } 402 | } 403 | 404 | } // namespace sampling_based 405 | } // namespace path_finding_visualizer -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "Game.h" 9 | 10 | int main() { 11 | sf::VideoMode videoMode(1600u, 960u); 12 | sf::RenderWindow window( 13 | videoMode, "Path-Finding Visualizer", 14 | sf::Style::Titlebar | sf::Style::Close | sf::Style::Resize); 15 | // setting frame limit 16 | window.setFramerateLimit(100u); 17 | 18 | sf::RenderTexture render_texture; 19 | if (!render_texture.create(700u, 700u)) { 20 | } 21 | 22 | ImGui::SFML::Init(window, false); 23 | 24 | // Initialize Game 25 | path_finding_visualizer::Game game(&window, &render_texture); 26 | 27 | // Game Loop 28 | while (game.running()) { 29 | // Update 30 | game.update(); 31 | 32 | // Render 33 | game.render(); 34 | } 35 | 36 | ImGui::SFML::Shutdown(); 37 | return 0; 38 | } 39 | --------------------------------------------------------------------------------