/// <summary>
/// AStar algorithm
/// </summary>
/// <param name="costMatrix"></param>
/// <param name="start"></param>
/// <param name="target"></param>
/// <param name="minDist"> heuristic estimation of min distance</param>
/// <returns></returns>
public int AStar(int[][] costMatrix, int start, int target, int[][] minDist)
{
int nodeCnt = costMatrix.Length;
int[] previous = new int[nodeCnt];
int[] distToTarget = new int[nodeCnt];
int[] distFromStart = new int[nodeCnt];
CostObject <int>[] costNodes = new CostObject <int> [nodeCnt];
SortedSet <CostObject <int> > unvisited = new SortedSet <CostObject <int> >();
HashSet <int> visited = new HashSet <int>();
for (int i = 0; i < nodeCnt; i++)
{
distToTarget[i] = distFromStart[i] = int.MaxValue;
previous[i] = -1; // none
costNodes[i] = new CostObject <int>(i, distToTarget[i]);
unvisited.Add(costNodes[i]);
}
distFromStart[start] = 0;
distToTarget[start] = distFromStart[start] + minDist[start][target];
unvisited.Add(costNodes[start]);
int crt = start;
while (unvisited.Count > 0 && crt != target)
{
// select crt as the one having min distance
// may be optimized with a heap
crt = unvisited.Min.Instance;
unvisited.Remove(unvisited.Min);
visited.Add(crt);
// unvisited.
for (int j = 0; j < costMatrix[crt].Length; j++)
{
if (costMatrix[crt][j] > 0 && !visited.Contains(j))
{
int possibleFromDist = distFromStart[crt] + costMatrix[crt][j];
// if score can be improved - TBD
if (unvisited.Contains(costNodes[j]) || possibleFromDist < distFromStart[j])
{
previous[j] = crt;
distFromStart[j] = possibleFromDist;
distToTarget[j] = distFromStart[j] + minDist[j][target];
}
// add j to unvisited if needed
unvisited.Add(costNodes[j]);
}
}
}
return(1);
}
public int Dijkstra(int[][] costMatrix, int start, int target)
{
int nodeCnt = costMatrix.Length;
int[] previous = new int[nodeCnt];
int[] distance = new int[nodeCnt];
CostObject <int>[] costNodes = new CostObject <int> [nodeCnt];
SortedSet <CostObject <int> > unvisited = new SortedSet <CostObject <int> >();
for (int i = 0; i < nodeCnt; i++)
{
distance[i] = int.MaxValue;
previous[i] = -1; // none
costNodes[i] = new CostObject <int>(i, distance[i]);
unvisited.Add(costNodes[i]);
}
distance[start] = 0;
// List<int> neighbors = new List<int>()
int crt = start;
while (unvisited.Count > 0 && crt != target)
{
// select crt as the one having min distance
// may be optimized with a heap
crt = unvisited.Min.Instance;
for (int j = 0; j < costMatrix[crt].Length; j++)
{
if (costMatrix[crt][j] > 0 && unvisited.Contains(costNodes[j]))
{
if (distance[j] > costMatrix[crt][j] + distance[crt])
{
distance[j] = costMatrix[crt][j] + distance[crt];
// refresh cost and reorder unvisited
unvisited.Remove(costNodes[j]);
costNodes[j].Cost = distance[j];
unvisited.Add(costNodes[j]);
previous[j] = crt;
}
}
}
unvisited.Remove(costNodes[crt]);
}
// results: distance and previous
return(1);
}
public int CompareTo(CostObject <T> other)
{
return(this.Cost.CompareTo(other.Cost));
}
