private static Int32Point?GetNeighbour4(Int32Point u, int neighbourIndex, LiveWireGraph graph, out double distance)
{
int width = graph.Width;
int height = graph.Height;
int x = u.X;
int y = u.Y;
distance = 0.0;
switch (neighbourIndex)
{
case 0:
if (x >= width - 1)
{
return(null);
}
else
{
distance = graph.EdgeData[y * width + x, 0];
return(new Int32Point(x + 1, y));
}
case 1:
if (y >= height - 1)
{
return(null);
}
else
{
distance = graph.EdgeData[y * width + x, 2];
return(new Int32Point(x, y + 1));
}
case 2:
if (x == 0)
{
return(null);
}
else
{
distance = graph.EdgeData[y * width + x - 1, 0];
return(new Int32Point(x - 1, y));
}
case 3:
if (y == 0)
{
return(null);
}
else
{
distance = graph.EdgeData[y * width + x - width, 2];
return(new Int32Point(x, y - 1));
}
default:
return(null);
}
}
public static void DijkstraBetweenPoints(LiveWireGraph graph, double[,] distanceMap, out List <Point> path, out List <int> segmentIndices, out TargetPathPoint pathIndex, Dictionary <Int32Point, int> pathPoints, params Point[] points)
{
// Which mode are we operating in
bool toPathMode = pathPoints != null;
// Initialisations
pathIndex = new TargetPathPoint(default(Int32Point), -1);
Int32Point currentPoint = (Int32Point)points.First();
int width = graph.Width;
int height = graph.Height;
int nodeCount = width * height;
double[] distances = new double[nodeCount];
segmentIndices = new List <int>();
bool[] visited = new bool[nodeCount];
Int32Point?[][] previous = new Int32Point?[width][];
for (int i = 0; i < width; i++)
{
previous[i] = new Int32Point?[height];
}
DateTime startTime = DateTime.Now;
List <Point> outputPoints = null;
// Loop for one extra in paths mode
int pointsLoopLength = toPathMode ? points.Length + 1 : points.Length;
for (int pointIndex = 1; pointIndex < pointsLoopLength; pointIndex++)
{
// Loop Initialisations
Int32Point targetPoint; // = (Int32Point)points[pointIndex];
bool toPaths = false;
if (!toPathMode || pointIndex < points.Length)
{
targetPoint = (Int32Point)points[pointIndex];
}
else
{
targetPoint = default(Int32Point);
toPaths = true;
}
FibonacciHeap <double, Int32Point> unvisitedheap = new FibonacciHeap <double, Int32Point>();
Node <double, Int32Point>[] nodeindex = new Node <double, Int32Point> [width * height];
for (int x = 0; x < nodeCount; x++)
{
distances[x] = double.MaxValue;
visited[x] = false;
}
for (int x = 0; x < width; x++)
{
for (int y = 0; y < height; y++)
{
previous[x][y] = null;
}
}
distances[currentPoint.Y * width + currentPoint.X] = 0;
Int32Point pN = new Int32Point(currentPoint.X, currentPoint.Y);
Node <double, Int32Point> pixelNode = new Node <double, Int32Point>(0, pN);
unvisitedheap.Insert(pixelNode);
nodeindex[currentPoint.Y * width + currentPoint.X] = pixelNode;
// Dijkstra's Algorithm between sourcePoint and nextPoint
while (unvisitedheap.Count > 0)
{
// Get next priority point u
Node <double, Int32Point> n = unvisitedheap.Minimum();
unvisitedheap.RemoveMinimum();
Int32Point u = n.value;
int uIndex = u.Y * width + u.X;
// if dist[u] == infinity then break
if (distances[uIndex] == double.MaxValue)
{
break;
}
if (!toPathMode)
{
// if u == target we are done
if (u.X == targetPoint.X && u.Y == targetPoint.Y)
{
break;
}
}
else
{
if (!toPaths)
{
if (u.X == targetPoint.X && u.Y == targetPoint.Y)
{
break;
}
}
else
{
// Search hash set rather than target point
if (pathPoints.ContainsKey(u))
{
pathIndex = new TargetPathPoint(u, pathPoints[u]);
break;
}
}
}
// for each possible neighbour v
for (int i = 0; i < 8; i++)
{
double distance;
Int32Point?nV = GetNeighbour8(u, i, graph, out distance);
if (nV == null)
{
continue;
}
Int32Point v = nV.Value;
int vIndex = v.Y * width + v.X;
if (visited[vIndex])
{
continue;
}
double newDistance = distances[uIndex] + distance;
double remainingDistance = (0.03 * Math.Sqrt(Math.Pow(v.X - targetPoint.X, 2.0) + Math.Pow(v.Y - targetPoint.Y, 2.0))) + 2 * (Math.Pow(distanceMap[v.X, v.Y], 2));
if (newDistance < distances[vIndex])
{
// Adjust V and re-prioritise
Node <double, Int32Point> vNode = nodeindex[vIndex];
if (vNode == null)
{
Int32Point p = new Int32Point(v.X, v.Y);
vNode = new Node <double, Int32Point>(newDistance + remainingDistance, p);
unvisitedheap.Insert(vNode);
nodeindex[vIndex] = vNode;
distances[vIndex] = newDistance;
previous[v.X][v.Y] = u;
}
else
{
distances[vIndex] = newDistance;
previous[v.X][v.Y] = u;
unvisitedheap.DecreaseKey(vNode, newDistance + remainingDistance);
}
}
}
visited[uIndex] = true;
}
// Finalisation
List <Point> shortestPathPoints = new List <Point>();
Point f = default(Point);
if (!toPathMode)
{
f = (Point)targetPoint;
}
else
{
if (toPaths)
{
f = (Point)pathIndex.Point;
}
else
{
f = (Point)targetPoint;
}
}
shortestPathPoints.Add(f);
int fIndex = (int)f.Y * width + (int)f.X;
while (previous[(int)f.X][(int)f.Y] != null)
{
shortestPathPoints.Insert(0, (Point)previous[(int)f.X][(int)f.Y]);
f = (Point)previous[(int)f.X][(int)f.Y];
fIndex = (int)f.Y * width + (int)f.X;
}
// Append to outputPoints
if (outputPoints == null)
{
outputPoints = shortestPathPoints;
}
else
{
segmentIndices.Add(outputPoints.Count);
foreach (Point p in shortestPathPoints)
{
outputPoints.Add(p);
}
}
// Preparation for next loop
if (!toPathMode)
{
currentPoint = (Int32Point)points[pointIndex];
}
else if (!toPaths)
{
currentPoint = (Int32Point)points[pointIndex];
}
}
path = outputPoints;
}