void Hungarian::print_status()
{
fprintf(stderr,"cost:\n");
print_cost();
fprintf(stderr,"assignment:\n");
print_assignment();
}
template <typename T, typename X> void core_solver_pretty_printer<T, X>::print() {
for (unsigned i = 0; i < nrows(); i++) {
print_row(i);
}
print_bottom_line();
print_cost();
print_x();
print_basis_heading();
print_lows();
print_upps();
print_exact_norms();
print_approx_norms();
m_out << std::endl;
}
void pathfinder::print_cost() {
print_cost(DIR_XP);
print_cost(DIR_XM);
print_cost(DIR_ZP);
print_cost(DIR_ZM);
}
std::vector<v3s16> pathfinder::get_Path(ServerEnvironment* env,
v3s16 source,
v3s16 destination,
unsigned int searchdistance,
unsigned int max_jump,
unsigned int max_drop,
algorithm algo) {
#ifdef PATHFINDER_CALC_TIME
timespec ts;
clock_gettime(CLOCK_REALTIME, &ts);
#endif
std::vector<v3s16> retval;
//check parameters
if (env == 0) {
ERROR_TARGET << "missing environment pointer" << std::endl;
return retval;
}
m_searchdistance = searchdistance;
m_env = env;
m_maxjump = max_jump;
m_maxdrop = max_drop;
m_start = source;
m_destination = destination;
m_min_target_distance = -1;
m_prefetch = true;
if (algo == A_PLAIN_NP) {
m_prefetch = false;
}
int min_x = MYMIN(source.X,destination.X);
int max_x = MYMAX(source.X,destination.X);
int min_y = MYMIN(source.Y,destination.Y);
int max_y = MYMAX(source.Y,destination.Y);
int min_z = MYMIN(source.Z,destination.Z);
int max_z = MYMAX(source.Z,destination.Z);
m_limits.X.min = min_x - searchdistance;
m_limits.X.max = max_x + searchdistance;
m_limits.Y.min = min_y - searchdistance;
m_limits.Y.max = max_y + searchdistance;
m_limits.Z.min = min_z - searchdistance;
m_limits.Z.max = max_z + searchdistance;
m_max_index_x = m_limits.X.max - m_limits.X.min;
m_max_index_y = m_limits.Y.max - m_limits.Y.min;
m_max_index_z = m_limits.Z.max - m_limits.Z.min;
//build data map
if (!build_costmap()) {
ERROR_TARGET << "failed to build costmap" << std::endl;
return retval;
}
#ifdef PATHFINDER_DEBUG
print_type();
print_cost();
print_ydir();
#endif
//validate and mark start and end pos
v3s16 StartIndex = getIndexPos(source);
v3s16 EndIndex = getIndexPos(destination);
path_gridnode& startpos = getIndexElement(StartIndex);
path_gridnode& endpos = getIndexElement(EndIndex);
if (!startpos.valid) {
VERBOSE_TARGET << "invalid startpos" <<
"Index: " << PPOS(StartIndex) <<
"Realpos: " << PPOS(getRealPos(StartIndex)) << std::endl;
return retval;
}
if (!endpos.valid) {
VERBOSE_TARGET << "invalid stoppos" <<
"Index: " << PPOS(EndIndex) <<
"Realpos: " << PPOS(getRealPos(EndIndex)) << std::endl;
return retval;
}
endpos.target = true;
startpos.source = true;
startpos.totalcost = 0;
bool update_cost_retval = false;
switch (algo) {
case DIJKSTRA:
update_cost_retval = update_all_costs(StartIndex,v3s16(0,0,0),0,0);
break;
case A_PLAIN_NP:
case A_PLAIN:
update_cost_retval = update_cost_heuristic(StartIndex,v3s16(0,0,0),0,0);
break;
default:
ERROR_TARGET << "missing algorithm"<< std::endl;
break;
}
if (update_cost_retval) {
#ifdef PATHFINDER_DEBUG
std::cout << "Path to target found!" << std::endl;
print_pathlen();
#endif
//find path
std::vector<v3s16> path;
build_path(path,EndIndex,0);
#ifdef PATHFINDER_DEBUG
std::cout << "Full index path:" << std::endl;
print_path(path);
#endif
//finalize path
std::vector<v3s16> full_path;
for (std::vector<v3s16>::iterator i = path.begin();
i != path.end(); ++i) {
full_path.push_back(getIndexElement(*i).pos);
}
#ifdef PATHFINDER_DEBUG
std::cout << "full path:" << std::endl;
print_path(full_path);
#endif
#ifdef PATHFINDER_CALC_TIME
timespec ts2;
clock_gettime(CLOCK_REALTIME, &ts2);
int ms = (ts2.tv_nsec - ts.tv_nsec)/(1000*1000);
int us = ((ts2.tv_nsec - ts.tv_nsec) - (ms*1000*1000))/1000;
int ns = ((ts2.tv_nsec - ts.tv_nsec) - ( (ms*1000*1000) + (us*1000)));
std::cout << "Calculating path took: " << (ts2.tv_sec - ts.tv_sec) <<
"s " << ms << "ms " << us << "us " << ns << "ns " << std::endl;
#endif
return full_path;
}
else {
#ifdef PATHFINDER_DEBUG
print_pathlen();
#endif
ERROR_TARGET << "failed to update cost map"<< std::endl;
}
//return
return retval;
}