/// <summary>
/// Calculates L1 (Manhattan) geodesic distance via Dijksta's algorithm as detailed in
/// http://www.numerical-tours.com/matlab/fastmarching_0_implementing/
/// </summary>
/// <param name="cost"></param>
/// <param name="sources"></param>
/// <param name="result"></param>
public static void GeodesicDistanceL1(GridField2d<double> field, double[] cost, IEnumerable<int> sources, IEnumerable<int> exclude = null)
{
// TODO handle additonal wrap modes
var dists = field.Values;
int nx = field.CountX;
int ny = field.CountY;
(double dx, double dy) = Vec2d.Abs(field.Scale);
var queue = new PriorityQueue<double, int>();
dists.Set(double.PositiveInfinity);
// enqueue sources
foreach (int i in sources)
{
dists[i] = 0.0;
queue.Insert(0.0, i);
}
// exclude
if (exclude != null)
{
foreach (int i in exclude)
dists[i] = 0.0;
}
// breadth first search from sources
while (queue.Count > 0)
{
(var d0, int i0) = queue.RemoveMin();
if (dists[i0] < d0) continue; // skip if lower value has been assigned
(int x0, int y0) = field.IndicesAt(i0);
// -x
if (x0 > 0)
TryUpdate(d0 + dx * cost[i0 - 1], i0 - 1);
// +x
if (x0 < nx - 1)
TryUpdate(d0 + dx * cost[i0 + 1], i0 + 1);
// -y
if (y0 > 0)
TryUpdate(d0 + dy * cost[i0 - nx], i0 - nx);
// +y
if (y0 < ny - 1)
TryUpdate(d0 + dy * cost[i0 + nx], i0 + nx);
// add to queue if less than current min
void TryUpdate(double distance, int index)
{
if (distance < dists[index])
{
dists[index] = distance;
queue.Insert(distance, index);
}
}
}
}
/// <summary>
/// Calculates L1 (Manhattan) geodesic distance via Dijksta's algorithm as detailed in
/// http://www.numerical-tours.com/matlab/fastmarching_0_implementing/
/// </summary>
/// <param name="cost"></param>
/// <param name="sources"></param>
/// <param name="field"></param>
public static void GeodesicDistanceL1(GridField2d<double> field, IEnumerable<int> sources, IEnumerable<int> exclude = null)
{
// TODO handle additonal wrap modes
var dists = field.Values;
int nx = field.CountX;
int ny = field.CountY;
(double dx, double dy) = Vec2d.Abs(field.Scale);
var queue = new Queue<int>();
dists.Set(double.PositiveInfinity);
// enqueue sources
foreach (int i in sources)
{
dists[i] = 0.0;
queue.Enqueue(i);
}
// exclude
if (exclude != null)
{
foreach (int i in exclude)
dists[i] = 0.0;
}
// breadth first search from sources
while (queue.Count > 0)
{
int i0 = queue.Dequeue();
var d0 = dists[i0];
(int x0, int y0) = field.IndicesAt(i0);
// -x
if (x0 > 0)
TryUpdate(d0 + dx, i0 - 1);
// +x
if (x0 < nx - 1)
TryUpdate(d0 + dx, i0 + 1);
// -y
if (y0 > 0)
TryUpdate(d0 + dy, i0 - nx);
// +y
if (y0 < ny - 1)
TryUpdate(d0 + dy, i0 + nx);
// add to queue if less than current min
void TryUpdate(double distance, int index)
{
if (distance < dists[index])
{
dists[index] = distance;
queue.Enqueue(index);
}
}
}
}
/// <summary>
///
/// </summary>
/// <param name="field"></param>
/// <param name="cost"></param>
/// <param name="sources"></param>
/// <param name="exclude"></param>
public static void GeodesicDistanceL2(GridField2d<double> field, double[] cost, IEnumerable<int> sources, IEnumerable<int> exclude = null)
{
// TODO handle additonal wrap modes
var dists = field.Values;
var nx = field.CountX;
var ny = field.CountY;
(var dx, var dy) = Vec2d.Abs(field.Scale);
var eikonal = new Eikonal2d(dx, dy);
var queue = new PriorityQueue<double, int>();
dists.Set(double.PositiveInfinity);
// enqueue sources
foreach (int i in sources)
{
dists[i] = 0.0;
queue.Insert(0.0, i);
}
// exclude
if (exclude != null)
{
foreach (var i in exclude)
dists[i] = 0.0;
}
// breadth first search from sources
while (queue.Count > 0)
{
(double d0, int i0) = queue.RemoveMin();
if (dists[i0] < d0) continue; // skip if lower value has been assigned
(int x0, int y0) = field.IndicesAt(i0);
if (x0 > 0) X(i0 - 1);
if (x0 < nx - 1) X(i0 + 1);
if (y0 > 0) Y(i0 - nx);
if (y0 < ny - 1) Y(i0 + nx);
// process x neighbor
void X(int index)
{
var d1 = dists[index];
if (d1 < d0) return; // no backtracking
double d2;
double minY = GetMinY(index); // will return infinity if neither neighbor has been visited
if (minY > double.MaxValue || !eikonal.Evaluate(d0, minY, cost[index], out d2))
d2 = d0 + dx * cost[index];
// add to queue if less than current min
if (d2 < d1)
{
dists[index] = d2;
queue.Insert(d2, index);
}
}
// process y neighbor
void Y(int index)
{
var d1 = dists[index];
if (d1 < d0) return; // no backtracking
double d2;
double minX = GetMinX(index); // will return infinity if neither neighbor has been visited
if (minX > double.MaxValue || !eikonal.Evaluate(minX, d0, cost[index], out d2))
d2 = d0 + dy * cost[index];
// add to queue if less than current min
if (d2 < d1)
{
dists[index] = d2;
queue.Insert(d2, index);
}
}
// returns the minimum adjacent value in the x
double GetMinX(int index)
{
if (x0 == 0) return dists[index + 1];
else if (x0 == nx - 1) return dists[index - 1];
return Math.Min(dists[index - 1], dists[index + 1]);
}
// returns the minimum adjacent value in the y
double GetMinY(int index)
{
if (y0 == 0) return dists[index + nx];
else if (y0 == ny - 1) return dists[index - nx];
return Math.Min(dists[index - nx], dists[index + nx]);
}
}
}
/// <summary>
///
/// </summary>
/// <param name="cost"></param>
/// <param name="sources"></param>
/// <param name="tags"></param>
/// <param name="result"></param>
private static void GeodesicDistanceL2Impl(GridField2d <double> cost, IEnumerable <int> sources, bool[] tags, double[] result)
{
// TODO handle wrap modes
var costVals = cost.Values;
var nx = cost.CountX;
var ny = cost.CountY;
(var dx, var dy) = Vec2d.Abs(cost.Scale).Components;
var pq = new PriorityQueue <(double, int)>((a, b) => a.Item1.CompareTo(b.Item1));
var eikonal = new Eikonal2d(dx, dy);
result.SetRange(double.PositiveInfinity, 0, cost.Count);
// enqueue sources
foreach (int index in sources)
{
result[index] = 0.0;
pq.Insert((0.0, index));
}
// breadth first search from sources
while (pq.Count > 0)
{
(double dist0, int index0) = pq.RemoveMin();
if (tags[index0])
{
continue; // skip if already accepted
}
tags[index0] = true;
(int i0, int j0) = cost.IndicesAt(index0);
// x neigbours
for (int i = -1; i < 2; i += 2)
{
if (!SlurMath.Contains(i0 + i, nx))
{
continue; // skip if out of bounds
}
int index1 = index0 + i;
if (!tags[index1])
{
double minAdj =
(j0 == 0) ? result[index1 + nx] :
(j0 == ny - 1) ? result[index1 - nx] :
Math.Min(result[index1 - nx], result[index1 + nx]);
double dist1 =
(minAdj > double.MaxValue) ? dist0 + dx * costVals[index1] :
eikonal.Evaluate(dist0, minAdj, costVals[index1]);
if (dist1 < result[index1])
{
result[index1] = dist1;
pq.Insert((dist1, index1));
}
}
}
// y neigbours
for (int j = -1; j < 2; j += 2)
{
if (!SlurMath.Contains(j0 + j, ny))
{
continue; // skip if out of bounds
}
int index1 = index0 + j * nx;
if (!tags[index1])
{
double minAdj =
(i0 == 0) ? result[index1 + 1] :
(i0 == nx - 1) ? result[index1 - 1] :
Math.Min(result[index1 - 1], result[index1 + 1]);
double dist1 =
(minAdj > double.MaxValue) ? dist0 + dy * costVals[index1] :
eikonal.Evaluate(minAdj, dist0, costVals[index1]);
if (dist1 < result[index1])
{
result[index1] = dist1;
pq.Insert((dist1, index1));
}
}
}
}
}
/// <summary>
/// Calculates L1 (Manhattan) geodesic distance via Dijksta's algorithm as detailed in
/// http://www.numerical-tours.com/matlab/fastmarching_0_implementing/
/// </summary>
/// <param name="cost"></param>
/// <param name="sources"></param>
/// <param name="result"></param>
public static void GeodesicDistanceL1(GridField2d <double> cost, IEnumerable <int> sources, double[] result)
{
// TODO handle wrap modes
var costVals = cost.Values;
int nx = cost.CountX;
int ny = cost.CountY;
(double dx, double dy) = Vec2d.Abs(cost.Scale).Components;
var pq = new PriorityQueue <(double, int)>((a, b) => a.Item1.CompareTo(b.Item1));
result.SetRange(double.PositiveInfinity, 0, cost.Count);
// enqueue sources
foreach (int index in sources)
{
result[index] = 0.0;
pq.Insert((0.0, index));
}
// breadth first search from sources
while (pq.Count > 0)
{
(double dist0, int index0) = pq.RemoveMin();
if (dist0 > result[index0])
{
continue; // skip if node has already been processed
}
(int i0, int j0) = cost.IndicesAt(index0);
// x neighbours
for (int i = -1; i < 2; i += 2)
{
if (!SlurMath.Contains(i0 + i, nx))
{
continue;
}
int index1 = index0 + i;
double dist1 = dist0 + costVals[index1] * dx;
if (dist1 < result[index1])
{
result[index1] = dist1;
pq.Insert((dist1, index1));
}
}
// y neigbours
for (int j = -1; j < 2; j += 2)
{
if (!SlurMath.Contains(j0 + j, ny))
{
continue;
}
int index1 = index0 + j * nx;
double dist1 = dist0 + costVals[index1] * dy;
if (dist1 < result[index1])
{
result[index1] = dist1;
pq.Insert((dist1, index1));
}
}
}
}
/// <summary>
/// Calculates L1 (Manhattan) geodesic distance via Dijksta's algorithm as detailed in
/// http://www.numerical-tours.com/matlab/fastmarching_0_implementing/
/// </summary>
/// <param name="cost"></param>
/// <param name="sources"></param>
/// <param name="result"></param>
public static void GeodesicDistanceL1(GridField2d <double> cost, IEnumerable <int> sources, double[] result)
{
// TODO
// handle additonal wrap modes
// add simplified bfs based version with obstacle threshold
var costVals = cost.Values;
int nx = cost.CountX;
int ny = cost.CountY;
(double dx, double dy) = Vec2d.Abs(cost.Scale);
var pq = new PriorityQueue <double, int>();
result.SetRange(double.PositiveInfinity, 0, cost.Count);
// enqueue sources
foreach (int index in sources)
{
result[index] = 0.0;
pq.Insert(0.0, index);
}
// breadth first search from sources
while (pq.Count > 0)
{
(double dist0, int index0) = pq.RemoveMin();
if (dist0 > result[index0])
{
continue; // skip if node has already been processed (TODO check if necessary)
}
(int i0, int j0) = cost.IndicesAt(index0);
// x neighbours
for (int i = -1; i < 2; i += 2)
{
if (!SlurMath.Contains(i0 + i, nx))
{
continue;
}
int index1 = index0 + i;
double dist1 = dist0 + costVals[index1] * dx;
if (dist1 < result[index1])
{
result[index1] = dist1;
pq.Insert(dist1, index1);
}
}
// y neigbours
for (int j = -1; j < 2; j += 2)
{
if (!SlurMath.Contains(j0 + j, ny))
{
continue;
}
int index1 = index0 + j * nx;
double dist1 = dist0 + costVals[index1] * dy;
if (dist1 < result[index1])
{
result[index1] = dist1;
pq.Insert(dist1, index1);
}
}
}
}
