/// <summary>
/// Remove point without locking / thread-safety
/// </summary>
public bool RemovePointUnsafe(T value, Vector3d pos)
{
Vector3i idx = Indexer.ToGrid(pos);
return(remove_point(value, idx, false));
}
public int to_linear(Vector3i ijk)
{
return(ijk.x + ni * (ijk.y + nj * ijk.z));
}
public void Set(Vector3i idx, bool val)
{
int i = idx.z * slab_size + idx.y * row_size + idx.x;
Bits[i] = val;
}
public Vector3i ToBlockLocal(Vector3i outer_index)
{
Vector3i block_idx = ToBlockIndex(outer_index);
return(outer_index - block_idx * BlockSize);
}
public float this[Vector3i idx]
{
get { return(grid[idx.x, idx.y, idx.z]); }
}
protected Vector3i bilerp(ref Vector3i v00, ref Vector3i v10, ref Vector3i v11, ref Vector3i v01, double tx, double ty)
{
Vector3d a = Vector3d.Lerp((Vector3d)v00, (Vector3d)v01, ty);
Vector3d b = Vector3d.Lerp((Vector3d)v10, (Vector3d)v11, ty);
Vector3d c = Vector3d.Lerp(a, b, tx);
return(new Vector3i((int)Math.Round(c.x), (int)Math.Round(c.y), (int)Math.Round(c.z)));
}
public Vector3d FromGrid(Vector3i gridpoint)
{
Vector3f pointf = CellSize * (Vector3f)gridpoint;
return((Vector3d)GridFrame.FromFrameP(pointf));
}
//! relative translation
public void Translate(Vector3i vTranslate)
{
Min.Add(vTranslate);
Max.Add(vTranslate);
}
public AxisAlignedBox3i(int fCubeSize)
{
Min = new Vector3i(0, 0, 0);
Max = new Vector3i(fCubeSize, fCubeSize, fCubeSize);
}
public AxisAlignedBox3i(int xmin, int ymin, int zmin, int xmax, int ymax, int zmax)
{
Min = new Vector3i(xmin, ymin, zmin);
Max = new Vector3i(xmax, ymax, zmax);
}
public int Distance(Vector3i v)
{
return((int)Math.Sqrt(DistanceSquared(v)));
}
public bool Contains(Vector3i v)
{
return((Min.x <= v.x) && (Min.y <= v.y) && (Min.z <= v.z) &&
(Max.x >= v.x) && (Max.y >= v.y) && (Max.z >= v.z));
}
public AxisAlignedBox3i(bool bIgnore)
{
Min = new Vector3i(int.MaxValue, int.MaxValue, int.MaxValue);
Max = new Vector3i(int.MinValue, int.MinValue, int.MinValue);
}
public bool Get(Vector3i idx)
{
int i = idx.z * slab_size + idx.y * row_size + idx.x;
return(Bits[i]);
}
void generate_graph(DenseGrid3f supportGrid, DenseGridTrilinearImplicit distanceField)
{
int ni = supportGrid.ni, nj = supportGrid.nj, nk = supportGrid.nk;
float dx = (float)CellSize;
Vector3f origin = this.GridOrigin;
// parameters for initializing cost grid
float MODEL_SPACE = 0.01f; // needs small positive so that points on triangles count as inside (eg on ground plane)
//float MODEL_SPACE = 2.0f*(float)CellSize;
float CRAZY_DISTANCE = 99999.0f;
bool UNIFORM_DISTANCE = true;
float MAX_DIST = 10 * (float)CellSize;
// parameters for sorting seeds
Vector3i center_idx = new Vector3i(ni / 2, 0, nk / 2); // middle
//Vector3i center_idx = new Vector3i(0, 0, 0); // corner
bool reverse_per_layer = true;
DenseGrid3f costGrid = new DenseGrid3f(supportGrid);
foreach ( Vector3i ijk in costGrid.Indices() ) {
Vector3d cell_center = new Vector3f(ijk.x * dx, ijk.y * dx, ijk.z * dx) + origin;
float f = (float)distanceField.Value(ref cell_center);
if (f <= MODEL_SPACE)
f = CRAZY_DISTANCE;
else if (UNIFORM_DISTANCE)
f = 1.0f;
else if (f > MAX_DIST)
f = MAX_DIST;
costGrid[ijk] = f;
}
// Find seeds on each layer, sort, and add to accumulated bottom-up seeds list.
// This sorting has an *enormous* effect on the support generation.
List<Vector3i> seeds = new List<Vector3i>();
List<Vector3i> layer_seeds = new List<Vector3i>();
for (int j = 0; j < nj; ++j) {
layer_seeds.Clear();
for (int k = 0; k < nk; ++k) {
for (int i = 0; i < ni; ++i) {
if (supportGrid[i, j, k] == SUPPORT_TIP_BASE)
layer_seeds.Add(new Vector3i(i, j, k));
}
}
layer_seeds.Sort((a, b) => {
Vector3i pa = a; pa.y = 0;
Vector3i pb = b; pb.y = 0;
int sa = (pa-center_idx).LengthSquared, sb = (pb-center_idx).LengthSquared;
return sa.CompareTo(sb);
});
// reversing sort order is intresting?
if(reverse_per_layer)
layer_seeds.Reverse();
seeds.AddRange(layer_seeds);
}
HashSet<Vector3i> seed_indices = new HashSet<Vector3i>(seeds);
// gives very different results...
if (ProcessBottomUp == false)
seeds.Reverse();
// for linear index a, is this a node we allow in graph? (ie graph bounds)
Func<int, bool> node_filter_f = (a) => {
Vector3i ai = costGrid.to_index(a);
// why not y check??
return ai.x > 0 && ai.z > 0 && ai.x != ni - 1 && ai.y != nj - 1 && ai.z != nk - 1;
};
// distance from linear index a to linear index b
// this defines the cost field we want to find shortest path through
Func<int, int, float> node_dist_f = (a, b) => {
Vector3i ai = costGrid.to_index(a), bi = costGrid.to_index(b);
if (bi.y >= ai.y) // b.y should always be a.y-1
return float.MaxValue;
float sg = supportGrid[bi];
// don't connect to tips
//if (sg == SUPPORT_TIP_BASE || sg == SUPPORT_TIP_TOP)
// return float.MaxValue;
if (sg == SUPPORT_TIP_TOP)
return float.MaxValue;
if (sg < 0)
return -999999; // if b is already used, we will terminate there, so this is a good choice
// otherwise cost is sqr-grid-distance + costGrid value (which is basically distance to surface)
float c = costGrid[b];
float f = (float)(Math.Sqrt((bi - ai).LengthSquared) * CellSize);
//float f = 0;
return c + f;
};
// which linear-index nbrs to consider for linear index a
Func<int, IEnumerable<int>> neighbour_f = (a) => {
Vector3i ai = costGrid.to_index(a);
return down_neighbours(ai, costGrid);
};
// when do we terminate
Func<int, bool> terminate_f = (a) => {
Vector3i ai = costGrid.to_index(a);
// terminate if we hit existing support path
if (seed_indices.Contains(ai) == false && supportGrid[ai] < 0)
return true;
// terminate if we hit ground plane
if (ai.y == 0)
return true;
return false;
};
DijkstraGraphDistance dijkstra = new DijkstraGraphDistance(ni * nj * nk, false,
node_filter_f, node_dist_f, neighbour_f);
dijkstra.TrackOrder = true;
List<int> path = new List<int>();
Graph = new DGraph3();
Dictionary<Vector3i, int> CellToGraph = new Dictionary<Vector3i, int>();
TipVertices = new HashSet<int>();
TipBaseVertices = new HashSet<int>();
GroundVertices = new HashSet<int>();
// seeds are tip-base points
for (int k = 0; k < seeds.Count; ++k) {
// add seed point (which is a tip-base vertex) as seed for dijkstra prop
int seed = costGrid.to_linear(seeds[k]);
dijkstra.Reset();
dijkstra.AddSeed(seed, 0);
// compute to termination (ground, existing node, etc)
int base_node = dijkstra.ComputeToNode(terminate_f);
if (base_node < 0)
base_node = dijkstra.GetOrder().Last();
// extract the path
path.Clear();
dijkstra.GetPathToSeed(base_node, path);
int N = path.Count;
// first point on path is termination point.
// create vertex for it if we have not yet
Vector3i basept_idx = supportGrid.to_index(path[0]);
int basept_vid;
if ( CellToGraph.TryGetValue(basept_idx, out basept_vid) == false ) {
Vector3d curv = get_cell_center(basept_idx);
if (basept_idx.y == 0) {
curv.y = 0;
}
basept_vid = Graph.AppendVertex(curv);
if (basept_idx.y == 0) {
GroundVertices.Add(basept_vid);
}
CellToGraph[basept_idx] = basept_vid;
}
int cur_vid = basept_vid;
// now walk up path and create vertices as necessary
for (int i = 0; i < N; ++i) {
int idx = path[i];
if ( supportGrid[idx] >= 0 )
supportGrid[idx] = SUPPORT_GRID_USED;
if ( i > 0 ) {
Vector3i next_idx = supportGrid.to_index(path[i]);
int next_vid;
if (CellToGraph.TryGetValue(next_idx, out next_vid) == false) {
Vector3d nextv = get_cell_center(next_idx);
next_vid = Graph.AppendVertex(nextv);
CellToGraph[next_idx] = next_vid;
}
Graph.AppendEdge(cur_vid, next_vid);
cur_vid = next_vid;
}
}
// seed was tip-base so we should always get back there. Then we
// explicitly add tip-top and edge to it.
if ( supportGrid[path[N-1]] == SUPPORT_TIP_BASE ) {
Vector3i vec_idx = supportGrid.to_index(path[N-1]);
TipBaseVertices.Add(CellToGraph[vec_idx]);
Vector3i tip_idx = vec_idx + Vector3i.AxisY;
int tip_vid;
if (CellToGraph.TryGetValue(tip_idx, out tip_vid) == false) {
Vector3d tipv = get_cell_center(tip_idx);
tip_vid = Graph.AppendVertex(tipv);
CellToGraph[tip_idx] = tip_vid;
Graph.AppendEdge(cur_vid, tip_vid);
TipVertices.Add(tip_vid);
}
}
}
/*
* Snap tips to surface
*/
gParallel.ForEach(TipVertices, (tip_vid) => {
bool snapped = false;
Vector3d v = Graph.GetVertex(tip_vid);
Frame3f hitF;
// try shooting ray straight up. if that hits, and point is close, we use it
if (MeshQueries.RayHitPointFrame(Mesh, MeshSpatial, new Ray3d(v, Vector3d.AxisY), out hitF)) {
if (v.Distance(hitF.Origin) < 2 * CellSize) {
v = hitF.Origin;
snapped = true;
}
}
// if that failed, try straight down
if (!snapped) {
if (MeshQueries.RayHitPointFrame(Mesh, MeshSpatial, new Ray3d(v, -Vector3d.AxisY), out hitF)) {
if (v.Distance(hitF.Origin) < CellSize) {
v = hitF.Origin;
snapped = true;
}
}
}
// if it missed, or hit pt was too far, find nearest point and try that
if (!snapped) {
hitF = MeshQueries.NearestPointFrame(Mesh, MeshSpatial, v);
if (v.Distance(hitF.Origin) < 2 * CellSize) {
v = hitF.Origin;
snapped = true;
}
// can this ever fail? tips should always be within 2 cells...
}
if (snapped)
Graph.SetVertex(tip_vid, v);
});
}
public AxisAlignedBox3i(int fWidth, int fHeight, int fDepth)
{
Min = new Vector3i(0, 0, 0);
Max = new Vector3i(fWidth, fHeight, fDepth);
}
Vector3d get_cell_center(Vector3i ijk)
{
return new Vector3d(ijk.x * CellSize, ijk.y * CellSize, ijk.z * CellSize) + this.GridOrigin;
}
public AxisAlignedBox3i(Vector3i vMin, Vector3i vMax)
{
Min = new Vector3i(Math.Min(vMin.x, vMax.x), Math.Min(vMin.y, vMax.y), Math.Min(vMin.z, vMax.z));
Max = new Vector3i(Math.Max(vMin.x, vMax.x), Math.Max(vMin.y, vMax.y), Math.Max(vMin.z, vMax.z));
}
protected Vector3i lerp(ref Vector3i a, ref Vector3i b, double t)
{
Vector3d c = Vector3d.Lerp((Vector3d)a, (Vector3d)b, t);
return(new Vector3i((int)Math.Round(c.x), (int)Math.Round(c.y), (int)Math.Round(c.z)));
}
public AxisAlignedBox3i(Vector3i vCenter, int fHalfWidth, int fHalfHeight, int fHalfDepth)
{
Min = new Vector3i(vCenter.x - fHalfWidth, vCenter.y - fHalfHeight, vCenter.z - fHalfDepth);
Max = new Vector3i(vCenter.x + fHalfWidth, vCenter.y + fHalfHeight, vCenter.z + fHalfDepth);
}
public MultigridIndexer3(Vector3i blockSize)
{
BlockSize = blockSize;
OuterShift = BlockShift = Vector3i.Zero;
}
public AxisAlignedBox3i(Vector3i vCenter, int fHalfSize)
{
Min = new Vector3i(vCenter.x - fHalfSize, vCenter.y - fHalfSize, vCenter.z - fHalfSize);
Max = new Vector3i(vCenter.x + fHalfSize, vCenter.y + fHalfSize, vCenter.z + fHalfSize);
}
public Vector3i FromBlock(Vector3i block_idx)
{
Vector3i outer_idx = (block_idx + BlockShift) * BlockSize;
return(outer_idx + OuterShift);
}
public AxisAlignedBox3i(Vector3i vCenter)
{
Min = Max = vCenter;
}
Vector3f cell_center(Vector3i ijk)
{
return(new Vector3f((float)ijk.x * CellSize + grid_origin[0],
(float)ijk.y * CellSize + grid_origin[1],
(float)ijk.z * CellSize + grid_origin[2]));
}
/// <summary>
/// Insert point without locking / thread-safety
/// </summary>
public void InsertPointUnsafe(T value, Vector3d pos)
{
Vector3i idx = Indexer.ToGrid(pos);
insert_point(value, idx, false);
}
public float this[Vector3i ijk] {
get { return(Buffer[ijk.x + ni * (ijk.y + nj * ijk.z)]); }
set { Buffer[ijk.x + ni * (ijk.y + nj * ijk.z)] = value; }
}
public int ToLinear(Vector3i idx)
{
return(idx.z * slab_size + idx.y * row_size + idx.x);
}
