public void Abs()
{
Vector3d v = new Vector3d(-1, -2, -3);
v.Abs();
Assert.AreEqual(v, new Vector3d(1, 2, 3));
}
/// <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(GridField3d <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, var nz) = cost.Count;
var nxy = cost.CountXY;
(var dx, var dy, var dz) = Vector3d.Abs(cost.Scale);
var eikonal = new Eikonal3d(dx, dy, dz);
var queue = new PriorityQueue <double, int>();
result.SetRange(double.PositiveInfinity, cost.CountXYZ);
// 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, int z0) = 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);
}
if (z0 > 0)
{
TryUpdateZ(i0 - nxy);
}
if (z0 < nz - 1)
{
TryUpdateZ(i0 + nxy);
}
// 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
double minZ = GetMinZ(index); // ''
if (minY > double.MaxValue || minZ > double.MaxValue || !eikonal.Solve(d0, minY, minZ, 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
double minZ = GetMinZ(index); // ''
if (minX > double.MaxValue || minZ > double.MaxValue || !eikonal.Solve(minX, d0, minZ, 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);
}
}
// process y neighbor
void TryUpdateZ(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
double minY = GetMinY(index); // ''
if (minX > double.MaxValue || minY > double.MaxValue || !eikonal.Solve(minX, minY, 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]));
}
// returns the minimum adjacent value in the z
double GetMinZ(int index)
{
if (z0 == 0)
{
return(result[index + nxy]);
}
else if (z0 == nz - 1)
{
return(result[index - nxy]);
}
return(Math.Min(result[index - nxy], result[index + nxy]));
}
}
}
/// <summary>
/// Calculates the L1 (Manhattan) 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 CalculateL1(GridField3d <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, var nz) = cost.Count;
var nxy = cost.CountXY;
(var dx, var dy, var dz) = Vector3d.Abs(cost.Scale);
var queue = new PriorityQueue <double, int>();
result.SetRange(double.PositiveInfinity, cost.CountXYZ);
// enqueue sources
foreach (int i in sources)
{
result[i] = 0.0;
queue.Insert(0.0, i);
}
// exclude
if (exclude != null)
{
foreach (int i in exclude)
{
result[i] = 0.0;
}
}
// breadth first search from sources
while (queue.Count > 0)
{
(var d0, int i0) = queue.RemoveMin();
if (result[i0] < d0)
{
continue; // skip if lower value has been assigned
}
(int x0, int y0, int z0) = cost.ToGridSpace(i0);
// -x
if (x0 > 0)
{
TryUpdate(d0 + dx * costVals[i0 - 1], i0 - 1);
}
// +x
if (x0 < nx - 1)
{
TryUpdate(d0 + dx * costVals[i0 + 1], i0 + 1);
}
// -y
if (y0 > 0)
{
TryUpdate(d0 + dy * costVals[i0 - nx], i0 - nx);
}
// +y
if (y0 < ny - 1)
{
TryUpdate(d0 + dy * costVals[i0 + nx], i0 + nx);
}
// -z
if (z0 > 0)
{
TryUpdate(d0 + dz * costVals[i0 - nxy], i0 - nxy);
}
// +z
if (z0 < nz - 1)
{
TryUpdate(d0 + dz * costVals[i0 + nxy], i0 + nxy);
}
// add to queue if less than current min
void TryUpdate(double distance, int index)
{
if (distance < result[index])
{
result[index] = distance;
queue.Insert(distance, index);
}
}
}
}
/// <summary>
/// Calculates the L1 (Manhattan) geodesic distance from the given sources.
/// </summary>
/// <param name="grid"></param>
/// <param name="sources"></param>
/// <param name="result"></param>
/// <param name="exclude"></param>
public static void CalculateL1(Grid3d grid, IEnumerable <int> sources, double[] result, IEnumerable <int> exclude = null)
{
// impl ref
// http://www.numerical-tours.com/matlab/fastmarching_0_implementing/
(var nx, var ny, var nz) = grid.Count;
var nxy = grid.CountXY;
(var dx, var dy, var dz) = Vector3d.Abs(grid.Scale);
var queue = new Queue <int>();
result.SetRange(double.PositiveInfinity, grid.CountXYZ);
// enqueue sources
foreach (int i in sources)
{
result[i] = 0.0;
queue.Enqueue(i);
}
// exclude
if (exclude != null)
{
foreach (int i in exclude)
{
result[i] = 0.0;
}
}
// breadth first search from sources
while (queue.Count > 0)
{
int i0 = queue.Dequeue();
var d0 = result[i0];
(int x0, int y0, int z0) = grid.ToGridSpace(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);
}
// -z
if (z0 > 0)
{
TryUpdate(d0 + dz, i0 - nxy);
}
// +z
if (z0 < nz - 1)
{
TryUpdate(d0 + dz, i0 + nxy);
}
// add to queue if less than current min
void TryUpdate(double distance, int index)
{
if (distance < result[index])
{
result[index] = distance;
queue.Enqueue(index);
}
}
}
}
public void Vector3dAbsWorks()
{
var v = new Vector3d(3, -4, -5);
Assert.True(v.Abs().Equivalent(new Vector3d(3, 4, 5)));
}
/// <summary>
///
/// </summary>
/// <param name="point"></param>
/// <param name="size"></param>
/// <returns></returns>
public static double Box(Vector3d point, Vector3d size)
{
var d = Vector3d.Abs(point) - size;
return(Math.Min(d.ComponentMax, 0.0) + Vector3d.Min(d, 0.0).Length);
}