/// <summary>
/// walk through graph from fromVtx, in direction of eid, until we hit the next junction vertex
/// </summary>
public static List <int> WalkToNextNonRegularVtx(DGraph3 graph, int fromVtx, int eid)
{
List <int> path = new List <int>();
path.Add(fromVtx);
int cur_vid = fromVtx;
int cur_eid = eid;
bool bContinue = true;
while (bContinue)
{
Index2i next = DGraph3Util.NextEdgeAndVtx(cur_eid, cur_vid, graph);
int next_eid = next.a;
int next_vtx = next.b;
if (next_eid == int.MaxValue)
{
if (graph.IsRegularVertex(next_vtx) == false)
{
path.Add(next_vtx);
bContinue = false;
}
else
{
throw new Exception("WalkToNextNonRegularVtx: have no next edge but vtx is regular - how?");
}
}
else
{
path.Add(next_vtx);
cur_vid = next_vtx;
cur_eid = next_eid;
}
}
return(path);
}
public GraphTubeMesher(GraphSupportGenerator support_gen)
{
Graph = support_gen.Graph;
TipVertices = support_gen.TipVertices;
GroundVertices = support_gen.GroundVertices;
SamplerCellSizeHint = support_gen.CellSize;
}
/// <summary>
/// If we are at edge eid, which as one vertex prev_vid, find 'other' vertex, and other edge connected to that vertex,
/// and return pair [next_edge, shared_vtx]
/// Returns [int.MaxValue, shared_vtx] if shared_vtx is not valence=2 (ie stops at boundaries and complex junctions)
/// </summary>
public static Index2i NextEdgeAndVtx(int eid, int prev_vid, DGraph3 graph)
{
Index2i ev = graph.GetEdgeV(eid);
if (ev.a == DGraph3.InvalidID)
{
return(Index2i.Max);
}
int next_vid = (ev.a == prev_vid) ? ev.b : ev.a;
if (graph.GetVtxEdgeCount(next_vid) != 2)
{
return(new Index2i(int.MaxValue, next_vid));
}
foreach (int next_eid in graph.VtxEdgesItr(next_vid))
{
if (next_eid != eid)
{
return(new Index2i(next_eid, next_vid));
}
}
return(Index2i.Max);
}
public void AppendGraph(DGraph3 graph, int gid = -1)
{
int[] mapV = new int[graph.MaxVertexID];
foreach (int vid in graph.VertexIndices())
{
mapV[vid] = this.AppendVertex(graph.GetVertex(vid));
}
foreach (int eid in graph.EdgeIndices())
{
Index2i ev = graph.GetEdgeV(eid);
int use_gid = (gid == -1) ? graph.GetEdgeGroup(eid) : gid;
this.AppendEdge(mapV[ev.a], mapV[ev.b], use_gid);
}
}
/// <summary>
/// foreach edge [vid,b] connected to junction vertex vid, remove, add new vertex c,
/// and then add new edge [b,c]. Optionally move c a bit back along edge from vid.
/// </summary>
public static void DisconnectJunction(DGraph3 graph, int vid, double shrinkFactor = 1.0)
{
Vector3d v = graph.GetVertex(vid);
int[] nbr_verts = graph.VtxVerticesItr(vid).ToArray();
for (int k = 0; k < nbr_verts.Length; ++k)
{
int eid = graph.FindEdge(vid, nbr_verts[k]);
graph.RemoveEdge(eid, true);
if (graph.IsVertex(nbr_verts[k]))
{
Vector3d newpos = Vector3d.Lerp(graph.GetVertex(nbr_verts[k]), v, shrinkFactor);
int newv = graph.AppendVertex(newpos);
graph.AppendEdge(nbr_verts[k], newv);
}
}
}
protected void compute_full(IEnumerable <int> Triangles, bool bIsFullMeshHint = false)
{
Graph = new DGraph3();
if (WantGraphEdgeInfo)
{
GraphEdges = new DVector <GraphEdgeInfo>();
}
Vertices = new Dictionary <Vector3d, int>();
// multithreaded precomputation of per-vertex values
double[] vertex_values = null;
if (PrecomputeVertexValues)
{
vertex_values = new double[Mesh.MaxVertexID];
IEnumerable <int> verts = Mesh.VertexIndices();
if (bIsFullMeshHint == false)
{
MeshVertexSelection vertices = new MeshVertexSelection(Mesh);
vertices.SelectTriangleVertices(Triangles);
verts = vertices;
}
gParallel.ForEach(verts, (vid) => {
vertex_values[vid] = ValueF(Mesh.GetVertex(vid));
});
VertexValueF = (vid) => { return(vertex_values[vid]); };
}
foreach (int tid in Triangles)
{
Vector3dTuple3 tv = new Vector3dTuple3();
Mesh.GetTriVertices(tid, ref tv.V0, ref tv.V1, ref tv.V2);
Index3i triVerts = Mesh.GetTriangle(tid);
Vector3d f = (VertexValueF != null) ?
new Vector3d(VertexValueF(triVerts.a), VertexValueF(triVerts.b), VertexValueF(triVerts.c))
: new Vector3d(ValueF(tv.V0), ValueF(tv.V1), ValueF(tv.V2));
// round f to 0 within epsilon?
if (f.x < 0 && f.y < 0 && f.z < 0)
{
continue;
}
if (f.x > 0 && f.y > 0 && f.z > 0)
{
continue;
}
Index3i triEdges = Mesh.GetTriEdges(tid);
if (f.x * f.y * f.z == 0)
{
int z0 = (f.x == 0) ? 0 : ((f.y == 0) ? 1 : 2);
int i1 = (z0 + 1) % 3, i2 = (z0 + 2) % 3;
if (f[i1] * f[i2] > 0)
{
continue; // single-vertex-crossing case, skip here and let other edges catch it
}
if (f[i1] == 0 || f[i2] == 0)
{
// on-edge case
int z1 = f[i1] == 0 ? i1 : i2;
if ((z0 + 1) % 3 != z1)
{
int tmp = z0; z0 = z1; z1 = tmp; // catch reverse-orientation cases
}
int e0 = add_or_append_vertex(Mesh.GetVertex(triVerts[z0]));
int e1 = add_or_append_vertex(Mesh.GetVertex(triVerts[z1]));
int graph_eid = Graph.AppendEdge(e0, e1, (int)TriangleCase.OnEdge);
if (graph_eid >= 0 && WantGraphEdgeInfo)
{
add_on_edge(graph_eid, tid, triEdges[z0], new Index2i(e0, e1));
}
}
else
{
// edge/vertex case
Util.gDevAssert(f[i1] * f[i2] < 0);
int vert_vid = add_or_append_vertex(Mesh.GetVertex(triVerts[z0]));
int i = i1, j = i2;
if (triVerts[j] < triVerts[i])
{
int tmp = i; i = j; j = tmp;
}
Vector3d cross = find_crossing(tv[i], tv[j], f[i], f[j]);
int cross_vid = add_or_append_vertex(cross);
add_edge_pos(triVerts[i], triVerts[j], cross);
int graph_eid = Graph.AppendEdge(vert_vid, cross_vid, (int)TriangleCase.EdgeVertex);
if (graph_eid >= 0 && WantGraphEdgeInfo)
{
add_edge_vert(graph_eid, tid, triEdges[(z0 + 1) % 3], triVerts[z0], new Index2i(vert_vid, cross_vid));
}
}
}
else
{
Index3i cross_verts = Index3i.Min;
int less_than = 0;
for (int tei = 0; tei < 3; ++tei)
{
int i = tei, j = (tei + 1) % 3;
if (f[i] < 0)
{
less_than++;
}
if (f[i] * f[j] > 0)
{
continue;
}
if (triVerts[j] < triVerts[i])
{
int tmp = i; i = j; j = tmp;
}
Vector3d cross = find_crossing(tv[i], tv[j], f[i], f[j]);
cross_verts[tei] = add_or_append_vertex(cross);
add_edge_pos(triVerts[i], triVerts[j], cross);
}
int e0 = (cross_verts.a == int.MinValue) ? 1 : 0;
int e1 = (cross_verts.c == int.MinValue) ? 1 : 2;
if (e0 == 0 && e1 == 2) // preserve orientation order
{
e0 = 2; e1 = 0;
}
// preserving orientation does not mean we get a *consistent* orientation across faces.
// To do that, we need to assign "sides". Either we have 1 less-than-0 or 1 greater-than-0 vtx.
// Arbitrary decide that we want loops oriented like bdry loops would be if we discarded less-than side.
// In that case, when we are "cutting off" one vertex, orientation would end up flipped
if (less_than == 1)
{
int tmp = e0; e0 = e1; e1 = tmp;
}
int ev0 = cross_verts[e0];
int ev1 = cross_verts[e1];
// [RMS] if function is garbage, we can end up w/ case where both crossings
// happen at same vertex, even though values are not the same (eg if
// some values are double.MaxValue). We will just fail in these cases.
if (ev0 != ev1)
{
Util.gDevAssert(ev0 != int.MinValue && ev1 != int.MinValue);
int graph_eid = Graph.AppendEdge(ev0, ev1, (int)TriangleCase.EdgeEdge);
if (graph_eid >= 0 && WantGraphEdgeInfo)
{
add_edge_edge(graph_eid, tid, new Index2i(triEdges[e0], triEdges[e1]), new Index2i(ev0, ev1));
}
}
}
}
Vertices = null;
}
public GraphTubeMesher(DGraph3 graph)
{
Graph = graph;
}
protected void compute_full(IEnumerable <int> Triangles)
{
Graph = new DGraph3();
if (WantGraphEdgeInfo)
{
GraphEdges = new DVector <GraphEdgeInfo>();
}
Vertices = new Dictionary <Vector3d, int>();
foreach (int tid in Triangles)
{
Vector3dTuple3 tv = new Vector3dTuple3();
Mesh.GetTriVertices(tid, ref tv.V0, ref tv.V1, ref tv.V2);
Vector3d f = new Vector3d(ValueF(tv.V0), ValueF(tv.V1), ValueF(tv.V2));
// round f to 0 within epsilon?
if (f.x < 0 && f.y < 0 && f.z < 0)
{
continue;
}
if (f.x > 0 && f.y > 0 && f.z > 0)
{
continue;
}
Index3i triVerts = Mesh.GetTriangle(tid);
Index3i triEdges = Mesh.GetTriEdges(tid);
if (f.x * f.y * f.z == 0)
{
int z0 = (f.x == 0) ? 0 : ((f.y == 0) ? 1 : 2);
int i1 = (z0 + 1) % 3, i2 = (z0 + 2) % 3;
if (f[i1] * f[i2] > 0)
{
continue; // single-vertex-crossing case, skip here and let other edges catch it
}
if (f[i1] == 0 || f[i2] == 0)
{
// on-edge case
int z1 = f[i1] == 0 ? i1 : i2;
int e0 = add_or_append_vertex(Mesh.GetVertex(triVerts[z0]));
int e1 = add_or_append_vertex(Mesh.GetVertex(triVerts[z1]));
int graph_eid = Graph.AppendEdge(e0, e1, (int)TriangleCase.OnEdge);
if (WantGraphEdgeInfo)
{
add_on_edge(graph_eid, tid, triEdges[z0]);
}
}
else
{
// edge/vertex case
Util.gDevAssert(f[i1] * f[i2] < 0);
int vert_vid = add_or_append_vertex(Mesh.GetVertex(triVerts[z0]));
int i = i1, j = i2;
if (triVerts[j] < triVerts[i])
{
int tmp = i; i = j; j = tmp;
}
Vector3d cross = find_crossing(tv[i], tv[j], f[i], f[j]);
int cross_vid = add_or_append_vertex(cross);
int graph_eid = Graph.AppendEdge(vert_vid, cross_vid, (int)TriangleCase.EdgeVertex);
if (WantGraphEdgeInfo)
{
add_edge_edge(graph_eid, tid, new Index2i(triEdges[(z0 + 1) % 3], triVerts[z0]));
}
}
}
else
{
Index3i cross_verts = Index3i.Min;
for (int ti = 0; ti < 3; ++ti)
{
int i = ti, j = (ti + 1) % 3;
if (f[i] * f[j] > 0)
{
continue;
}
if (triVerts[j] < triVerts[i])
{
int tmp = i; i = j; j = tmp;
}
Vector3d cross = find_crossing(tv[i], tv[j], f[i], f[j]);
cross_verts[ti] = add_or_append_vertex(cross);
}
int e0 = (cross_verts.a == int.MinValue) ? 1 : 0;
int e1 = (cross_verts.c == int.MinValue) ? 1 : 2;
int ev0 = cross_verts[e0];
int ev1 = cross_verts[e1];
Util.gDevAssert(ev0 != int.MinValue && ev1 != int.MinValue);
int graph_eid = Graph.AppendEdge(ev0, ev1, (int)TriangleCase.EdgeEdge);
if (WantGraphEdgeInfo)
{
add_edge_edge(graph_eid, tid, new Index2i(triEdges[e0], triEdges[e1]));
}
}
}
Vertices = null;
}
/// <summary>
/// Decompose graph into simple polylines and polygons.
/// </summary>
public static Curves ExtractCurves(DGraph3 graph)
{
Curves c = new Curves();
c.Loops = new List <DCurve3>();
c.Paths = new List <DCurve3>();
HashSet <int> used = new HashSet <int>();
// find boundary and junction vertices
HashSet <int> boundaries = new HashSet <int>();
HashSet <int> junctions = new HashSet <int>();
foreach (int vid in graph.VertexIndices())
{
if (graph.IsBoundaryVertex(vid))
{
boundaries.Add(vid);
}
if (graph.IsJunctionVertex(vid))
{
junctions.Add(vid);
}
}
// walk paths from boundary vertices
foreach (int start_vid in boundaries)
{
int vid = start_vid;
int eid = graph.GetVtxEdges(vid)[0];
if (used.Contains(eid))
{
continue;
}
DCurve3 path = new DCurve3()
{
Closed = false
};
path.AppendVertex(graph.GetVertex(vid));
while (true)
{
used.Add(eid);
Index2i next = NextEdgeAndVtx(eid, vid, graph);
eid = next.a;
vid = next.b;
path.AppendVertex(graph.GetVertex(vid));
if (boundaries.Contains(vid) || junctions.Contains(vid))
{
break; // done!
}
}
c.Paths.Add(path);
}
// ok we should be done w/ boundary verts now...
boundaries.Clear();
foreach (int start_vid in junctions)
{
foreach (int outgoing_eid in graph.VtxEdgesItr(start_vid))
{
if (used.Contains(outgoing_eid))
{
continue;
}
int vid = start_vid;
int eid = outgoing_eid;
DCurve3 path = new DCurve3()
{
Closed = false
};
path.AppendVertex(graph.GetVertex(vid));
while (true)
{
used.Add(eid);
Index2i next = NextEdgeAndVtx(eid, vid, graph);
eid = next.a;
vid = next.b;
path.AppendVertex(graph.GetVertex(vid));
if (eid == int.MaxValue || junctions.Contains(vid))
{
break; // done!
}
}
// we could end up back at our start junction vertex!
if (vid == start_vid)
{
path.RemoveVertex(path.VertexCount - 1);
path.Closed = true;
c.Loops.Add(path);
// need to mark incoming edge as used...but is it valid now?
//Util.gDevAssert(eid != int.MaxValue);
if (eid != int.MaxValue)
{
used.Add(eid);
}
}
else
{
c.Paths.Add(path);
}
}
}
// all that should be left are continuous loops...
foreach (int start_eid in graph.EdgeIndices())
{
if (used.Contains(start_eid))
{
continue;
}
int eid = start_eid;
Index2i ev = graph.GetEdgeV(eid);
int vid = ev.a;
DCurve3 poly = new DCurve3()
{
Closed = true
};
poly.AppendVertex(graph.GetVertex(vid));
while (true)
{
used.Add(eid);
Index2i next = NextEdgeAndVtx(eid, vid, graph);
eid = next.a;
vid = next.b;
poly.AppendVertex(graph.GetVertex(vid));
if (eid == int.MaxValue || junctions.Contains(vid))
{
throw new Exception("how did this happen??");
}
if (used.Contains(eid))
{
break;
}
}
poly.RemoveVertex(poly.VertexCount - 1);
c.Loops.Add(poly);
}
return(c);
}
public DGraph3(DGraph3 copy) : base()
{
vertices = new DVector <double>();
AppendGraph(copy);
}
/// <summary>
/// Erode inwards from open boundary vertices of graph (ie vtx with single edge).
/// Resulting graph is not compact (!)
/// </summary>
public static void ErodeOpenSpurs(DGraph3 graph)
{
var used = new HashSet <int>(); // do we need this?
// find boundary and junction vertices
var boundaries = new HashSet <int>();
var junctions = new HashSet <int>();
foreach (int vid in graph.VertexIndices())
{
if (graph.IsBoundaryVertex(vid))
{
boundaries.Add(vid);
}
if (graph.IsJunctionVertex(vid))
{
junctions.Add(vid);
}
}
// walk paths from boundary vertices
foreach (int start_vid in boundaries)
{
if (graph.IsVertex(start_vid) == false)
{
continue;
}
int vid = start_vid;
int eid = graph.GetVtxEdges(vid)[0];
if (used.Contains(eid))
{
continue;
}
var pathE = new List <int>();
if (pathE != null)
{
pathE.Add(eid);
}
while (true)
{
used.Add(eid);
Index2i next = NextEdgeAndVtx(eid, vid, graph);
eid = next.a;
vid = next.b;
if (boundaries.Contains(vid) || junctions.Contains(vid))
{
break; // done!
}
if (pathE != null)
{
pathE.Add(eid);
}
}
// delete this path
foreach (int path_eid in pathE)
{
graph.RemoveEdge(path_eid, true);
}
}
}
/// <summary>
/// Decompose graph into simple polylines and polygons.
/// </summary>
public static Curves ExtractCurves(DGraph3 graph,
bool bWantLoopIndices = false,
Func <int, bool> CurveOrientationF = null)
{
var c = new Curves();
c.Loops = new List <DCurve3>();
c.Paths = new List <DCurve3>();
if (bWantLoopIndices)
{
c.LoopEdges = new List <List <int> >();
c.PathEdges = new List <List <int> >();
}
var used = new HashSet <int>();
// find boundary and junction vertices
var boundaries = new HashSet <int>();
var junctions = new HashSet <int>();
foreach (int vid in graph.VertexIndices())
{
if (graph.IsBoundaryVertex(vid))
{
boundaries.Add(vid);
}
if (graph.IsJunctionVertex(vid))
{
junctions.Add(vid);
}
}
// walk paths from boundary vertices
foreach (int start_vid in boundaries)
{
int vid = start_vid;
int eid = graph.GetVtxEdges(vid)[0];
if (used.Contains(eid))
{
continue;
}
bool reverse = (CurveOrientationF != null) ? CurveOrientationF(eid) : false;
var path = new DCurve3()
{
Closed = false
};
List <int> pathE = (bWantLoopIndices) ? new List <int>() : null;
path.AppendVertex(graph.GetVertex(vid));
if (pathE != null)
{
pathE.Add(eid);
}
while (true)
{
used.Add(eid);
Index2i next = NextEdgeAndVtx(eid, vid, graph);
eid = next.a;
vid = next.b;
path.AppendVertex(graph.GetVertex(vid));
if (boundaries.Contains(vid) || junctions.Contains(vid))
{
break; // done!
}
if (pathE != null)
{
pathE.Add(eid);
}
}
if (reverse)
{
path.Reverse();
}
c.Paths.Add(path);
if (pathE != null)
{
Util.gDevAssert(pathE.Count == path.VertexCount - 1);
if (reverse)
{
pathE.Reverse();
}
c.PathEdges.Add(pathE);
}
}
// ok we should be done w/ boundary verts now...
//boundaries.Clear();
c.BoundaryV = boundaries;
foreach (int start_vid in junctions)
{
foreach (int outgoing_eid in graph.VtxEdgesItr(start_vid))
{
if (used.Contains(outgoing_eid))
{
continue;
}
int vid = start_vid;
int eid = outgoing_eid;
bool reverse = (CurveOrientationF != null) ? CurveOrientationF(eid) : false;
var path = new DCurve3()
{
Closed = false
};
List <int> pathE = (bWantLoopIndices) ? new List <int>() : null;
path.AppendVertex(graph.GetVertex(vid));
if (pathE != null)
{
pathE.Add(eid);
}
while (true)
{
used.Add(eid);
Index2i next = NextEdgeAndVtx(eid, vid, graph);
eid = next.a;
vid = next.b;
path.AppendVertex(graph.GetVertex(vid));
if (eid == int.MaxValue || junctions.Contains(vid))
{
break; // done!
}
if (pathE != null)
{
pathE.Add(eid);
}
}
// we could end up back at our start junction vertex!
if (vid == start_vid)
{
path.RemoveVertex(path.VertexCount - 1);
path.Closed = true;
if (reverse)
{
path.Reverse();
}
c.Loops.Add(path);
if (pathE != null)
{
Util.gDevAssert(pathE.Count == path.VertexCount);
if (reverse)
{
pathE.Reverse();
}
c.LoopEdges.Add(pathE);
}
// need to mark incoming edge as used...but is it valid now?
//Util.gDevAssert(eid != int.MaxValue);
if (eid != int.MaxValue)
{
used.Add(eid);
}
}
else
{
if (reverse)
{
path.Reverse();
}
c.Paths.Add(path);
if (pathE != null)
{
Util.gDevAssert(pathE.Count == path.VertexCount - 1);
if (reverse)
{
pathE.Reverse();
}
c.PathEdges.Add(pathE);
}
}
}
}
c.JunctionV = junctions;
// all that should be left are continuous loops...
foreach (int start_eid in graph.EdgeIndices())
{
if (used.Contains(start_eid))
{
continue;
}
int eid = start_eid;
Index2i ev = graph.GetEdgeV(eid);
int vid = ev.a;
bool reverse = (CurveOrientationF != null) ? CurveOrientationF(eid) : false;
var poly = new DCurve3()
{
Closed = true
};
List <int> polyE = (bWantLoopIndices) ? new List <int>() : null;
poly.AppendVertex(graph.GetVertex(vid));
if (polyE != null)
{
polyE.Add(eid);
}
while (true)
{
used.Add(eid);
Index2i next = NextEdgeAndVtx(eid, vid, graph);
eid = next.a;
vid = next.b;
poly.AppendVertex(graph.GetVertex(vid));
if (polyE != null)
{
polyE.Add(eid);
}
if (eid == int.MaxValue || junctions.Contains(vid))
{
throw new Exception("how did this happen??");
}
if (used.Contains(eid))
{
break;
}
}
poly.RemoveVertex(poly.VertexCount - 1);
if (reverse)
{
poly.Reverse();
}
c.Loops.Add(poly);
if (polyE != null)
{
polyE.RemoveAt(polyE.Count - 1);
Util.gDevAssert(polyE.Count == poly.VertexCount);
if (reverse)
{
polyE.Reverse();
}
c.LoopEdges.Add(polyE);
}
}
return(c);
}
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);
});
}
void constrained_smooth(DGraph3 graph, double surfDist, double dotThresh, double alpha, int rounds)
{
int NV = graph.MaxVertexID;
Vector3d[] pos = new Vector3d[NV];
for (int ri = 0; ri < rounds; ++ri) {
gParallel.ForEach(graph.VertexIndices(), (vid) => {
Vector3d v = graph.GetVertex(vid);
if ( GroundVertices.Contains(vid) || TipVertices.Contains(vid) ) {
pos[vid] = v;
return;
}
// for tip base vertices, we could allow them to move down and away within angle cone...
if (TipBaseVertices.Contains(vid)) {
pos[vid] = v;
return;
}
// compute smoothed position of vtx
Vector3d centroid = Vector3d.Zero; int nbr_count = 0;
foreach (int nbr_vid in graph.VtxVerticesItr(vid)) {
centroid += graph.GetVertex(nbr_vid);
nbr_count++;
}
if (nbr_count == 1) {
pos[vid] = v;
return;
}
centroid /= nbr_count;
Vector3d vnew = (1 - alpha) * v + (alpha) * centroid;
// make sure we don't violate angle constraint to any nbrs
int attempt = 0;
try_again:
foreach ( int nbr_vid in graph.VtxVerticesItr(vid)) {
Vector3d dv = graph.GetVertex(nbr_vid) - vnew;
dv.Normalize();
double dot = dv.Dot(Vector3d.AxisY);
if ( Math.Abs(dot) < dotThresh ) {
if (attempt++ < 3) {
vnew = Vector3d.Lerp(v, vnew, 0.66);
goto try_again;
} else {
pos[vid] = v;
return;
}
}
}
// offset from nearest point on surface
Frame3f fNearest = MeshQueries.NearestPointFrame(Mesh, MeshSpatial, vnew, true);
Vector3d vNearest = fNearest.Origin;
double dist = vnew.Distance(vNearest);
bool inside = MeshSpatial.IsInside(vnew);
if (inside || dist < surfDist) {
Vector3d normal = fNearest.Z;
// don't push down?
if (normal.Dot(Vector3d.AxisY) < 0) {
normal.y = 0; normal.Normalize();
}
vnew = fNearest.Origin + surfDist * normal;
}
pos[vid] = vnew;
});
foreach (int vid in graph.VertexIndices())
graph.SetVertex(vid, pos[vid]);
}
}
