shortest path [RELEASE] Sarogahtyps Simple Shortest Paths - SSSP V-2.0

[sarogahtyp 1109]

Sarogahtyps Simple Shortest Paths - SSSP V-1.0

the simplest way to find your path 😉

 

Actually there is the Dijkstra algorithm and the A* algorithm available.

Both are able to get a shortest path through a "2D"-map and A* is always faster than Dijkstra on this. How much faster A* is depends on the size of the given situation.

As more nodes and connections the given situation has as more A*'si advantage comes in play.

 

Dijkstra's advantage is that it should find the true shortest path always while A* just finds one short Path.

 

Also Dijkstra is much better able to solve the shortest path problem in a house with multiple floors (3D-problem).

A* can solve this as well but it's often slower there and finds much longer shortest paths in that situation.

 

A* will solve this through a multiple floor handler which is currently not implemented for it.

sqf implementation of Dijkstra algorithm:

Spoiler

/*  
	Author: Sarogahtyp
	Title: SSSP(D) - Sarogahtyps Simple Shortest Path - Dijkstra algorithm
                                                          https://en.wikipedia.org/wiki/Dijkstra

	Description: Dijkstra-algorithm - sqf implementation
                     This algorithm is much more time consuming than A* algorithm if only 1 floor is necessary
                     if multiple floors are needed then it has to be tested if A* (multiple floor solution) is faster or not

	Arguments:
        if you use alternative format at 3. then you have to use alternative format at 4 as well and vice versa
        if you have precalculated distances (maybe at mission start) for 4. then alternative format is faster
         
1.	number - ID of start node

2.  number - ID of end node

3.  array - nodes list - format: [ [ID1, positionASL], [ID2, positionASL], ..., [IDn, positionASL] ]
               ID1-IDn - numbers, ID of the specific node 
			   ID range has to be 0 to n and each ID between 0 and n has to have a node (no leaks)
			   ID may provided unsorted cause they get sorted anyways
		    positionASL - position above sea level

     format(alternative, faster): [ ID1, ID2, ..., IDn ]

4.  array - connections list - format: [[fromID, toID], [fromID, toID], ..., [fromID, toID]]
            fromID - number, connections start node ID
            toID - number, connections end node ID

         format (alternative, faster): [[distance, fromID, toID], [distance, fromID, toID], ..., [distance, fromID, toID]]
                                       distance can be any distance format you prefer (distance, distanceSqr, distance2D may be worse with multiple floors)

   returned nodes array (3.) - array of nodeIDs in order to pass from first (start node) to last (end node) element or
					           boolean false - indicating that input data was not valid
                               boolean true - indicating that there is no possible solution
*/

private _correctList = params [ ["_startNode", objNull, [0]],  // numbers allowed only
         ["_endNode", objNull, [0]],    // numbers allowed only
         ["_allNodes", objNull, [ [] ]], // arrays allowed only
         ["_allConnections", objNull, [ [] ]], // arrays allowed only
		  "_oneway" ];

// check for valid parameter data types
if (!_correctList) exitWith {_allNodes = false; false};

//check if first element of _allNodes is number or array and if it is array then with exact 2 elements
private _correctNodes = _allNodes params [["_firstElementNodes", objNull, [0, []], [2]]];

//check if first element of _allConnections is an array and if it has 2 or 3 elements
private _correctConnections = _allConnections params [["_firstElementConnections", objNull, [[]], [2, 3]]];

// exit if at least one of both first elements has wrong format
if (!_correctNodes || !_correctConnections) exitWith {_allNodes = false; false};
// determine arguments 3. and 4. format
private _alternativeFormat = count _firstElementConnections isEqualTo 3;

private _solution = [];
private _sqrt2 = 1.41421;

//set maximum possible squared distance
private _maxDist = (worldSize * _sqrt2)^2;

//ensures that ID is the same as the array index of each element 
_allNodes sort true;
 
if (!_alternativeFormat) then
{
 // calculate distances and insert as first element of each connection
 _allConnections = _allConnections apply 
 {
  _fromID = _x select 0;
  _toID = _x select 1;
  _fromPos = (_allNodes select _fromID) select 1;
  _toPos = (_allNodes select _toID) select 1;
  _dist = _fromPos distanceSqr _toPos;
  [_dist, _fromID, _toID]
 };
};

// insert distance to start value, predecessor node ID and already viewed state for each node.
// delete positions as not needed anymore

