Node<Plot>* search(std::map<Plot*, std::vector<Plot*> >& path, Plot& start, Plot& goal) { Node<Plot>* node_goal = new Node<Plot>(goal); Node<Plot>* node_start = new Node<Plot>(start); PriorityQueue queue; std::unordered_map<Plot*, int> closeList; queue.insert(node_start); int iter = 0; while (!queue.empty()) { iter++; Node<Plot> *current(*(queue.begin())); queue.erase(queue.begin()); std::vector<Plot*>& neighbours(path[current->node]); for (Plot *p : neighbours) { Node<Plot>* next = new Node<Plot>(*p); next->g = current->g + manhattan_distance(current->node, p); next->h = manhattan_distance(p, &goal); next->f = next->g + next->h; next->x = current->x + 1; // nb turns next->parent = current; if (p == node_goal->node) { std::cout << "nbIter: " << iter << std::endl; return next; } if (isWorthTrying(next, queue, closeList)) { queue.insert(next); } } closeList[current->node] = current->f; delete current; } return nullptr; }
IGL_INLINE bool igl::decimate( const Eigen::MatrixXd & OV, const Eigen::MatrixXi & OF, const std::function<void( const int, const Eigen::MatrixXd &, const Eigen::MatrixXi &, const Eigen::MatrixXi &, const Eigen::VectorXi &, const Eigen::MatrixXi &, const Eigen::MatrixXi &, double &, Eigen::RowVectorXd &)> & cost_and_placement, const std::function<bool( const Eigen::MatrixXd &, const Eigen::MatrixXi &, const Eigen::MatrixXi &, const Eigen::VectorXi &, const Eigen::MatrixXi &, const Eigen::MatrixXi &, const std::set<std::pair<double,int> > &, const std::vector<std::set<std::pair<double,int> >::iterator > &, const Eigen::MatrixXd &, const int, const int, const int, const int, const int)> & stopping_condition, const std::function<bool( const Eigen::MatrixXd & ,/*V*/ const Eigen::MatrixXi & ,/*F*/ const Eigen::MatrixXi & ,/*E*/ const Eigen::VectorXi & ,/*EMAP*/ const Eigen::MatrixXi & ,/*EF*/ const Eigen::MatrixXi & ,/*EI*/ const std::set<std::pair<double,int> > & ,/*Q*/ const std::vector<std::set<std::pair<double,int> >::iterator > &,/*Qit*/ const Eigen::MatrixXd & ,/*C*/ const int /*e*/ )> & pre_collapse, const std::function<void( const Eigen::MatrixXd & , /*V*/ const Eigen::MatrixXi & , /*F*/ const Eigen::MatrixXi & , /*E*/ const Eigen::VectorXi & ,/*EMAP*/ const Eigen::MatrixXi & , /*EF*/ const Eigen::MatrixXi & , /*EI*/ const std::set<std::pair<double,int> > & , /*Q*/ const std::vector<std::set<std::pair<double,int> >::iterator > &, /*Qit*/ const Eigen::MatrixXd & , /*C*/ const int , /*e*/ const int , /*e1*/ const int , /*e2*/ const int , /*f1*/ const int , /*f2*/ const bool /*collapsed*/ )> & post_collapse, const Eigen::MatrixXi & OE, const Eigen::VectorXi & OEMAP, const Eigen::MatrixXi & OEF, const Eigen::MatrixXi & OEI, Eigen::MatrixXd & U, Eigen::MatrixXi & G, Eigen::VectorXi & J, Eigen::VectorXi & I ) { // Decimate 1 using namespace Eigen; using namespace std; // Working copies Eigen::MatrixXd V = OV; Eigen::MatrixXi F = OF; Eigen::MatrixXi E = OE; Eigen::VectorXi EMAP = OEMAP; Eigen::MatrixXi EF = OEF; Eigen::MatrixXi EI = OEI; typedef std::set<std::pair<double,int> > PriorityQueue; PriorityQueue Q; std::vector<PriorityQueue::iterator > Qit; Qit.resize(E.rows()); // If an edge were collapsed, we'd collapse it to these points: MatrixXd C(E.rows(),V.cols()); for(int e = 0;e<E.rows();e++) { double cost = e; RowVectorXd p(1,3); cost_and_placement(e,V,F,E,EMAP,EF,EI,cost,p); C.row(e) = p; Qit[e] = Q.insert(std::pair<double,int>(cost,e)).first; } int prev_e = -1; bool clean_finish = false; while(true) { if(Q.empty()) { break; } if(Q.begin()->first == std::numeric_limits<double>::infinity()) { // min cost edge is infinite cost break; } int e,e1,e2,f1,f2; if(collapse_edge( cost_and_placement, pre_collapse, post_collapse, V,F,E,EMAP,EF,EI,Q,Qit,C,e,e1,e2,f1,f2)) { if(stopping_condition(V,F,E,EMAP,EF,EI,Q,Qit,C,e,e1,e2,f1,f2)) { clean_finish = true; break; } }else { if(prev_e == e) { assert(false && "Edge collapse no progress... bad stopping condition?"); break; } // Edge was not collapsed... must have been invalid. collapse_edge should // have updated its cost to inf... continue } prev_e = e; } // remove all IGL_COLLAPSE_EDGE_NULL faces MatrixXi F2(F.rows(),3); J.resize(F.rows()); int m = 0; for(int f = 0;f<F.rows();f++) { if( F(f,0) != IGL_COLLAPSE_EDGE_NULL || F(f,1) != IGL_COLLAPSE_EDGE_NULL || F(f,2) != IGL_COLLAPSE_EDGE_NULL) { F2.row(m) = F.row(f); J(m) = f; m++; } } F2.conservativeResize(m,F2.cols()); J.conservativeResize(m); VectorXi _1; remove_unreferenced(V,F2,U,G,_1,I); return clean_finish; }