static void check_same(IndexPriorityQueue PQ_Index, DynamicPriorityQueue <TestDynamicNode> PQ_Dynamic)
{
List <int> indices = new List <int>(PQ_Index);
List <TestDynamicNode> nodes = new List <TestDynamicNode>(PQ_Dynamic);
Util.gDevAssert(indices.Count == nodes.Count);
for (int i = 0; i < indices.Count; ++i)
{
Util.gDevAssert(indices[i] == nodes[i].id);
}
}
public DijkstraSP(EdgeWeightedDigraph G, int s)
{
edgeTo = new DirectedEdge[G.V];
distTo = new double[G.V];
pq = new IndexPriorityQueue<double>(G.V);
for (int v = 0; v < G.V; v++)
distTo[v] = Double.PositiveInfinity;
distTo[s] = 0.0;
pq.Insert(s, 0.0);
while (!pq.isEmpty())
relax(G, pq.Del());
}
private IndexPriorityQueue<Double> pq; // eligible crossing edges
#endregion Fields
#region Constructors
public PrimMST(EdgeWeightedGraph G)
{
edgeTo = new Edge[G.V];
distTo = new double[G.V];
marked = new bool[G.V];
for (int v = 0; v < G.V; v++)
distTo[v] = Double.PositiveInfinity;
pq = new IndexPriorityQueue<double>(G.V);
distTo[0] = 0.0;
pq.Insert(0, 0.0); // Initialize pq with 0, weight 0.
while (!pq.isEmpty())
visit(G, pq.Del()); // Add closest vertex to tree.
}
public void IndexMinPQ_Enqueue()
{
var q = new IndexPriorityQueue<int>();
var minItem = int.MaxValue;
for (int i = 0; i < items.Length; i++)
{
// After adding each item, the min item should be on top
q.Enqueue(i, items[i]);
minItem = Math.Min(items[i], minItem);
Assert.IsTrue(q.Contains(i));
Assert.AreEqual(q.MinKey(), minItem);
}
}
static void time_index_pq(int MAXID, int MAXCOUNT, int mod, int rounds, LocalProfiler profiler)
{
profiler.Start("index_all");
profiler.Start("index_initialize");
IndexPriorityQueue PQ_Index = new IndexPriorityQueue(MAXID);
profiler.StopAndAccumulate("index_initialize");
for (int ri = 0; ri < rounds; ++ri)
{
profiler.Start("index_push");
int count = 0;
int id = 0;
while (count < MAXCOUNT)
{
id = (id + mod) % MAXID;
PQ_Index.Enqueue(id, count);
count++;
}
profiler.StopAndAccumulate("index_push");
profiler.Start("index_update");
Random r = new Random(31337);
id = 0; count = 0;
while (count++ < MAXCOUNT)
{
id = (id + mod) % MAXID;
float new_p = count + ((r.Next() % 1000) - 1000);
PQ_Index.Update(id, new_p);
}
profiler.StopAndAccumulate("index_update");
profiler.Start("index_pop");
while (PQ_Index.Count > 0)
{
int index_id = PQ_Index.Dequeue();
}
profiler.StopAndAccumulate("index_pop");
}
profiler.StopAndAccumulate("index_all");
}
public void IndexMinPQ_Contains()
{
var q = new IndexPriorityQueue<int>();
Assert.IsFalse(q.Contains(1));
q.Enqueue(1, 10);
q.Enqueue(1, 20);
q.Enqueue(1, 30);
Assert.IsTrue(q.Contains(1));
q.Dequeue();
Assert.IsTrue(q.Contains(1));
q.Dequeue();
Assert.IsTrue(q.Contains(1));
q.Dequeue();
Assert.IsFalse(q.Contains(1));
}
public void IndexMinPQ_Dequeue()
{
var q = new IndexPriorityQueue<int>();
for (int i = 0; i < items.Length; i++)
{
q.Enqueue(i, items[i]);
}
var sortedItems = new List<int>(items);
sortedItems.Sort();
foreach (var item in sortedItems)
{
// The top of the queue returns the items in sorted order
var minIndex = q.Dequeue();
Assert.IsFalse(q.Contains(minIndex));
Assert.AreEqual(items[minIndex], item);
}
}
/// <summary>
/// 初始化搜索状态
/// </summary>
protected override void InitSearch(int source, int target, NavGraph graph)
{
isHavePath = false;
this.graph = graph;
this.source = source;
this.target = target;
if (spt == null)
{
costToNode = new double[graph.TotalV];
costToEnd = new double[graph.TotalV];
spt = new GraphEdge[graph.TotalV];
searchFrontier = new GraphEdge[graph.TotalV];
}
for (int i = 0; i < graph.TotalV; i++)
{
costToNode[i] = double.PositiveInfinity;
costToEnd[i] = double.PositiveInfinity;
spt[i] = null;
searchFrontier[i] = null;
}
spt[source] = new GraphEdge(source, source, 0);
searchFrontier[source] = spt[source];
costToNode[source] = 0;
costToEnd[source] = 0;
pq = new IndexPriorityQueue <double>(costToEnd, (i, j) => (float)(i - j), PriorityOrder.MinFirst);
}
public static void Merge(StreamReader[] streams)
{
int N = streams.Length;
IndexPriorityQueue <string> pq = new IndexPriorityQueue <string>(N);//最大堆，从大到小排序
for (int i = 0; i < N; ++i)
{
if (!streams[i].EndOfStream)
{
pq.Insert(i, streams[i].ReadLine());//i是保存的元素的流在数组的索引
}
}
while (!pq.IsEmpty())
{
Console.WriteLine(pq.Min());
int i = pq.DeleteMin();
if (!streams[i].EndOfStream)
{
pq.Insert(i, streams[i].ReadLine());//替换为新的值
}
}
}
public Dijkstra(EdgeWeightedDigraph digraph, int start) : base(digraph, start)
{
priorityQueue = new IndexPriorityQueue <float>(digraph.VertexCount);
FindPath(digraph, start);
}
private bool search(int source, int target)
{
//int num = 0;
if (source == target)
{
//当起点和终点相同时
Debug.Log("目标和起点节点相同" + target);
return(true);
}
//Debug.Log("起始点是"+source+"终点是"+target);
costToNode[source] = 0;
costToEnd[source] = 0;
IndexPriorityQueue <double> pq = new IndexPriorityQueue <double>(costToEnd, (i, j) => (float)(i - j), PriorityOrder.MinFirst);
searchFrontier[source] = new GraphEdge(source, source);
while (!pq.isEmpty())
{
//Debug.Log("A*");
int index = pq.DequeueIndex();
spt[index] = searchFrontier[index];
//Debug.Log("最小花费的点是"+index+"花费是"+costToEnd[index]);
DebugController.instance.AddPathLine(graph.GetNode(spt[index].From).Pos, graph.GetNode(spt[index].To).Pos);
if (index == target)
{
//Debug.Log("A*总的搜索次数是"+num);
return(true);
}
//Debug.Log("" + graph.GetAdjoinEdges(index).Count);
foreach (var e in graph.GetAdjoinEdges(index))
{
//num++;
double costNode = costToNode[e.From] + e.Cost;
double costEnd = costNode + heuristic(e.To, target);
//Debug.Log("边的总花费是" + costEnd+"边的花费是"+e.Cost+"上一个节点的花费是"+costToNode[e.From]+"启发因子花费是"+ heuristic(e.To, target));
if (searchFrontier[e.To] == null)
{
costToNode[e.To] = costNode;
costToEnd[e.To] = costEnd;
searchFrontier[e.To] = e;
pq.ChangePriority(e.To, costEnd);
}
else
{
//Debug.Log("哈哈哈哈"+costNode+",,"+costToNode[e.To]);
if (costNode < costToNode[e.To] && spt[e.To] == null)
{
//否则只有当spt上没有这个节点
//Debug.Log("啦啦啦啦");
costToEnd[e.To] = costEnd;
costToNode[e.To] = costNode;
searchFrontier[e.To] = e;
pq.ChangePriority(e.To, costEnd);
}
}
}
}
return(false);
}
private bool search(int source, int target)
{
int num = 0;
if (source == target)
{
spt[target] = new GraphEdge(target, target);
Debug.Log("一样");
return(true);
}
IndexPriorityQueue <float> queue = new IndexPriorityQueue <float>(costToNode, (i, j) => (i - j), PriorityOrder.MinFirst);
searchFrontier[source] = new GraphEdge(source, source);
//searchFrontier[]
//int t = queue.TopIndex();
//Debug.Log("起点处理完后最小的点是" + t+"花费是"+costToNode[t]);
while (!queue.isEmpty())
{
var index = queue.DequeueIndex();
spt[index] = searchFrontier[index];
DebugController.instance.AddPathLine(graph.GetNode(spt[index].From).Pos, graph.GetNode(spt[index].To).Pos);
if (index == target)
{
//Debug.Log("DJ总的搜索次数是"+num);
return(true);
}
//queue.ChangePriority(index,float.PositiveInfinity);
#region 邻接边的寻找
foreach (var e in graph.GetAdjoinEdges(index))
{
num++;
float costNode = costToNode[e.From] + e.Cost;
//float costEnd = costNode + heuristic(e.To, target);
if (searchFrontier[e.To] == null)
{
costToNode[e.To] = costNode;
searchFrontier[e.To] = e;
queue.ChangePriority(e.To, costNode);
}
else
if (costNode < costToNode[e.To] && spt[e.To] == null)
{
//否则只有当spt上没有这个节点
Debug.Log("lalalal");
costToNode[e.To] = costNode;
searchFrontier[e.To] = e;
queue.ChangePriority(e.To, costNode);
}
}
#endregion
}
return(false);
}
List <Polygon2d> decompose_cluster_up(DMesh3 mesh)
{
optimize_mesh(mesh);
mesh.CompactInPlace();
mesh.DiscardTriangleGroups(); mesh.EnableTriangleGroups(0);
double minLength = Settings.MaxBridgeWidthMM * 0.75;
double minArea = minLength * minLength;
Dictionary <int, double> areas = new Dictionary <int, double>();
Dictionary <int, HashSet <int> > trisets = new Dictionary <int, HashSet <int> >();
HashSet <int> active_groups = new HashSet <int>();
Action <int, int> add_tri_to_group = (tid, gid) => {
mesh.SetTriangleGroup(tid, gid);
areas[gid] = areas[gid] + mesh.GetTriArea(tid);
trisets[gid].Add(tid);
};
Action <int, int> add_group_to_group = (gid, togid) => {
var set = trisets[togid];
foreach (int tid in trisets[gid])
{
mesh.SetTriangleGroup(tid, togid);
set.Add(tid);
}
areas[togid] += areas[gid];
active_groups.Remove(gid);
};
Func <IEnumerable <int>, int> find_min_area_group = (tri_itr) => {
int min_gid = -1; double min_area = double.MaxValue;
foreach (int tid in tri_itr)
{
int gid = mesh.GetTriangleGroup(tid);
double a = areas[gid];
if (a < min_area)
{
min_area = a;
min_gid = gid;
}
}
return(min_gid);
};
foreach (int eid in MeshIterators.InteriorEdges(mesh))
{
Index2i et = mesh.GetEdgeT(eid);
if (mesh.GetTriangleGroup(et.a) != 0 || mesh.GetTriangleGroup(et.b) != 0)
{
continue;
}
int gid = mesh.AllocateTriangleGroup();
areas[gid] = 0;
trisets[gid] = new HashSet <int>();
active_groups.Add(gid);
add_tri_to_group(et.a, gid);
add_tri_to_group(et.b, gid);
}
foreach (int tid in mesh.TriangleIndices())
{
if (mesh.GetTriangleGroup(tid) != 0)
{
continue;
}
int gid = find_min_area_group(mesh.TriTrianglesItr(tid));
add_tri_to_group(tid, gid);
}
IndexPriorityQueue pq = new IndexPriorityQueue(mesh.MaxGroupID);
foreach (var pair in areas)
{
pq.Insert(pair.Key, (float)pair.Value);
}
while (pq.Count > 0)
{
int gid = pq.First;
pq.Remove(gid);
if (areas[gid] > minArea) // ??
{
break;
}
List <int> nbr_groups = find_neighbour_groups(mesh, gid, trisets[gid]);
int min_gid = -1; double min_area = double.MaxValue;
foreach (int ngid in nbr_groups)
{
double a = areas[ngid];
if (a < min_area)
{
min_area = a;
min_gid = ngid;
}
}
if (min_gid != -1)
{
add_group_to_group(gid, min_gid);
pq.Remove(min_gid);
pq.Insert(min_gid, (float)areas[min_gid]);
}
}
List <Polygon2d> result = new List <Polygon2d>();
int[][] sets = FaceGroupUtil.FindTriangleSetsByGroup(mesh);
foreach (var set in sets)
{
result.Add(make_poly(mesh, set));
}
return(result);
}
public static void test_pq_debuggable()
{
int MAXID = 10;
IndexPriorityQueue QIndex = new IndexPriorityQueue(MAXID);
DynamicPriorityQueue <TestDynamicNode> QDynamic = new DynamicPriorityQueue <TestDynamicNode>();
TestDynamicNode[] dyn_nodes = new TestDynamicNode[MAXID];
bool verbose = false;
//int n = 1;
for (int i = 0; i < MAXID; ++i)
{
//n = (n + 17) % 17;
int id = i;
float priority = 1.0f - (float)i / 10.0f;
QIndex.Enqueue(id, priority);
if (verbose)
{
System.Console.WriteLine("i = {0}", i);
}
QIndex.DebugPrint();
if (verbose)
{
System.Console.WriteLine("---", i);
}
dyn_nodes[i] = new TestDynamicNode()
{
id = id
};
QDynamic.Enqueue(dyn_nodes[i], priority);
QDynamic.DebugPrint();
}
System.Console.WriteLine("Dequeing...");
for (int i = 0; i < MAXID; ++i)
{
float newp = (float)((i + MAXID / 2) % MAXID) / 10.0f;
QIndex.Update(i, newp);
QDynamic.Update(dyn_nodes[i], newp);
//System.Console.WriteLine("UPDATE {0} {1}", QIndex.First, QDynamic.First.id);
//QIndex.DebugPrint();
//System.Console.WriteLine("---", i);
//QDynamic.DebugPrint();
Util.gDevAssert(QIndex.First == QDynamic.First.id);
}
for (int i = 0; i < MAXID; ++i)
{
int id = QIndex.Dequeue();
var node = QDynamic.Dequeue();
Util.gDevAssert(id == node.id);
if (verbose)
{
System.Console.WriteLine("DEQUEUE {0} {1}", id, node.id);
}
if (verbose)
{
QIndex.DebugPrint();
}
if (verbose)
{
System.Console.WriteLine("---", i);
}
if (verbose)
{
QDynamic.DebugPrint();
}
}
}
public static void test_pq()
{
System.Console.WriteLine("testing priority queues...");
int MAXID = 1000000;
int MAXCOUNT = 100;
int mod = 31337;
for (int kk = 0; kk < 3; ++kk)
{
if (kk == 1)
{
MAXCOUNT = MAXID / 10;
}
else if (kk == 2)
{
MAXCOUNT = MAXID;
}
IndexPriorityQueue PQ_Index = new IndexPriorityQueue(MAXID);
DynamicPriorityQueue <TestDynamicNode> PQ_Dynamic = new DynamicPriorityQueue <TestDynamicNode>();
MemoryPool <TestDynamicNode> Dynamic_Pool = new MemoryPool <TestDynamicNode>();
Dictionary <int, TestDynamicNode> DynamicNodes = new Dictionary <int, TestDynamicNode>();
System.Console.WriteLine("inserting {0} of {1}", MAXCOUNT, MAXID);
int count = 0;
int id = 0;
while (count < MAXCOUNT)
{
id = (id + mod) % MAXID;
PQ_Index.Enqueue(id, count);
TestDynamicNode node = Dynamic_Pool.Allocate();
node.Initialize(id);
PQ_Dynamic.Enqueue(node, count);
DynamicNodes[id] = node;
count++;
}
Util.gDevAssert(PQ_Index.IsValidQueue());
Util.gDevAssert(PQ_Dynamic.IsValidQueue());
Util.gDevAssert(PQ_Index.Count == PQ_Dynamic.Count);
Util.gDevAssert(PQ_Index.First == PQ_Dynamic.First.id);
check_same(PQ_Index, PQ_Dynamic);
Random r = new Random(31337);
System.Console.WriteLine("updating...");
id = 0; count = 0;
while (count++ < MAXCOUNT)
{
id = (id + mod) % MAXID;
float new_p = count + ((r.Next() % 1000) - 1000);
PQ_Index.Update(id, new_p);
PQ_Dynamic.Update(DynamicNodes[id], new_p);
}
Util.gDevAssert(PQ_Index.IsValidQueue());
Util.gDevAssert(PQ_Dynamic.IsValidQueue());
check_same(PQ_Index, PQ_Dynamic);
System.Console.WriteLine("removing...");
while (PQ_Index.Count > 0)
{
int index_id = PQ_Index.Dequeue();
TestDynamicNode node = PQ_Dynamic.Dequeue();
Util.gDevAssert(index_id == node.id);
}
}
}
