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