int add_or_append_vertex(Vector3d pos) { int vid; if (Vertices.TryGetValue(pos, out vid) == false) { vid = Graph.AppendVertex(pos); Vertices.Add(pos, vid); } return(vid); }
/// <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); } } }
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); }); }