{
 _nodeID = if(typeName _x isEqualTo "ARRAY") then {_x select 0} else {_x};
 _dist = if(_nodeID isEqualTo _startNode) then {0} else {_maxDist};
 _allNodes set [_forEachIndex, [_dist, _maxDist, false, _nodeID]];
} forEach _allNodes;

_solutionFound = false;

// define a name for the script scope to be able to break all loops
scopeName "main";

while {true} do
{
 // select node with shortest distance value (_allNodes is sortet ascending by distance)
 private _shortest = _maxDist;
 private _currentNode= [];

 {
  if (!(_x select 2) && {(_x select 0) < _shortest}) then 
  {
   _shortest = _x select 0;
   _currentNode = _x;
  };
 } count _allNodes;
 
 //if there is no unviewed node then break all scopes - no solution possible
 if (_currentNode isEqualTo []) then {breakTo "main"};
 
 _currID = _currentNode select 3;
 
 // change viewed state of current node to true
 _currentNode set [2, true];

 //get all connections to all neighbors of current node
 _neighbConns = _allConnections select
 {
  (_x select 1 isEqualTo _currID) || (_x select 2 isEqualTo _currID)
 };

 //get all not viewed neighbors of current node
 _neighbNodes = _allNodes select
 {
  private _neighbID = _x select 3;
  (!(_x select 2) && (_neighbConns findIf {(_x select 1 isEqualTo _neighbID) || (_x select 2 isEqualTo _neighbID)} > -1))
 };
 
 //process all neighbor nodes of current node
 {
  private _neighbID = _x select 3;
  
  // find the index of the connection between current node and this neighbor node
  // get distance value of the connection
   
  private _thisDist = (_currentNode select 0) + ((_neighbConns select (_neighbConns findIf {_neighbID isEqualTo (_x select 1) || _neighbID isEqualTo (_x select 2)})) select 0);
  
  // if we found a shorter distance then set current node as predecessor of this neighbor node and update the found distance for it
  if ( (_x select 0) > _thisDist ) then
  {
   _x set [0, _thisDist];
   _x set [1, _currID];
  
   //check if this neighbor is the end node
   if (_neighbID isEqualTo _endNode) then
   {
    _solutionFound = true;

	breakTo "main";    //breaks all scopes except main scope
   };
  };
 } count _neighbNodes;
};

//exit if there is no solution for the given nodes/connections set to get from start node to end node
if (!_solutionFound) exitWith {_allNodes = true; true};

{
 _allNodes set [_forEachIndex, [(_x select 3), (_x select 1)]];
} forEach _allNodes;

_allNodes sort true;

// build solution path array from end node to start node
private _currNode = _allNodes select _endNode;

while {!((_currNode select 0) isEqualTo _startNode)} do
{
 //current nodes ID is part of solution
 _solution pushBack (_currNode select 0);
 
 //next current node is predecessor of actual current node
 _currNode = _allNodes select (_currNode select 1);
};

_solution pushBack _startNode;

// reverse it to get start point first and end point last
reverse _solution;

_allNodes resize (count _solution);

{
 _allNodes set [_forEachIndex, _x];
} forEach _solution;

_solution

 

 

Usage:

 

First you should read the script header. The script supports 2 different formats of node/connection lists. the first syntax of parameter 3. and 4. is needed if you don't like to precalculate the distances/weights for all connections. In this case you will deliver the node positions as well and the script calculates the distances itself.

The alternative syntax for parameter 3. and 4. has no positions but those distances instead.

 

I recommend to implement the script to armas function library by registering it via description.ext file. But it will work with or without  a manually done compiling as well.

 

The following lines executes the script twice and writes the solution to .rpt-file via diag_log command. One time the precompiled script is spawned the second time it gets called (works only if environment is scheduled).

Yes, you can use execVM as well. It has the same usage as the spawned function:

 

Spoiler

private _nodeThings = [3,1,0,2];
private _nodeThings2 = [3,1,0,2];

private _index = [0, 3, _nodeThings, [[10, 0, 1], [9,0,2], [15, 1, 3], [5, 2, 3]]] spawn saroSSSP_fnc_shortPathDijkstra;

waitUntil {scriptDone _index};

// DEBUG
_notext = " ";
_text = "Nodes after spawn";
diag_Log _text;
diag_log _nodeThings;
diag_log _notext;
// DEBUG

