/// <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)); } } } } }