/// <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>
/// Calculates the L2 (Euclidiean) geodesic distance from the given sources.
/// </summary>
/// <param name="cost"></param>
/// <param name="sources"></param>
/// <param name="result"></param>
/// <param name="exclude"></param>
public static void CalculateL2(GridField2d <double> cost, IEnumerable <int> sources, double[] result, IEnumerable <int> exclude = null)
{
// impl ref
// http://www.numerical-tours.com/matlab/fastmarching_0_implementing/
var costVals = cost.Values;
(var nx, var ny) = cost.Count;
(var dx, var dy) = Vector2d.Abs(cost.Scale);
var eikonal = new Eikonal2d(dx, dy);
var queue = new PriorityQueue <double, int>();
result.SetRange(double.PositiveInfinity, cost.CountXY);
// enqueue sources
foreach (int i in sources)
{
result[i] = 0.0;
queue.Insert(0.0, i);
}
// exclude
if (exclude != null)
{
foreach (var i in exclude)
{
result[i] = 0.0;
}
}
// breadth first search from sources
while (queue.Count > 0)
{
(double d0, int i0) = queue.RemoveMin();
if (result[i0] < d0)
{
continue; // skip if lower value has been assigned
}
(int x0, int y0) = cost.ToGridSpace(i0);
if (x0 > 0)
{
TryUpdateX(i0 - 1);
}
if (x0 < nx - 1)
{
TryUpdateX(i0 + 1);
}
if (y0 > 0)
{
TryUpdateY(i0 - nx);
}
if (y0 < ny - 1)
{
TryUpdateY(i0 + nx);
}
// process x neighbor
void TryUpdateX(int index)
{
var d1 = result[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.Solve(d0, minY, costVals[index], out d2))
{
d2 = d0 + dx * costVals[index];
}
// add to queue if less than current min
if (d2 < d1)
{
result[index] = d2;
queue.Insert(d2, index);
}
}
// process y neighbor
void TryUpdateY(int index)
{
var d1 = result[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.Solve(minX, d0, costVals[index], out d2))
{
d2 = d0 + dy * costVals[index];
}
// add to queue if less than current min
if (d2 < d1)
{
result[index] = d2;
queue.Insert(d2, index);
}
}
// returns the minimum adjacent value in the x
double GetMinX(int index)
{
if (x0 == 0)
{
return(result[index + 1]);
}
else if (x0 == nx - 1)
{
return(result[index - 1]);
}
return(Math.Min(result[index - 1], result[index + 1]));
}
// returns the minimum adjacent value in the y
double GetMinY(int index)
{
if (y0 == 0)
{
return(result[index + nx]);
}
else if (y0 == ny - 1)
{
return(result[index - nx]);
}
return(Math.Min(result[index - nx], result[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));
}
}
}
}
}