_solution = [0, 3, _nodeThings2, [[10, 0, 1], [9,0,2], [15, 1, 3], [5, 2, 3]]] call saroSSSP_fnc_shortPathDijkstra;

// DEBUG
_notext = " ";
_text = "Nodes after call";
diag_Log _text;
diag_log _solution;
diag_log _notext;
// DEBUG

 

 

 

The output for the situation shown in picture above is:

 1:56:25 "Nodes after spawn"
 1:56:25 [0,2,3]
 1:56:25 " "
 1:56:25 "Nodes after call"
 1:56:25 [0,2,3]
 1:56:25 " "

Yeah, correct solution ... it works 😄

 

 

sqf implementation of A*-algorithm

Spoiler

 /*  
	Author: Sarogahtyp
	Title: SSSP(A) - Sarogahtyps Simple Shortest Path - A*-algorithm
                                                          https://en.wikipedia.org/wiki/A*_search_algorithm

	Description: A*-algorithm - sqf implementation
                     This algorithm is much faster than -Dijkstra algorithm but currently support not more than one floor/layer

	Arguments: Precalculated distances are not supportet here as A* doesn't need more than a few distances. 
	           It would be just senseless to hold all probably needed distances in memory.
         
1.	number - ID of start node

2.  number - ID of end node

3.  array - nodes list - format: [ [ID1, positionASL], [ID2, positionASL], ..., [IDn, positionASL] ]
               ID1-IDn - numbers, ID of the specific node 
			   ID range has to be 0 to n and each ID between 0 and n has to have a node (no leaks)
			   ID may provided unsorted cause they get sorted anyways
		    positionASL - position above sea level

4.  array - connections list - format: [[fromID, toID], [fromID, toID], ..., [fromID, toID]]
            fromID - number, connections start node ID
            toID - number, connections end node ID

   returned nodes array (3.) - array of nodeIDs in order to pass from first (start node) to last (end node) element or
					           boolean false - indicating that input data was not valid
                               boolean true - indicating that there is no possible solution
*/

	
private _correctList = params [ ["_startNode", objNull, [0]],  // numbers allowed only
         ["_endNode", objNull, [0]],    // numbers allowed only
         ["_allNodes", objNull, [ [] ]], // arrays allowed only
         ["_allConnections", objNull, [ [] ]]]; // arrays allowed only



// check for valid parameter data types
if (!_correctList) exitWith {_allNodes = false; false};

//check if first element of _allNodes is an array with exact 2 elements
private _correctNodes = _allNodes params [["_firstElementNodes", objNull, [[]], [2]]];

//check if first element of _allConnections is an array with 3 elements
private _correctConnections = _allConnections params [["_firstElementConnections", objNull, [[]], [2]]];

// exit if at least one of both first elements has wrong format
if (!_correctNodes || !_correctConnections) exitWith {_allNodes = false; false};
// determine arguments 3. and 4. format

private _tempNode = _startNode;
_startNode = _endNode;
_endNode = _tempNode;
_tempNode = nil;

#ifndef SQRT2
 #define SQRT2 1.41421
#endif

//set maximum possible squared distance
private _maxDist = (worldSize * SQRT2)^2;
private _maxDistDoubl = 2 * _maxDist;

// store f-value (heuristic sum) - Predecessor ID - g-value (real sum) - Node ID - Node Position

_allNodes sort true;
private _openList = [[0, _maxDist, 0, (_allNodes select _startNode select 0)]]; 

private _closedList = [];
private _currentNode = _maxDist;

// define a name for the script scope to be able to break all loops
scopeName "main";
_solutionFound = false;

