MotionPlanning/motion_planning/testDijsktra.m

22 lines
1.7 KiB
Matlab

%{
points =
Columns 1 through 12:
-28.9550 -19.3630 -91.9319 -174.3179 113.6085 9.7763 -11.6645 -1.4659 47.0429 110.2832 65.9202 151.0450
104.4775 -103.5474 110.5317 -125.3647 -107.9741 -113.7357 164.8849 -95.2513 129.1750 -145.0642 -86.2545 104.4775
2.0000 1.3435 0.8803 -1.4950 0.1942 1.7297 1.0660 1.8824 0.3651 0.1280 1.7537 -2.0000
0 -1.5026 -1.6799 0.6708 1.9308 -0.6309 0.0462 -1.0443 1.5297 1.3055 1.4785 0
%}
visGraphTest = [ NaN 1208.2459 NaN NaN 255.8516 221.6238 62.8332 201.6116 NaN;
1208.2459 NaN 226.0445 156.4833 133.0452 30.8691 268.5426 19.7264 242.0113;
NaN 226.0445 NaN NaN NaN 246.2527 NaN 224.7904 140.2197;
NaN 156.4833 NaN NaN 288.4511 NaN 332.7176 NaN NaN;
255.8516 133.0452 NaN 288.4511 NaN NaN 300.2422 NaN 246.3142;
221.6238 30.8691 246.2527 NaN NaN NaN 279.4443 NaN 245.7527;
62.8332 268.5426 NaN 332.7176 300.2422 279.4443 NaN NaN NaN;
201.6116 19.7264 224.7904 NaN NaN NaN NaN NaN 1229.6089;
NaN 242.0113 140.2197 NaN 246.3142 245.7527 NaN 1229.6089 NaN];
[distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(7, visGraphTest)