// @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; }
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() ); }
void tick(bool includeOldAudio = false){ bool success = false; StreamFrameMap streamFrames; streamFrames[videoStream] = Frame::CreateEmpty(); streamFrames[audioStream] = Frame::CreateEmpty(); while(!IsEof() && !success) { try { int audioQueueTargetSize = audioDevice->GetBlockSize() * 4; while( frameQueue.size() < (unsigned int)targetFrameQueueSize || (hasAudioStream() && audioHandler->getAudioQueueSize() < audioQueueTargetSize)) { if(frameQueue.size() >= (unsigned int)maxFrameQueueSize) break; bool frameDecoded = decodeFrame(streamFrames); if(!frameDecoded) throw VideoException(VideoException::EDecodingVideo); if(streamFrames[videoStream]->finished != 0){ frameQueue.push(streamFrames[videoStream]->Clone()); streamFrames[videoStream] = Frame::CreateEmpty(); } if(streamFrames[audioStream]->finished != 0){ // only enqueue audio that's newer than the current video time, // eg. on seeking we might encounter audio that's older than the frames in the frame queue. if(streamFrames[audioStream]->GetSamples().size() > 0 && (includeOldAudio || streamFrames[audioStream]->GetSamples()[0].ts >= timeHandler->GetTime())) { audioHandler->EnqueueAudio(streamFrames[audioStream]->GetSamples()); }else{ FlogD("skipping old audio samples: " << streamFrames[audioStream]->GetSamples().size()); } streamFrames[audioStream] = Frame::CreateEmpty(); } } // sync framequeue target size with number of frames needed for audio queue if(targetFrameQueueSize < (int)frameQueue.size()){ targetFrameQueueSize = std::max((int)frameQueue.size(), minFrameQueueSize); } success = true; } catch(VideoException e) { Retry(Str("Exception in tick: " << e.what())); } } }
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 PriorityQueueTest::fillQueue( void ) { for ( unsigned i = 0; i < FillSize; ++i ) { Dummyer::Ptr d( new Dummyer(i) ); _dummies->push( Dummy::Ptr(d) ); d = 0; } }
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) ) ); } }
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); }
int main(){ PriorityQueue<int> pq; for(int i=0;i<10;i++){ pq.push(i); } while(!pq.empty()){ cout<<pq.top()<<" "; pq.pop(); } }
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; }
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; } }
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() ); }
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; }
//- "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; }
PriorityQueue VectorialSearch::search(string query, double alpha, double gamma){ string token; ifstream index; PriorityQueue top; // cout << alpha << " " << gamma << endl; double w_t, w_dt; int previous_doc_id = 0; int word_id, doc_id, freq, pos; unordered_map<unsigned int, double> acc_text; unordered_map<unsigned int, double> acc_anchor; ifstream input; Tokenizer t(query, Stopwords::instance()->get_value()); // this->reset_distance(); // Calculating tfidf for each word in query // TODO: Accumulate tfidf of documents in order to have vectorial product while(t.size() > 0){ token = t.getToken(); if(token.size() > 0){ std::cout << "\tProcessing token: " << token << std::endl; this->set_text_search(); if(!((*this->vocabulary).find(token) == (*this->vocabulary).end())){ this->cossine_similarity(token, acc_text, INDEX_SORTED_FILE_NAME); } this->set_anchor_search(); if(!((*this->vocabulary).find(token) == (*this->vocabulary).end())){ this->cossine_similarity(token, acc_anchor, INDEX_SORTED_FILE_NAME); } } } for (auto item : acc_text){ Ranking rank; // item.first => doc_id // item.second => acc[doc_id] rank.id = item.first; rank.rank = 0.0; if(this->w_d_text.find(rank.id) == this->w_d_text.end()){ this->w_d_text[rank.id] = 0.0; } else { rank.rank = this->w_d_text[rank.id] > 0 ? (acc_text[rank.id] / this->w_d_text[rank.id]) : 0.0; } if(this->w_d_anchor.find(rank.id) == this->w_d_anchor.end()){ this->w_d_anchor[rank.id] = 0.0; } else { double sum = this->w_d_anchor[rank.id] ? (alpha*((acc_anchor[rank.id] / this->w_d_anchor[rank.id]))) : 0.0; rank.rank += sum; } if (this->pagerank.find(rank.id) != this->pagerank.end()){ rank.rank += (gamma*this->pagerank[rank.id]); } top.push(rank); } return top; }
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 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 }
void prim(Graph graph, char startNode){ PriorityQueue priorityQueue; Node node; node.key = INT_MAX; node.pi = '\0'; // for (auto it = graph.getVertices().begin(); it != graph.getVertices().end(); it++){ // node.name = *it; // priorityQueue.push(node); // } string tempString = ""; for (auto it = graph.getVertices().begin(); it != graph.getVertices().end(); it++) tempString += *it; cout << tempString << "!!!" << endl; for (int i = 0; i < tempString.length(); i++){ node.name = tempString.c_str()[i]; priorityQueue.push(node); } // Node node; // node.key = INT_MAX; // node.pi = '\0'; // node.name = 'i'; // priorityQueue.push(node); // node.name = 'a'; // priorityQueue.push(node); // node.name = 'b'; // priorityQueue.push(node); // node.name = 'c'; // priorityQueue.push(node); // node.name = 'd'; // priorityQueue.push(node); // node.name = 'e'; // priorityQueue.push(node); // node.name = 'f'; // priorityQueue.push(node); // node.name = 'g'; // priorityQueue.push(node); // node.name = 'h'; // priorityQueue.push(node); cout << "finished inserting vertices" << endl; priorityQueue.decreaseKey(startNode, 0, '\0'); while (!priorityQueue.empty()){ Node node = priorityQueue.extractMin(); cout << "extracted: " << node.name << ": " << node.key << ", " << node.pi << endl; if (node.name != node.pi && node.pi != '\0') cout << "(" << node.name << ", " << node.pi << ") " << endl; for (auto it = graph.getEdges().begin(); it != graph.getEdges().end(); it++){ cout << "checking " << it->first << endl; if (it->first.c_str()[0] == node.name || it->first.c_str()[1] == node.name){// Adjacency char adjacency = (it->first.c_str()[0] == node.name)?it->first.c_str()[1]:it->first.c_str()[0]; cout << "going for adj" << adjacency << endl; if (priorityQueue.find(adjacency) && priorityQueue.getNode(adjacency).key > graph.getWeight(node.name, adjacency)){ cout << "decreasingKey: " << adjacency << ", w: " << graph.getWeight(node.name, adjacency) << ", " << node.name << endl; priorityQueue.decreaseKey(adjacency, graph.getWeight(node.name, adjacency), node.name); } } } } cout << endl; }
void PriorityQueueTest::enqueue( void ) { sleep( 0.1 ); // Give parent thread a chance to block on the queue before we push to it. CPPUNIT_ASSERT_NO_THROW( _queue->push( 42 ) ); }
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() { // 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; }