while {true} do
{
 private _shortestHeuristic = _maxDistDoubl;
 private _shortestReal = _maxDist;
 
 // get node with best f-value (heuristic sum)
 {
  if ((_x select 0) < _shortestHeuristic) then 
  {
   _shortestHeuristic = _x select 0;
   _currentNode = _x;
  };
 } count _openList;
 
 private _openNum = count _openList;
 
 //if the open list is empty then break all scopes - no solution possible
 if (_openNum isEqualTo 0) then {breakTo "main"};

 private _currID = _currentNode select 3;
 
 //delete current Node from open list and add it to closed list
 //_openList = _openList - _currentNode;
 
 _d = _openList deleteAt (_openList findIf {(_x select 3) isEqualTo _currID});
 
 _closedList pushBack [(_currentNode select 3), (_currentNode select 1)];
 
 // break to script scope if solution is found
 if (_currID isEqualTo _endNode) then
 {
  _solutionFound = true;
  breakTo "main";    //breaks all scopes except main scope
 };
 
 //get all connections to all neighbors of current node
 _neighbConns = _allConnections select
 {
  (_x select 0 isEqualTo _currID) || (_x select 1 isEqualTo _currID)
 };

 //get all neighbors of current node which are not on closed list
 _neighbNodes = _allNodes select
 {
  private _neighbID = _x select 0;
   ((_closedList findIf {(_x select 0) isEqualTo _neighbID} isEqualTo -1) && 
   (_neighbConns findIf {(_x select 0) isEqualTo _neighbID || (_x select 1) isEqualTo _neighbID} > -1))
 };
 
 {
  private _neighbID = _x select 0;

  _tentative_g = (_currentNode select 2) + ((_allNodes select _currID select 1) distanceSqr (_allNodes select _neighbID select 1));
  _neighInOListIndex = _openList findIf {(_x select 3) isEqualTo _neighbID};
  _heurDist = (_allNodes select _endNode select 1) distanceSqr (_allNodes select _neighbID select 1);

  if (_neighInOListIndex > -1) then
  {
   if (_tentative_g < (_openList select _neighInOListIndex select 2)) then
   {
    _d = _openList set [_neighInOListIndex, [(_tentative_g + _heurDist), _currID, _tentative_g, _neighbID]];
   };
  }
  else
  {
   _d = _openList pushBack [(_tentative_g + _heurDist), _currID, _tentative_g, _neighbID];
  };
 } count _neighbNodes;
};

//exit if there is no solution for the given nodes/connections set to get from start node to end node
if (!_solutionFound) exitWith {_allNodes = true; true};

// build solution path array from end node to start node

private _currNodeIndex = _closedList findIf {(_x select 0) isEqualTo _endNode};
private _currNode = _closedList select _currNodeIndex;

_allNodes resize 0;

_allNodes pushBack (_currNode select 0);

while {!((_currNode select 0) isEqualTo _startNode)} do
{
 _currNodeIndex = _closedList findIf {(_x select 0) isEqualTo (_currNode select 1)};
 _currNode = _closedList select _currNodeIndex;
 
 //current nodes ID is part of solution
 _allNodes pushBack (_currNode select 0);
};

_allNodes

 

 

A* - Dijkstra runtime comparsion videos

 

Planned features:

- A* algorithm improvement to solve 3D problems (multiple floors)

- Travelling Salesman algorithms

 

Known issues:

-Dijkstra is currently not getting always the true shortest path due to a bug in implementation

 

Changelog

v 2.0 (Download: Scripts Zip - Mission)

- added implementation for A*-algorithm

- minor performance optimizations on Dijkstra

- added demo mission which is able to compare both algorithms

 

v 1.1 (Download SQF)

-performance optimizations - 65-70% less readability / 30-35% better performance

 

v 1.0 (Download: SQF - Mission)

- initial release

[johnnyboy 3797]

That is a THOB! (THing of Beauty).  Thanks much Sarogahtyp.  Excellent work.  And your code reads beautifully.  Very cool that you provided 2 methods also (one with distances pre-calculated, and one not).

[sarogahtyp 1109]

Demo mission:

https://www.dropbox.com/s/bmzgi487r6iw8fv/Dijkstra_demo.zip?dl=0

 

Demo video:

 

[genesis92x 810]

I love this stuff 🙂 There is a lot to learn here! 

 

 

[sarogahtyp 1109]

Performance Update

 

Changelog

v 1.1 (Download SQF)

-performance optimizations - 65-70% less readability / 30-35% better performance

[sarogahtyp 1109]

Update

 

Changelog

v 2.0 (Download: Scripts Zip - Mission)

- added implementation for A*-algorithm

- minor performance optimizations on Dijkstra

- added demo mission which is able to compare both algorithms

[sarogahtyp 1109]

A* vs. Dijkstra Videos - out of the demo mission using 20 - 2000 nodes and 64 - 6778 connections:

Spoiler

A* vs. Dijkstra - 20 nodes - 64 connections

 

A* vs. Dijkstra - 50 nodes - 164 connections

 

A* vs. Dijkstra - 100 nodes - 330 connections

 

A* vs. Dijkstra - 150 nodes - 537 connections

 

 

A* vs. Dijkstra - 300 nodes - 1029 connections

 

A* vs. Dijkstra - 600 nodes - 2066 connections

 

