Navigation System featuring Dijkstra Algorithm

    Diese Seite verwendet Cookies. Durch die Nutzung unserer Seite erklären Sie sich damit einverstanden, dass wir Cookies setzen. Weitere Informationen

    • Navigation System featuring Dijkstra Algorithm

      Hey Folks,

      recently i was playing a bit with setting up the dijkstra algorithm in arma.
      With the following code you will be able to navigate from your position to a marker on the map.

      Have Fun!

      Video Tutorial:

      Tutorial on YouTube


      Parts of the Algorithm:

      • Create Road Map, Gets all connected roads on the map and save them in an array
      • Run the Dijkstra Algorithm, Defines the graph for the RoadMap with recent starting point
      • Finding the Path, Gets the position of the destination marker on the map and finds the shortest path
      • Create Local Markers, Create markers for every path node
      • Delete Markers on Passing, When passing the navigation markers they will be deleted from the gui


      C-Quellcode: Create Road Map

      1. // Create Road Map
      2. _roadMap = [];
      3. _nextRoads = [];
      4. _finishedRoads = [];
      5. _startRoads = player nearRoads 10;
      6. _firstRoad = _startRoads select 0;
      7. _nextRoads pushBack _firstRoad;
      8. _iterationCounter = 0;
      9. while {_iterationCounter < 1000} do
      10. {
      11. _nextRoad = _nextRoads deleteAt 0;
      12. _connectedRoads = roadsConnectedTo _nextRoad;
      13. {
      14. _distance = _x distance _nextRoad;
      15. _roadMap pushBack [_nextRoad, _x, _distance];
      16. if((_finishedRoads find _x) == -1) then
      17. {
      18. _nextRoads pushBack _x;
      19. };
      20. } foreach _connectedRoads;
      21. _finishedRoads pushBack _nextRoad;
      22. _iterationCounter = _iterationCounter +1;
      23. };
      Alles anzeigen

      C-Quellcode: Run the Dijkstra Algorithm

      1. // Run the Dijkstra Algorithm
      2. _startRoads = player nearRoads 10;
      3. _startRoad = _startRoads select 0;
      4. // Init Distances
      5. _distanceArray = [];
      6. _workQueue = [];
      7. _distanceArray pushBack [_startRoad, 0, null];
      8. _visitedRoads = [];
      9. _visitedRoads pushBack _startRoad;
      10. _workQueue pushBack [0, _startRoad];
      11. while { count _workQueue > 0} do
      12. {
      13. _workItem = _workQueue deleteAt 0;
      14. _actualRoad = _workItem select 1;
      15. // Get the connected Roads out the RoadMap
      16. {
      17. _road = _x select 0;
      18. _connRoad = _x select 1;
      19. _connDistance = _x select 2;
      20. if ((_road == _actualRoad) && !(_connRoad in _visitedRoads)) then // Find Connected Roads, not yet visited
      21. {
      22. _visitedRoads pushBack _connRoad; // Save connected road as visited
      23. // Calculate Distance between the Roads
      24. _roadDistance = _connDistance;
      25. // Search for Parent in Distance Array and get his Distance
      26. {
      27. _parentRoad = _x select 0;
      28. _parentDistance = _x select 1;
      29. _parentParent = _x select 2;
      30. if(_parentRoad == _road) then
      31. {
      32. _roadDistance = _roadDistance + _parentDistance; // Add distance of parent to new distance
      33. };
      34. } foreach _distanceArray;
      35. // Save new Road in Distance Array
      36. _distanceArray pushBack [_connRoad, _roadDistance, _road]; // Add connected road to Distance Array
      37. _workQueue pushBack [_roadDistance, _connRoad]; // Add connected road to queue
      38. };
      39. } forEach _roadMap;
      40. if(count _workQueue > 0) then {_workQueue sort true;};
      41. };
      Alles anzeigen

      C-Quellcode: Finding the Path

      1. // Now Finding the Shortest Path to Destination
      2. _destinationPath = [];
      3. _destinationLength = 0;
      4. _startNode = _distanceArray select 0;
      5. // Get the Destination Node from Marker
      6. _destinationMarker = allMapMarkers select ((count allMapMarkers) -1);
      7. _nearestDestinationRoad = (getMarkerPos(_destinationMarker) nearRoads 10) select 0;
      8. _selectedNode = [];
      9. // Find DestinationRoad in Array
      10. {
      11. _nodeRoadx = _x select 0;
      12. if(_nodeRoadx == _nearestDestinationRoad) then
      13. {
      14. _selectedNode = _x;
      15. }
      16. } foreach _distanceArray;
      17. // Get the Distance to Destination
      18. _destinationLength = _selectedNode select 1;
      19. diag_log format ["StartNode: %1, SelectNode: %2, DestinationLength: %3", _startNode, _selectedNode, _destinationLength];
      20. // Get the Path to Destination
      21. while{!(_selectedNode isEqualTo _startNode)} do
      22. {
      23. _nodeRoad = _selectedNode select 0; // Select the Road in the Node
      24. _destinationPath pushBack _nodeRoad; // Save the Road in the Path
      25. _nodeParent = _selectedNode select 2; // Node Parent to find
      26. // Find Node Parent in Distance Array
      27. {
      28. _nodeRoadx = _x select 0;
      29. if(_nodeRoadx == _nodeParent) then
      30. {
      31. _selectedNode = _x;
      32. }
      33. } foreach _distanceArray;
      34. };
      35. _destinationPath pushBack (_startNode select 0);
      Alles anzeigen

      C-Quellcode: Create Local Markers

      1. // Create Local Markers to navigate to the path
      2. {
      3. _streetMarker = "VR_3DSelector_01_exit_F" createVehicleLocal getPos(_x);
      4. // _mapMarker = createMarkerLocal ["markername",[getPos(_x select 0) select 0,getPos(_x select 0) select 1]];
      5. // _mapMarker setMarkerShapeLocal "ICON";
      6. // _mapMarker setMarkerTypeLocal "DOT";
      7. } foreach _destinationPath;

      C-Quellcode: Delete Markers on Passing

      1. // Delete Local Markers when passing them
      2. [] spawn {
      3. while{true} do
      4. {
      5. _nearestObjects = nearestObjects [player, [], 10];
      6. {
      7. if(typeOf _x == "VR_3DSelector_01_exit_F") then
      8. {
      9. deleteVehicle _x;
      10. }
      11. } foreach _nearestObjects;
      12. sleep 0.1;
      13. }
      14. }
      Alles anzeigen
    • Hey,

      I have a problem on Tanoa. The first part with the 1000 _iterationCounter loads like 20seconds.
      And dont work for me because it hangs in the

      Quellcode

      1. while { count _workQueue > 0} do
      2. {
      3. }
      part.

      It runs over and over again but dont finish, because _workQueue gets never under 0.
      I tried it with differtent way lengths. No chance. Still loops in workQueue.

      Any idea?
    • Spiderswine schrieb:

      So here we go,

      sry for the late answer, my studies take quite some time at the moment, but finally i was able to upload my recent code to github, here's the link:

      github.com/Spiderswine/Arma3Navigation

      Feel free to write me if anything does not work so i can take a look.


      Cheers,

      Luke
      I updated @Spiderswine files with private variables and some other performant options etc. in addition I put it all into one file you only need to run with some params.
      Here it is, you need to give start pos/object & destination pos/object as params.
      It returns automaticly the destination path as array of road objects, so you can use it for everything you need.

      C-Quellcode: createRoadMap

      1. /*
      2. File: fn_createRoadMap.sqf
      3. Author: Spiderswine
      4. Edit: nflug
      5. */
      6. params [
      7. "_destinationObject",
      8. "_object"
      9. ];
      10. private _roads = [];
      11. private _finishedCrossways = [];
      12. private _visitedCrossways = [];
      13. private _islandRoads = [];
      14. private _crosswayMap = [];
      15. private _firstCrossway = objNull;
      16. private _nearPlayerRoads = _object nearRoads 1000;
      17. private _startRoad = _nearPlayerRoads select 0;
      18. private _finishedRoads = [];
      19. private _roadQueue = [];
      20. _roadQueue pushBack _startRoad;
      21. while {isNull _firstCrossway} do {
      22. private _handleRoad = _roadQueue deleteAt 0;
      23. private _connectedRoads = roadsConnectedTo _handleRoad;
      24. if(count _connectedRoads > 2) then {_firstCrossway = _handleRoad;};
      25. {
      26. private _connectedRoad = _x;
      27. if((_finishedRoads find _connectedRoad) isEqualTo -1) then {
      28. _roadQueue pushBack _connectedRoad;
      29. };
      30. } forEach _connectedRoads;
      31. _finishedRoads pushBack _handleRoad;
      32. };
      33. private _crosswayQueue = [];
      34. _finishedRoads = [];
      35. _crosswayQueue pushBack _firstCrossway;
      36. _visitedCrossways pushBack _firstCrossway;
      37. while {count _crosswayQueue > 0} do {
      38. private _handleCrossway = _crosswayQueue deleteAt 0;
      39. private _connectedCrosswayRoads = roadsConnectedTo _handleCrossway;
      40. {
      41. private _handleNextCrossway = _x;
      42. private _roads = [];
      43. private _finishedRoads = [];
      44. private _crossways = [];
      45. private _roadQueue = [];
      46. private _roadLength = 0;
      47. private _pos = _connectedCrosswayRoads find _handleNextCrossway;
      48. _finishedRoads append _connectedCrosswayRoads;
      49. _finishedRoads deleteAt _pos;
      50. _roadQueue pushBack _handleCrossway;
      51. while {count _roadQueue > 0} do {
      52. private _handleRoad = _roadQueue deleteAt 0;
      53. private _connectedRoads = roadsConnectedTo _handleRoad;
      54. if(count _connectedRoads > 2) then {_crossways pushBack _handleRoad;};
      55. if(count _crossways < 2) then {
      56. {
      57. private _connectedRoad = _x;
      58. if((_finishedRoads find _connectedRoad) isEqualTo -1) then {
      59. private _distance = _connectedRoad distance _handleRoad;
      60. private _roadLength = _roadLength + _distance;
      61. _roads pushBack [_handleRoad, _connectedRoad, _distance];
      62. _roadQueue pushBack _connectedRoad;
      63. };
      64. } forEach _connectedRoads;
      65. };
      66. _finishedRoads pushBack _handleRoad;
      67. };
      68. if(count _crossways > 1) then {_crosswayMap pushBack [_crossways select 0, _crossways select 1, _roadLength];};
      69. _islandRoads pushBack _roads;
      70. if((_visitedCrossways find (_crossways select 1)) isEqualTo -1) then {
      71. _crosswayQueue pushBack (_crossways select 1);
      72. _visitedCrossways pushBack (_crossways select 1);
      73. };
      74. } forEach _connectedCrosswayRoads;
      75. };
      76. private _nearestDestinationRoad = (_destinationObject nearRoads 1000) select 0;
      77. private _destinationCrossway = ObjNull;
      78. _finishedRoads = [];
      79. _roadQueue = [];
      80. _roadQueue pushBack _nearestDestinationRoad;
      81. while {isNull _destinationCrossway} do {
      82. private _handleRoad = _roadQueue deleteAt 0;
      83. private _connectedRoads = roadsConnectedTo _handleRoad;
      84. if(count _connectedRoads > 2) then {_destinationCrossway = _handleRoad;};
      85. {
      86. private _connectedRoad = _x;
      87. if((_finishedRoads find _connectedRoad) isEqualTo -1) then {
      88. _roadQueue pushBack _connectedRoad;
      89. };
      90. } forEach _connectedRoads;
      91. _finishedRoads pushBack _handleRoad;
      92. };
      93. private _distanceArray = [];
      94. private _workQueue = [];
      95. private _finishedCrossways = [];
      96. private _iterationCounter = 0;
      97. private _targetReached = false;
      98. _distanceArray pushBack [_firstCrossway, 0, objNull];
      99. _finishedCrossways pushBack _firstCrossway;
      100. _workQueue pushBack [0, _firstCrossway];
      101. while {count _workQueue > 0 && _iterationCounter < 10000 && _targetReached isEqualTo false} do {
      102. private _workItem = _workQueue deleteAt 0;
      103. private _actualCrossway = _workItem select 1;
      104. if(_actualCrossway isEqualTo _destinationCrossway) then {_targetReached = true};
      105. private _connCrossways = _crosswayMap select {(_x select 0) isEqualTo _actualCrossway};
      106. {
      107. _x params [
      108. "_crossway",
      109. "_connCrossway",
      110. "_connectionWeight"
      111. ];
      112. private _parent = _distanceArray select {(_x select 0 IsEqualTo _crossway)} select 0;
      113. private _parentDistance = _parent select 1;
      114. private _heuretic = 0;
      115. private _crosswayDistance = _connectionWeight + _parentDistance + _heuretic;
      116. private _posWorkQueue = _workQueue findIf {(_x select 1) isEqualTo _connCrossway};
      117. if((_posWorkQueue isEqualTo -1) && (!(_connCrossway in _finishedCrossways))) then {
      118. _workQueue pushBack [_crosswayDistance, _connCrossway];
      119. };
      120. private _posDistArray = _distanceArray findIf {(_x select 0) isEqualTo _connCrossway};
      121. if (_posDistArray != -1) then{
      122. private _oldDistance = (_distanceArray select _posDistArray) select 1;
      123. if(_oldDistance > _crosswayDistance) then {
      124. _distanceArray set [_posDistArray, [_connCrossway, _crosswayDistance, _crossway]];
      125. };
      126. } else {
      127. _distanceArray pushBack [_connCrossway, _crosswayDistance, _crossway];
      128. };
      129. _iterationCounter = _iterationCounter +1;
      130. } forEach _connCrossways;
      131. _finishedCrossways pushBack _actualCrossway;
      132. if(count _workQueue > 0) then {_workQueue sort true;};
      133. };
      134. private _destinationPath = [];
      135. private _destinationLength = 0;
      136. private _startNode = _distanceArray select 0;
      137. private _selectedNode = _distanceArray select {_x select 0 isEqualTo _destinationCrossway} select 0;
      138. _destinationLength = _selectedNode select 1;
      139. while {!(_selectedNode isEqualTo _startNode)} do {
      140. private _nodeRoad = _selectedNode select 0;
      141. private _nodeParent = _selectedNode select 2;
      142. _destinationPath pushBack _nodeRoad;
      143. _selectedNode = _distanceArray select {_x select 0 isEqualTo _nodeParent} select 0;
      144. };
      145. _destinationPath pushBack (_startNode select 0);
      146. reverse _destinationPath;
      147. private _allRoads = [];
      148. private _counter = 0;
      149. while {_counter < (count _destinationPath -1)} do {
      150. private _startRoad = _destinationPath select _counter;
      151. private _endRoad = _destinationPath select (_counter + 1);
      152. private _entryPosition = _islandRoads findIf {((_x select 0) select 0) isEqualTo _startRoad && (_x select (count _x -1)) select 1 isEqualTo _endRoad};
      153. private _entryRoads = _islandRoads select _entryPosition;
      154. {
      155. _allRoads pushBack (_x select 0);
      156. } forEach _entryRoads;
      157. _counter = _counter + 1;
      158. };
      159. _allRoads;
      Alles anzeigen
    • A little Update.

      If you really want to find the nearest road don't use

      Quellcode

      1. (object/pos nearRoads 1000) select 0;

      Use this code to get the nearest road because Arma3 does handle it like in that picture for the first road.
      On the picture the nearRoads command was used for the middle position of Feruz Abad with a 50meters circle.



      Use this code instead

      Quellcode

      1. private _firstRoad = objNull;
      2. private _meterCount = 20;
      3. while {isNull _firstRoad} do {
      4. if(isNil {(_destinationObject nearRoads _meterCount) select 0;}) then {
      5. _meterCount = _meterCount + 5;
      6. } else {
      7. _firstRoad = (_destinationObject nearRoads _meterCount) select 0;
      8. };
      9. };