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