A* vs. Dijkstra - 2000 nodes - 6778 connections

 

 

Edited May 2, 2019 by sarogahtyp

[.kju 3245]

Just in case - maybe worth comparing:

 

 

 

[sarogahtyp 1109]

10 minutes ago, .kju said:

Just in case - maybe worth comparing:

 

 

 

 

Thank you for linking it. I think Dijkstra is not the best solution for a 2D-Map-Navi but I'll definitly look into it.

[madrussian 347]

Many thanks for this!  Quick question about input parameters.  Should connections list include each connection both ways?  For example, if node1 is connected to node2, should connections list include just [fromID1, toID2]?  Or in this case, does connections list need both [fromID1, toID2] and [fromID2, toID1]?

 

Also, in case first choice is the correct (connections list needing just [fromID1, toID2]), if you go ahead and provide both ([fromID1, toID2] and [fromID2, toID1]), will this still work? ... And if so will it take significant performance hit?

 

[sarogahtyp 1109]

For performance reasons I did decide to not support oneway connections.

this mean that it isn't important if a connection is established from fromID to toID or vice versa. both will work and each connection listed in the array will be handled. you don't need to provide a connection twice.

 

If you really think that oneway connections are important (like jumping out of a window where you cant go back) then just tell and I ll think about implementing it as an option.

[madrussian 347]

Thanks, good to know!  I don't particularly need one-way connections, now or for the foreseeable future.  Still curious though (after carefully reading your answer a couple of times):

 

