├── .gitattributes ├── AStar ├── CPP │ ├── .gitattributes │ ├── .gitignore │ ├── Astar.sln │ └── Astar │ │ ├── Astar.cpp │ │ ├── Astar.vcxproj │ │ ├── Astar.vcxproj.filters │ │ ├── maps.h │ │ └── pathPlanning.h └── Matlab │ ├── AStar.m │ ├── AStarP.m │ └── Map.m ├── PRM Node Based ├── GoodP.m ├── Map.m ├── nPRM.m └── pathCheck.m ├── PRM ├── GoodP.m ├── Map.m ├── PRM.m └── pathCheck.m └── PSO ├── Map.m ├── PSO.mp4 ├── PSO_2.mp4 ├── PSO_3(2).mp4 ├── PSO_3.mp4 ├── bestPath.m ├── costFunc.m └── main_pso.m /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | -------------------------------------------------------------------------------- /AStar/CPP/.gitattributes: -------------------------------------------------------------------------------- 1 | ############################################################################### 2 | # Set default behavior to automatically normalize line endings. 3 | ############################################################################### 4 | * text=auto 5 | 6 | ############################################################################### 7 | # Set default behavior for command prompt diff. 8 | # 9 | # This is need for earlier builds of msysgit that does not have it on by 10 | # default for csharp files. 11 | # Note: This is only used by command line 12 | ############################################################################### 13 | #*.cs diff=csharp 14 | 15 | ############################################################################### 16 | # Set the merge driver for project and solution files 17 | # 18 | # Merging from the command prompt will add diff markers to the files if there 19 | # are conflicts (Merging from VS is not affected by the settings below, in VS 20 | # the diff markers are never inserted). Diff markers may cause the following 21 | # file extensions to fail to load in VS. An alternative would be to treat 22 | # these files as binary and thus will always conflict and require user 23 | # intervention with every merge. To do so, just uncomment the entries below 24 | ############################################################################### 25 | #*.sln merge=binary 26 | #*.csproj merge=binary 27 | #*.vbproj merge=binary 28 | #*.vcxproj merge=binary 29 | #*.vcproj merge=binary 30 | #*.dbproj merge=binary 31 | #*.fsproj merge=binary 32 | #*.lsproj merge=binary 33 | #*.wixproj merge=binary 34 | #*.modelproj merge=binary 35 | #*.sqlproj merge=binary 36 | #*.wwaproj merge=binary 37 | 38 | ############################################################################### 39 | # behavior for image files 40 | # 41 | # image files are treated as binary by default. 42 | ############################################################################### 43 | #*.jpg binary 44 | #*.png binary 45 | #*.gif binary 46 | 47 | ############################################################################### 48 | # diff behavior for common document formats 49 | # 50 | # Convert binary document formats to text before diffing them. This feature 51 | # is only available from the command line. Turn it on by uncommenting the 52 | # entries below. 53 | ############################################################################### 54 | #*.doc diff=astextplain 55 | #*.DOC diff=astextplain 56 | #*.docx diff=astextplain 57 | #*.DOCX diff=astextplain 58 | #*.dot diff=astextplain 59 | #*.DOT diff=astextplain 60 | #*.pdf diff=astextplain 61 | #*.PDF diff=astextplain 62 | #*.rtf diff=astextplain 63 | #*.RTF diff=astextplain 64 | -------------------------------------------------------------------------------- /AStar/CPP/.gitignore: -------------------------------------------------------------------------------- 1 | ## Ignore Visual Studio temporary files, build results, and 2 | ## files generated by popular Visual Studio add-ons. 3 | ## 4 | ## Get latest from https://github.com/github/gitignore/blob/master/VisualStudio.gitignore 5 | 6 | # User-specific files 7 | *.rsuser 8 | *.suo 9 | *.user 10 | *.userosscache 11 | *.sln.docstates 12 | 13 | # User-specific files (MonoDevelop/Xamarin Studio) 14 | *.userprefs 15 | 16 | # Build results 17 | [Dd]ebug/ 18 | [Dd]ebugPublic/ 19 | [Rr]elease/ 20 | [Rr]eleases/ 21 | x64/ 22 | x86/ 23 | [Aa][Rr][Mm]/ 24 | [Aa][Rr][Mm]64/ 25 | bld/ 26 | [Bb]in/ 27 | [Oo]bj/ 28 | [Ll]og/ 29 | 30 | # Visual Studio 2015/2017 cache/options directory 31 | .vs/ 32 | # Uncomment if you have tasks that create the project's static files in wwwroot 33 | #wwwroot/ 34 | 35 | # Visual Studio 2017 auto generated files 36 | Generated\ Files/ 37 | 38 | # MSTest test Results 39 | [Tt]est[Rr]esult*/ 40 | [Bb]uild[Ll]og.* 41 | 42 | # NUNIT 43 | *.VisualState.xml 44 | TestResult.xml 45 | 46 | # Build Results of an ATL Project 47 | [Dd]ebugPS/ 48 | [Rr]eleasePS/ 49 | dlldata.c 50 | 51 | # Benchmark Results 52 | BenchmarkDotNet.Artifacts/ 53 | 54 | # .NET Core 55 | project.lock.json 56 | project.fragment.lock.json 57 | artifacts/ 58 | 59 | # StyleCop 60 | StyleCopReport.xml 61 | 62 | # Files built by Visual Studio 63 | *_i.c 64 | *_p.c 65 | *_h.h 66 | *.ilk 67 | *.meta 68 | *.obj 69 | *.iobj 70 | *.pch 71 | *.pdb 72 | *.ipdb 73 | *.pgc 74 | *.pgd 75 | *.rsp 76 | *.sbr 77 | *.tlb 78 | *.tli 79 | *.tlh 80 | *.tmp 81 | *.tmp_proj 82 | *_wpftmp.csproj 83 | *.log 84 | *.vspscc 85 | *.vssscc 86 | .builds 87 | *.pidb 88 | *.svclog 89 | *.scc 90 | 91 | # Chutzpah Test files 92 | _Chutzpah* 93 | 94 | # Visual C++ cache files 95 | ipch/ 96 | *.aps 97 | *.ncb 98 | *.opendb 99 | *.opensdf 100 | *.sdf 101 | *.cachefile 102 | *.VC.db 103 | *.VC.VC.opendb 104 | 105 | # Visual Studio profiler 106 | *.psess 107 | *.vsp 108 | *.vspx 109 | *.sap 110 | 111 | # Visual Studio Trace Files 112 | *.e2e 113 | 114 | # TFS 2012 Local Workspace 115 | $tf/ 116 | 117 | # Guidance Automation Toolkit 118 | *.gpState 119 | 120 | # ReSharper is a .NET coding add-in 121 | _ReSharper*/ 122 | *.[Rr]e[Ss]harper 123 | *.DotSettings.user 124 | 125 | # JustCode is a .NET coding add-in 126 | .JustCode 127 | 128 | # TeamCity is a build add-in 129 | _TeamCity* 130 | 131 | # DotCover is a Code Coverage Tool 132 | *.dotCover 133 | 134 | # AxoCover is a Code Coverage Tool 135 | .axoCover/* 136 | !.axoCover/settings.json 137 | 138 | # Visual Studio code coverage results 139 | *.coverage 140 | *.coveragexml 141 | 142 | # NCrunch 143 | _NCrunch_* 144 | .*crunch*.local.xml 145 | nCrunchTemp_* 146 | 147 | # MightyMoose 148 | *.mm.* 149 | AutoTest.Net/ 150 | 151 | # Web workbench (sass) 152 | .sass-cache/ 153 | 154 | # Installshield output folder 155 | [Ee]xpress/ 156 | 157 | # DocProject is a documentation generator add-in 158 | DocProject/buildhelp/ 159 | DocProject/Help/*.HxT 160 | DocProject/Help/*.HxC 161 | DocProject/Help/*.hhc 162 | DocProject/Help/*.hhk 163 | DocProject/Help/*.hhp 164 | DocProject/Help/Html2 165 | DocProject/Help/html 166 | 167 | # Click-Once directory 168 | publish/ 169 | 170 | # Publish Web Output 171 | *.[Pp]ublish.xml 172 | *.azurePubxml 173 | # Note: Comment the next line if you want to checkin your web deploy settings, 174 | # but database connection strings (with potential passwords) will be unencrypted 175 | *.pubxml 176 | *.publishproj 177 | 178 | # Microsoft Azure Web App publish settings. Comment the next line if you want to 179 | # checkin your Azure Web App publish settings, but sensitive information contained 180 | # in these scripts will be unencrypted 181 | PublishScripts/ 182 | 183 | # NuGet Packages 184 | *.nupkg 185 | # The packages folder can be ignored because of Package Restore 186 | **/[Pp]ackages/* 187 | # except build/, which is used as an MSBuild target. 188 | !**/[Pp]ackages/build/ 189 | # Uncomment if necessary however generally it will be regenerated when needed 190 | #!**/[Pp]ackages/repositories.config 191 | # NuGet v3's project.json files produces more ignorable files 192 | *.nuget.props 193 | *.nuget.targets 194 | 195 | # Microsoft Azure Build Output 196 | csx/ 197 | *.build.csdef 198 | 199 | # Microsoft Azure Emulator 200 | ecf/ 201 | rcf/ 202 | 203 | # Windows Store app package directories and files 204 | AppPackages/ 205 | BundleArtifacts/ 206 | Package.StoreAssociation.xml 207 | _pkginfo.txt 208 | *.appx 209 | 210 | # Visual Studio cache files 211 | # files ending in .cache can be ignored 212 | *.[Cc]ache 213 | # but keep track of directories ending in .cache 214 | !?*.[Cc]ache/ 215 | 216 | # Others 217 | ClientBin/ 218 | ~$* 219 | *~ 220 | *.dbmdl 221 | *.dbproj.schemaview 222 | *.jfm 223 | *.pfx 224 | *.publishsettings 225 | orleans.codegen.cs 226 | 227 | # Including strong name files can present a security risk 228 | # (https://github.com/github/gitignore/pull/2483#issue-259490424) 229 | #*.snk 230 | 231 | # Since there are multiple workflows, uncomment next line to ignore bower_components 232 | # (https://github.com/github/gitignore/pull/1529#issuecomment-104372622) 233 | #bower_components/ 234 | 235 | # RIA/Silverlight projects 236 | Generated_Code/ 237 | 238 | # Backup & report files from converting an old project file 239 | # to a newer Visual Studio version. Backup files are not needed, 240 | # because we have git ;-) 241 | _UpgradeReport_Files/ 242 | Backup*/ 243 | UpgradeLog*.XML 244 | UpgradeLog*.htm 245 | ServiceFabricBackup/ 246 | *.rptproj.bak 247 | 248 | # SQL Server files 249 | *.mdf 250 | *.ldf 251 | *.ndf 252 | 253 | # Business Intelligence projects 254 | *.rdl.data 255 | *.bim.layout 256 | *.bim_*.settings 257 | *.rptproj.rsuser 258 | *- Backup*.rdl 259 | 260 | # Microsoft Fakes 261 | FakesAssemblies/ 262 | 263 | # GhostDoc plugin setting file 264 | *.GhostDoc.xml 265 | 266 | # Node.js Tools for Visual Studio 267 | .ntvs_analysis.dat 268 | node_modules/ 269 | 270 | # Visual Studio 6 build log 271 | *.plg 272 | 273 | # Visual Studio 6 workspace options file 274 | *.opt 275 | 276 | # Visual Studio 6 auto-generated workspace file (contains which files were open etc.) 277 | *.vbw 278 | 279 | # Visual Studio LightSwitch build output 280 | **/*.HTMLClient/GeneratedArtifacts 281 | **/*.DesktopClient/GeneratedArtifacts 282 | **/*.DesktopClient/ModelManifest.xml 283 | **/*.Server/GeneratedArtifacts 284 | **/*.Server/ModelManifest.xml 285 | _Pvt_Extensions 286 | 287 | # Paket dependency manager 288 | .paket/paket.exe 289 | paket-files/ 290 | 291 | # FAKE - F# Make 292 | .fake/ 293 | 294 | # JetBrains Rider 295 | .idea/ 296 | *.sln.iml 297 | 298 | # CodeRush personal settings 299 | .cr/personal 300 | 301 | # Python Tools for Visual Studio (PTVS) 302 | __pycache__/ 303 | *.pyc 304 | 305 | # Cake - Uncomment if you are using it 306 | # tools/** 307 | # !tools/packages.config 308 | 309 | # Tabs Studio 310 | *.tss 311 | 312 | # Telerik's JustMock configuration file 313 | *.jmconfig 314 | 315 | # BizTalk build output 316 | *.btp.cs 317 | *.btm.cs 318 | *.odx.cs 319 | *.xsd.cs 320 | 321 | # OpenCover UI analysis results 322 | OpenCover/ 323 | 324 | # Azure Stream Analytics local run output 325 | ASALocalRun/ 326 | 327 | # MSBuild Binary and Structured Log 328 | *.binlog 329 | 330 | # NVidia Nsight GPU debugger configuration file 331 | *.nvuser 332 | 333 | # MFractors (Xamarin productivity tool) working folder 334 | .mfractor/ 335 | 336 | # Local History for Visual Studio 337 | .localhistory/ 338 | 339 | # BeatPulse healthcheck temp database 340 | healthchecksdb -------------------------------------------------------------------------------- /AStar/CPP/Astar.sln: -------------------------------------------------------------------------------- 1 |  2 | Microsoft Visual Studio Solution File, Format Version 12.00 3 | # Visual Studio Version 16 4 | VisualStudioVersion = 16.0.28803.156 5 | MinimumVisualStudioVersion = 10.0.40219.1 6 | Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "Astar", "Astar\Astar.vcxproj", "{9C1AD2F7-BE25-434E-BE8B-9C8F20628BB7}" 7 | EndProject 8 | Global 9 | GlobalSection(SolutionConfigurationPlatforms) = preSolution 10 | Debug|x64 = Debug|x64 11 | Debug|x86 = Debug|x86 12 | Release|x64 = Release|x64 13 | Release|x86 = Release|x86 14 | EndGlobalSection 15 | GlobalSection(ProjectConfigurationPlatforms) = postSolution 16 | {9C1AD2F7-BE25-434E-BE8B-9C8F20628BB7}.Debug|x64.ActiveCfg = Debug|x64 17 | {9C1AD2F7-BE25-434E-BE8B-9C8F20628BB7}.Debug|x64.Build.0 = Debug|x64 18 | {9C1AD2F7-BE25-434E-BE8B-9C8F20628BB7}.Debug|x86.ActiveCfg = Debug|Win32 19 | {9C1AD2F7-BE25-434E-BE8B-9C8F20628BB7}.Debug|x86.Build.0 = Debug|Win32 20 | {9C1AD2F7-BE25-434E-BE8B-9C8F20628BB7}.Release|x64.ActiveCfg = Release|x64 21 | {9C1AD2F7-BE25-434E-BE8B-9C8F20628BB7}.Release|x64.Build.0 = Release|x64 22 | {9C1AD2F7-BE25-434E-BE8B-9C8F20628BB7}.Release|x86.ActiveCfg = Release|Win32 23 | {9C1AD2F7-BE25-434E-BE8B-9C8F20628BB7}.Release|x86.Build.0 = Release|Win32 24 | EndGlobalSection 25 | GlobalSection(SolutionProperties) = preSolution 26 | HideSolutionNode = FALSE 27 | EndGlobalSection 28 | GlobalSection(ExtensibilityGlobals) = postSolution 29 | SolutionGuid = {EC024BE7-D9FD-433D-BB01-2C07D53271AD} 30 | EndGlobalSection 31 | EndGlobal 32 | -------------------------------------------------------------------------------- /AStar/CPP/Astar/Astar.cpp: -------------------------------------------------------------------------------- 1 | /* Astar path finder - 30 x 15 map - 2 | Start point is random on the Y-axis & 0 on the X-axis - 3 | End point is at (14, 29) - 4 | Cost, computational time, graph, and path are outputted 5 | */ 6 | 7 | #include "pathPlanning.h" 8 | #include 9 | #include 10 | #include 11 | 12 | int main() { 13 | srand((time(NULL))); 14 | clock_t clockStart = clock(); 15 | int randS = rand() % 14; 16 | maps m; 17 | point startP(0,randS), endP(29,14); 18 | aStar as; 19 | 20 | if (as.check(startP, endP, m)) { 21 | list path; 22 | int c = as.path(path); 23 | for (int y = 0; y < m.width; y++) { 24 | for (int x = 0; x < m.height; x++) { 25 | if (m(x, y) == 1) cout << "@"; 26 | else { 27 | if (point(x, y) == startP) cout << "S"; 28 | else if (point(x, y) == endP) cout << "E"; 29 | else if (find(path.begin(), path.end(), point(x, y)) != path.end()) cout << "x"; 30 | else cout << "."; 31 | } 32 | } 33 | cout << endl; 34 | } 35 | 36 | clock_t clockEnd = clock(); 37 | float timeCal = ((float)clockEnd - (float)clockStart); 38 | cout << "\nTime to Calculate the route: " << timeCal << "ms" << endl; 39 | cout << "\nStart: " << "[" << startP.x << ", " << startP.y << "] " << "\nEnd: " << "[" << endP.x << ", " << endP.y << "] "; 40 | cout << "\nPath cost " << c << ": "; 41 | for (list::iterator i = path.begin(); i != path.end(); i++) { 42 | cout << "[" << (*i).x << ", " << (*i).y << "], "; 43 | } 44 | 45 | } 46 | cout << endl; 47 | return 0; 48 | } 49 | -------------------------------------------------------------------------------- /AStar/CPP/Astar/Astar.vcxproj: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | Debug 6 | Win32 7 | 8 | 9 | Release 10 | Win32 11 | 12 | 13 | Debug 14 | x64 15 | 16 | 17 | Release 18 | x64 19 | 20 | 21 | 22 | 16.0 23 | {9C1AD2F7-BE25-434E-BE8B-9C8F20628BB7} 24 | Win32Proj 25 | Astar 26 | 10.0 27 | 28 | 29 | 30 | Application 31 | true 32 | v142 33 | Unicode 34 | 35 | 36 | Application 37 | false 38 | v142 39 | true 40 | Unicode 41 | 42 | 43 | Application 44 | true 45 | v142 46 | Unicode 47 | 48 | 49 | Application 50 | false 51 | v142 52 | true 53 | Unicode 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | true 75 | 76 | 77 | true 78 | 79 | 80 | false 81 | 82 | 83 | false 84 | 85 | 86 | 87 | 88 | 89 | Level3 90 | Disabled 91 | true 92 | WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) 93 | true 94 | 95 | 96 | Console 97 | true 98 | 99 | 100 | 101 | 102 | 103 | 104 | Level3 105 | Disabled 106 | true 107 | _DEBUG;_CONSOLE;%(PreprocessorDefinitions) 108 | true 109 | 110 | 111 | Console 112 | true 113 | 114 | 115 | 116 | 117 | 118 | 119 | Level3 120 | MaxSpeed 121 | true 122 | true 123 | true 124 | WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) 125 | true 126 | 127 | 128 | Console 129 | true 130 | true 131 | true 132 | 133 | 134 | 135 | 136 | 137 | 138 | Level3 139 | MaxSpeed 140 | true 141 | true 142 | true 143 | NDEBUG;_CONSOLE;%(PreprocessorDefinitions) 144 | true 145 | 146 | 147 | Console 148 | true 149 | true 150 | true 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | -------------------------------------------------------------------------------- /AStar/CPP/Astar/Astar.vcxproj.filters: -------------------------------------------------------------------------------- 1 |  2 | 3 | 4 | 5 | {4FC737F1-C7A5-4376-A066-2A32D752A2FF} 6 | cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx 7 | 8 | 9 | {93995380-89BD-4b04-88EB-625FBE52EBFB} 10 | h;hh;hpp;hxx;hm;inl;inc;ipp;xsd 11 | 12 | 13 | {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} 14 | rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms 15 | 16 | 17 | 18 | 19 | Source Files 20 | 21 | 22 | 23 | 24 | Header Files 25 | 26 | 27 | Header Files 28 | 29 | 30 | -------------------------------------------------------------------------------- /AStar/CPP/Astar/maps.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | using namespace std; 5 | 6 | class maps { 7 | public: 8 | maps() { 9 | int emptyMap[15][30] = { {0} 10 | }; 11 | 12 | pair obstacle[] = { 13 | make_pair(2, 2), make_pair(3, 2), make_pair(3, 3), make_pair(4,2), make_pair(5, 2), make_pair(9,2), make_pair(10, 2), make_pair(11,2), make_pair(12, 2), make_pair(13, 2), 14 | make_pair(2, 12), make_pair(2,13), make_pair(2, 14), make_pair(2, 15), make_pair(2, 16), make_pair(3, 16), make_pair(3, 17), 15 | make_pair(5, 12), make_pair(5, 13), make_pair(5, 14), make_pair(5, 15), make_pair(6, 15), make_pair(7, 15), make_pair(8, 15), make_pair(9, 15), 16 | make_pair(3, 7), make_pair(4, 7), make_pair(5, 7), make_pair(6,7), make_pair(7, 7), 17 | make_pair(6, 19), make_pair(7, 19), make_pair(8, 19), make_pair(9, 19), make_pair(10, 19), make_pair(10, 20), 18 | make_pair(12, 11), make_pair(12, 12), make_pair(12, 13), make_pair(12, 14), 19 | make_pair(2, 22), make_pair(2, 23), make_pair(2, 24), make_pair(2,25), make_pair(2, 26), make_pair(2,27), make_pair(2, 28), make_pair(2,29), 20 | make_pair(5,26), make_pair(6, 26), make_pair(7,26), make_pair(8, 26), make_pair(9,26), make_pair(10, 26), make_pair(11,26), make_pair(12, 26), make_pair(13,26), make_pair(14, 26), 21 | }; 22 | 23 | for (const auto& p: obstacle) { 24 | emptyMap[p.first][p.second] = 1; 25 | } 26 | 27 | width = 15; height = 30; 28 | for (int r = 0; r < width; r++) 29 | for (int s = 0; s < height; s++) 30 | m[s][r] = emptyMap[r][s]; 31 | } 32 | int operator() (int x, int y) { return m[x][y]; } 33 | int m[30][15], width, height; 34 | 35 | }; -------------------------------------------------------------------------------- /AStar/CPP/Astar/pathPlanning.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | 6 | #include "maps.h" 7 | 8 | class point { 9 | public: 10 | point(int a = 0, int b = 0) { x = a; y = b; } 11 | bool operator == (const point& op) { return op.x == x && op.y == y; } 12 | point operator + (const point& op) { return point(op.x + x, op.y + y); } 13 | int x, y; 14 | }; 15 | 16 | class node { 17 | public: 18 | bool operator == (const node& op) { return post == op.post; } 19 | bool operator == (const point& op) { return post == op; } 20 | bool operator < (const node& op) { return dist + cost < op.dist + op.cost; } 21 | point post, parent; 22 | int dist, cost; 23 | }; 24 | 25 | class aStar { 26 | public: 27 | aStar() { 28 | directions[0] = point(-1, -1); // NW 29 | directions[1] = point(1, -1); // SW 30 | directions[2] = point(-1, 1); // NE 31 | directions[3] = point(1, 1); // SE 32 | directions[4] = point(0, -1); // W 33 | directions[5] = point(-1, 0); // N 34 | directions[6] = point(0, 1); // E 35 | directions[7] = point(1, 0); // S 36 | } 37 | 38 | int calcDist(point & p) { 39 | // Euclidean Distance 40 | int x = end.x - p.x, y = end.y - p.y; 41 | return((int)sqrt(x * x + y * y)); 42 | } 43 | 44 | bool checkValid(point & p) { 45 | return (p.x > -1 && p.y > -1 && p.x < m.height && p.y < m.width); 46 | } 47 | 48 | bool checkPoint(point & p, int cost) { 49 | list::iterator i; 50 | i = find(closedN.begin(), closedN.end(), p); 51 | if (i != closedN.end()) { 52 | if ((*i).cost + (*i).dist < cost) return true; 53 | else { closedN.erase(i); return false; } 54 | } 55 | i = find(openN.begin(), openN.end(), p); 56 | if (i != openN.end()) { 57 | if ((*i).cost + (*i).dist < cost) return true; 58 | else { openN.erase(i); return false; } 59 | } 60 | return false; 61 | } 62 | 63 | bool fillOpenNode(node & n) { 64 | int nc, dist; 65 | point direction; 66 | 67 | for (int x = 0; x < 8; x++) { 68 | direction = n.post + directions[x]; 69 | if (direction == end) return true; 70 | 71 | if (checkValid(direction) && m(direction.x, direction.y) != 1) { 72 | nc = 1 + n.cost; // nc = cost of each step + remaining distance estimate 73 | dist = calcDist(direction); 74 | if (!checkPoint(direction, nc + dist)) { 75 | node m; 76 | m.cost = nc; m.dist = dist; 77 | m.post = direction; 78 | m.parent = n.post; 79 | openN.push_back(m); 80 | } 81 | } 82 | } 83 | return false; 84 | } 85 | 86 | 87 | bool check(point & s, point & e, maps & ms) { 88 | node n; end = e; start = s; m = ms; 89 | n.cost = 0; n.post = s; n.parent = 0; n.dist = calcDist(s); 90 | openN.push_back(n); 91 | while (!openN.empty()) { 92 | node n = openN.front(); 93 | openN.pop_front(); 94 | closedN.push_back(n); 95 | if (fillOpenNode(n)) return true; 96 | } 97 | return false; 98 | } 99 | 100 | int path(list & path) { 101 | path.push_front(end); 102 | int cost = 1 + closedN.back().cost; 103 | path.push_front(closedN.back().post); 104 | point parent = closedN.back().parent; 105 | 106 | for (list::reverse_iterator i = closedN.rbegin(); i != closedN.rend(); i++) { 107 | if ((*i).post == parent && !((*i).post == start)) { 108 | path.push_front((*i).post); 109 | parent = (*i).parent; 110 | } 111 | } 112 | path.push_front(start); 113 | return cost; 114 | } 115 | 116 | maps m; 117 | point end, start, directions[8]; 118 | list openN, closedN; 119 | }; -------------------------------------------------------------------------------- /AStar/Matlab/AStar.m: -------------------------------------------------------------------------------- 1 | % Astar 2 | 3 | % clear 4 | clc; 5 | clear; 6 | close all; 7 | 8 | t = cputime; % run time 9 | 10 | %% Environments 11 | Environ = 1; 12 | MAX_X = 20; % x range 13 | MAX_Y = 40; % y range 14 | 15 | % Grid of Map 16 | MAP = zeros(MAX_X,MAX_Y); 17 | 18 | % Get the different environment setting depending on values: 1, 2, 3 19 | [start, target, obstacles] = Map(Environ); 20 | 21 | %target 22 | target_map = int8(MAP); 23 | target_map(target(1),target(2)) = 1; 24 | 25 | % create obstacles 26 | for i = 1:1:size(obstacles,1) 27 | MAP(obstacles(i,1),obstacles(i,2)) = 1; 28 | end 29 | 30 | %% Astar pathfinding algorithm 31 | path = AStarP(start(2),start(1),MAP,target_map); 32 | 33 | %% if path is found visualize 34 | 35 | if size(path,2)>1 36 | 37 | % visualize environmnet 38 | hold on 39 | imagesc((MAP')) 40 | colormap(flipud(gray)); 41 | axis([1 MAX_X 1 MAX_Y]) 42 | plot(path(end,1),path(end,2),'kh','MarkerFaceColor','g'); % start 43 | plot(path(1,1),path(1,2),'mh','MarkerFaceColor','m'); % target 44 | plot(path(:,1),path(:,2),'r'); % path 45 | legend('Start','Goal', 'Path', 'Location', 'best') % legend 46 | title(['A Star in Environment ', num2str(Environ)]) % title 47 | 48 | else 49 | % if path is not found 50 | error('No Path!') 51 | end 52 | 53 | %% Time and cost function 54 | 55 | % time 56 | e = cputime-t; 57 | 58 | % cost function 59 | d = diff([path(:,2), path(:,1)]); 60 | total_length = sum(sqrt(sum(d.*d,2))); 61 | fprintf("Processing Time: %.2f sec \t Path Length: %.2f bits \n", e, total_length); 62 | -------------------------------------------------------------------------------- /AStar/Matlab/AStarP.m: -------------------------------------------------------------------------------- 1 | % Astar process 2 | 3 | function path=AStarP(X,Y,MAP,target) 4 | 5 | % Initialize 6 | [H,W] = size(MAP); % height and width of matrix 7 | g_cost = zeros(H,W); % g-cost (distance from starting node) 8 | h_cost = single(zeros(H,W)); % hue (h-cost) (distance from end node) 9 | f_cost = single(inf(H,W)); % f-cost for open list (g-cost + h-cost) 10 | par_x = int16(zeros(H,W)); % parent reg of x 11 | par_y = int16(zeros(H,W)); % parent reg of y 12 | connect_Dist = 2; % degree of connection 13 | o_matrix = int8(zeros(H,W)); % open spots 14 | c_matrix = int8(zeros(H,W)); % closed spots 15 | c_matrix(MAP==1) = 1; 16 | 17 | %% Create grid to be looked at 18 | 19 | check = ones(2*connect_Dist+1); 20 | temp = 2*connect_Dist+2; 21 | mid = connect_Dist+1; 22 | 23 | % grids (neighbours) to be investigated around current point (brute force) 24 | for i=1:connect_Dist-1 25 | check(i,i) = 0; 26 | check(temp-i,i) = 0; 27 | check(i,temp-i) = 0; 28 | check(temp-i,temp-i) = 0; 29 | check(mid,i) = 0; 30 | check(mid,temp-i) = 0; 31 | check(i,mid) = 0; 32 | check(temp-i,mid) = 0; 33 | end 34 | check(mid,mid) = 0; 35 | 36 | % nearest (around) 37 | [row, col] = find(check==1); 38 | near = [row col]-(connect_Dist+1); 39 | next_Near = size(col,1); 40 | 41 | %% H cost (heuristic) process = distance from end node (target) 42 | [col, row] = find(target==1); 43 | goal_Reg = [row col]; % goal 44 | nodes_F = size(goal_Reg,1); 45 | 46 | % calculate cost 47 | for k=1:size(target,1) 48 | for j=1:size(target,2) 49 | if MAP(k,j)==0 50 | ma = goal_Reg-(repmat([j k],(nodes_F),1)); 51 | distance = min(sqrt(sum(abs(ma).^2,2))); 52 | h_cost(k,j) = distance; 53 | end 54 | end 55 | end 56 | 57 | %% F-cost = G-cost + H-cost 58 | f_cost(Y,X) = h_cost(Y,X); 59 | o_matrix(Y,X) = 1; 60 | 61 | % ends when no options are available 62 | while 1==1 63 | temp_Fcost = min(min(f_cost)); 64 | 65 | % if no more paths exist 66 | if temp_Fcost==inf 67 | path = inf; 68 | create_Path = 0; 69 | break 70 | end 71 | [nowY,nowX] = find(f_cost==temp_Fcost); 72 | nowY = nowY(1); 73 | nowX = nowX(1); 74 | 75 | % if it reaches the target 76 | if target(nowY,nowX)==1 77 | create_Path = 1; 78 | break 79 | end 80 | 81 | % Open space to close space 82 | o_matrix(nowY,nowX) = 0; 83 | f_cost(nowY,nowX) = inf; 84 | c_matrix(nowY,nowX) = 1; 85 | 86 | for p=1:next_Near 87 | i = near(p,1); %Y 88 | j = near(p,2); %X 89 | 90 | % if path is an option 91 | if nowY+i<1||nowY+i>H||nowX+j<1||nowX+j>W 92 | continue 93 | end 94 | 95 | flag=1; 96 | % if path is open 97 | if c_matrix(nowY+i,nowX+j)==0 98 | 99 | % check if path does not pass obstacles 100 | if (abs(i)>1 || abs(j)>1) 101 | tempC = 2*max(abs(i),abs(j))-1; 102 | for K=1:tempC 103 | YPOS = round(K*i/tempC); 104 | XPOS = round(K*j/tempC); 105 | 106 | if MAP(nowY+YPOS,nowX+XPOS)==1 107 | flag=0; % not okay 108 | end 109 | 110 | end 111 | end 112 | 113 | % if okay 114 | if flag==1 115 | temp_gScore = g_cost(nowY,nowX) + sqrt(i^2+j^2); 116 | if o_matrix(nowY+i,nowX+j)==0 117 | o_matrix(nowY+i,nowX+j) = 1; 118 | elseif temp_gScore >= g_cost(nowY+i,nowX+j) 119 | continue 120 | end 121 | 122 | par_x(nowY+i,nowX+j) = nowX; 123 | par_y(nowY+i,nowX+j) = nowY; 124 | g_cost(nowY+i,nowX+j) = temp_gScore; 125 | f_cost(nowY+i,nowX+j) = temp_gScore+h_cost(nowY+i,nowX+j); 126 | end 127 | end 128 | end 129 | end 130 | 131 | %% add to path 132 | 133 | k=2; 134 | if create_Path 135 | path(1,:) = [nowY nowX]; 136 | while create_Path 137 | x_temp = par_x(nowY,nowX); 138 | nowY = par_y(nowY,nowX); 139 | nowX = x_temp; 140 | path(k,:) = [nowY nowX]; 141 | k = k+1; 142 | if (nowX== X && nowY==Y) 143 | break 144 | end 145 | end 146 | end 147 | end 148 | -------------------------------------------------------------------------------- /AStar/Matlab/Map.m: -------------------------------------------------------------------------------- 1 | % Environment set up 2 | 3 | function [start, target, obstacles] = Map(value) 4 | if value == 1 5 | 6 | % environment 1 7 | start = [10,2]; 8 | target = [10,39]; 9 | obstacles = [1,7; 2,7; 3,7; 4,7; 5,7; 6,7; 7,7; 10 | 1,8; 2,8; 3,8; 4,8; 5,8; 6,8; 7,8; 11 | 14,7; 15,7; 16,7; 17,7; 18,7; 19,7; 20,7; 12 | 14,8; 15,8; 16,8; 17,8; 18,8; 19,8; 20,8; 13 | 6,15; 7,15; 8,15; 9,15; 10,15; 11,15; 12,15; 13,15; 14,15; 15,15; 14 | 6,16; 7,16; 8,16; 9,16; 10,16; 11,16; 12,16; 13,16; 14,16; 15,16; 15 | 3,23; 4,23; 5,23; 6,23; 7,23; 8,23; 16 | 3,24; 4,24; 5,24; 6,24; 7,24; 8,24; 17 | 13,23; 14,23; 15,23; 16,23; 17,23; 18,23; 18 | 13,24; 14,24; 15,24; 16,24; 17,24; 18,24; 19 | 7,31; 8,31; 9,31; 10,31; 11,31; 12,31; 13,31; 14,31; 20 | 7,32; 8,32; 9,32; 10,32; 11,32; 12,32; 13,32; 14,32;]; 21 | elseif value == 2 22 | 23 | % environment 2 24 | start = [2,2]; 25 | target = [19,39]; 26 | obstacles = [3,4; 4,4; 5,4; 6,4; 7,4; 8,4; 9,4; 10,4; 11,4; 12,4; 13,4; 3,5; 4,5; 5,5; 6,5; 7,5; 8,5; 9,5; 10,5; 11,5; 12,5; 13,5; 27 | 3,6; 4,6; 5,6; 6,6; 7,6; 8,6; 9,6; 10,6; 11,6; 12,6; 13,6; 3,7; 4,7; 5,7; 6,7; 7,7; 8,7; 9,7; 10,7; 11,7; 12,7; 13,7; 28 | 6,8; 7,8; 8,8; 9,8; 10,8; 11,8; 12,8; 13,8; 6,9; 7,9; 8,9; 9,9; 10,9; 11,9; 12,9; 13,9; 6,10; 7,10; 8,10; 9,10; 10,10; 11,10; 12,10; 13,10; 29 | 6,11; 7,11; 8,11; 9,11; 10,11; 11,11; 12,11; 13,11; 6,12; 7,12; 8,12; 9,12; 10,12; 11,12; 12,12; 13,12; 6,13; 7,13; 8,13; 9,13; 10,13; 11,13; 12,13; 13,13; 30 | 6,14; 7,14; 8,14; 9,14; 10,14; 11,14; 12,14; 13,14; 11,15; 12,15; 13,15; 11,16; 12,16; 13,16; 11,17; 12,17; 13,17; 11,18; 12,18; 13,18; 11,9; 12,9; 13,9; 31 | 11,19; 12,19; 13,19; 11,20; 12,20; 13,20; 11,21; 12,21; 13,21; 11,22; 12,22; 13,22; 14,22; 15,22; 16,22; 17,22; 18,22; 11,23; 12,23; 13,23; 14,23; 15,23; 16,23; 17,23; 18,23; 32 | 11,24; 12,24; 13,24; 14,24; 15,24; 16,24; 17,24; 18,24; 11,25; 12,25; 13,25; 14,25; 15,25; 16,25; 17,25; 18,25; 11,26; 12,26; 13,26; 14,26; 15,26; 16,26; 17,26; 18,26; 33 | 11,27; 12,27; 13,27; 14,27; 15,27; 16,27; 17,27; 18,27; 16,28; 17,28; 18,28; 16,29; 17,29; 18,29; 16,30; 17,30; 18,30; 16,31; 17,31; 18,31; 16,32; 17,32; 18,32; 34 | 11,34; 12,34; 13,34; 11,35; 12,35; 13,35; 11,36; 12,36; 13,36; 11,37; 12,37; 13,37; 11,38; 12,38; 13,38; 11,39; 12,39; 13,39; 11,40; 12,40; 13,40; 35 | 7,34; 8,34; 9,34; 10,34; 7,35; 8,35; 9,35; 10,35; 7,36; 8,36; 9,36; 10,36; 7,37; 8,37; 9,37; 10,37; 7,38; 8,38; 9,38; 10,38; 7,39; 8,39; 9,39; 10,39; 7,40; 8,40; 9,40; 10,40; 36 | 1,21; 2,21; 3,21; 4,21; 5,21; 6,21; 7,21; 1,22; 2,22; 3,22; 4,22; 5,22; 6,22; 7,22; 1,23; 2,23; 3,23; 4,23; 5,23; 6,23; 7,23; 1,24; 2,24; 3,24; 4,24; 5,24; 6,24; 7,24; 37 | 1,24; 2,24; 3,24; 4,24; 5,24; 6,24; 7,24; 1,25; 2,25; 3,25; 4,25; 5,25; 6,25; 7,25; 1,26; 2,26; 3,26; 4,26; 5,26; 6,26; 7,26; 1,27; 2,27; 3,27; 4,27; 5,27; 6,27; 7,27; 38 | 8,21; 8,22; 8,23; 8,24; 8,25; 8,26; 8,27; 7,20; 8,20; 7,19; 8,19; 39 | 16,4; 17,4; 18,4; 19,4; 20,4; 16,5; 17,5; 18,5; 19,5; 20,5; 16,6; 17,6; 18,6; 19,6; 20,6; 16,7; 17,7; 18,7; 19,7; 20,7; 16,8; 17,8; 18,8; 19,8; 20,8; 15,4; 15,5; 15,6; 15,7; 15,8;]; 40 | else 41 | 42 | % environment 3 43 | start = [2,39]; 44 | target = [19,39]; 45 | obstacles = [1,14; 2,14; 3,14; 4,14; 5,14; 6,14; 7,14; 8,14; 1,13; 2,13; 3,13; 4,13; 5,13; 6,13; 7,13; 8,13; 1,12; 2,12; 3,12; 4,12; 5,12; 6,12; 7,12; 8,12; 46 | 1,11; 2,11; 3,11; 4,11; 5,11; 6,11; 7,11; 8,11; 1,10; 2,10; 3,10; 4,10; 5,10; 6,10; 7,10; 8,10; 1,9; 2,9; 3,9; 4,9; 5,9; 6,9; 7,9; 8,9; 47 | 10,12; 11,12; 12,12; 13,12; 14,12; 15,12; 16,12; 10,13; 11,13; 12,13; 13,13; 14,13; 15,13; 16,13; 10,14; 11,14; 12,14; 13,14; 14,14; 15,14; 16,14; 48 | 10,15; 11,15; 12,15; 13,15; 14,15; 15,15; 16,15; 10,16; 11,16; 12,16; 13,16; 14,16; 15,16; 16,16; 10,17; 11,17; 12,17; 13,17; 14,17; 15,17; 16,17; 49 | 10,18; 11,18; 12,18; 13,18; 14,18; 15,18; 16,18; 10,19; 11,19; 12,19; 13,19; 14,19; 15,19; 16,19; 10,20; 11,20; 12,20; 13,20; 14,20; 15,20; 16,20; 50 | 15,21; 16,21; 15,22; 16,22; 15,23; 16,23; 15,24; 16,24; 15,26; 16,26; 15,25; 16,25; 15,27; 16,27; 19,25; 20,25; 19,26; 20,26; 19,27; 20,27; 19,28; 20,28; 19,29; 20,29; 51 | 11,1; 12,1; 13,1; 14,1; 11,2; 12,2; 13,2; 14,2; 11,3; 12,3; 13,3; 14,3; 11,4; 12,4; 13,4; 14,4; 11,5; 12,5; 13,5; 14,5; 11,6; 12,6; 13,6; 14,6; 52 | 1,15; 2,15; 3,15; 4,15; 5,15; 6,15; 7,15; 8,15; 1,16; 2,16; 3,16; 4,16; 5,16; 6,16; 7,16; 8,16; 1,17; 2,17; 3,17; 4,17; 5,17; 6,17; 7,17; 8,17; 53 | 1,18; 2,18; 3,18; 4,18; 5,18; 6,18; 7,18; 8,18; 1,19; 2,19; 3,19; 4,19; 1,20; 2,20; 3,20; 4,20; 1,22; 2,22; 3,22; 4,22; 1,21; 2,21; 3,21; 4,21; 54 | 1,22; 2,22; 3,22; 4,22; 1,23; 2,23; 3,23; 4,23; 1,24; 2,24; 3,24; 4,24; 1,25; 2,25; 3,25; 4,25; 1,26; 2,26; 3,26; 4,26; 1,27; 2,27; 3,27; 4,27; 55 | 1,28; 2,28; 3,28; 4,28; 1,29; 2,29; 3,29; 4,29; 1,30; 2,30; 3,30; 4,30; 1,31; 2,31; 3,31; 4,31; 1,32; 2,32; 3,32; 4,32; 1,33; 2,33; 3,33; 4,33; 56 | 1,34; 2,34; 3,34; 4,34; 1,35; 2,35; 3,35; 4,35; 3,36; 4,36; 3,37; 4,37; 7,39; 8,39; 9,39; 10,39; 11,39; 12,39; 7,38; 8,38; 9,38; 10,38; 11,38; 12,38; 57 | 7,37; 8,37; 9,37; 10,37; 11,37; 12,37; 7,36; 8,36; 9,36; 10,36; 11,36; 12,36; 7,35; 8,35; 9,35; 10,35; 11,35; 12,35; 7,34; 8,34; 9,34; 10,34; 11,34; 12,34; 58 | 7,33; 8,33; 9,33; 10,33; 11,33; 12,33; 7,32; 8,32; 9,32; 10,32; 11,32; 12,32; 7,31; 8,31; 9,31; 10,31; 11,31; 12,31; 7,30; 8,30; 9,30; 10,30; 11,30; 12,30; 59 | 7,29; 8,29; 9,29; 10,29; 11,29; 12,29; 7,28; 8,28; 9,28; 10,28; 11,28; 12,28; 15,28; 16,28; 15,29; 16,29; 15,30; 16,30; 7,40; 8,40; 9,40; 10,40; 11,40; 12,40; 60 | 9,28; 10,28; 8,28; 11,28; 8,27; 11,27; 11,26; 8,26; 9,27; 10,27; 9,26; 10,26;]; 61 | end 62 | -------------------------------------------------------------------------------- /PRM Node Based/GoodP.m: -------------------------------------------------------------------------------- 1 | % check if point in the map is valid 2 | 3 | function check=GoodP(point,map) 4 | check=true; 5 | % check if collission free in the map 6 | if ~(point(1)>=1 && point(1)<=size(map,1) && point(2)>=1 && point(2)<=size(map,2) && map(point(1),point(2))==1) 7 | check=false; 8 | end 9 | -------------------------------------------------------------------------------- /PRM Node Based/Map.m: -------------------------------------------------------------------------------- 1 | % Environment set up 2 | 3 | function [start, target, obstacles] = Map(value) 4 | if value == 1 5 | 6 | % environment 1 7 | start = [2 10]; 8 | target = [39 10]; 9 | obstacles = [1,7; 2,7; 3,7; 4,7; 5,7; 6,7; 7,7; 10 | 1,8; 2,8; 3,8; 4,8; 5,8; 6,8; 7,8; 11 | 14,7; 15,7; 16,7; 17,7; 18,7; 19,7; 20,7; 12 | 14,8; 15,8; 16,8; 17,8; 18,8; 19,8; 20,8; 13 | 6,15; 7,15; 8,15; 9,15; 10,15; 11,15; 12,15; 13,15; 14,15; 15,15; 14 | 6,16; 7,16; 8,16; 9,16; 10,16; 11,16; 12,16; 13,16; 14,16; 15,16; 15 | 3,23; 4,23; 5,23; 6,23; 7,23; 8,23; 16 | 3,24; 4,24; 5,24; 6,24; 7,24; 8,24; 17 | 13,23; 14,23; 15,23; 16,23; 17,23; 18,23; 18 | 13,24; 14,24; 15,24; 16,24; 17,24; 18,24; 19 | 7,31; 8,31; 9,31; 10,31; 11,31; 12,31; 13,31; 14,31; 20 | 7,32; 8,32; 9,32; 10,32; 11,32; 12,32; 13,32; 14,32;]; 21 | elseif value == 2 22 | 23 | % environment two 24 | start = [2,2]; 25 | target = [39,19]; 26 | obstacles = [3,4; 4,4; 5,4; 6,4; 7,4; 8,4; 9,4; 10,4; 11,4; 12,4; 13,4; 3,5; 4,5; 5,5; 6,5; 7,5; 8,5; 9,5; 10,5; 11,5; 12,5; 13,5; 27 | 3,6; 4,6; 5,6; 6,6; 7,6; 8,6; 9,6; 10,6; 11,6; 12,6; 13,6; 3,7; 4,7; 5,7; 6,7; 7,7; 8,7; 9,7; 10,7; 11,7; 12,7; 13,7; 28 | 6,8; 7,8; 8,8; 9,8; 10,8; 11,8; 12,8; 13,8; 6,9; 7,9; 8,9; 9,9; 10,9; 11,9; 12,9; 13,9; 6,10; 7,10; 8,10; 9,10; 10,10; 11,10; 12,10; 13,10; 29 | 6,11; 7,11; 8,11; 9,11; 10,11; 11,11; 12,11; 13,11; 6,12; 7,12; 8,12; 9,12; 10,12; 11,12; 12,12; 13,12; 6,13; 7,13; 8,13; 9,13; 10,13; 11,13; 12,13; 13,13; 30 | 6,14; 7,14; 8,14; 9,14; 10,14; 11,14; 12,14; 13,14; 11,15; 12,15; 13,15; 11,16; 12,16; 13,16; 11,17; 12,17; 13,17; 11,18; 12,18; 13,18; 11,9; 12,9; 13,9; 31 | 11,19; 12,19; 13,19; 11,20; 12,20; 13,20; 11,21; 12,21; 13,21; 11,22; 12,22; 13,22; 14,22; 15,22; 16,22; 17,22; 18,22; 11,23; 12,23; 13,23; 14,23; 15,23; 16,23; 17,23; 18,23; 32 | 11,24; 12,24; 13,24; 14,24; 15,24; 16,24; 17,24; 18,24; 11,25; 12,25; 13,25; 14,25; 15,25; 16,25; 17,25; 18,25; 11,26; 12,26; 13,26; 14,26; 15,26; 16,26; 17,26; 18,26; 33 | 11,27; 12,27; 13,27; 14,27; 15,27; 16,27; 17,27; 18,27; 16,28; 17,28; 18,28; 16,29; 17,29; 18,29; 16,30; 17,30; 18,30; 16,31; 17,31; 18,31; 16,32; 17,32; 18,32; 34 | 11,34; 12,34; 13,34; 11,35; 12,35; 13,35; 11,36; 12,36; 13,36; 11,37; 12,37; 13,37; 11,38; 12,38; 13,38; 11,39; 12,39; 13,39; 11,40; 12,40; 13,40; 35 | 7,34; 8,34; 9,34; 10,34; 7,35; 8,35; 9,35; 10,35; 7,36; 8,36; 9,36; 10,36; 7,37; 8,37; 9,37; 10,37; 7,38; 8,38; 9,38; 10,38; 7,39; 8,39; 9,39; 10,39; 7,40; 8,40; 9,40; 10,40; 36 | 1,21; 2,21; 3,21; 4,21; 5,21; 6,21; 7,21; 1,22; 2,22; 3,22; 4,22; 5,22; 6,22; 7,22; 1,23; 2,23; 3,23; 4,23; 5,23; 6,23; 7,23; 1,24; 2,24; 3,24; 4,24; 5,24; 6,24; 7,24; 37 | 1,24; 2,24; 3,24; 4,24; 5,24; 6,24; 7,24; 1,25; 2,25; 3,25; 4,25; 5,25; 6,25; 7,25; 1,26; 2,26; 3,26; 4,26; 5,26; 6,26; 7,26; 1,27; 2,27; 3,27; 4,27; 5,27; 6,27; 7,27; 38 | 8,21; 8,22; 8,23; 8,24; 8,25; 8,26; 8,27; 7,20; 8,20; 7,19; 8,19; 39 | 16,4; 17,4; 18,4; 19,4; 20,4; 16,5; 17,5; 18,5; 19,5; 20,5; 16,6; 17,6; 18,6; 19,6; 20,6; 16,7; 17,7; 18,7; 19,7; 20,7; 16,8; 17,8; 18,8; 19,8; 20,8; 15,4; 15,5; 15,6; 15,7; 15,8;]; 40 | 41 | else 42 | 43 | % environment 3 44 | start = [39,2]; 45 | target = [39,19]; 46 | obstacles = [1,14; 2,14; 3,14; 4,14; 5,14; 6,14; 7,14; 8,14; 1,13; 2,13; 3,13; 4,13; 5,13; 6,13; 7,13; 8,13; 1,12; 2,12; 3,12; 4,12; 5,12; 6,12; 7,12; 8,12; 47 | 1,11; 2,11; 3,11; 4,11; 5,11; 6,11; 7,11; 8,11; 1,10; 2,10; 3,10; 4,10; 5,10; 6,10; 7,10; 8,10; 1,9; 2,9; 3,9; 4,9; 5,9; 6,9; 7,9; 8,9; 48 | 10,12; 11,12; 12,12; 13,12; 14,12; 15,12; 16,12; 10,13; 11,13; 12,13; 13,13; 14,13; 15,13; 16,13; 10,14; 11,14; 12,14; 13,14; 14,14; 15,14; 16,14; 49 | 10,15; 11,15; 12,15; 13,15; 14,15; 15,15; 16,15; 10,16; 11,16; 12,16; 13,16; 14,16; 15,16; 16,16; 10,17; 11,17; 12,17; 13,17; 14,17; 15,17; 16,17; 50 | 10,18; 11,18; 12,18; 13,18; 14,18; 15,18; 16,18; 10,19; 11,19; 12,19; 13,19; 14,19; 15,19; 16,19; 10,20; 11,20; 12,20; 13,20; 14,20; 15,20; 16,20; 51 | 15,21; 16,21; 15,22; 16,22; 15,23; 16,23; 15,24; 16,24; 15,26; 16,26; 15,25; 16,25; 15,27; 16,27; 19,25; 20,25; 19,26; 20,26; 19,27; 20,27; 19,28; 20,28; 19,29; 20,29; 52 | 11,1; 12,1; 13,1; 14,1; 11,2; 12,2; 13,2; 14,2; 11,3; 12,3; 13,3; 14,3; 11,4; 12,4; 13,4; 14,4; 11,5; 12,5; 13,5; 14,5; 11,6; 12,6; 13,6; 14,6; 53 | 1,15; 2,15; 3,15; 4,15; 5,15; 6,15; 7,15; 8,15; 1,16; 2,16; 3,16; 4,16; 5,16; 6,16; 7,16; 8,16; 1,17; 2,17; 3,17; 4,17; 5,17; 6,17; 7,17; 8,17; 54 | 1,18; 2,18; 3,18; 4,18; 5,18; 6,18; 7,18; 8,18; 1,19; 2,19; 3,19; 4,19; 1,20; 2,20; 3,20; 4,20; 1,22; 2,22; 3,22; 4,22; 1,21; 2,21; 3,21; 4,21; 55 | 1,22; 2,22; 3,22; 4,22; 1,23; 2,23; 3,23; 4,23; 1,24; 2,24; 3,24; 4,24; 1,25; 2,25; 3,25; 4,25; 1,26; 2,26; 3,26; 4,26; 1,27; 2,27; 3,27; 4,27; 56 | 1,28; 2,28; 3,28; 4,28; 1,29; 2,29; 3,29; 4,29; 1,30; 2,30; 3,30; 4,30; 1,31; 2,31; 3,31; 4,31; 1,32; 2,32; 3,32; 4,32; 1,33; 2,33; 3,33; 4,33; 57 | 1,34; 2,34; 3,34; 4,34; 1,35; 2,35; 3,35; 4,35; 3,36; 4,36; 3,37; 4,37; 7,39; 8,39; 9,39; 10,39; 11,39; 12,39; 7,38; 8,38; 9,38; 10,38; 11,38; 12,38; 58 | 7,37; 8,37; 9,37; 10,37; 11,37; 12,37; 7,36; 8,36; 9,36; 10,36; 11,36; 12,36; 7,35; 8,35; 9,35; 10,35; 11,35; 12,35; 7,34; 8,34; 9,34; 10,34; 11,34; 12,34; 59 | 7,33; 8,33; 9,33; 10,33; 11,33; 12,33; 7,32; 8,32; 9,32; 10,32; 11,32; 12,32; 7,31; 8,31; 9,31; 10,31; 11,31; 12,31; 7,30; 8,30; 9,30; 10,30; 11,30; 12,30; 60 | 7,29; 8,29; 9,29; 10,29; 11,29; 12,29; 7,28; 8,28; 9,28; 10,28; 11,28; 12,28; 15,28; 16,28; 15,29; 16,29; 15,30; 16,30; 7,40; 8,40; 9,40; 10,40; 11,40; 12,40; 61 | 9,28; 10,28; 8,28; 11,28; 8,27; 11,27; 11,26; 8,26; 9,27; 10,27; 9,26; 10,26;]; 62 | end 63 | -------------------------------------------------------------------------------- /PRM Node Based/nPRM.m: -------------------------------------------------------------------------------- 1 | % Probabilistic Roadmap Method (PRMN) improved by random sampling in the 2 | % bounding box array to ensure a more efficient distribution of roadmap 3 | % nodes. 4 | 5 | % clear 6 | clc; 7 | clear; 8 | close all; 9 | 10 | t = cputime; % run time 11 | 12 | %% Environments 13 | Environ = 1; 14 | MAX_X = 20; % x range 15 | MAX_Y = 40; % y range 16 | 17 | % Grid of Map (one to visualize one to calcuate) 18 | MAP = zeros(MAX_X,MAX_Y); % use to visualize 19 | map = ones(MAX_X,MAX_Y); % use to calculate 20 | 21 | % Get the different environment setting depending on values: 1, 2, 3 22 | [start, target, obstacles] = Map(Environ); 23 | 24 | % create obstacles 25 | for i = 1: 1:size(obstacles,1) 26 | MAP(obstacles(i,1),obstacles(i,2)) = 1; 27 | map(obstacles(i,1),obstacles(i,2)) = 0; 28 | end 29 | 30 | map = map'; 31 | node = 250; % number of points in the PRM 32 | 33 | % visualize environmnet 34 | hold on 35 | imagesc((MAP')) 36 | colormap(flipud(gray)); 37 | axis([1 MAX_X 1 MAX_Y]) 38 | 39 | temp = [start;target]; % start and target as first points 40 | p1 = plot(temp(1,2), temp(1,1),'kh','MarkerFaceColor','g'); %start 41 | p2 = plot(temp(2,2), temp(2,1),'mh','MarkerFaceColor','m'); %target 42 | 43 | %% Bounding box (improvement of PRM) 44 | 45 | % bounding box around obstacles to randomly sample in them 46 | con_com = bwlabel(MAP); 47 | props = regionprops(con_com); 48 | split = max(con_com(:)); 49 | 50 | % get the white space (empty space) in the bounding boxes 51 | for io = 1:split 52 | count = 0; 53 | for mn = floor(props(io).BoundingBox(1)):floor(props(io).BoundingBox(1))+floor(props(io).BoundingBox(3)) 54 | for mm = floor(props(io).BoundingBox(2)):floor(props(io).BoundingBox(2))+floor(props(io).BoundingBox(4)) 55 | if mn < 1 || mn > 40 || mm < 1 || mm > 40 56 | continue; 57 | end 58 | if MAP(mm, mn) == 0 59 | count = count+1; 60 | end 61 | end 62 | end 63 | white_space(io) = count; 64 | end 65 | 66 | % random sampling in the bounding box and anywhere 67 | io = 1; 68 | temp_length = 2; % slowly accumulate sum till reaches total nodes declared 69 | while length(temp)0 125 | [A, I] = min(X,[],1); 126 | n = X(I(4),:); % check smallest cost element 127 | X= [X(1:I(4)-1,:);X(I(4)+1:end,:)]; % delete element (currently) 128 | 129 | if n(1)==2 % check 130 | path_F = true; 131 | break; 132 | end 133 | 134 | % iterate through all adjacency from the node 135 | for mv=1:length(adjacency{n(1),1}) 136 | temp1 = adjacency{n(1),1}(mv); 137 | 138 | % if not already in p_index 139 | if length(p_index)==0 || length(find(p_index(:,1)==temp1))==0 140 | historic_c = n(2)+hist(temp(n(1),:),temp(temp1,:)); % historic cost 141 | heuristic_c = heu(temp(temp1,:),target); % heuristic cost 142 | total_c = historic_c+heuristic_c; % total cost 143 | 144 | add = true; % add if better cost 145 | if length(find(X(:,1)==temp1))>=1 146 | I = find(X(:,1)==temp1); 147 | if X(I,4)0 172 | path = [temp(p_index(prev,1),:);path]; 173 | prev = p_index(prev,5); 174 | end 175 | 176 | %visualize the bounding box 177 | for id =1:length(props) 178 | b1 = rectangle('Position', [props(id).BoundingBox(2) props(id).BoundingBox(1) props(id).BoundingBox(4) props(id).BoundingBox(3)], 'EdgeColor', 'g'); 179 | end 180 | 181 | % visualize the path 182 | p5 = plot(path(:,2),path(:,1),'color','r'); 183 | %legend([p1 p2 p3(1) p4 p5], {'Start','Target', 'Nodes', 'Potental Paths', 'Path'}, 'Location', 'southeast') 184 | %legend([p1 p2 p3(1) p5], {'Start','Target', 'Nodes', 'Path'}, 'Location', 'bestoutside') 185 | title(['PRM Node Based in Environment ', num2str(Environ)]) % title 186 | hold off 187 | 188 | %% Time and cost function (recalculated just in case) 189 | 190 | % time 191 | e = cputime-t; 192 | 193 | % cost function 194 | d = diff([path(:,2), path(:,1)]); 195 | total_length = sum(sqrt(sum(d.*d,2))); 196 | fprintf("Processing Time: %.2f sec \t Path Length: %.2f bits \n", e, total_length); 197 | 198 | %% Cost function calculation 199 | 200 | % historic 201 | function h=hist(a,b) 202 | h = sqrt(sum((a-b).^2)); 203 | end 204 | 205 | % heuristic 206 | function h=heu(c,d) 207 | h = sqrt(sum((c-d).^2)); 208 | end 209 | 210 | -------------------------------------------------------------------------------- /PRM Node Based/pathCheck.m: -------------------------------------------------------------------------------- 1 | % check if path is valid 2 | 3 | function check=pathCheck(n,n_P,map) 4 | check=true; 5 | d=atan2(n_P(1)-n(1),n_P(2)-n(2)); 6 | 7 | for r=0:0.5:sqrt(sum((n-n_P).^2)) 8 | posCheck=n+r.*[sin(d) cos(d)]; 9 | 10 | % check if valid 11 | if ~(GoodP(ceil(posCheck),map) && GoodP(floor(posCheck),map) && GoodP([ceil(posCheck(1)) floor(posCheck(2))],map) && GoodP([floor(posCheck(1)) ceil(posCheck(2))],map)) 12 | check=false; 13 | break; 14 | end 15 | 16 | % check if new path is valid 17 | if ~GoodP(n_P,map), 18 | check=false; 19 | end 20 | end -------------------------------------------------------------------------------- /PRM/GoodP.m: -------------------------------------------------------------------------------- 1 | % check if point in the map is valid 2 | 3 | function check=GoodP(point,map) 4 | check=true; 5 | % check if collission free in the map 6 | if ~(point(1)>=1 && point(1)<=size(map,1) && point(2)>=1 && point(2)<=size(map,2) && map(point(1),point(2))==1) 7 | check=false; 8 | end 9 | -------------------------------------------------------------------------------- /PRM/Map.m: -------------------------------------------------------------------------------- 1 | % Environment set up 2 | 3 | function [start, target, obstacles] = Map(value) 4 | if value == 1 5 | 6 | % environment 1 7 | start = [2 10]; 8 | target = [39 10]; 9 | obstacles = [1,7; 2,7; 3,7; 4,7; 5,7; 6,7; 7,7; 10 | 1,8; 2,8; 3,8; 4,8; 5,8; 6,8; 7,8; 11 | 14,7; 15,7; 16,7; 17,7; 18,7; 19,7; 20,7; 12 | 14,8; 15,8; 16,8; 17,8; 18,8; 19,8; 20,8; 13 | 6,15; 7,15; 8,15; 9,15; 10,15; 11,15; 12,15; 13,15; 14,15; 15,15; 14 | 6,16; 7,16; 8,16; 9,16; 10,16; 11,16; 12,16; 13,16; 14,16; 15,16; 15 | 3,23; 4,23; 5,23; 6,23; 7,23; 8,23; 16 | 3,24; 4,24; 5,24; 6,24; 7,24; 8,24; 17 | 13,23; 14,23; 15,23; 16,23; 17,23; 18,23; 18 | 13,24; 14,24; 15,24; 16,24; 17,24; 18,24; 19 | 7,31; 8,31; 9,31; 10,31; 11,31; 12,31; 13,31; 14,31; 20 | 7,32; 8,32; 9,32; 10,32; 11,32; 12,32; 13,32; 14,32;]; 21 | elseif value == 2 22 | 23 | % environment two 24 | start = [2,2]; 25 | target = [39,19]; 26 | obstacles = [3,4; 4,4; 5,4; 6,4; 7,4; 8,4; 9,4; 10,4; 11,4; 12,4; 13,4; 3,5; 4,5; 5,5; 6,5; 7,5; 8,5; 9,5; 10,5; 11,5; 12,5; 13,5; 27 | 3,6; 4,6; 5,6; 6,6; 7,6; 8,6; 9,6; 10,6; 11,6; 12,6; 13,6; 3,7; 4,7; 5,7; 6,7; 7,7; 8,7; 9,7; 10,7; 11,7; 12,7; 13,7; 28 | 6,8; 7,8; 8,8; 9,8; 10,8; 11,8; 12,8; 13,8; 6,9; 7,9; 8,9; 9,9; 10,9; 11,9; 12,9; 13,9; 6,10; 7,10; 8,10; 9,10; 10,10; 11,10; 12,10; 13,10; 29 | 6,11; 7,11; 8,11; 9,11; 10,11; 11,11; 12,11; 13,11; 6,12; 7,12; 8,12; 9,12; 10,12; 11,12; 12,12; 13,12; 6,13; 7,13; 8,13; 9,13; 10,13; 11,13; 12,13; 13,13; 30 | 6,14; 7,14; 8,14; 9,14; 10,14; 11,14; 12,14; 13,14; 11,15; 12,15; 13,15; 11,16; 12,16; 13,16; 11,17; 12,17; 13,17; 11,18; 12,18; 13,18; 11,9; 12,9; 13,9; 31 | 11,19; 12,19; 13,19; 11,20; 12,20; 13,20; 11,21; 12,21; 13,21; 11,22; 12,22; 13,22; 14,22; 15,22; 16,22; 17,22; 18,22; 11,23; 12,23; 13,23; 14,23; 15,23; 16,23; 17,23; 18,23; 32 | 11,24; 12,24; 13,24; 14,24; 15,24; 16,24; 17,24; 18,24; 11,25; 12,25; 13,25; 14,25; 15,25; 16,25; 17,25; 18,25; 11,26; 12,26; 13,26; 14,26; 15,26; 16,26; 17,26; 18,26; 33 | 11,27; 12,27; 13,27; 14,27; 15,27; 16,27; 17,27; 18,27; 16,28; 17,28; 18,28; 16,29; 17,29; 18,29; 16,30; 17,30; 18,30; 16,31; 17,31; 18,31; 16,32; 17,32; 18,32; 34 | 11,34; 12,34; 13,34; 11,35; 12,35; 13,35; 11,36; 12,36; 13,36; 11,37; 12,37; 13,37; 11,38; 12,38; 13,38; 11,39; 12,39; 13,39; 11,40; 12,40; 13,40; 35 | 7,34; 8,34; 9,34; 10,34; 7,35; 8,35; 9,35; 10,35; 7,36; 8,36; 9,36; 10,36; 7,37; 8,37; 9,37; 10,37; 7,38; 8,38; 9,38; 10,38; 7,39; 8,39; 9,39; 10,39; 7,40; 8,40; 9,40; 10,40; 36 | 1,21; 2,21; 3,21; 4,21; 5,21; 6,21; 7,21; 1,22; 2,22; 3,22; 4,22; 5,22; 6,22; 7,22; 1,23; 2,23; 3,23; 4,23; 5,23; 6,23; 7,23; 1,24; 2,24; 3,24; 4,24; 5,24; 6,24; 7,24; 37 | 1,24; 2,24; 3,24; 4,24; 5,24; 6,24; 7,24; 1,25; 2,25; 3,25; 4,25; 5,25; 6,25; 7,25; 1,26; 2,26; 3,26; 4,26; 5,26; 6,26; 7,26; 1,27; 2,27; 3,27; 4,27; 5,27; 6,27; 7,27; 38 | 8,21; 8,22; 8,23; 8,24; 8,25; 8,26; 8,27; 7,20; 8,20; 7,19; 8,19; 39 | 16,4; 17,4; 18,4; 19,4; 20,4; 16,5; 17,5; 18,5; 19,5; 20,5; 16,6; 17,6; 18,6; 19,6; 20,6; 16,7; 17,7; 18,7; 19,7; 20,7; 16,8; 17,8; 18,8; 19,8; 20,8; 15,4; 15,5; 15,6; 15,7; 15,8;]; 40 | 41 | else 42 | 43 | % environment 3 44 | start = [39,2]; 45 | target = [39,19]; 46 | obstacles = [1,14; 2,14; 3,14; 4,14; 5,14; 6,14; 7,14; 8,14; 1,13; 2,13; 3,13; 4,13; 5,13; 6,13; 7,13; 8,13; 1,12; 2,12; 3,12; 4,12; 5,12; 6,12; 7,12; 8,12; 47 | 1,11; 2,11; 3,11; 4,11; 5,11; 6,11; 7,11; 8,11; 1,10; 2,10; 3,10; 4,10; 5,10; 6,10; 7,10; 8,10; 1,9; 2,9; 3,9; 4,9; 5,9; 6,9; 7,9; 8,9; 48 | 10,12; 11,12; 12,12; 13,12; 14,12; 15,12; 16,12; 10,13; 11,13; 12,13; 13,13; 14,13; 15,13; 16,13; 10,14; 11,14; 12,14; 13,14; 14,14; 15,14; 16,14; 49 | 10,15; 11,15; 12,15; 13,15; 14,15; 15,15; 16,15; 10,16; 11,16; 12,16; 13,16; 14,16; 15,16; 16,16; 10,17; 11,17; 12,17; 13,17; 14,17; 15,17; 16,17; 50 | 10,18; 11,18; 12,18; 13,18; 14,18; 15,18; 16,18; 10,19; 11,19; 12,19; 13,19; 14,19; 15,19; 16,19; 10,20; 11,20; 12,20; 13,20; 14,20; 15,20; 16,20; 51 | 15,21; 16,21; 15,22; 16,22; 15,23; 16,23; 15,24; 16,24; 15,26; 16,26; 15,25; 16,25; 15,27; 16,27; 19,25; 20,25; 19,26; 20,26; 19,27; 20,27; 19,28; 20,28; 19,29; 20,29; 52 | 11,1; 12,1; 13,1; 14,1; 11,2; 12,2; 13,2; 14,2; 11,3; 12,3; 13,3; 14,3; 11,4; 12,4; 13,4; 14,4; 11,5; 12,5; 13,5; 14,5; 11,6; 12,6; 13,6; 14,6; 53 | 1,15; 2,15; 3,15; 4,15; 5,15; 6,15; 7,15; 8,15; 1,16; 2,16; 3,16; 4,16; 5,16; 6,16; 7,16; 8,16; 1,17; 2,17; 3,17; 4,17; 5,17; 6,17; 7,17; 8,17; 54 | 1,18; 2,18; 3,18; 4,18; 5,18; 6,18; 7,18; 8,18; 1,19; 2,19; 3,19; 4,19; 1,20; 2,20; 3,20; 4,20; 1,22; 2,22; 3,22; 4,22; 1,21; 2,21; 3,21; 4,21; 55 | 1,22; 2,22; 3,22; 4,22; 1,23; 2,23; 3,23; 4,23; 1,24; 2,24; 3,24; 4,24; 1,25; 2,25; 3,25; 4,25; 1,26; 2,26; 3,26; 4,26; 1,27; 2,27; 3,27; 4,27; 56 | 1,28; 2,28; 3,28; 4,28; 1,29; 2,29; 3,29; 4,29; 1,30; 2,30; 3,30; 4,30; 1,31; 2,31; 3,31; 4,31; 1,32; 2,32; 3,32; 4,32; 1,33; 2,33; 3,33; 4,33; 57 | 1,34; 2,34; 3,34; 4,34; 1,35; 2,35; 3,35; 4,35; 3,36; 4,36; 3,37; 4,37; 7,39; 8,39; 9,39; 10,39; 11,39; 12,39; 7,38; 8,38; 9,38; 10,38; 11,38; 12,38; 58 | 7,37; 8,37; 9,37; 10,37; 11,37; 12,37; 7,36; 8,36; 9,36; 10,36; 11,36; 12,36; 7,35; 8,35; 9,35; 10,35; 11,35; 12,35; 7,34; 8,34; 9,34; 10,34; 11,34; 12,34; 59 | 7,33; 8,33; 9,33; 10,33; 11,33; 12,33; 7,32; 8,32; 9,32; 10,32; 11,32; 12,32; 7,31; 8,31; 9,31; 10,31; 11,31; 12,31; 7,30; 8,30; 9,30; 10,30; 11,30; 12,30; 60 | 7,29; 8,29; 9,29; 10,29; 11,29; 12,29; 7,28; 8,28; 9,28; 10,28; 11,28; 12,28; 15,28; 16,28; 15,29; 16,29; 15,30; 16,30; 7,40; 8,40; 9,40; 10,40; 11,40; 12,40; 61 | 9,28; 10,28; 8,28; 11,28; 8,27; 11,27; 11,26; 8,26; 9,27; 10,27; 9,26; 10,26;]; 62 | end 63 | -------------------------------------------------------------------------------- /PRM/PRM.m: -------------------------------------------------------------------------------- 1 | % Probabilistic Roadmap Method (PRM) 2 | 3 | % clear 4 | clc; 5 | clear; 6 | close all; 7 | 8 | t = cputime; % run time 9 | 10 | %% Environments 11 | Environ = 2; 12 | MAX_X = 20; % x range 13 | MAX_Y = 40; % y range 14 | 15 | % Grid of Map (one to visualize one to calcuate) 16 | MAP = zeros(MAX_X,MAX_Y); % use to visualize 17 | map = ones(MAX_X,MAX_Y); % use to calculate 18 | 19 | % Get the different environment setting depending on values: 1, 2, 3 20 | [start, target, obstacles] = Map(Environ); 21 | 22 | % create obstacles 23 | for i = 1: 1:size(obstacles,1) 24 | MAP(obstacles(i,1),obstacles(i,2)) = 1; 25 | map(obstacles(i,1),obstacles(i,2)) = 0; 26 | end 27 | 28 | map = map'; 29 | node = 250; % number of points in the PRM 30 | 31 | % visualize environmnet 32 | hold on 33 | imagesc((MAP')) 34 | colormap(flipud(gray)); 35 | axis([1 MAX_X 1 MAX_Y]) 36 | 37 | temp = [start;target]; % start and target as first points 38 | p1 = plot(temp(1,2), temp(1,1),'kh','MarkerFaceColor','g'); %start 39 | p2 = plot(temp(2,2), temp(2,1),'mh','MarkerFaceColor','m'); %target 40 | 41 | %% Random Sampling 42 | 43 | % get random nodes until total nodes is reach 44 | while length(temp)0 74 | [A, I] = min(X,[],1); 75 | n = X(I(4),:); % check smallest cost element 76 | X = [X(1:I(4)-1,:);X(I(4)+1:end,:)]; % delete element (currently) 77 | 78 | if n(1)==2 % check 79 | path_F = true; 80 | break; 81 | end 82 | 83 | % iterate through all adjacency from the node 84 | for mv=1:length(adjacency{n(1),1}) 85 | temp1 = adjacency{n(1),1}(mv); 86 | 87 | % if not already in p_index 88 | if length(p_index)==0 || length(find(p_index(:,1)==temp1))==0 89 | historic_c = n(2)+hist(temp(n(1),:),temp(temp1,:)); % historic cost 90 | heuristic_c = heu(temp(temp1,:),target); % heuristic cost 91 | total_c = historic_c+heuristic_c; % total cost 92 | 93 | add = true; % add if better cost 94 | if length(find(X(:,1)==temp1))>=1 95 | I = find(X(:,1)==temp1); 96 | if X(I,4)0 120 | path = [temp(p_index(prev,1),:);path]; 121 | prev = p_index(prev,5); 122 | end 123 | 124 | % visualize the path 125 | p5 = plot(path(:,2),path(:,1),'color','r'); 126 | %legend([p1 p2 p3(1) p4 p5], {'Start','Target', 'Nodes', 'Potental Paths', 'Path'}, 'Location', 'southeast') 127 | legend([p1 p2 p3(1) p5], {'Start','Target', 'Nodes', 'Path'}, 'Location', 'bestoutside') 128 | title(['PRM in Environment ', num2str(Environ)]) % title 129 | hold off 130 | 131 | %% Time and cost function (recalculated just in case) 132 | 133 | % time 134 | e = cputime-t; 135 | 136 | % cost function 137 | d = diff([path(:,2), path(:,1)]); 138 | total_length = sum(sqrt(sum(d.*d,2))); 139 | fprintf("Processing Time: %.2f sec \t Path Length: %.2f bits \n", e, total_length); 140 | %saveas(p5,'image1.png'); 141 | 142 | %% Cost function calculation 143 | 144 | % historic 145 | function h=hist(a,b) 146 | h = sqrt(sum((a-b).^2)); 147 | end 148 | 149 | % heuristic 150 | function h=heu(c,d) 151 | h = sqrt(sum((c-d).^2)); 152 | end -------------------------------------------------------------------------------- /PRM/pathCheck.m: -------------------------------------------------------------------------------- 1 | % check if path is valid 2 | 3 | function check=pathCheck(n,n_P,map) 4 | check=true; 5 | d=atan2(n_P(1)-n(1),n_P(2)-n(2)); 6 | 7 | for r=0:0.5:sqrt(sum((n-n_P).^2)) 8 | posCheck=n+r.*[sin(d) cos(d)]; 9 | 10 | % check if valid 11 | if ~(GoodP(ceil(posCheck),map) && GoodP(floor(posCheck),map) && GoodP([ceil(posCheck(1)) floor(posCheck(2))],map) && GoodP([floor(posCheck(1)) ceil(posCheck(2))],map)) 12 | check=false; 13 | break; 14 | end 15 | 16 | % check if new path is valid 17 | if ~GoodP(n_P,map), 18 | check=false; 19 | end 20 | end -------------------------------------------------------------------------------- /PSO/Map.m: -------------------------------------------------------------------------------- 1 | % Environment set up 2 | 3 | function [start, target, obstacles] = Map(value) 4 | if value == 1 5 | 6 | % environment 1 7 | start = [10,2]; 8 | target = [10,39]; 9 | obstacles = [1,7; 2,7; 3,7; 4,7; 5,7; 6,7; 7,7; 10 | 1,8; 2,8; 3,8; 4,8; 5,8; 6,8; 7,8; 11 | 14,7; 15,7; 16,7; 17,7; 18,7; 19,7; 20,7; 12 | 14,8; 15,8; 16,8; 17,8; 18,8; 19,8; 20,8; 13 | 6,15; 7,15; 8,15; 9,15; 10,15; 11,15; 12,15; 13,15; 14,15; 15,15; 14 | 6,16; 7,16; 8,16; 9,16; 10,16; 11,16; 12,16; 13,16; 14,16; 15,16; 15 | 3,23; 4,23; 5,23; 6,23; 7,23; 8,23; 16 | 3,24; 4,24; 5,24; 6,24; 7,24; 8,24; 17 | 13,23; 14,23; 15,23; 16,23; 17,23; 18,23; 18 | 13,24; 14,24; 15,24; 16,24; 17,24; 18,24; 19 | 7,31; 8,31; 9,31; 10,31; 11,31; 12,31; 13,31; 14,31; 20 | 7,32; 8,32; 9,32; 10,32; 11,32; 12,32; 13,32; 14,32;]; 21 | elseif value == 2 22 | 23 | % environment 2 24 | start = [2,2]; 25 | target = [19,39]; 26 | obstacles = [3,4; 4,4; 5,4; 6,4; 7,4; 8,4; 9,4; 10,4; 11,4; 12,4; 13,4; 3,5; 4,5; 5,5; 6,5; 7,5; 8,5; 9,5; 10,5; 11,5; 12,5; 13,5; 27 | 3,6; 4,6; 5,6; 6,6; 7,6; 8,6; 9,6; 10,6; 11,6; 12,6; 13,6; 3,7; 4,7; 5,7; 6,7; 7,7; 8,7; 9,7; 10,7; 11,7; 12,7; 13,7; 28 | 6,8; 7,8; 8,8; 9,8; 10,8; 11,8; 12,8; 13,8; 6,9; 7,9; 8,9; 9,9; 10,9; 11,9; 12,9; 13,9; 6,10; 7,10; 8,10; 9,10; 10,10; 11,10; 12,10; 13,10; 29 | 6,11; 7,11; 8,11; 9,11; 10,11; 11,11; 12,11; 13,11; 6,12; 7,12; 8,12; 9,12; 10,12; 11,12; 12,12; 13,12; 6,13; 7,13; 8,13; 9,13; 10,13; 11,13; 12,13; 13,13; 30 | 6,14; 7,14; 8,14; 9,14; 10,14; 11,14; 12,14; 13,14; 11,15; 12,15; 13,15; 11,16; 12,16; 13,16; 11,17; 12,17; 13,17; 11,18; 12,18; 13,18; 11,9; 12,9; 13,9; 31 | 11,19; 12,19; 13,19; 11,20; 12,20; 13,20; 11,21; 12,21; 13,21; 11,22; 12,22; 13,22; 14,22; 15,22; 16,22; 17,22; 18,22; 11,23; 12,23; 13,23; 14,23; 15,23; 16,23; 17,23; 18,23; 32 | 11,24; 12,24; 13,24; 14,24; 15,24; 16,24; 17,24; 18,24; 11,25; 12,25; 13,25; 14,25; 15,25; 16,25; 17,25; 18,25; 11,26; 12,26; 13,26; 14,26; 15,26; 16,26; 17,26; 18,26; 33 | 11,27; 12,27; 13,27; 14,27; 15,27; 16,27; 17,27; 18,27; 16,28; 17,28; 18,28; 16,29; 17,29; 18,29; 16,30; 17,30; 18,30; 16,31; 17,31; 18,31; 16,32; 17,32; 18,32; 34 | 11,34; 12,34; 13,34; 11,35; 12,35; 13,35; 11,36; 12,36; 13,36; 11,37; 12,37; 13,37; 11,38; 12,38; 13,38; 11,39; 12,39; 13,39; 11,40; 12,40; 13,40; 35 | 7,34; 8,34; 9,34; 10,34; 7,35; 8,35; 9,35; 10,35; 7,36; 8,36; 9,36; 10,36; 7,37; 8,37; 9,37; 10,37; 7,38; 8,38; 9,38; 10,38; 7,39; 8,39; 9,39; 10,39; 7,40; 8,40; 9,40; 10,40; 36 | 1,21; 2,21; 3,21; 4,21; 5,21; 6,21; 7,21; 1,22; 2,22; 3,22; 4,22; 5,22; 6,22; 7,22; 1,23; 2,23; 3,23; 4,23; 5,23; 6,23; 7,23; 1,24; 2,24; 3,24; 4,24; 5,24; 6,24; 7,24; 37 | 1,24; 2,24; 3,24; 4,24; 5,24; 6,24; 7,24; 1,25; 2,25; 3,25; 4,25; 5,25; 6,25; 7,25; 1,26; 2,26; 3,26; 4,26; 5,26; 6,26; 7,26; 1,27; 2,27; 3,27; 4,27; 5,27; 6,27; 7,27; 38 | 8,21; 8,22; 8,23; 8,24; 8,25; 8,26; 8,27; 7,20; 8,20; 7,19; 8,19; 39 | 16,4; 17,4; 18,4; 19,4; 20,4; 16,5; 17,5; 18,5; 19,5; 20,5; 16,6; 17,6; 18,6; 19,6; 20,6; 16,7; 17,7; 18,7; 19,7; 20,7; 16,8; 17,8; 18,8; 19,8; 20,8; 15,4; 15,5; 15,6; 15,7; 15,8;]; 40 | else 41 | 42 | % environment 3 43 | start = [2,39]; 44 | target = [19,39]; 45 | obstacles = [1,14; 2,14; 3,14; 4,14; 5,14; 6,14; 7,14; 8,14; 1,13; 2,13; 3,13; 4,13; 5,13; 6,13; 7,13; 8,13; 1,12; 2,12; 3,12; 4,12; 5,12; 6,12; 7,12; 8,12; 46 | 1,11; 2,11; 3,11; 4,11; 5,11; 6,11; 7,11; 8,11; 1,10; 2,10; 3,10; 4,10; 5,10; 6,10; 7,10; 8,10; 1,9; 2,9; 3,9; 4,9; 5,9; 6,9; 7,9; 8,9; 47 | 10,12; 11,12; 12,12; 13,12; 14,12; 15,12; 16,12; 10,13; 11,13; 12,13; 13,13; 14,13; 15,13; 16,13; 10,14; 11,14; 12,14; 13,14; 14,14; 15,14; 16,14; 48 | 10,15; 11,15; 12,15; 13,15; 14,15; 15,15; 16,15; 10,16; 11,16; 12,16; 13,16; 14,16; 15,16; 16,16; 10,17; 11,17; 12,17; 13,17; 14,17; 15,17; 16,17; 49 | 10,18; 11,18; 12,18; 13,18; 14,18; 15,18; 16,18; 10,19; 11,19; 12,19; 13,19; 14,19; 15,19; 16,19; 10,20; 11,20; 12,20; 13,20; 14,20; 15,20; 16,20; 50 | 15,21; 16,21; 15,22; 16,22; 15,23; 16,23; 15,24; 16,24; 15,26; 16,26; 15,25; 16,25; 15,27; 16,27; 19,25; 20,25; 19,26; 20,26; 19,27; 20,27; 19,28; 20,28; 19,29; 20,29; 51 | 11,1; 12,1; 13,1; 14,1; 11,2; 12,2; 13,2; 14,2; 11,3; 12,3; 13,3; 14,3; 11,4; 12,4; 13,4; 14,4; 11,5; 12,5; 13,5; 14,5; 11,6; 12,6; 13,6; 14,6; 52 | 1,15; 2,15; 3,15; 4,15; 5,15; 6,15; 7,15; 8,15; 1,16; 2,16; 3,16; 4,16; 5,16; 6,16; 7,16; 8,16; 1,17; 2,17; 3,17; 4,17; 5,17; 6,17; 7,17; 8,17; 53 | 1,18; 2,18; 3,18; 4,18; 5,18; 6,18; 7,18; 8,18; 1,19; 2,19; 3,19; 4,19; 1,20; 2,20; 3,20; 4,20; 1,22; 2,22; 3,22; 4,22; 1,21; 2,21; 3,21; 4,21; 54 | 1,22; 2,22; 3,22; 4,22; 1,23; 2,23; 3,23; 4,23; 1,24; 2,24; 3,24; 4,24; 1,25; 2,25; 3,25; 4,25; 1,26; 2,26; 3,26; 4,26; 1,27; 2,27; 3,27; 4,27; 55 | 1,28; 2,28; 3,28; 4,28; 1,29; 2,29; 3,29; 4,29; 1,30; 2,30; 3,30; 4,30; 1,31; 2,31; 3,31; 4,31; 1,32; 2,32; 3,32; 4,32; 1,33; 2,33; 3,33; 4,33; 56 | 1,34; 2,34; 3,34; 4,34; 1,35; 2,35; 3,35; 4,35; 3,36; 4,36; 3,37; 4,37; 7,39; 8,39; 9,39; 10,39; 11,39; 12,39; 7,38; 8,38; 9,38; 10,38; 11,38; 12,38; 57 | 7,37; 8,37; 9,37; 10,37; 11,37; 12,37; 7,36; 8,36; 9,36; 10,36; 11,36; 12,36; 7,35; 8,35; 9,35; 10,35; 11,35; 12,35; 7,34; 8,34; 9,34; 10,34; 11,34; 12,34; 58 | 7,33; 8,33; 9,33; 10,33; 11,33; 12,33; 7,32; 8,32; 9,32; 10,32; 11,32; 12,32; 7,31; 8,31; 9,31; 10,31; 11,31; 12,31; 7,30; 8,30; 9,30; 10,30; 11,30; 12,30; 59 | 7,29; 8,29; 9,29; 10,29; 11,29; 12,29; 7,28; 8,28; 9,28; 10,28; 11,28; 12,28; 15,28; 16,28; 15,29; 16,29; 15,30; 16,30; 7,40; 8,40; 9,40; 10,40; 11,40; 12,40; 60 | 9,28; 10,28; 8,28; 11,28; 8,27; 11,27; 11,26; 8,26; 9,27; 10,27; 9,26; 10,26;]; 61 | end 62 | -------------------------------------------------------------------------------- /PSO/PSO.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/greenbtong/Path_Planning_Algorithm/b6c64264b757e7d68634f2eb6bd431b436780235/PSO/PSO.mp4 -------------------------------------------------------------------------------- /PSO/PSO_2.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/greenbtong/Path_Planning_Algorithm/b6c64264b757e7d68634f2eb6bd431b436780235/PSO/PSO_2.mp4 -------------------------------------------------------------------------------- /PSO/PSO_3(2).mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/greenbtong/Path_Planning_Algorithm/b6c64264b757e7d68634f2eb6bd431b436780235/PSO/PSO_3(2).mp4 -------------------------------------------------------------------------------- /PSO/PSO_3.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/greenbtong/Path_Planning_Algorithm/b6c64264b757e7d68634f2eb6bd431b436780235/PSO/PSO_3.mp4 -------------------------------------------------------------------------------- /PSO/bestPath.m: -------------------------------------------------------------------------------- 1 | % best solution (best path) 2 | 3 | function gBest2=bestPath(gBest1,S) 4 | 5 | % path 6 | temp1 = linspace(0,1,numel([S.s(1) gBest1.x S.t(1)])); 7 | temp2 = linspace(0,1,100); 8 | xx = spline(temp1, [S.s(1) gBest1.x S.t(1)], temp2); 9 | yy = spline(temp1, [S.s(2) gBest1.y S.t(2)], temp2); 10 | 11 | % obstacle (check if path doesnt break the rule) 12 | wrong = 0; 13 | for k=1:numel(S.obs(1,:)) % Number of Obstacles 14 | d = sqrt((xx-S.obs(1,k)).^2+(yy-S.obs(2,k)).^2); 15 | m = max(1-d/S.robs(k),0); 16 | wrong = wrong + mean(m); 17 | end 18 | 19 | % gBest2 = (handling points x, handling points y, path x, path y, 20 | % distance, violation value, true or false if path is allowed) 21 | gBest2 = struct('X', [S.s(1) gBest1.x S.t(1)], 'Y', [S.s(2) gBest1.y S.t(2)], ... 22 | 'xx', xx, 'yy', yy, 'dis', sum(sqrt(diff(xx).^2 + diff(yy).^2)), ... 23 | 'vio', wrong, 'allowed', (wrong==0)); 24 | 25 | -------------------------------------------------------------------------------- /PSO/costFunc.m: -------------------------------------------------------------------------------- 1 | % cost function 2 | 3 | function [z, gBest]=costFunc(temp,S) 4 | 5 | % get the best solution (path) 6 | gBest = bestPath(temp,S); 7 | 8 | % get the cost (distance) 9 | z = gBest.dis*(1 + 100 * gBest.vio); 10 | 11 | -------------------------------------------------------------------------------- /PSO/main_pso.m: -------------------------------------------------------------------------------- 1 | % Particle Swarm Optimization (PSO) 2 | 3 | % clear 4 | clc; 5 | clear; 6 | close all; 7 | 8 | t = cputime; % run time 9 | %% Environments 10 | Environ = 2; 11 | MAX_X = 20; % x range 12 | MAX_Y = 40; % y range 13 | 14 | % Grid of Map 15 | MAP = zeros(MAX_X,MAX_Y); 16 | 17 | % Get the different environment setting depending on values: 1, 2, 3 18 | [start, target, obstacles] = Map(Environ); 19 | 20 | % create obstacles 21 | for i = 1: 1:size(obstacles,1) 22 | MAP(obstacles(i,1),obstacles(i,2)) = 1; 23 | end 24 | 25 | % create struct to for easier passing around 26 | % struct = [start, target, obstacles, radius of obstacles, min, max, number 27 | % of handle points, MAP] 28 | S = struct('s', start, 't', target, 'obs', obstacles', 'robs', .62*ones(1,size(obstacles,1)), ... 29 | 'min', [1 1], 'max', [MAX_X MAX_Y], 'n', 4, 'MAP', MAP); 30 | 31 | %% PSO Parameters 32 | 33 | cost_Func = @(x) costFunc(x,S); % cost function 34 | vSize = [1 S.n]; % decision variables 35 | iter = 3; % iterations 36 | pop = 200; % Swarm Size 37 | weight = 1; % inertia weight 38 | w_Damp = 1; % damping ratio (inertia weight) 39 | c1 = 2; % personal learning coefficient 40 | c2 = 2; % global learning coefficient 41 | 42 | % velocity 43 | alpha=0.1; 44 | max_Vel = [alpha*(S.max(1)-S.min(1)) alpha*(S.max(2)-S.min(2))]; % maximum velocity 45 | min_Vel = [-alpha*(S.max(1)-S.min(1)) -alpha*(S.max(2)-S.min(2))]; % minimum velocity 46 | 47 | %% Initialization 48 | 49 | % global best 50 | gBest.cost = inf; 51 | 52 | % Pso struct = [position, velocity, cost, path, best = {best position, best 53 | % cost, best path}] 54 | temp_struct = struct('pos', [], 'vel', [], 'cost', [], 'path', [], 'best', []); 55 | temp_struct.best = struct('pos', [], 'cost', [], 'path', []); 56 | pso = repmat(temp_struct,pop,1); 57 | 58 | % Get the first values (initialize loop) 59 | for i=1:pop 60 | 61 | % position of starting handling points 62 | if i > 1 63 | % random the points 64 | pso(i).pos.x = unifrnd(S.min(1),S.max(1),1,S.n); 65 | pso(i).pos.y = unifrnd(S.min(2),S.max(2),1,S.n); 66 | else 67 | % line from start to target 68 | xx = linspace(S.s(1), S.t(1), S.n+2); 69 | yy = linspace(S.s(2), S.t(2), S.n+2); 70 | pso(i).pos.x = xx(2:end-1); 71 | pso(i).pos.y = yy(2:end-1); 72 | end 73 | 74 | % velocity 75 | pso(i).vel.x = zeros(vSize); 76 | pso(i).vel.y = zeros(vSize); 77 | 78 | % cost value 79 | [pso(i).cost, pso(i).path] = cost_Func(pso(i).pos); 80 | 81 | % update personal best 82 | pso(i).best.pos = pso(i).pos; 83 | pso(i).best.cost = pso(i).cost; 84 | pso(i).best.path = pso(i).path; 85 | 86 | % update global best 87 | if pso(i).best.costS.max(1)); 128 | pso(i).vel.x(outside) = -pso(i).vel.x(outside); 129 | outside = (pso(i).pos.yS.max(2)); 130 | pso(i).vel.y(outside) = -pso(i).vel.y(outside); 131 | 132 | % update position boundaries 133 | pso(i).pos.x = max(pso(i).pos.x,S.min(1)); 134 | pso(i).pos.x = min(pso(i).pos.x,S.max(1)); 135 | pso(i).pos.y = max(pso(i).pos.y,S.min(2)); 136 | pso(i).pos.y = min(pso(i).pos.y,S.max(2)); 137 | 138 | % calculate cost 139 | [pso(i).cost, pso(i).path] = cost_Func(pso(i).pos); 140 | 141 | % update personal best 142 | if pso(i).cost1 161 | 162 | % if the cost function doesnt change per iteration 163 | if best_Cost(it) == best_Cost(it-1) && gBest.path.vio == 0 164 | temp = temp +1; 165 | else % if the path violates the rules reset the early break 166 | temp = 0; 167 | end 168 | 169 | % if the PSO is lost or path goes stright through obstacles 170 | if best_Cost(it) < 26 || gBest.path.vio ~= 0 || best_Cost(it) > 70 171 | temp1 = temp1+1; 172 | else 173 | temp1 = 0; 174 | end 175 | end 176 | 177 | % if no change in cost and rule breaking end iteration 178 | if temp > 25 179 | break; 180 | end 181 | 182 | % if rules are broken for 20 iterations re-random the handler points 183 | if temp1 > 20 184 | temp1 = 0; 185 | for i=1:pop 186 | if i > 1 187 | % random the points 188 | pso(i).pos.x = unifrnd(S.min(1),S.max(1),1,S.n); 189 | pso(i).pos.y = unifrnd(S.min(2),S.max(2),1,S.n); 190 | else 191 | % line from start to target 192 | xx = linspace(S.s(1), S.t(1), S.n+2); 193 | yy = linspace(S.s(2), S.t(2), S.n+2); 194 | pso(i).pos.x = xx(2:end-1); 195 | pso(i).pos.y = yy(2:end-1); 196 | end 197 | 198 | % reset everything with these new random points 199 | % cost function record is still recorded 200 | pso(i).vel.x=zeros(vSize); 201 | pso(i).vel.y=zeros(vSize); 202 | [pso(i).cost, pso(i).path] = cost_Func(pso(i).pos); 203 | pso(i).best.pos = pso(i).pos; 204 | pso(i).best.cost = pso(i).cost; 205 | pso(i).best.path = pso(i).path; 206 | 207 | % global best 208 | gBest = pso(i).best; 209 | 210 | end 211 | end 212 | 213 | % inertia weight damping 214 | weight = weight*w_Damp; 215 | 216 | %% Show Iteration Information 217 | if gBest.path.allowed 218 | passed = '||'; 219 | else 220 | passed = ['|| Violation = ' num2str(gBest.path.vio)]; 221 | end 222 | fprintf('Iteration: %d || Current Best Cost = %f %s \n', it, best_Cost(it), passed); 223 | 224 | %%visualize the iterations 225 | hh = figure(1); 226 | plotIt(gBest.path,S); 227 | legend('Start', 'Target', 'Handle Points', 'Path', 'Location', 'southeast') 228 | title(['PSO in Environment ', num2str(Environ)]) % title 229 | %FF = getframe(hh); 230 | %writeVideo(output, FF); 231 | pause(0.01); 232 | 233 | end 234 | %close(output); % save video 235 | 236 | %% Final plot 237 | clf(figure(1)) 238 | figure(1) 239 | plotIt(gBest.path,S); % plot 240 | legend('Start', 'Target', 'Handle Points', 'Path', 'Location', 'best') % legend 241 | title(['PSO in Environment ', num2str(Environ)]) % title 242 | %% Time and cost function 243 | 244 | % %cost function plot 245 | % figure; 246 | % plot(best_Cost,'LineWidth',2); 247 | % xlabel('Iteration'); 248 | % ylabel('Best Cost'); 249 | % grid on; 250 | 251 | % time 252 | e = cputime-t; 253 | 254 | % cost function 255 | d = diff([gBest.path.xx(:) gBest.path.yy(:)]); 256 | total_length = sum(sqrt(sum(d.*d,2))); 257 | fprintf("Processing Time: %.2f sec \t Path Length: %.2f bits \t Iteration: %d \n", e, total_length, it-1); 258 | 259 | %% Functions 260 | 261 | %plot functions 262 | function plotIt(gBest,S) 263 | hold on 264 | imagesc((S.MAP')) 265 | colormap(flipud(gray)); 266 | axis([1 20 1 40]) 267 | plot(S.s(1), S.s(2), 'mh','MarkerSize',10,'MarkerFaceColor','m'); %start 268 | plot(S.t(1), S.t(2), 'kh','MarkerSize',10,'MarkerFaceColor','g'); %finish 269 | plot(gBest.X, gBest.Y,'bo'); % handling points 270 | plot(gBest.xx, gBest.yy,'r'); % path 271 | hold off; 272 | end 273 | --------------------------------------------------------------------------------