public static int distance(bool[,] passable, int startX, int startY, int goalX, int goalY) { int[,] distanceMatrix = new int[passable.GetLength(0), passable.GetLength(1)]; for (int i = 0; i < distanceMatrix.GetLength(0); i++) { for (int j = 0; j < distanceMatrix.GetLength(1); j++) { distanceMatrix[i, j] = -1; } } distanceMatrix[startX, startY] = 0; PriorityQueue<Node> openSet = new PriorityQueue<Node>(); Node initial = new Node(startX, startY, 0, euclideanDistance(startX, startY, goalX, goalY)); openSet.Add(initial); while (true) { if (openSet.IsEmpty()) { // we failed to find the goal return -1; } Node current = openSet.PopFront(); if (current.x == goalX && current.y == goalY) { // we found it! return current.costToGetHere; } // search all the neighbours List<Node> neighbours = current.generateNeighbours(passable, distanceMatrix, goalX, goalY); openSet.AddRange(neighbours); } }
static int FindPath(char[,] map, int row, int col, Point start, Point end) { int[,] gscore = new int[row, col]; int[,] fscore = new int[row, col]; Color[,] status = new Color[row, col]; PriorityQueue<Point> queue = new PriorityQueue<Point>(); queue.Comparer = Compare; gscore[start.Y, start.X] = 0; start.F = fscore[start.Y, start.X] = Heuristic(start, end); queue.Push(start); status[start.Y, start.X] = Color.Grey; while (!queue.IsEmpty()) { Point cur = queue.Pop(); if (cur.X == end.X && cur.Y == end.Y) return fscore[cur.Y, cur.X]; foreach (var n in GetNeighbors(cur, map, row, col)) { if (status[n.Y, n.X] == Color.Black) continue; int tentative = gscore[cur.Y, cur.X] + 1; bool useTentative = true; if (status[n.Y, n.X] == Color.Grey && gscore[n.Y, n.X] <= tentative) useTentative = false; if (useTentative) { gscore[n.Y, n.X] = tentative; n.F = fscore[n.Y, n.X] = tentative + Heuristic(n, end); queue.Push(n); n.P = cur; } } } return int.MinValue; }
private void ExpandClusterOrder(VectorDataOptics pointDBS) { var neighbors = kdTree.PointsWithinRadiusOfWithDistance(pointDBS, Epsilon); pointDBS.Visited = true; pointDBS.ReachabilityDistance = double.PositiveInfinity; pointDBS.SetCoreDistance(neighbors, Epsilon, MinPts); clusterOrdering.Add(pointDBS); if (!double.IsPositiveInfinity(pointDBS.CoreDistance)) { var orderSeeds = new PriorityQueue<VectorDataOptics>(); Update(neighbors, pointDBS, orderSeeds); while (orderSeeds.IsEmpty() == false) { var currentObject = orderSeeds .Poll(); var neighborsCurrent = kdTree .PointsWithinRadiusOfWithDistance(pointDBS, Epsilon); currentObject.Visited = true; currentObject.SetCoreDistance(neighborsCurrent, Epsilon, MinPts); clusterOrdering.Add(currentObject); if (!double.IsPositiveInfinity(currentObject.CoreDistance)) { Update(neighborsCurrent, currentObject, orderSeeds); } } } }
/// <summary> /// based on the pseudocode on wiki page(http://en.wikipedia.org/wiki/Dijkstra%27s_algorithm) /// </summary> static int using_Dijkstra_s_algorithm_with_priority_queue() { var len = 80; var node_map = new Node[len, len]; var queue = new PriorityQueue(len * len); Node source = null; Node goal = null; var row = 0; var col = 0; foreach (var line in File.ReadAllLines("matrix.txt")) { col = 0; foreach (var num in line.Split(new char[] { ',' })) { var node = new Node(Convert.ToInt32(num)) { row = row, col = col }; if (row == 0 && col == 0) source = node; if (row == len - 1 && col == len - 1) goal = node; // node map is mainly used to get neighbors node_map[row, col] = node; queue.Enqueue(node); col++; } row++; } // set the source's distance to zero to kick start the process queue.Update(source, 0); // code for getting neighbor, using closure with the neighbor_list to make life a little easier var neighbor_list = new Node[4]; // 0:left 1:up 2:right 3:down Action<Node> prepare_neighbor_list = n => { neighbor_list[0] = n.col - 1 < 0 ? null : node_map[n.row, n.col - 1]; neighbor_list[1] = n.row - 1 < 0 ? null : node_map[n.row - 1, n.col]; neighbor_list[2] = n.col + 1 >= len ? null : node_map[n.row, n.col + 1]; neighbor_list[3] = n.row + 1 >= len ? null : node_map[n.row + 1, n.col]; }; var total = 0; while (queue.IsEmpty() == false) { var u = queue.DequeueMin(); if (u.distance == int.MaxValue) break; // all remaining vertices are inaccessible from source if (u == goal) { while (u != null) { total += u.cost; u = u.previous; } break; } // call this method before using neighbor_list array prepare_neighbor_list(u); foreach (var v in neighbor_list) { if (v == null) continue; // like when u is edge cell in the matrix var alt = u.distance + u.cost + v.cost; if (alt < v.distance) { v.previous = u; queue.Update(v, alt); } } } return total; }
private void CGI_Cut() { // -- mark verteices on strokes -- int n = mesh.VertexCount, label = 0; bool[] labeled = new bool[n]; bool[] tag = new bool[n]; foreach (int v in this.sourceVertices) { vnode[v].label = label; labeled[v] = true; tag[v] = true; } label = 1; foreach (int v in this.sinkVertices) { vnode[v].label = label; labeled[v] = true; tag[v] = true; } // -- push to priority queue -- PriorityQueue iQue = new PriorityQueue(); foreach (int v in this.sourceVertices) { foreach (int adj in mesh.AdjVV[v]) { GraphNode node = vnode[adj]; node.label = vnode[v].label; node.dis = Vrt_wij(v, adj); tag[adj] = true; iQue.Insert(node); } } foreach (int v in this.sinkVertices) { foreach (int adj in mesh.AdjVV[v]) { GraphNode node = vnode[adj]; node.label = vnode[v].label; node.dis = Vrt_wij(v, adj); tag[adj] = true; iQue.Insert(node); } } // -- region growing -- while (!iQue.IsEmpty()) { GraphNode node = iQue.DeleteMin() as GraphNode; int v = node.index; labeled[v] = true; foreach (int adj in mesh.AdjVV[v]) { if (labeled[adj]) // -- already labeled -- { continue; } else { double cost = Vrt_wij(v, adj); GraphNode adjNode = vnode[adj]; adjNode.label = node.label; if (tag[adj]) // -- already in the queue -- { if (adjNode.dis > cost) { adjNode.dis = cost; iQue.Update(adjNode); } } else // -- a fresh vertex -- { adjNode.dis = cost; tag[adj] = true; iQue.Insert(adjNode); } } } } // -- convert to facets -- List<int> risidual = new List<int>(); for (int i = 0, j = 0; i < mesh.FaceCount; ++i, j+=3) { int c0 = mesh.FaceIndex[j]; int c1 = mesh.FaceIndex[j+1]; int c2 = mesh.FaceIndex[j+2]; if (vnode[c0].label == vnode[c1].label && vnode[c0].label == vnode[c2].label) { fnode[i].label = vnode[c0].label; } else { fnode[i].label = -1; risidual.Add(i); } } // -- deal with boundary faces -- while (risidual.Count > 0) { List<int> vlist = new List<int>(); vlist.AddRange(risidual); risidual.Clear(); foreach (int f in vlist) { double min = double.MaxValue; int minid = -1; foreach (int adj in mesh.AdjFF[f]) { if (fnode[adj].label < 0) continue; double c = Face_wij1(f, adj); if (c < min) { min = c; minid = adj; } } if (minid != -1) fnode[f].label = fnode[minid].label; else risidual.Add(f); } } // -- patch the results -- int index = this.patches.Count + 1; Patch p = new Patch(index); if (patchid == null) patchid = new byte[mesh.FaceCount]; for (int i = 0; i < mesh.FaceCount; ++i) { if (fnode[i].label == 0) { patchid[i] = (byte)index; p.faces.Add(i); } } this.all_patches.Add(p); }
private void patch_type_decompose() { int n = mesh.FaceCount; for (int i = 0; i < n; ++i) // -- reset the labels -- fnode[i].label = -1; // -- label seed faces -- bool[] visited = new bool[n]; bool[] labeled = new bool[n]; if (patchid == null) patchid = new byte[n]; List<int> sources = new List<int>(); List<int> targets = new List<int>(); foreach (List<int> flist in this.facesOnStrokes) { if (flist.Count < 1) continue; sources.Add(flist[0]); targets.Add(flist[flist.Count - 1]); } byte pid = patchid[sources[0]]; // -- the patch strokes lies on -- foreach (int f in sources) { fnode[f].label = 0; visited[f] = true; labeled[f] = true; } int label = 1; foreach (int f in targets) { fnode[f].label = label; visited[f] = true; labeled[f] = true; } // -- region growing -- PriorityQueue iQue = new PriorityQueue(); foreach (int f in sources) { foreach (int adj in mesh.AdjFF[f]) { if (patchid[adj] != pid) continue; fnode[adj].label = fnode[f].label; fnode[adj].dis = Face_wij(f, adj); iQue.Insert(fnode[adj]); visited[adj] = true; } } foreach (int f in targets) { foreach (int adj in mesh.AdjFF[f]) { if (patchid[adj] != pid) continue; fnode[adj].label = fnode[f].label; fnode[adj].dis = Face_wij(f, adj); iQue.Insert(fnode[adj]); visited[adj] = true; } } while (!iQue.IsEmpty()) { GraphNode node = iQue.DeleteMin() as GraphNode; int f = node.index; labeled[f] = true; foreach (int adj in mesh.AdjFF[f]) { if (patchid[adj] != pid) continue; if (labeled[adj]) { continue; } else if (visited[adj]) { double wij = Face_wij(f, adj); if (fnode[adj].dis > wij) { fnode[adj].label = fnode[f].label; fnode[adj].dis = wij; iQue.Update(fnode[adj]); } } else { fnode[adj].label = fnode[f].label; fnode[adj].dis = Face_wij(f, adj); iQue.Insert(fnode[adj]); visited[adj] = true; } } } // -- patch the results -- int index = this.all_patches.Count+1; Patch p = new Patch(index); p.prev_label = (int)pid; for (int i = 0; i < n; ++i) { if (fnode[i].label == 0) { patchid[i] = (byte)index; p.faces.Add(i); } } this.patches.Add(p); this.all_patches.Add(p); // -- global patches -- }