I understand now you support only two-way connections and that we only need to provide each connection once.  <- Considering this, will we hurt performance by providing each connection twice (one per each way)?  (Because in my case it's easier to build an array of connections that includes each connection both ways...)  Hope that makes sense.

[sarogahtyp 1109]

idk, depends on how many connections you provide.

the performance hit will mainly come with this part of code (assuming u use dijkstra code):

 _neighbConns = _allConnections select
 {
  (_x select 1 isEqualTo _currID) || (_x select 2 isEqualTo _currID)
 };

if u provide connections twice then the array _allConnections is double sized and the selection above should last longer.

this selection is done once for each provided node (maximum).

[madrussian 347]

Gotcha, that makes sense.  OK if I have many nodes, I'll go ahead and pair it down to squeeze out max performance!

[mrcurry 508]

Bit of a necro @sarogahtyp but you should revisit this one now in this year of our plague lord, 2021.

Using the new(-ish) HashMaps to define the graph you ought be able to squeeze a fair bit more performance out of it and should allow you to add one way connections for "free".

 

I'm sure this has crossed your mind but for A* handling 3D floors I'd say it's all about the right heuristic.

You should be able to use this psuedo heuristic to bring the algorithm to the right floor fairly quickly:

(_target distance2DSqr _x)*(1+_target heightDiff _x)

[pierremgi 4906]

Interesting topic,... with this persistent question on my side:
 How can we use this tool?  I mean, in what manner that can change something in AIs behavior? Is this a project for some FSMs replacing Vanilla ones?

 

About AI paths, I never released an advanced waypoint I made for visiting the houses within a radius. I failed on 2 big problems:

- the path between 2 positions (of a same house) can be inexistent, so the move fails;

- this kind of waypoint is supposed to work in combat mode, and the result is just... boring! For example, when AIs visit a military cargo tower (land_cargo_Tower_V1... and others), they often suicide jumping from top to ground, skipping stairs... One detail among myriad of others. I gave up.

 

Here is a little video, for basic and starting conversation about paths.

- in open field, waypoints are calculated and calculatePath + EH "pathCaculated" show the result, along with the behavior, the type of vehicle... the roads... Nothing surprising when we read the BIKI page about waypoints and AI behaviors. The problems arise when scripters are not aware of what an obstacle can be for an AI.

A rock is a rock. A house is a house... But a fence, a destroyed fence, a wall, a destroyed wall... that's too much subtle, unfortunately.

 

inside houses, the ordered moves can be on existing positions only. Unfortunately here, the paths between these positions are not guaranteed. And if a path exists between 2 points in a same room!, there is no guarantee that way is the shortest (see video). No matter the behavior here, stealth or combat, you AI will follow the existing path (something to take into account if you plan to add some furniture in a room!)

 

I really hope your approach will issue one day an improvement for all of that.

Have fun. 

 

[gc8 978]

Had lot of fun testing your script @sarogahtyp !

 

I did simple 2D pathfinding test:

 

 

 

The only thing I noticed was that fn_shortPathDijkstra.sqf takes the nodes list but it modifies the original array.

 

Edit: Another thing I noticed is that if the end node is not connected and the dijkstra  therefore fails to find the path then the return value of the function is not empty array but true (boolean)

 

 

ty for this script 🙂

[sarogahtyp 1109]

Okay, this topic is a to big thing for me to handle it in my beach hollidays armed with a mobile phone only.

Maybe I ll come back to this and answering the 3 latest posts when I m back on my gaming machine in a couple of days.

 

Cheers...

[madrussian 347]

On 4/26/2019 at 8:17 PM, sarogahtyp said:

 

Known issues:

-Dijkstra is currently not getting always the true shortest path due to a bug in implementation

 

Changelog

v 2.0 (Download: Scripts Zip - Mission)

- added implementation for A*-algorithm

- minor performance optimizations on Dijkstra

- added demo mission which is able to compare both algorithms

 

v 1.1 (Download SQF)

-performance optimizations - 65-70% less readability / 30-35% better performance

 

v 1.0 (Download: SQF - Mission)

- initial release

 

Is this known issue "Dijkstra currently not getting always the true shortest path" fixed in any version?  If not, any idea where the bug is?  I may take a crack at fixing it.

[sarogahtyp 1109]

It should be located in fn_shortPathDijkstra.sqf but where exactly ... I ve no clue.

Im not even sure if I tested it correctly and if the bug is really one.

Therefore you should firstly test if those bug is there using the test mission.

As far as I remember I did a strict implementation of the algorithm shown on wikipedia. therefore I always wondered why it is not finding the shortest path always.

[madrussian 347]

43 minutes ago, sarogahtyp said:

It should be located in fn_shortPathDijkstra.sqf but where exactly ... I ve no clue.

Im not even sure if I tested it correctly and if the bug is really one.

Therefore you should firstly test if those bug is there using the test mission.

As far as I remember I did a strict implementation of the algorithm shown on wikipedia. therefore I always wondered why it is not finding the shortest path always.

 

Roger, many thanks!  Ok in terms of result, seems like I found a way to get it consistently not finding shortest path.  I'll dive into the code and have a look for a solution.

[madrussian 347]

Ok, based on some visualization tests, plus some very limited code inspection, it looks like the very first instance of a found solution is returned.  Whether or not it's actually the shortest.
 

If this is true and it can be fixed, maybe user can prioritize either first solution found (completing faster) -or- shortest solution found (completing slower), via a function parameter?

[madrussian 347]

I found your bug!  (99.9% sure now)

 

On 4/12/2022 at 11:31 AM, madrussian said:

Ok, based on some visualization tests, plus some very limited code inspection, it looks like the very first instance of a found solution is returned.  Whether or not it's actually the shortest.

 

^ This was on the right track, but wasn't quite it.

 

Let's revisit Dijkstra's algorithm real quick from that wikipedia page:

 

Quote

Let the node at which we are starting be called the initial node. Let the distance of node Y be the distance from the initial node to Y. Dijkstra's algorithm will initially start with infinite distances and will try to improve them step by step.

1. Mark all nodes unvisited. Create a set of all the unvisited nodes called the unvisited set.
2. Assign to every node a tentative distance value: set it to zero for our initial node and to infinity for all other nodes. The tentative distance of a node v is the length of the shortest path discovered so far between the node v and the starting node. Since initially no path is known to any other vertex than the source itself (which is a path of length zero), all other tentative distances are initially set to infinity. Set the initial node as current.
3. For the current node, consider all of its unvisited neighbors and calculate their tentative distances through the current node. Compare the newly calculated tentative distance to the current assigned value and assign the smaller one. For example, if the current node A is marked with a distance of 6, and the edge connecting it with a neighbor B has length 2, then the distance to B through A will be 6 + 2 = 8. If B was previously marked with a distance greater than 8 then change it to 8. Otherwise, the current value will be kept.
4. When we are done considering all of the unvisited neighbors of the current node, mark the current node as visited and remove it from the unvisited set. A visited node will never be checked again.
5. If the destination node has been marked visited (when planning a route between two specific nodes) or if the smallest tentative distance among the nodes in the unvisited set is infinity (when planning a complete traversal; occurs when there is no connection between the initial node and remaining unvisited nodes), then stop. The algorithm has finished.
6. Otherwise, select the unvisited node that is marked with the smallest tentative distance, set it as the new current node, and go back to step 3.

When planning a route, it is actually not necessary to wait until the destination node is "visited" as above: the algorithm can stop once the destination node has the smallest tentative distance among all "unvisited" nodes (and thus could be selected as the next "current").

 

In your code, it appears during the part where neighbor nodes are evaluated (in step 3 above), if any neighbor node is the destination node, you're pre-maturely declaring a solution.  So the way you've got it works to provide a complete solution (i.e. start to finish), but isn't necessarily the shortest.  In fact in my experience, it is often not the shortest.

 

To fix everything up (so it will actually provide shortest possible path), we must instead only check whether current node is the destination node (in step 5 above), and only then declare a solution.

 

I tried to fix yours up, but got confused, so I started over from scratch.  Here's a working implementation.  Not super optimized (yet), but in my (rather extensive) testing so far, it is always providing the actual shortest path:

Spoiler

// Note - User arrays will not be modified
// _userNodes in format [_id, _posASL] assumed to be sorted by id, starting with id zero and no gaps!
// _userConnections in format [_id1, _id2]

// TBD - Consider optimization by storing (separate) list of unvisited nodes
// TBD - Consider optimization by removing connections that no longer apply (if that's even possible?)

private ["_maxDist","_solution","_nodes","_node","_currentNode","_connections","_connection","_id1","_id2",
        "_currentID","_shortestDist","_neighborID","_neighborNode","_visited","_neighborDistToStartTentative",
        "_currentDistToStartTentative","_connectionDist","_dist","_nextCurrentNode","_distToStartTentative"];

params ["_startID","_endID","_userNodes","_userConnections"];

_maxDist = 10e10;
_solution = [];

_nodes = _userNodes apply {
    _node = _x;
    _node + [_maxDist, -1, false]
};
// _nodeX params ["_id","_posASL","_distToStartTentative","_nodeBackID","_visited"];

_currentNode = _nodes # _startID;
_currentNode set [2, 0];

_connections = _userConnections apply {
    _connection = _x;
    _id1 = _connection # 0;
    _id2 = _connection # 1;
    _connection + [((_nodes # _id1) # 1) vectorDistance ((_nodes # _id2) # 1)]
};
// _connectionX params ["_id1","_id2","_connectionDist"];

while {true} do {
    _currentID = _currentNode # 0;

    if (_currentID == _endID) exitWith {
        // Solution found! Iterate back through _nodeBackIDs!
        while {true} do {
            _solution pushBack _currentID;
            if (_currentID == _startID) exitWith {};
            _currentID = _currentNode # 3;
            _currentNode = _nodes # _currentID;
        };
    };
    {
        _connection = _x;
        if ((_connection find _currentID) >= 0) then {
            _neighborID = (_connection - [_currentID]) # 0;
            _neighborNode = (_nodes # _neighborID);
            _visited = _neighborNode # 4;
            if (! _visited) then {
                _neighborDistToStartTentative = _neighborNode # 2;
                _currentDistToStartTentative = _currentNode # 2;
                _connectionDist = _connection # 2;
                _dist = _currentDistToStartTentative + _connectionDist;
                if (_dist < _neighborDistToStartTentative) then {
                    _neighborNode set [2, _dist];
                    _neighborNode set [3, _currentID];
                };
            };
        };
    } foreach _connections;

    _currentNode set [4, true];

    _nextCurrentNode = objNull;
    _shortestDist = _maxDist;
    {
        _node = _x;
        _visited = _node # 4;
        if (! _visited) then {
            _distToStartTentative = _node # 2;
            if (_distToStartTentative < _shortestDist) then {
                _nextCurrentNode = _node;
                _shortestDist = _distToStartTentative;
            };
        };
    } foreach _nodes;

    if (_shortestDist == _maxDist) exitWith { _solution = true }; // <- No solution

    _currentNode = _nextCurrentNode;
};

_solution

^ Update - Edge case connection problems present here, use code in Edit below instead of this.

Hope that somehow helps. 😉

 

Also btw- They do say this explicitly:

 

Quote

When planning a route, it is actually not necessary to wait until the destination node is "visited" as above: the algorithm can stop once the destination node has the smallest tentative distance among all "unvisited" nodes (and thus could be selected as the next "current").

So apparently the destination node doesn't technically have to become the current node to declare a solution, provided the other checks are completed.  Which is maybe how you were trying to implement?

 

Regardless, hats off to you Sarogahtyp and many thanks for taking on this pathfinding stuff!  Your work is incredibly inspiring, and quite integral to much of what I'm working on lately. 🙏

 

Edit - Corrected edge cases related to connections, made connection distance calcs optional, & streamlined here and there:

Spoiler

// _nodePosASLs IDed implicitly (will not be modified)
// _connections in format [_id1, _id2] (will not be modified)
// _connectionDists corresponding to _connections - If empty will be calculated (in which case original array will be modified)

// TBD - Consider optimization by storing (separate) list of unvisited nodes
// TBD - Consider optimization by removing connections that no longer apply (if that's even possible?)

private ["_maxDist","_solution","_nodeInfos","_currentID","_currentNodeInfo","_i","_connection","_neighborID",
        "_neighborNodeInfo","_visited","_neighborDistToStartTentative","_currentDistToStartTentative","_connectionDist",
        "_dist","_shortestDist","_nodeInfo","_distToStartTentative"];

params ["_startID","_endID","_nodePosASLs","_connections",["_connectionDists", []]];

_maxDist = 10e10;
_solution = [];

_nodeInfos = _nodePosASLs apply {
    [_maxDist, -1, false]
};
// _nodeInfoX params ["_distToStartTentative","_nodeBackID","_visited"]

_currentID = _startID;
_currentNodeInfo = _nodeInfos # _startID;
_currentNodeInfo set [0, 0];

if ((count _connectionDists) == 0) then {
    _connectionDists = _connections apply {
        (_nodePosASLs # (_x # 0)) vectorDistance (_nodePosASLs # (_x # 1))
    };
};

while {true} do {
    if (_currentID == _endID) exitWith {
        // Solution found! Iterate back through _nodeBackIDs!
        while {true} do {
            _solution pushBack _currentID;
            if (_currentID == _startID) exitWith {};
            _currentID = _currentNodeInfo # 1;
            _currentNodeInfo = _nodeInfos # _currentID;
        };
        reverse _solution;
    };
    for "_i" from 0 to ((count _connections) - 1) do {
        _connection = _connections # _i;
        if ((_connection find _currentID) >= 0) then {
            _neighborID = (_connection - [_currentID]) # 0;
            _neighborNodeInfo = (_nodeInfos # _neighborID);
            _visited = _neighborNodeInfo # 2;
            if (! _visited) then {
                _neighborDistToStartTentative = _neighborNodeInfo # 0;
                _currentDistToStartTentative = _currentNodeInfo # 0;
                _connectionDist = _connectionDists # _i;
                _dist = _currentDistToStartTentative + _connectionDist;
                if (_dist < _neighborDistToStartTentative) then {
                    _neighborNodeInfo set [0, _dist];
                    _neighborNodeInfo set [1, _currentID];
                };
            };
        };
    };

    _currentNodeInfo set [2, true];

    _shortestDist = _maxDist;
    for "_i" from 0 to ((count _nodeInfos) - 1) do {
        _nodeInfo = _nodeInfos # _i;
        _visited = _nodeInfo # 2;
        if (! _visited) then {
            _distToStartTentative = _nodeInfo # 0;
            if (_distToStartTentative < _shortestDist) then {
                _currentID = _i;
                _shortestDist = _distToStartTentative;
            };
        };
    };

    if (_shortestDist == _maxDist) exitWith { _solution = true }; // <- No solution

    _currentNodeInfo = _nodeInfos # _currentID;
};

_solution

 

 

[sarogahtyp 1109]

 

okay, this was a hard 'n heavy bug hunt^^

I crawled the code multiple hours back and forth and forth and back and I did not find any big issue with it.

The bottom line is that there is no issue with the algorithm itself in the code.

The issue was the distance calculation for the connections.

I used distanceSqr to be a "smart guy" and write a fast code. But fast is b*llshit if it is just wrong.

 

Why is it wrong there to use squared distances for the connections?

the picture shows it:

 

 

The problem is the Theorem of Pythagoras. (Which is very funny if you ever read my name backwards)

if you want to go from A to B and use squared distances then the distance between A and B is the same as the sum of the distances between A and C and between C and B.

 

That means using squared distances will often end up in selecting the longer way.

[johnnyboy 3797]

4 hours ago, sarogahtyp said:

The problem is the Theorem of Pythagoras. (Which is very funny if you ever read my name backwards)

Haha, that is funny!  😀

 

Thanks much to you and @madrussian for the great work!  Its an honor to be associated with you super smart guys!