public static EdgeSpan find_edge_path(DMesh3 mesh, int v0, int v1) { if (v0 == v1) { throw new Exception("same vertices!"); } int eid = mesh.FindEdge(v0, v1); if (eid >= 0) { return(EdgeSpan.FromEdges(mesh, new List <int>() { eid })); } DijkstraGraphDistance dist = DijkstraGraphDistance.MeshVertices(mesh); //DijkstraGraphDistance dist = DijkstraGraphDistance.MeshVerticesSparse(mesh); dist.AddSeed(v0, 0); dist.ComputeToNode(v1); List <int> vpath = new List <int>(); bool bOK = dist.GetPathToSeed(v1, vpath); Util.gDevAssert(bOK); vpath.Reverse(); return(EdgeSpan.FromVertices(mesh, vpath)); }
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); } }); }