/// <summary>
///
/// </summary>
/// <param name="field"></param>
/// <param name="result"></param>
/// <param name="parallel"></param>
public static void GetCurl(this GridField3d <Vector3d> field, Vector3d[] result, bool parallel = false)
{
// implementation reference
// http://www.math.harvard.edu/archive/21a_spring_09/PDF/13-05-curl-and-divergence.pdf
if (parallel)
{
Parallel.ForEach(Partitioner.Create(0, field.CountXYZ), range => Body(range.Item1, range.Item2));
}
else
{
Body(0, field.CountXYZ);
}
void Body(int from, int to)
{
var vals = field.Values;
(var nx, var ny, var nz) = field.Count;
int nxy = field.CountXY;
(var dx, var dy, var dz) = (0.5 / field.Scale);
(int di, int dj, int dk) = field.GetBoundaryOffsets();
(int i, int j, int k) = field.ToGridSpace(from);
for (int index = from; index < to; index++, i++)
{
if (i == nx)
{
j++; i = 0;
}
if (j == ny)
{
k++; j = 0;
}
Vector3d tx0 = (i == 0) ? vals[index + di] : vals[index - 1];
Vector3d tx1 = (i == nx - 1) ? vals[index - di] : vals[index + 1];
Vector3d ty0 = (j == 0) ? vals[index + dj] : vals[index - nx];
Vector3d ty1 = (j == ny - 1) ? vals[index - dj] : vals[index + nx];
Vector3d tz0 = (k == 0) ? vals[index + dk] : vals[index - nxy];
Vector3d tz1 = (k == nz - 1) ? vals[index - dk] : vals[index + nxy];
result[index] = new Vector3d(
(ty1.Z - ty0.Z) * dy - (tz1.Y - tz0.Y) * dz,
(tz1.X - tz0.X) * dz - (tx1.Z - tx0.Z) * dx,
(tx1.Y - tx0.Y) * dx - (ty1.X - ty0.X) * dy);
}
}
}
/// <summary>
///
/// </summary>
/// <param name="field"></param>
/// <param name="result"></param>
/// <param name="parallel"></param>
public static void GetLaplacian(this GridField3d <double> field, double[] result, bool parallel = false)
{
if (parallel)
{
Parallel.ForEach(Partitioner.Create(0, field.CountXYZ), range => Body(range.Item1, range.Item2));
}
else
{
Body(0, field.CountXYZ);
}
void Body(int from, int to)
{
var vals = field.Values;
(var nx, var ny, var nz) = field.Count;
int nxy = field.CountXY;
(var dx, var dy, var dz) = field.Scale;
dx = 1.0 / (dx * dx);
dy = 1.0 / (dy * dy);
dz = 1.0 / (dz * dz);
(int di, int dj, int dk) = field.GetBoundaryOffsets();
(int i, int j, int k) = field.ToGridSpace(from);
for (int index = from; index < to; index++, i++)
{
if (i == nx)
{
j++; i = 0;
}
if (j == ny)
{
k++; j = 0;
}
double tx0 = (i == 0) ? vals[index + di] : vals[index - 1];
double tx1 = (i == nx - 1) ? vals[index - di] : vals[index + 1];
double ty0 = (j == 0) ? vals[index + dj] : vals[index - nx];
double ty1 = (j == ny - 1) ? vals[index - dj] : vals[index + nx];
double tz0 = (k == 0) ? vals[index + dk] : vals[index - nxy];
double tz1 = (k == nz - 1) ? vals[index - dk] : vals[index + nxy];
double t = vals[index] * 2.0;
result[index] = (tx0 + tx1 - t) * dx + (ty0 + ty1 - t) * dy + (tz0 + tz1 - t) * dz;
}
}
}
/// <summary>
///
/// </summary>
/// <param name="field"></param>
/// <param name="result"></param>
/// <param name="parallel"></param>
public static void GetGradient(this GridField3d <double> field, Vector3d[] result, bool parallel = false)
{
if (parallel)
{
Parallel.ForEach(Partitioner.Create(0, field.CountXYZ), range => Body(range.Item1, range.Item2));
}
else
{
Body(0, field.CountXYZ);
}
void Body(int from, int to)
{
var vals = field.Values;
(var nx, var ny, var nz) = field.Count;
int nxy = field.CountXY;
(var dx, var dy, var dz) = (0.5 / field.Scale);
(int di, int dj, int dk) = field.GetBoundaryOffsets();
(int i, int j, int k) = field.ToGridSpace(from);
for (int index = from; index < to; index++, i++)
{
if (i == nx)
{
j++; i = 0;
}
if (j == ny)
{
k++; j = 0;
}
double tx0 = (i == 0) ? vals[index + di] : vals[index - 1];
double tx1 = (i == nx - 1) ? vals[index - di] : vals[index + 1];
double ty0 = (j == 0) ? vals[index + dj] : vals[index - nx];
double ty1 = (j == ny - 1) ? vals[index - dj] : vals[index + nx];
double tz0 = (k == 0) ? vals[index + dk] : vals[index - nxy];
double tz1 = (k == nz - 1) ? vals[index - dk] : vals[index + nxy];
result[index] = new Vector3d((tx1 - tx0) * dx, (ty1 - ty0) * dy, (tz1 - tz0) * dz);
}
}
}
/// <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);
}
}
}
}