void Pathfinder::buildPath(std::vector<v3s16> &path, v3s16 pos, int level) { level ++; if (level > 700) { ERROR_TARGET << LVL "Pathfinder: path is too long aborting" << std::endl; return; } PathGridnode &g_pos = getIndexElement(pos); if (!g_pos.valid) { ERROR_TARGET << LVL "Pathfinder: invalid next pos detected aborting" << std::endl; return; } g_pos.is_element = true; //check if source reached if (g_pos.source) { path.push_back(pos); return; } buildPath(path, pos + g_pos.sourcedir, level); path.push_back(pos); }
bool Pathfinder::updateAllCosts(v3s16 ipos, v3s16 srcdir, int current_cost, int level) { PathGridnode &g_pos = getIndexElement(ipos); g_pos.totalcost = current_cost; g_pos.sourcedir = srcdir; level ++; //check if target has been found if (g_pos.target) { m_min_target_distance = current_cost; DEBUG_OUT(LVL " Pathfinder: target found!" << std::endl); return true; } bool retval = false; std::vector<v3s16> directions; directions.push_back(v3s16( 1,0, 0)); directions.push_back(v3s16(-1,0, 0)); directions.push_back(v3s16( 0,0, 1)); directions.push_back(v3s16( 0,0,-1)); for (unsigned int i=0; i < directions.size(); i++) { if (directions[i] != srcdir) { PathCost cost = g_pos.getCost(directions[i]); if (cost.valid) { directions[i].Y = cost.direction; v3s16 ipos2 = ipos + directions[i]; if (!isValidIndex(ipos2)) { DEBUG_OUT(LVL " Pathfinder: " << PP(ipos2) << " out of range, max=" << PP(m_limits.MaxEdge) << std::endl); continue; } PathGridnode &g_pos2 = getIndexElement(ipos2); if (!g_pos2.valid) { VERBOSE_TARGET << LVL "Pathfinder: no data for new position: " << PP(ipos2) << std::endl; continue; } assert(cost.value > 0); int new_cost = current_cost + cost.value; // check if there already is a smaller path if ((m_min_target_distance > 0) && (m_min_target_distance < new_cost)) { return false; } if ((g_pos2.totalcost < 0) || (g_pos2.totalcost > new_cost)) { DEBUG_OUT(LVL "Pathfinder: updating path at: "<< PP(ipos2) << " from: " << g_pos2.totalcost << " to "<< new_cost << std::endl); if (updateAllCosts(ipos2, invert(directions[i]), new_cost, level)) { retval = true; } } else { DEBUG_OUT(LVL "Pathfinder:" " already found shorter path to: " << PP(ipos2) << std::endl); } } else { DEBUG_OUT(LVL "Pathfinder:" " not moving to invalid direction: " << PP(directions[i]) << std::endl); } } } return retval; }
std::vector<v3s16> Pathfinder::getPath(ServerEnvironment *env, v3s16 source, v3s16 destination, unsigned int searchdistance, unsigned int max_jump, unsigned int max_drop, PathAlgorithm 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 == PA_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.MinEdge.X = min_x - searchdistance; m_limits.MinEdge.Y = min_y - searchdistance; m_limits.MinEdge.Z = min_z - searchdistance; m_limits.MaxEdge.X = max_x + searchdistance; m_limits.MaxEdge.Y = max_y + searchdistance; m_limits.MaxEdge.Z = max_z + searchdistance; v3s16 diff = m_limits.MaxEdge - m_limits.MinEdge; m_max_index_x = diff.X; m_max_index_y = diff.Y; m_max_index_z = diff.Z; delete m_nodes_container; if (diff.getLength() > 5) { m_nodes_container = new MapGridNodeContainer(this); } else { m_nodes_container = new ArrayGridNodeContainer(this, diff); } #ifdef PATHFINDER_DEBUG printType(); printCost(); printYdir(); #endif //validate and mark start and end pos v3s16 StartIndex = getIndexPos(source); v3s16 EndIndex = getIndexPos(destination); PathGridnode &startpos = getIndexElement(StartIndex); PathGridnode &endpos = getIndexElement(EndIndex); if (!startpos.valid) { VERBOSE_TARGET << "invalid startpos" << "Index: " << PP(StartIndex) << "Realpos: " << PP(getRealPos(StartIndex)) << std::endl; return retval; } if (!endpos.valid) { VERBOSE_TARGET << "invalid stoppos" << "Index: " << PP(EndIndex) << "Realpos: " << PP(getRealPos(EndIndex)) << std::endl; return retval; } endpos.target = true; startpos.source = true; startpos.totalcost = 0; bool update_cost_retval = false; switch (algo) { case PA_DIJKSTRA: update_cost_retval = updateAllCosts(StartIndex, v3s16(0, 0, 0), 0, 0); break; case PA_PLAIN_NP: case PA_PLAIN: update_cost_retval = updateCostHeuristic(StartIndex, v3s16(0, 0, 0), 0, 0); break; default: ERROR_TARGET << "missing PathAlgorithm"<< std::endl; break; } if (update_cost_retval) { #ifdef PATHFINDER_DEBUG std::cout << "Path to target found!" << std::endl; printPathLen(); #endif //find path std::vector<v3s16> path; buildPath(path, EndIndex, 0); #ifdef PATHFINDER_DEBUG std::cout << "Full index path:" << std::endl; printPath(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; printPath(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 printPathLen(); #endif ERROR_TARGET << "failed to update cost map"<< std::endl; } //return return retval; }
bool pathfinder::update_cost_heuristic( v3s16 ipos, v3s16 srcdir, int current_cost, int level) { path_gridnode& g_pos = getIndexElement(ipos); g_pos.totalcost = current_cost; g_pos.sourcedir = srcdir; level ++; //check if target has been found if (g_pos.target) { m_min_target_distance = current_cost; DEBUG_OUT(LVL " Pathfinder: target found!" << std::endl); return true; } bool retval = false; std::vector<v3s16> directions; directions.push_back(v3s16( 1,0, 0)); directions.push_back(v3s16(-1,0, 0)); directions.push_back(v3s16( 0,0, 1)); directions.push_back(v3s16( 0,0,-1)); v3s16 direction = get_dir_heuristic(directions,g_pos); while (direction != v3s16(0,0,0) && (!retval)) { if (direction != srcdir) { path_cost cost = g_pos.get_cost(direction); if (cost.valid) { direction.Y = cost.direction; v3s16 ipos2 = ipos + direction; if (!valid_index(ipos2)) { DEBUG_OUT(LVL " Pathfinder: " << PPOS(ipos2) << " out of range (" << m_limits.X.max << "," << m_limits.Y.max << "," << m_limits.Z.max <<")" << std::endl); direction = get_dir_heuristic(directions,g_pos); continue; } path_gridnode& g_pos2 = getIndexElement(ipos2); if (!g_pos2.valid) { VERBOSE_TARGET << LVL "Pathfinder: no data for new position: " << PPOS(ipos2) << std::endl; direction = get_dir_heuristic(directions,g_pos); continue; } assert(cost.value > 0); int new_cost = current_cost + cost.value; // check if there already is a smaller path if ((m_min_target_distance > 0) && (m_min_target_distance < new_cost)) { DEBUG_OUT(LVL "Pathfinder:" " already longer than best already found path " << PPOS(ipos2) << std::endl); return false; } if ((g_pos2.totalcost < 0) || (g_pos2.totalcost > new_cost)) { DEBUG_OUT(LVL "Pathfinder: updating path at: "<< PPOS(ipos2) << " from: " << g_pos2.totalcost << " to "<< new_cost << " srcdir=" << PPOS(invert(direction))<< std::endl); if (update_cost_heuristic(ipos2,invert(direction), new_cost,level)) { retval = true; } } else { DEBUG_OUT(LVL "Pathfinder:" " already found shorter path to: " << PPOS(ipos2) << std::endl); } } else { DEBUG_OUT(LVL "Pathfinder:" " not moving to invalid direction: " << PPOS(direction) << std::endl); } } else { DEBUG_OUT(LVL "Pathfinder:" " skipping srcdir: " << PPOS(direction) << std::endl); } direction = get_dir_heuristic(directions,g_pos); } return retval; }