int main()
{
// push , pop , top test
{
PriorityQueue<int> p;
assert(p.getSize() == 0);
assert(p.getCapacity() == 21);
p.push(3);p.push(2);p.push(7);p.push(11);p.push(23);
p.pop();p.pop();
p.push(3);p.push(2);p.push(43);p.push(21);p.push(25);
p.pop();p.pop();
vector<int> vcmp = {7, 11, 21, 23, 25, 43};
assert(p.getSize() == 6);
assert( vcmp == print_queue<int>(p));
assert(p.getCapacity() == 21);
}
// size , capacity , clear , destroy test
{
PriorityQueue<int> p;
assert(p.getSize() == 0);
assert(p.getCapacity() == 21);
for(auto i = 0; i<21; i++){
p.push(i);
}
assert(p.getSize() == 21);
assert(p.getCapacity() == 42);
p.clear();
assert(p.getSize() == 0);
assert(p.getCapacity() == 42);
p.destroy();
assert(p.getSize() == 0);
assert(p.getCapacity() == 0);
}
// heapsort test
{
vector<int> v = {1, 16, 12, 2, 25, 5, 6};
vector<int> cmpv = {1, 2, 5, 6, 12, 16, 25};
heap_sort(v.begin(), v.end());
assert(v == cmpv);
}
cout<< "All TestCases Pass."<<endl;
return 0;
}
void PriorityQueueTest::testThreadFill( void )
{
CPPUNIT_ASSERT( _queue->empty() );
ClassFuncThread<PriorityQueueTest> fillQueueThread( this, &PriorityQueueTest::fillQueue );
CPPUNIT_ASSERT_NO_THROW( fillQueueThread.start() );
for ( unsigned i = 0; i < FillSize; ++i ) {
Dummy::Ptr d( _dummies->pop() );
CPPUNIT_ASSERT( d );
CPPUNIT_ASSERT( d->isValid() );
Dummyer::Ptr d2( d );
CPPUNIT_ASSERT( d2 );
CPPUNIT_ASSERT( d2->isValid() );
CPPUNIT_ASSERT( d->refs() );
CPPUNIT_ASSERT_EQUAL( i, d->val() );
d = 0;
d2 = 0;
}
CPPUNIT_ASSERT_NO_THROW( fillQueueThread.join() );
CPPUNIT_ASSERT( _queue->empty() );
}
void PriorityQueueTest::testOrder( void )
{
_queue->push( 5, 0.0 );
_queue->push( 6, 0.0 );
_queue->push( 3, 1.0 );
_queue->push( 4, 1.0 );
_queue->push( 1, 2.0 );
_queue->push( 2, 2.0 );
CPPUNIT_ASSERT_EQUAL( 1U, _queue->pop() );
CPPUNIT_ASSERT_EQUAL( 2U, _queue->pop() );
CPPUNIT_ASSERT_EQUAL( 3U, _queue->pop() );
CPPUNIT_ASSERT_EQUAL( 4U, _queue->pop() );
CPPUNIT_ASSERT_EQUAL( 5U, _queue->pop() );
CPPUNIT_ASSERT_EQUAL( 6U, _queue->pop() );
}
// @snip <sh19910711/contest:setlib/disjoints_set.cpp>
template <typename GraphType> const typename std::vector<typename GraphType::Edge> get_minimum_spanning_forest( const GraphType& G ) {
typedef typename std::vector<typename GraphType::Edge> Edges;
typedef typename GraphType::Edge GraphEdge;
typedef typename GraphType::Edges GraphEdges;
typedef std::priority_queue<GraphEdge, Edges, std::greater<GraphEdge> > PriorityQueue;
typedef setlib::DisjointSets UnionFind;
Edges res;
PriorityQueue E;
UnionFind uf(G.num_vertices);
for ( int i = 0; i < G.num_vertices; ++ i ) {
for ( typename GraphEdges::const_iterator it_i = G.vertex_edges[i].begin(); it_i != G.vertex_edges[i].end(); ++ it_i ) {
const GraphEdge& e = **it_i;
E.push(GraphEdge(e.from, e.to, e.weight));
}
}
while ( ! E.empty() ) {
GraphEdge e = E.top();
E.pop();
if ( ! uf.same(e.from, e.to) ) {
res.push_back(e);
uf.merge(e.from, e.to);
}
}
return res;
}
vector<T> print_queue(PriorityQueue<T> &q){
vector<T> ret;
while(!q.isEmpty()) {
ret.push_back(q.getTop());
q.pop();
}
return ret;
}
void PriorityQueueTest::squareQueue( void )
{
for ( unsigned i = 0; i < FillSize; ++i ) {
unsigned v;
CPPUNIT_ASSERT_NO_THROW( v = _queue->pop() );
CPPUNIT_ASSERT_NO_THROW( _squared->push( sqr(v) ) );
}
}
void PriorityQueueTest::testPush( void )
{
CPPUNIT_ASSERT( _queue->empty() );
CPPUNIT_ASSERT_NO_THROW( _queue->push( 42 ) );
CPPUNIT_ASSERT_EQUAL( 1U, _queue->size() );
CPPUNIT_ASSERT_EQUAL( 42U, _queue->pop() );
CPPUNIT_ASSERT( _queue->empty() );
}
void dumpContents( const string & msg, PriorityQueue & pq )
{
cout << msg << ":" << endl;
while( !pq.empty( ) )
{
cout << pq.top( ) << endl;
pq.pop( );
}
}
int main(){
PriorityQueue<int> pq;
for(int i=0;i<10;i++){
pq.push(i);
}
while(!pq.empty()){
cout<<pq.top()<<" ";
pq.pop();
}
}
float* Dijkstra::dijkstra(Graph *&graph, int s)
{
int n = graph->getVerticesNum();
if ((s < 0)||(s >= n))
return 0;
int m = graph->getRealSize();
Data** dist = new Data*[n];
int* up = new int[n];
for (int i = 0; i < n; i++){
up[i] = 0;
dist[i] = new DataFloat(i, FLT_MAX);
}
dist[s]->priority = 0;
PriorityQueue *queue = new PriorityQueue(dist, n, 4);
Edge** edges = graph->getEdgeSet();
int edgeCount = m;
while ((edgeCount != 0) && (!queue->isEmpty()))
{
Data* tmp = queue->pop();
int v = ((DataFloat*)tmp)->v;
int v0 = -1;
float delta;
for (int i = 0; i < m; i++)
{
v0 = -1;
if (edges[i]->K == v)
v0 = edges[i]->N;
if (edges[i]->N == v)
v0 = edges[i]->K;
if (v0 == -1) continue;
//edgeCount--;
delta = dist[v0]->priorities - (dist[v]->priorities + graph->getWeight(v, v0));
if (delta > 0){
dist[v0]->priorities = graph->getWeight(v, v0) + dist[v]->priorities;
up[v0] = v;
}
}
}
float *result = new float[n];
for (int i = 0; i < n; i++)
result[i] = dist[i]->priorities;
for (int i = 0; i < n; i++)
delete dist[i];
delete []dist;
delete queue;
delete []up;
return result;
}
void PriorityQueueTest::testSynchronize( void )
{
ClassFuncThread<PriorityQueueTest> enqueueThread( this, &PriorityQueueTest::enqueue );
CPPUNIT_ASSERT_NO_THROW( enqueueThread.start() );
unsigned v;
CPPUNIT_ASSERT_NO_THROW( v = _queue->pop() );
CPPUNIT_ASSERT_EQUAL( 42U, v );
CPPUNIT_ASSERT_NO_THROW( enqueueThread.join() );
CPPUNIT_ASSERT( _queue->empty() );
}
int main()
{
PriorityQueue* pq = new PriorityQueue;
pq->push(7);
pq->push(3);
pq->push(10);
pq->push(1);
cout << pq->pop() << endl;
cout << pq->pop() << endl;
cout << pq->pop() << endl;
cout << pq->pop() << endl;
delete pq;
return 0;
}
inline
void inpainting_loop(VertexListGraph& g, InpaintingVisitorType vis,
BoundaryStatusMap& boundaryStatusMap, PriorityQueue& boundaryNodeQueue,
NearestNeighborFinder find_inpainting_source,
PatchInpainter inpaint_patch)
{
typedef typename boost::graph_traits<VertexListGraph>::vertex_descriptor Vertex;
typedef typename boost::graph_traits<VertexListGraph>::edge_descriptor Edge;
typedef typename boost::property_traits<PositionMap>::value_type PositionValueType;
// When this function is called, the priority-queue should already be filled
// with all the hole-vertices (which should also have "Color::black()" color value).
// So, the only thing this function does is run the inpainting loop. All of the
// actual code is externalized in the functors and visitors (vis, find_inpainting_source, inpaint_patches, etc.).
while(true)
{
// find the next target to in-paint:
Vertex targetNode;
do
{
if( boundaryNodeQueue.empty() )
{
std::cout << "Queue is empty, exiting." << std::endl;
return; //terminate if the queue is empty.
}
targetNode = boundaryNodeQueue.top();
boundaryNodeQueue.pop();
} while( get(boundaryStatusMap, targetNode) == false );
// Notify the visitor that we have a hole target center.
vis.discover_vertex(targetNode, g);
// next, we want to find a source patch-center that matches best to our target-patch:
Vertex source_patch_center = find_inpainting_source(targetNode);
vis.vertex_match_made(targetNode, source_patch_center, g);
// finally, do the in-painting of the target patch from the source patch.
// the inpaint_patch functor should take care of iterating through the vertices in both
// patches and call "vis.paint_vertex(target, source, g)" on the individual vertices.
inpaint_patch(targetNode, source_patch_center, g, vis);
if(!vis.accept_painted_vertex(targetNode, g))
{
throw std::runtime_error("Vertex was not painted successfully!");
}
vis.finish_vertex(targetNode, g);
} // end main iteration loop
};
void heap_sort(T &begin, T &end) {
PriorityQueue<int> p;
T it = begin;
while(it!=end) {
p.push(*it);
it++;
}
it = begin;
while(it!=end) {
*it = p.getTop();
p.pop();
it++;
}
}
Node getNode(char name){
if (find(name)){
PriorityQueue tempQueue;
while (top().name != name){
tempQueue.push(top());
pop();
}
Node node = top();
while (!tempQueue.empty()){
push(tempQueue.top());
tempQueue.pop();
}
return node;
}
}
static bool A_star(DGR& dg, Weight& w, Digraph::Node s, Digraph::Node t, int k, DistMap& dist, PredMap& pred)
{
if(s == t) return false;
typedef QNode Node;
typedef std::vector<QNode> NodeCon;
typedef std::priority_queue<Node,NodeCon> PriorityQueue;
PriorityQueue pq;
int cnt = 0;
pred[s] = INVALID;
pq.push(Node(s, INVALID, 0, dist[s]));
while(!pq.empty())
{
Node node = pq.top();
Digraph::Node u = node.u;
pred[u] = node.parent;
//cout<<"->pop:f("<<Digraph::id(u)<<")="<<node.g+p.h
// <<"\tg("<<Digraph::id(u)<<")="<<node.g
// <<"\th("<<Digraph::id(u)<<")="<<node.h
// <<endl;
pq.pop();
if(u == t)
{
//cout<<"---------------------"<<endl;
cnt++;
}
if(cnt == k)
{
break;
}
for(typename DGR::OutArcIt e(dg,u);e!=INVALID;++e)
{
Digraph::Node v = dg.target(e);
pred[v] = u;
Node child_node(v, u, node.g + w[e], dist[v]);
//cout<<"\t->push:f("<<Digraph::id(v)<<")="<<child_node.g+child_node.h
//<<"\tg("<<Digraph::id(v)<<")="<<child_node.g
//<<"\th("<<Digraph::id(v)<<")="<<child_node.h
//<<endl;
pq.push(child_node);
}
}
return (cnt == k);
}
void PriorityQueueTest::testPushPop( void )
{
CPPUNIT_ASSERT( _queue->empty() );
for ( unsigned i = 0; i < FillSize; ++i ) {
CPPUNIT_ASSERT_EQUAL( i, _queue->size() );
CPPUNIT_ASSERT_NO_THROW( _queue->push( i ) );
CPPUNIT_ASSERT_EQUAL( i + 1, _queue->size() );
}
for ( unsigned i = FillSize; i > 0; --i ) {
CPPUNIT_ASSERT_EQUAL( i, _queue->size() );
CPPUNIT_ASSERT_EQUAL( FillSize - i, _queue->pop() );
CPPUNIT_ASSERT_EQUAL( i - 1, _queue->size() );
}
CPPUNIT_ASSERT( _queue->empty() );
}
FramePtr fetchFrame()
{
bool wasStepIntoQueue = stepIntoQueue;
if(frameQueue.empty() && IsEof() && !reportedEof)
{
reportedEof = true;
messageCallback(MEof, "eof");
}
if(stepIntoQueue && !frameQueue.empty())
{
stepIntoQueue = false;
timeHandler->SetTime(timeFromTs(frameQueue.top()->GetPts()) + .001);
audioHandler->discardQueueUntilTs(timeHandler->GetTime());
}
double time = timeHandler->GetTime();
FramePtr newFrame = 0;
// Throw away all old frames (timestamp older than now) except for the last
// and set the pFrame pointer to that.
int poppedFrames = 0;
while(!frameQueue.empty() && timeFromTs(frameQueue.top()->GetPts()) < time)
{
newFrame = frameQueue.top();
frameQueue.pop();
poppedFrames++;
}
if(poppedFrames > 1){
FlogD("skipped " << poppedFrames - 1 << " frames");
}
if(newFrame != 0 && (newFrame->GetPts() >= time || wasStepIntoQueue))
return newFrame;
return 0;
}
inline void sweep(Range const& range, PriorityQueue& queue,
InitializationVisitor& initialization_visitor,
EventVisitor& event_visitor,
InterruptPolicy const& interrupt_policy)
{
typedef typename PriorityQueue::value_type event_type;
initialization_visitor.apply(range, queue, event_visitor);
while (! queue.empty())
{
event_type event = queue.top();
queue.pop();
event_visitor.apply(event, queue);
if (interrupt_policy.enabled && interrupt_policy.apply(event))
{
break;
}
}
geofeatures_boost::ignore_unused(interrupt_policy);
}
int main()
{
int p;
int n;
n=scan_d();
vector<int> a(n);
for(int i=0;i<n;i++)
{
a[i]=scan_d();
}
PriorityQueue<int> x;
x.build(a);
while(!x.empty())
{
println_d(x.pop());
}
getchar();
return 0;
}
void myerase(Node val)
{
PriorityQueue <Node, std::vector<Node>, Compare> tmp;
while (this->c.size() > 0)
{
auto first = this->c.begin();
if (first->getTab() == val.getTab())
{
this->pop();
break;
}
tmp.push(*first);
this->pop();
}
while (tmp.size() > 0)
{
this->push(tmp.top());
tmp.pop();
}
}
void decreaseKey(char node, int newKey, char newPi){
cout << "decreaseKey(" << node << ", " << newKey << ", " << newPi << endl;
if (find(node)){
cout << "found" << endl;
PriorityQueue tempQueue;
while (top().name != node){
cout << "popping out" << endl;
tempQueue.push(top());
pop();
}
Node node = top();
cout << "orig: " << node.name << ", " << node.pi << ", " << node.key << endl;
pop();
node.key = newKey;
node.pi = newPi;
push(node);
while (!tempQueue.empty()){
cout << "pushing back" << endl;
push(tempQueue.top());
tempQueue.pop();
}
}
cout << "leaving" << endl;
}
void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter,
entity_pos_t x0, entity_pos_t z0, entity_pos_t r,
entity_pos_t range, const Goal& goal, pass_class_t passClass, Path& path)
{
UpdateGrid(); // TODO: only need to bother updating if the terrain changed
PROFILE("ComputeShortPath");
// ScopeTimer UID__(L"ComputeShortPath");
m_DebugOverlayShortPathLines.clear();
if (m_DebugOverlay)
{
// Render the goal shape
m_DebugOverlayShortPathLines.push_back(SOverlayLine());
m_DebugOverlayShortPathLines.back().m_Color = CColor(1, 0, 0, 1);
switch (goal.type)
{
case CCmpPathfinder::Goal::POINT:
{
SimRender::ConstructCircleOnGround(GetSimContext(), goal.x.ToFloat(), goal.z.ToFloat(), 0.2f, m_DebugOverlayShortPathLines.back(), true);
break;
}
case CCmpPathfinder::Goal::CIRCLE:
{
SimRender::ConstructCircleOnGround(GetSimContext(), goal.x.ToFloat(), goal.z.ToFloat(), goal.hw.ToFloat(), m_DebugOverlayShortPathLines.back(), true);
break;
}
case CCmpPathfinder::Goal::SQUARE:
{
float a = atan2f(goal.v.X.ToFloat(), goal.v.Y.ToFloat());
SimRender::ConstructSquareOnGround(GetSimContext(), goal.x.ToFloat(), goal.z.ToFloat(), goal.hw.ToFloat()*2, goal.hh.ToFloat()*2, a, m_DebugOverlayShortPathLines.back(), true);
break;
}
}
}
// List of collision edges - paths must never cross these.
// (Edges are one-sided so intersections are fine in one direction, but not the other direction.)
std::vector<Edge> edges;
std::vector<Edge> edgesAA; // axis-aligned squares
// Create impassable edges at the max-range boundary, so we can't escape the region
// where we're meant to be searching
fixed rangeXMin = x0 - range;
fixed rangeXMax = x0 + range;
fixed rangeZMin = z0 - range;
fixed rangeZMax = z0 + range;
{
// (The edges are the opposite direction to usual, so it's an inside-out square)
Edge e0 = { CFixedVector2D(rangeXMin, rangeZMin), CFixedVector2D(rangeXMin, rangeZMax) };
Edge e1 = { CFixedVector2D(rangeXMin, rangeZMax), CFixedVector2D(rangeXMax, rangeZMax) };
Edge e2 = { CFixedVector2D(rangeXMax, rangeZMax), CFixedVector2D(rangeXMax, rangeZMin) };
Edge e3 = { CFixedVector2D(rangeXMax, rangeZMin), CFixedVector2D(rangeXMin, rangeZMin) };
edges.push_back(e0);
edges.push_back(e1);
edges.push_back(e2);
edges.push_back(e3);
}
// List of obstruction vertexes (plus start/end points); we'll try to find paths through
// the graph defined by these vertexes
std::vector<Vertex> vertexes;
// Add the start point to the graph
CFixedVector2D posStart(x0, z0);
fixed hStart = (posStart - NearestPointOnGoal(posStart, goal)).Length();
Vertex start = { posStart, fixed::Zero(), hStart, 0, Vertex::OPEN, QUADRANT_NONE, QUADRANT_ALL };
vertexes.push_back(start);
const size_t START_VERTEX_ID = 0;
// Add the goal vertex to the graph.
// Since the goal isn't always a point, this a special magic virtual vertex which moves around - whenever
// we look at it from another vertex, it is moved to be the closest point on the goal shape to that vertex.
Vertex end = { CFixedVector2D(goal.x, goal.z), fixed::Zero(), fixed::Zero(), 0, Vertex::UNEXPLORED, QUADRANT_NONE, QUADRANT_ALL };
vertexes.push_back(end);
const size_t GOAL_VERTEX_ID = 1;
// Add terrain obstructions
{
u16 i0, j0, i1, j1;
NearestTile(rangeXMin, rangeZMin, i0, j0);
NearestTile(rangeXMax, rangeZMax, i1, j1);
AddTerrainEdges(edgesAA, vertexes, i0, j0, i1, j1, r, passClass, *m_Grid);
}
// Find all the obstruction squares that might affect us
CmpPtr<ICmpObstructionManager> cmpObstructionManager(GetSimContext(), SYSTEM_ENTITY);
std::vector<ICmpObstructionManager::ObstructionSquare> squares;
cmpObstructionManager->GetObstructionsInRange(filter, rangeXMin - r, rangeZMin - r, rangeXMax + r, rangeZMax + r, squares);
// Resize arrays to reduce reallocations
vertexes.reserve(vertexes.size() + squares.size()*4);
edgesAA.reserve(edgesAA.size() + squares.size()); // (assume most squares are AA)
// Convert each obstruction square into collision edges and search graph vertexes
for (size_t i = 0; i < squares.size(); ++i)
{
CFixedVector2D center(squares[i].x, squares[i].z);
CFixedVector2D u = squares[i].u;
CFixedVector2D v = squares[i].v;
// Expand the vertexes by the moving unit's collision radius, to find the
// closest we can get to it
CFixedVector2D hd0(squares[i].hw + r + EDGE_EXPAND_DELTA, squares[i].hh + r + EDGE_EXPAND_DELTA);
CFixedVector2D hd1(squares[i].hw + r + EDGE_EXPAND_DELTA, -(squares[i].hh + r + EDGE_EXPAND_DELTA));
// Check whether this is an axis-aligned square
bool aa = (u.X == fixed::FromInt(1) && u.Y == fixed::Zero() && v.X == fixed::Zero() && v.Y == fixed::FromInt(1));
Vertex vert;
vert.status = Vertex::UNEXPLORED;
vert.quadInward = QUADRANT_NONE;
vert.quadOutward = QUADRANT_ALL;
vert.p.X = center.X - hd0.Dot(u); vert.p.Y = center.Y + hd0.Dot(v); if (aa) vert.quadInward = QUADRANT_BR; vertexes.push_back(vert);
vert.p.X = center.X - hd1.Dot(u); vert.p.Y = center.Y + hd1.Dot(v); if (aa) vert.quadInward = QUADRANT_TR; vertexes.push_back(vert);
vert.p.X = center.X + hd0.Dot(u); vert.p.Y = center.Y - hd0.Dot(v); if (aa) vert.quadInward = QUADRANT_TL; vertexes.push_back(vert);
vert.p.X = center.X + hd1.Dot(u); vert.p.Y = center.Y - hd1.Dot(v); if (aa) vert.quadInward = QUADRANT_BL; vertexes.push_back(vert);
// Add the edges:
CFixedVector2D h0(squares[i].hw + r, squares[i].hh + r);
CFixedVector2D h1(squares[i].hw + r, -(squares[i].hh + r));
CFixedVector2D ev0(center.X - h0.Dot(u), center.Y + h0.Dot(v));
CFixedVector2D ev1(center.X - h1.Dot(u), center.Y + h1.Dot(v));
CFixedVector2D ev2(center.X + h0.Dot(u), center.Y - h0.Dot(v));
CFixedVector2D ev3(center.X + h1.Dot(u), center.Y - h1.Dot(v));
if (aa)
{
Edge e = { ev1, ev3 };
edgesAA.push_back(e);
}
else
{
Edge e0 = { ev0, ev1 };
Edge e1 = { ev1, ev2 };
Edge e2 = { ev2, ev3 };
Edge e3 = { ev3, ev0 };
edges.push_back(e0);
edges.push_back(e1);
edges.push_back(e2);
edges.push_back(e3);
}
// TODO: should clip out vertexes and edges that are outside the range,
// to reduce the search space
}
ENSURE(vertexes.size() < 65536); // we store array indexes as u16
if (m_DebugOverlay)
{
// Render the obstruction edges
for (size_t i = 0; i < edges.size(); ++i)
{
m_DebugOverlayShortPathLines.push_back(SOverlayLine());
m_DebugOverlayShortPathLines.back().m_Color = CColor(0, 1, 1, 1);
std::vector<float> xz;
xz.push_back(edges[i].p0.X.ToFloat());
xz.push_back(edges[i].p0.Y.ToFloat());
xz.push_back(edges[i].p1.X.ToFloat());
xz.push_back(edges[i].p1.Y.ToFloat());
SimRender::ConstructLineOnGround(GetSimContext(), xz, m_DebugOverlayShortPathLines.back(), true);
}
for (size_t i = 0; i < edgesAA.size(); ++i)
{
m_DebugOverlayShortPathLines.push_back(SOverlayLine());
m_DebugOverlayShortPathLines.back().m_Color = CColor(0, 1, 1, 1);
std::vector<float> xz;
xz.push_back(edgesAA[i].p0.X.ToFloat());
xz.push_back(edgesAA[i].p0.Y.ToFloat());
xz.push_back(edgesAA[i].p0.X.ToFloat());
xz.push_back(edgesAA[i].p1.Y.ToFloat());
xz.push_back(edgesAA[i].p1.X.ToFloat());
xz.push_back(edgesAA[i].p1.Y.ToFloat());
xz.push_back(edgesAA[i].p1.X.ToFloat());
xz.push_back(edgesAA[i].p0.Y.ToFloat());
xz.push_back(edgesAA[i].p0.X.ToFloat());
xz.push_back(edgesAA[i].p0.Y.ToFloat());
SimRender::ConstructLineOnGround(GetSimContext(), xz, m_DebugOverlayShortPathLines.back(), true);
}
}
// Do an A* search over the vertex/visibility graph:
// Since we are just measuring Euclidean distance the heuristic is admissible,
// so we never have to re-examine a node once it's been moved to the closed set.
// To save time in common cases, we don't precompute a graph of valid edges between vertexes;
// we do it lazily instead. When the search algorithm reaches a vertex, we examine every other
// vertex and see if we can reach it without hitting any collision edges, and ignore the ones
// we can't reach. Since the algorithm can only reach a vertex once (and then it'll be marked
// as closed), we won't be doing any redundant visibility computations.
PROFILE_START("A*");
PriorityQueue open;
PriorityQueue::Item qiStart = { START_VERTEX_ID, start.h };
open.push(qiStart);
u16 idBest = START_VERTEX_ID;
fixed hBest = start.h;
while (!open.empty())
{
// Move best tile from open to closed
PriorityQueue::Item curr = open.pop();
vertexes[curr.id].status = Vertex::CLOSED;
// If we've reached the destination, stop
if (curr.id == GOAL_VERTEX_ID)
{
idBest = curr.id;
break;
}
// Sort the edges so ones nearer this vertex are checked first by CheckVisibility,
// since they're more likely to block the rays
std::sort(edgesAA.begin(), edgesAA.end(), EdgeSort(vertexes[curr.id].p));
std::vector<EdgeAA> edgesLeft;
std::vector<EdgeAA> edgesRight;
std::vector<EdgeAA> edgesBottom;
std::vector<EdgeAA> edgesTop;
SplitAAEdges(vertexes[curr.id].p, edgesAA, edgesLeft, edgesRight, edgesBottom, edgesTop);
// Check the lines to every other vertex
for (size_t n = 0; n < vertexes.size(); ++n)
{
if (vertexes[n].status == Vertex::CLOSED)
continue;
// If this is the magical goal vertex, move it to near the current vertex
CFixedVector2D npos;
if (n == GOAL_VERTEX_ID)
{
npos = NearestPointOnGoal(vertexes[curr.id].p, goal);
// To prevent integer overflows later on, we need to ensure all vertexes are
// 'close' to the source. The goal might be far away (not a good idea but
// sometimes it happens), so clamp it to the current search range
npos.X = clamp(npos.X, rangeXMin, rangeXMax);
npos.Y = clamp(npos.Y, rangeZMin, rangeZMax);
}
else
{
npos = vertexes[n].p;
}
// Work out which quadrant(s) we're approaching the new vertex from
u8 quad = 0;
if (vertexes[curr.id].p.X <= npos.X && vertexes[curr.id].p.Y <= npos.Y) quad |= QUADRANT_BL;
if (vertexes[curr.id].p.X >= npos.X && vertexes[curr.id].p.Y >= npos.Y) quad |= QUADRANT_TR;
if (vertexes[curr.id].p.X <= npos.X && vertexes[curr.id].p.Y >= npos.Y) quad |= QUADRANT_TL;
if (vertexes[curr.id].p.X >= npos.X && vertexes[curr.id].p.Y <= npos.Y) quad |= QUADRANT_BR;
// Check that the new vertex is in the right quadrant for the old vertex
if (!(vertexes[curr.id].quadOutward & quad))
{
// Hack: Always head towards the goal if possible, to avoid missing it if it's
// inside another unit
if (n != GOAL_VERTEX_ID)
{
continue;
}
}
bool visible =
CheckVisibilityLeft(vertexes[curr.id].p, npos, edgesLeft) &&
CheckVisibilityRight(vertexes[curr.id].p, npos, edgesRight) &&
CheckVisibilityBottom(vertexes[curr.id].p, npos, edgesBottom) &&
CheckVisibilityTop(vertexes[curr.id].p, npos, edgesTop) &&
CheckVisibility(vertexes[curr.id].p, npos, edges);
/*
// Render the edges that we examine
m_DebugOverlayShortPathLines.push_back(SOverlayLine());
m_DebugOverlayShortPathLines.back().m_Color = visible ? CColor(0, 1, 0, 0.5) : CColor(1, 0, 0, 0.5);
std::vector<float> xz;
xz.push_back(vertexes[curr.id].p.X.ToFloat());
xz.push_back(vertexes[curr.id].p.Y.ToFloat());
xz.push_back(npos.X.ToFloat());
xz.push_back(npos.Y.ToFloat());
SimRender::ConstructLineOnGround(GetSimContext(), xz, m_DebugOverlayShortPathLines.back(), false);
//*/
if (visible)
{
fixed g = vertexes[curr.id].g + (vertexes[curr.id].p - npos).Length();
// If this is a new tile, compute the heuristic distance
if (vertexes[n].status == Vertex::UNEXPLORED)
{
// Add it to the open list:
vertexes[n].status = Vertex::OPEN;
vertexes[n].g = g;
vertexes[n].h = DistanceToGoal(npos, goal);
vertexes[n].pred = curr.id;
// If this is an axis-aligned shape, the path must continue in the same quadrant
// direction (but not go into the inside of the shape).
// Hack: If we started *inside* a shape then perhaps headed to its corner (e.g. the unit
// was very near another unit), don't restrict further pathing.
if (vertexes[n].quadInward && !(curr.id == START_VERTEX_ID && g < fixed::FromInt(8)))
vertexes[n].quadOutward = ((~vertexes[n].quadInward) & quad) & 0xF;
if (n == GOAL_VERTEX_ID)
vertexes[n].p = npos; // remember the new best goal position
PriorityQueue::Item t = { (u16)n, g + vertexes[n].h };
open.push(t);
// Remember the heuristically best vertex we've seen so far, in case we never actually reach the target
if (vertexes[n].h < hBest)
{
idBest = (u16)n;
hBest = vertexes[n].h;
}
}
else // must be OPEN
{
// If we've already seen this tile, and the new path to this tile does not have a
// better cost, then stop now
if (g >= vertexes[n].g)
continue;
// Otherwise, we have a better path, so replace the old one with the new cost/parent
vertexes[n].g = g;
vertexes[n].pred = curr.id;
// If this is an axis-aligned shape, the path must continue in the same quadrant
// direction (but not go into the inside of the shape).
if (vertexes[n].quadInward)
vertexes[n].quadOutward = ((~vertexes[n].quadInward) & quad) & 0xF;
if (n == GOAL_VERTEX_ID)
vertexes[n].p = npos; // remember the new best goal position
open.promote((u16)n, g + vertexes[n].h);
}
}
}
}
// Reconstruct the path (in reverse)
for (u16 id = idBest; id != START_VERTEX_ID; id = vertexes[id].pred)
{
Waypoint w = { vertexes[id].p.X, vertexes[id].p.Y };
path.m_Waypoints.push_back(w);
}
PROFILE_END("A*");
}
int main()
{
srand(time(0));
cout<<"Creating new doubly linked list\n";
D_LinkedList<int> dll(10);
cout<<"Outputting list\n";
dll.printForward();
cout<<"\n\n";
cout<<"Appending Values\n";
for (int var = 10; var < 20; ++var) {
dll.append(var+1);
}
cout<<"Outputting list with list Size: "<<dll.size()<<endl;
dll.printForward();
cout<<"\n\n";
cout<<"Prepending Values\n";
for (int var = 10; var > 0; --var) {
dll.prepend(var-1);
}
cout<<"Outputting list with list Size: "<<dll.size()<<endl;
dll.printForward();
cout<<"\n\n";
cout<<"Outputting list in reverse order with list Size: "<<dll.size()<<endl;
dll.printBackward();
cout<<"\n\n";
cout<<"First: "<<dll.first()<<endl;
cout<<"Last: "<<dll.last()<<endl;
cout<<"\n\n";
cout<<"Pulling First: "<<dll.pullFront()<<endl;
cout<<"Pulling Last: "<<dll.pullBack()<<endl;
cout<<"Outputting list with list Size: "<<dll.size()<<endl;
dll.printForward();
cout<<"\n\n";
cout<<"Inserting value 100 after\n";
dll.insertAfter(9,100);
cout<<"Outputting list with list Size: "<<dll.size()<<endl;
dll.printForward();
cout<<"\n\n";
cout<<"Inserting value 50 before\n";
dll.insertBefore(9,50);
cout<<"Outputting list with list Size: "<<dll.size()<<endl;
dll.printForward();
cout<<"\n\n";
cout<<"Testing operator[] \n";
dll[9]=75;
cout<<dll.get(9)<<endl;
cout<<"\n\n";
cout<<"Testing Extract\n";
cout<<dll.extract(9)<<endl;
cout<<"Outputting list with list Size: "<<dll.size()<<endl;
dll.printForward();
cout<<"\n\n";
cout<<"\n****************************************************************\n";
cout<<"Creating new circular linked list\n";
C_LinkedList<int> cll(10);
cout<<"Outputting list\n";
cll.printList();
cout<<"\n\n";
cout<<"Appending Values\n";
for (int var = 10; var < 20; ++var) {
cll.append(var+1);
}
cout<<"Outputting list with list Size: "<<cll.size()<<endl;
cll.printList();
cout<<"\n\n";
cout<<"Prepending Values\n";
for (int var = 10; var > 0; --var) {
cll.prepend(var-1);
}
cout<<"Outputting list with list Size: "<<cll.size()<<endl;
cll.printList();
cout<<"\n\n";
cout<<"First: "<<cll.first()<<endl;
cout<<"Last: "<<cll.last()<<endl;
cout<<"\n\n";
cout<<"Pulling First: "<<cll.pullFront()<<endl;
cout<<"Pulling Last: "<<cll.pullBack()<<endl;
cout<<"Outputting list with list Size: "<<cll.size()<<endl;
cll.printList();
cout<<"\n\n";
cout<<"Inserting value 100 after\n";
cll.insertAfter(9,100);
cout<<"Outputting list with list Size: "<<cll.size()<<endl;
cll.printList();
cout<<"\n\n";
cout<<"Inserting value 50 before\n";
cll.insertBefore(9,50);
cout<<"Outputting list with list Size: "<<cll.size()<<endl;
cll.printList();
cout<<"\n\n";
cout<<"Testing operator[] \n";
cll[9]=75;
cout<<cll.get(9)<<endl;
cout<<"\n\n";
cout<<"Testing Extract\n";
cout<<cll.extract(9)<<endl;
cout<<"Outputting list with list Size: "<<cll.size()<<endl;
cll.printList();
cout<<"\n\n";
Stack<int> stack(0);
cout << "Populating Stack"<<endl;
for (int var = 0; var < 10; ++var) {
stack.push(var+1);
}
cout<<"Stack Size: "<<stack.size()<<endl;
cout<<"\n\n";
cout<<"Testing top() and pop() functions\n";
cout<<stack.top()<<endl;
stack.pop();
cout<<stack.pop()<<endl;
cout<<"\n\n";
cout<<"testing copy construcor\n";
Stack<int> newStack(stack);
cout<<"stack top(): "<<stack.top()<<endl;
cout<<"stack size(): "<<stack.size()<<endl;
cout<<"newStack top(): "<<newStack.top()<<endl;
cout<<"newStack size(): "<<newStack.size()<<endl;
cout<<"\n\n";
// cout<<"Clearing newStack\n";
// newStack.clear();
// cout<<"stack size(): "<<stack.size()<<endl;
// cout<<"stack top(): "<<stack.top()<<endl;
// cout<<"newStack size(): "<<newStack.size()<<endl;//outputs 0
// cout<<"newStack top(): "<<newStack.top()<<endl;//causes subscript error
// cout<<"\n\n";
Queue<int> queue(0);
cout << "Populating queue"<<endl;
for (int var = 0; var < 10; ++var) {
queue.pushBack(var+1);
}
cout<<"Queue Size: "<<queue.size()<<endl;
cout<<"\n\n";
cout<<"Testing front() and back() functions\n";
cout<<queue.front()<<endl;
cout<<queue.back()<<endl;
cout<<"\n\n";
cout<<"Testing PopFront() function\n";
cout<<queue.popFront()<<endl;
cout<<"Queue Size: "<<queue.size()<<endl;
cout<<"\n\n";
cout<<"testing copy construcor\n";
Queue<int> newQueue(queue);
cout<<"queue front(): "<<queue.front()<<endl;
cout<<"queue size(): "<<queue.size()<<endl;
cout<<"newQueue front(): "<<newQueue.front()<<endl;
cout<<"newQueue size(): "<<newQueue.size()<<endl;
cout<<"\n\n";
cout<<"Popping Valued from newQueue\n";
for (int i = 0; i < 5; ++i) {
cout<<newQueue.popFront()<<endl;
}
cout<<"queue front(): "<<queue.front()<<endl;
cout<<"queue size(): "<<queue.size()<<endl;
cout<<"newQueue front(): "<<newQueue.front()<<endl;
cout<<"newQueue size(): "<<newQueue.size()<<endl;
cout<<"\n\n";
cout<<"***********************************\nTesting Priority Linked List\n";
P_LinkedList<int> pll;
for (int i = 0; i < 20; ++i) {
pll.append(i+1);
}
cout<<"Outputting list of size: "<<pll.size()<<endl;
pll.printList();
cout<<"\n\n";
cout<<"Outputting peekMax()\n";
cout<<pll.peekMax()<<endl;
cout<<"Outputting popMax()\n";
cout<<pll.popMax()<<endl;
cout<<"Outputting list of size: "<<pll.size()<<endl;
pll.printList();
cout<<"\n\n";
cout<<"Outputting peekMin()\n";
cout<<pll.peekMin()<<endl;
cout<<"Outputting popMin()\n";
cout<<pll.popMin()<<endl;
cout<<"Outputting list of size: "<<pll.size()<<endl;
pll.printList();
cout<<"\n\n";
cout<<"*****************************************\nTesting Priority Queue\n";
PriorityQueue<int> pq;
for (int i = 0; i < 20; ++i) {
pq.push((rand()%30)+1);
}
cout<<"Outputting list of size: "<<pq.size()<<endl;
pq.printQueue();
cout<<"\n\n";
cout<<"OutPutting front()\n";
cout<< pq.front() <<endl;
cout<<"OutPutting back()\n";
cout<< pq.back() <<endl;
cout<<"\n\n";
cout<<"pop()ing 10 elements\n";
for (int i = 0; i < 10; ++i) {
cout<<pq.pop()<<endl;
}
cout<<"\n\n";
cout<<"Outputting list of size: "<<pq.size()<<endl;
pq.printQueue();
cout<<"\n\n";
cout<<"Instanciating descending object\n";
PriorityQueue<int> pqd(SortOrder::DEC);
for (int i = 0; i < 10; ++i) {
pqd.push((rand()%15)+1);
}
cout<<"Outputting list of size: "<<pqd.size()<<endl;
pqd.printQueue();
cout<<"\n\n";
cout<<"OutPutting front()\n";
cout<< pqd.front() <<endl;
cout<<"OutPutting back()\n";
cout<< pqd.back() <<endl;
cout<<"\n\n";
cout<<"pop()ing 5 elements\n";
for (int i = 0; i < 5; ++i) {
cout<<pqd.pop()<<endl;
}
cout<<"\n\n";
cout<<"Outputting list of size: "<<pqd.size()<<endl;
pqd.printQueue();
cout<<"\n\n";
cout<<"*************************************\nTesting Sorted Linked List\n";
S_LinkedList<int> sll(SortOrder::ASC);
for (int i = 0; i < 100; ++i) {
sll.push((rand()%50)+1);
}
cout<<"Outputting lis of size: "<<sll.size()<<endl;
sll.printList();
cout<<"\n\n";
return 0;
}
void emptyFrameQueue(){
while(!frameQueue.empty()){
frameQueue.pop();
}
}
void EstimatePropagator::propagate(OptimizableGraph::VertexSet& vset,
const EstimatePropagator::PropagateCost& cost,
const EstimatePropagator::PropagateAction& action,
double maxDistance,
double maxEdgeCost)
{
reset();
PriorityQueue frontier;
for (OptimizableGraph::VertexSet::iterator vit=vset.begin(); vit!=vset.end(); ++vit){
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*vit);
AdjacencyMap::iterator it = _adjacencyMap.find(v);
assert(it != _adjacencyMap.end());
it->second._distance = 0.;
it->second._parent.clear();
it->second._frontierLevel = 0;
frontier.push(&it->second);
}
while(! frontier.empty()){
AdjacencyMapEntry* entry = frontier.pop();
OptimizableGraph::Vertex* u = entry->child();
double uDistance = entry->distance();
//cerr << "uDistance " << uDistance << endl;
// initialize the vertex
if (entry->_frontierLevel > 0) {
action(entry->edge(), entry->parent(), u);
}
/* std::pair< OptimizableGraph::VertexSet::iterator, bool> insertResult = */ _visited.insert(u);
OptimizableGraph::EdgeSet::iterator et = u->edges().begin();
while (et != u->edges().end()){
OptimizableGraph::Edge* edge = static_cast<OptimizableGraph::Edge*>(*et);
++et;
int maxFrontier = -1;
OptimizableGraph::VertexSet initializedVertices;
for (size_t i = 0; i < edge->vertices().size(); ++i) {
OptimizableGraph::Vertex* z = static_cast<OptimizableGraph::Vertex*>(edge->vertex(i));
AdjacencyMap::iterator ot = _adjacencyMap.find(z);
if (ot->second._distance != numeric_limits<double>::max()) {
initializedVertices.insert(z);
maxFrontier = (max)(maxFrontier, ot->second._frontierLevel);
}
}
assert(maxFrontier >= 0);
for (size_t i = 0; i < edge->vertices().size(); ++i) {
OptimizableGraph::Vertex* z = static_cast<OptimizableGraph::Vertex*>(edge->vertex(i));
if (z == u)
continue;
size_t wasInitialized = initializedVertices.erase(z);
double edgeDistance = cost(edge, initializedVertices, z);
if (edgeDistance > 0. && edgeDistance != std::numeric_limits<double>::max() && edgeDistance < maxEdgeCost) {
double zDistance = uDistance + edgeDistance;
//cerr << z->id() << " " << zDistance << endl;
AdjacencyMap::iterator ot = _adjacencyMap.find(z);
assert(ot!=_adjacencyMap.end());
if (zDistance < ot->second.distance() && zDistance < maxDistance){
//if (ot->second.inQueue)
//cerr << "Updating" << endl;
ot->second._distance = zDistance;
ot->second._parent = initializedVertices;
ot->second._edge = edge;
ot->second._frontierLevel = maxFrontier + 1;
frontier.push(&ot->second);
}
}
if (wasInitialized > 0)
initializedVertices.insert(z);
}
}
}
// writing debug information like cost for reaching each vertex and the parent used to initialize
#ifdef DEBUG_ESTIMATE_PROPAGATOR
cerr << "Writing cost.dat" << endl;
ofstream costStream("cost.dat");
for (AdjacencyMap::const_iterator it = _adjacencyMap.begin(); it != _adjacencyMap.end(); ++it) {
HyperGraph::Vertex* u = it->second.child();
costStream << "vertex " << u->id() << " cost " << it->second._distance << endl;
}
cerr << "Writing init.dat" << endl;
ofstream initStream("init.dat");
vector<AdjacencyMapEntry*> frontierLevels;
for (AdjacencyMap::iterator it = _adjacencyMap.begin(); it != _adjacencyMap.end(); ++it) {
if (it->second._frontierLevel > 0)
frontierLevels.push_back(&it->second);
}
sort(frontierLevels.begin(), frontierLevels.end(), FrontierLevelCmp());
for (vector<AdjacencyMapEntry*>::const_iterator it = frontierLevels.begin(); it != frontierLevels.end(); ++it) {
AdjacencyMapEntry* entry = *it;
OptimizableGraph::Vertex* to = entry->child();
initStream << "calling init level = " << entry->_frontierLevel << "\t (";
for (OptimizableGraph::VertexSet::iterator pit = entry->parent().begin(); pit != entry->parent().end(); ++pit) {
initStream << " " << (*pit)->id();
}
initStream << " ) -> " << to->id() << endl;
}
#endif
}
//- "k_nearest_neighbor"
//- Find the K nearest neighbors to a point.
//-
//- Description:
//- This algorithm is based on the best-first search. The goal of this
//- algorithm is to minimize the number of nodes visited by using the
//- distance to each subtree's bounding box to avoid visiting subtrees
//- which could not possibly contain one of the k nearest objects.
//-
template <class Z> CubitStatus KDDTree<Z>::k_nearest_neighbor
(CubitVector &q, int k, double &closest_dist, DLIList<Z> &nearest_neighbors,
typename KDDTree<Z>::DistSqFunc dist_sq_point_data
)
{
//// Create the priority queues
PriorityQueue<KDDTreeNode<Z>*> *queue = new PriorityQueue<KDDTreeNode<Z>*> (KDDTree<Z>::less_than_func);
PriorityQueue<KDDTreeNode<Z>*> *queueTemp = new PriorityQueue<KDDTreeNode<Z>*> (KDDTree<Z>::less_than_func);
KDDTreeNode<Z> *element = root;
// push this node on the queue
element->set_dist (min_dist_sq (q, element->safetyBox));
element->set_dist_data (DD_SAFETY);
queue->push (element);
// if the k closest nodes on the tree are not leaf-nodes, expand the closest
// non-leaf node
while ( !queue->empty() )
{
element = queue->top();
queue->pop();
if (element->get_dist_data() == DD_LEAF)
{
// this node is a leaf, so it can be pushed onto the temporary queue
queueTemp->push (element);
}
else
{
// one of the top k nodes is a non-leaf node, so expand it
if (element->left)
{
element->left->set_dist (min_dist_sq (q, element->left->safetyBox));
element->left->set_dist_data (DD_SAFETY);
queue->push (element->left);
}
if (element->right)
{
element->right->set_dist (min_dist_sq (q, element->right->safetyBox));
element->right->set_dist_data (DD_SAFETY);
queue->push (element->right);
}
element->set_dist (dist_sq_point_data (q, element->data));
element->set_dist_data (DD_LEAF);
queue->push (element);
// take all the elements in the temporary queue and reinsert them into
// the actual queue
while ( !queueTemp->empty() )
{
queue->push ( queueTemp->top() );
queueTemp->pop ();
}
}
if (queueTemp->size() == k)
{
// success-- place the k nodes into the nearest_neighbors list
element = queueTemp->top();
queueTemp->pop();
closest_dist = element->get_dist();
nearest_neighbors.append (element->data);
while ( !queueTemp->empty() )
{
nearest_neighbors.append ( queueTemp->top()->data );
queueTemp->pop();
}
return CUBIT_SUCCESS;
}
}
return CUBIT_FAILURE;
}
