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 --
}
