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; }
void Piece::flashRun() { MoveTo* mt = MoveTo::create(0.5, getRealPos() + ORIGIN); this->runAction(mt); }
bool pathfinder::build_costmap() { INFO_TARGET << "Pathfinder build costmap: (" << m_limits.X.min << "," << m_limits.Z.min << ") (" << m_limits.X.max << "," << m_limits.Z.max << ")" << std::endl; m_data.resize(m_max_index_x); for (int x = 0; x < m_max_index_x; x++) { m_data[x].resize(m_max_index_z); for (int z = 0; z < m_max_index_z; z++) { m_data[x][z].resize(m_max_index_y); int surfaces = 0; for (int y = 0; y < m_max_index_y; y++) { v3s16 ipos(x,y,z); v3s16 realpos = getRealPos(ipos); MapNode current = m_env->getMap().getNodeNoEx(realpos); MapNode below = m_env->getMap().getNodeNoEx(realpos + v3s16(0,-1,0)); if ((current.param0 == CONTENT_IGNORE) || (below.param0 == CONTENT_IGNORE)) { DEBUG_OUT("Pathfinder: " << PPOS(realpos) << " current or below is invalid element" << std::endl); if (current.param0 == CONTENT_IGNORE) { m_data[x][z][y].type = 'i'; DEBUG_OUT(x << "," << y << "," << z << ": " << 'i' << std::endl); } continue; } //don't add anything if it isn't an air node if ((current.param0 != CONTENT_AIR) || (below.param0 == CONTENT_AIR )) { DEBUG_OUT("Pathfinder: " << PPOS(realpos) << " not on surface" << std::endl); if (current.param0 != CONTENT_AIR) { m_data[x][z][y].type = 's'; DEBUG_OUT(x << "," << y << "," << z << ": " << 's' << std::endl); } else { m_data[x][z][y].type = '-'; DEBUG_OUT(x << "," << y << "," << z << ": " << '-' << std::endl); } continue; } surfaces++; m_data[x][z][y].valid = true; m_data[x][z][y].pos = realpos; m_data[x][z][y].type = 'g'; DEBUG_OUT(x << "," << y << "," << z << ": " << 'a' << std::endl); if (m_prefetch) { m_data[x][z][y].directions[DIR_XP] = calc_cost(realpos,v3s16( 1,0, 0)); m_data[x][z][y].directions[DIR_XM] = calc_cost(realpos,v3s16(-1,0, 0)); m_data[x][z][y].directions[DIR_ZP] = calc_cost(realpos,v3s16( 0,0, 1)); m_data[x][z][y].directions[DIR_ZM] = calc_cost(realpos,v3s16( 0,0,-1)); } } if (surfaces >= 1 ) { for (int y = 0; y < m_max_index_y; y++) { if (m_data[x][z][y].valid) { m_data[x][z][y].surfaces = surfaces; } } } } } return true; }