From 69367aa7998d3817db1d4b101f36a6e25b1becf8 Mon Sep 17 00:00:00 2001 From: sapier Date: Sun, 17 Mar 2013 17:03:44 +0000 Subject: [PATCH] Add Dijkstra A* and A* without prefetching pathfind algorithms --- doc/lua_api.txt | 16 +- src/CMakeLists.txt | 1 + src/environment.cpp | 23 + src/environment.h | 3 + src/pathfinder.cpp | 1081 +++++++++++++++++++++++++++++++++++++++++ src/pathfinder.h | 345 +++++++++++++ src/scriptapi_env.cpp | 66 +++ src/scriptapi_env.h | 6 + 8 files changed, 1540 insertions(+), 1 deletion(-) create mode 100644 src/pathfinder.cpp create mode 100644 src/pathfinder.h diff --git a/doc/lua_api.txt b/doc/lua_api.txt index de73ecd3..285f3d20 100644 --- a/doc/lua_api.txt +++ b/doc/lua_api.txt @@ -1185,7 +1185,21 @@ methods: - get_perlin(seeddiff, octaves, persistence, scale) ^ Return world-specific perlin noise (int(worldseed)+seeddiff) - clear_objects() - ^ clear all objects in the environments + ^ clear all objects in the environments +- line_of_sight(pos1,pos2,stepsize) ->true/false + ^ checkif there is a direct line of sight between pos1 and pos2 + ^ pos1 First position + ^ pos2 Second position + ^ stepsize smaller gives more accurate results but requires more computing + time. Default is 1. +-find_path(pos1,pos2,searchdistance,max_jump,max_drop,algorithm) -> table containing path + ^ returns a table of 3d points representing a path from pos1 to pos2 or nil + ^ pos1: start position + ^ pos2: end position + ^ searchdistance: number of blocks to search in each direction + ^ max_jump: maximum height difference to consider walkable + ^ max_drop: maximum height difference to consider droppable + ^ algorithm: A*_noprefetch(default), A*, Dijkstra - spawn_tree (pos, {treedef}) ^ spawns L-System tree at given pos with definition in treedef table treedef={ diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index d6182861..1ed1c862 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -264,6 +264,7 @@ set(common_SRCS clientserver.cpp staticobject.cpp serverlist.cpp + pathfinder.cpp util/serialize.cpp util/directiontables.cpp util/numeric.cpp diff --git a/src/environment.cpp b/src/environment.cpp index 385e5b95..fc7972b2 100644 --- a/src/environment.cpp +++ b/src/environment.cpp @@ -364,6 +364,29 @@ ServerMap & ServerEnvironment::getServerMap() return *m_map; } +bool ServerEnvironment::line_of_sight(v3f pos1, v3f pos2, float stepsize) +{ + float distance = pos1.getDistanceFrom(pos2); + + //calculate normalized direction vector + v3f normalized_vector = v3f((pos2.X - pos1.X)/distance, + (pos2.Y - pos1.Y)/distance, + (pos2.Z - pos1.Z)/distance); + + //find out if there's a node on path between pos1 and pos2 + for (float i = 1; i < distance; i += stepsize) { + v3s16 pos = floatToInt(v3f(normalized_vector.X * i, + normalized_vector.Y * i, + normalized_vector.Z * i) +pos1,BS); + + MapNode n = getMap().getNodeNoEx(pos); + + if(n.param0 != CONTENT_AIR) { + return false; + } + } + return true; +} void ServerEnvironment::serializePlayers(const std::string &savedir) { diff --git a/src/environment.h b/src/environment.h index 7f73a546..a3e43dbb 100644 --- a/src/environment.h +++ b/src/environment.h @@ -298,6 +298,9 @@ public: // This makes stuff happen void step(f32 dtime); + //check if there's a line of sight between two positions + bool line_of_sight(v3f pos1, v3f pos2, float stepsize=1.0); + private: /* diff --git a/src/pathfinder.cpp b/src/pathfinder.cpp new file mode 100644 index 00000000..c7621177 --- /dev/null +++ b/src/pathfinder.cpp @@ -0,0 +1,1081 @@ +/* +Minetest +Copyright (C) 2013 sapier, sapier at gmx dot net + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU Lesser General Public License as published by +the Free Software Foundation; either version 2.1 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU Lesser General Public License for more details. + +You should have received a copy of the GNU Lesser General Public License along +with this program; if not, write to the Free Software Foundation, Inc., +51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. +*/ + +/******************************************************************************/ +/* Includes */ +/******************************************************************************/ + +#include "pathfinder.h" + +#ifdef PATHFINDER_DEBUG +#include +#endif +#ifdef PATHFINDER_CALC_TIME + #include +#endif + +/******************************************************************************/ +/* Typedefs and macros */ +/******************************************************************************/ + +//#define PATHFINDER_CALC_TIME + +/** shortcut to print a 3d pos */ +#define PPOS(pos) "(" << pos.X << "," << pos.Y << "," << pos.Z << ")" + +#define LVL "(" << level << ")" << + +#ifdef PATHFINDER_DEBUG +#define DEBUG_OUT(a) std::cout << a +#define INFO_TARGET std::cout +#define VERBOSE_TARGET std::cout +#define ERROR_TARGET std::cout +#else +#define DEBUG_OUT(a) while(0) +#define INFO_TARGET infostream +#define VERBOSE_TARGET verbosestream +#define ERROR_TARGET errorstream +#endif + +/******************************************************************************/ +/* implementation */ +/******************************************************************************/ + +std::vector get_Path(ServerEnvironment* env, + v3s16 source, + v3s16 destination, + unsigned int searchdistance, + unsigned int max_jump, + unsigned int max_drop, + algorithm algo) { + + pathfinder searchclass; + + return searchclass.get_Path(env, + source,destination, + searchdistance,max_jump,max_drop,algo); +} + +/******************************************************************************/ +path_cost::path_cost() +: valid(false), + value(0), + direction(0), + updated(false) +{ + //intentionaly empty +} + +/******************************************************************************/ +path_cost::path_cost(const path_cost& b) { + valid = b.valid; + direction = b.direction; + value = b.value; + updated = b.updated; +} + +/******************************************************************************/ +path_cost& path_cost::operator= (const path_cost& b) { + valid = b.valid; + direction = b.direction; + value = b.value; + updated = b.updated; + + return *this; +} + +/******************************************************************************/ +path_gridnode::path_gridnode() +: valid(false), + target(false), + source(false), + totalcost(-1), + sourcedir(v3s16(0,0,0)), + surfaces(0), + pos(v3s16(0,0,0)), + is_element(false), + type('u') +{ + //intentionaly empty +} + +/******************************************************************************/ +path_gridnode::path_gridnode(const path_gridnode& b) +: valid(b.valid), + target(b.target), + source(b.source), + totalcost(b.totalcost), + sourcedir(b.sourcedir), + surfaces(b.surfaces), + pos(b.pos), + is_element(b.is_element), + type(b.type) + { + + directions[DIR_XP] = b.directions[DIR_XP]; + directions[DIR_XM] = b.directions[DIR_XM]; + directions[DIR_ZP] = b.directions[DIR_ZP]; + directions[DIR_ZM] = b.directions[DIR_ZM]; +} + +/******************************************************************************/ +path_gridnode& path_gridnode::operator= (const path_gridnode& b) { + valid = b.valid; + target = b.target; + source = b.source; + is_element = b.is_element; + totalcost = b.totalcost; + sourcedir = b.sourcedir; + surfaces = b.surfaces; + pos = b.pos; + type = b.type; + + directions[DIR_XP] = b.directions[DIR_XP]; + directions[DIR_XM] = b.directions[DIR_XM]; + directions[DIR_ZP] = b.directions[DIR_ZP]; + directions[DIR_ZM] = b.directions[DIR_ZM]; + + return *this; +} + +/******************************************************************************/ +path_cost path_gridnode::get_cost(v3s16 dir) { + if (dir.X > 0) { + return directions[DIR_XP]; + } + if (dir.X < 0) { + return directions[DIR_XM]; + } + if (dir.Z > 0) { + return directions[DIR_ZP]; + } + if (dir.Z < 0) { + return directions[DIR_ZM]; + } + path_cost retval; + return retval; +} + +/******************************************************************************/ +void path_gridnode::set_cost(v3s16 dir,path_cost cost) { + if (dir.X > 0) { + directions[DIR_XP] = cost; + } + if (dir.X < 0) { + directions[DIR_XM] = cost; + } + if (dir.Z > 0) { + directions[DIR_ZP] = cost; + } + if (dir.Z < 0) { + directions[DIR_ZM] = cost; + } +} + +/******************************************************************************/ +std::vector 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 retval; + + //check parameters + if (env == 0) { + std::cout << "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()) { + std::cout << "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) { + std::cout << "invalid startpos" << + "Index: " << PPOS(StartIndex) << + "Realpos: " << PPOS(getRealPos(StartIndex)) << std::endl; + return retval; + } + if (!endpos.valid) { + std::cout << "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: + std::cout << "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 path; + build_path(path,EndIndex,0); + +#ifdef PATHFINDER_DEBUG + std::cout << "Full index path:" << std::endl; + print_path(path); +#endif + + //optimize path + std::vector optimized_path; + + std::vector::iterator startpos = path.begin(); + optimized_path.push_back(source); + + for (std::vector::iterator i = path.begin(); + i != path.end(); i++) { + if (!m_env->line_of_sight( + tov3f(getIndexElement(*startpos).pos), + tov3f(getIndexElement(*i).pos))) { + optimized_path.push_back(getIndexElement(*(i-1)).pos); + startpos = (i-1); + } + } + + optimized_path.push_back(destination); + +#ifdef PATHFINDER_DEBUG + std::cout << "Optimized path:" << std::endl; + print_path(optimized_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 optimized_path; + } + else { +#ifdef PATHFINDER_DEBUG + print_pathlen(); +#endif + std::cout << "failed to update cost map"<< std::endl; + } + + + //return + return retval; +} + +/******************************************************************************/ +pathfinder::pathfinder() : + m_max_index_x(0), + m_max_index_y(0), + m_max_index_z(0), + m_searchdistance(0), + m_maxdrop(0), + m_maxjump(0), + m_min_target_distance(0), + m_prefetch(true), + m_start(0,0,0), + m_destination(0,0,0), + m_limits(), + m_data(), + m_env(0) +{ + //intentionaly empty +} + +/******************************************************************************/ +v3s16 pathfinder::getRealPos(v3s16 ipos) { + + v3s16 retval = ipos; + + retval.X += m_limits.X.min; + retval.Y += m_limits.Y.min; + retval.Z += m_limits.Z.min; + + return retval; +} + +/******************************************************************************/ +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; +} + +/******************************************************************************/ +path_cost pathfinder::calc_cost(v3s16 pos,v3s16 dir) { + path_cost retval; + + retval.updated = true; + + v3s16 pos2 = pos + dir; + + //check limits + if ( (pos2.X < m_limits.X.min) || + (pos2.X >= m_limits.X.max) || + (pos2.Z < m_limits.Z.min) || + (pos2.Z >= m_limits.Z.max)) { + DEBUG_OUT("Pathfinder: " << PPOS(pos2) << + " no cost -> out of limits" << std::endl); + return retval; + } + + MapNode node_at_pos2 = m_env->getMap().getNodeNoEx(pos2); + + //did we get information about node? + if (node_at_pos2.param0 == CONTENT_IGNORE ) { + VERBOSE_TARGET << "Pathfinder: (1) area at pos: " + << PPOS(pos2) << " not loaded"; + return retval; + } + + if (node_at_pos2.param0 == CONTENT_AIR) { + MapNode node_below_pos2 = + m_env->getMap().getNodeNoEx(pos2 + v3s16(0,-1,0)); + + //did we get information about node? + if (node_below_pos2.param0 == CONTENT_IGNORE ) { + VERBOSE_TARGET << "Pathfinder: (2) area at pos: " + << PPOS((pos2 + v3s16(0,-1,0))) << " not loaded"; + return retval; + } + + if (node_below_pos2.param0 != CONTENT_AIR) { + retval.valid = true; + retval.value = 1; + retval.direction = 0; + DEBUG_OUT("Pathfinder: "<< PPOS(pos) + << " cost same height found" << std::endl); + } + else { + v3s16 testpos = pos2 - v3s16(0,-1,0); + MapNode node_at_pos = m_env->getMap().getNodeNoEx(testpos); + + while ((node_at_pos.param0 != CONTENT_IGNORE) && + (node_at_pos.param0 == CONTENT_AIR) && + (testpos.Y > m_limits.Y.min)) { + testpos += v3s16(0,-1,0); + node_at_pos = m_env->getMap().getNodeNoEx(testpos); + } + + //did we find surface? + if ((testpos.Y >= m_limits.Y.min) && + (node_at_pos.param0 != CONTENT_IGNORE) && + (node_at_pos.param0 != CONTENT_AIR)) { + if (((pos2.Y - testpos.Y)*-1) <= m_maxdrop) { + retval.valid = true; + retval.value = 2; + //difference of y-pos +1 (target node is ABOVE solid node) + retval.direction = ((testpos.Y - pos2.Y) +1); + DEBUG_OUT("Pathfinder cost below height found" << std::endl); + } + else { + INFO_TARGET << "Pathfinder:" + " distance to surface below to big: " + << (testpos.Y - pos2.Y) << " max: " << m_maxdrop + << std::endl; + } + } + else { + DEBUG_OUT("Pathfinder: no surface below found" << std::endl); + } + } + } + else { + v3s16 testpos = pos2; + MapNode node_at_pos = m_env->getMap().getNodeNoEx(testpos); + + while ((node_at_pos.param0 != CONTENT_IGNORE) && + (node_at_pos.param0 != CONTENT_AIR) && + (testpos.Y < m_limits.Y.max)) { + testpos += v3s16(0,1,0); + node_at_pos = m_env->getMap().getNodeNoEx(testpos); + } + + //did we find surface? + if ((testpos.Y <= m_limits.Y.max) && + (node_at_pos.param0 == CONTENT_AIR)) { + + if (testpos.Y - pos2.Y <= m_maxjump) { + retval.valid = true; + retval.value = 2; + retval.direction = (testpos.Y - pos2.Y); + DEBUG_OUT("Pathfinder cost above found" << std::endl); + } + else { + DEBUG_OUT("Pathfinder: distance to surface above to big: " + << (testpos.Y - pos2.Y) << " max: " << m_maxjump + << std::endl); + } + } + else { + DEBUG_OUT("Pathfinder: no surface above found" << std::endl); + } + } + return retval; +} + +/******************************************************************************/ +v3s16 pathfinder::getIndexPos(v3s16 pos) { + + v3s16 retval = pos; + retval.X -= m_limits.X.min; + retval.Y -= m_limits.Y.min; + retval.Z -= m_limits.Z.min; + + return retval; +} + +/******************************************************************************/ +path_gridnode& pathfinder::getIndexElement(v3s16 ipos) { + return m_data[ipos.X][ipos.Z][ipos.Y]; +} + +/******************************************************************************/ +bool pathfinder::valid_index(v3s16 index) { + if ( (index.X < m_max_index_x) && + (index.Y < m_max_index_y) && + (index.Z < m_max_index_z) && + (index.X >= 0) && + (index.Y >= 0) && + (index.Z >= 0)) + return true; + + return false; +} + +/******************************************************************************/ +v3s16 pathfinder::invert(v3s16 pos) { + v3s16 retval = pos; + + retval.X *=-1; + retval.Y *=-1; + retval.Z *=-1; + + return retval; +} + +/******************************************************************************/ +bool pathfinder::update_all_costs( 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 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) { + path_cost cost = g_pos.get_cost(directions[i]); + + if (cost.valid) { + directions[i].Y = cost.direction; + + v3s16 ipos2 = ipos + directions[i]; + + 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); + continue; + } + + path_gridnode& g_pos2 = getIndexElement(ipos2); + + if (!g_pos2.valid) { + VERBOSE_TARGET << LVL "Pathfinder: no data for new position: " + << PPOS(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)) { + int old_cost = g_pos2.totalcost; + DEBUG_OUT(LVL "Pathfinder: updating path at: "<< + PPOS(ipos2) << " from: " << old_cost << " to "<< + new_cost << std::endl); + if (update_all_costs(ipos2,invert(directions[i]), + 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(directions[i]) << std::endl); + } + } + } + return retval; +} + +/******************************************************************************/ +int pathfinder::get_manhattandistance(v3s16 pos) { + + int min_x = MYMIN(pos.X,m_destination.X); + int max_x = MYMAX(pos.X,m_destination.X); + int min_z = MYMIN(pos.Z,m_destination.Z); + int max_z = MYMAX(pos.Z,m_destination.Z); + + return (max_x - min_x) + (max_z - min_z); +} + +/******************************************************************************/ +v3s16 pathfinder::get_dir_heuristic(std::vector& directions,path_gridnode& g_pos) { + int minscore = -1; + v3s16 retdir = v3s16(0,0,0); + v3s16 srcpos = g_pos.pos; + DEBUG_OUT("Pathfinder: remaining dirs at beginning:" + << directions.size() << std::endl); + + for (std::vector::iterator iter = directions.begin(); + iter != directions.end(); + iter ++) { + + v3s16 pos1 = v3s16(srcpos.X + iter->X,0,srcpos.Z+iter->Z); + + int cur_manhattan = get_manhattandistance(pos1); + path_cost cost = g_pos.get_cost(*iter); + + if (!cost.updated) { + cost = calc_cost(g_pos.pos,*iter); + g_pos.set_cost(*iter,cost); + } + + if (cost.valid) { + int score = cost.value + cur_manhattan; + + if ((minscore < 0)|| (score < minscore)) { + minscore = score; + retdir = *iter; + } + } + } + + if (retdir != v3s16(0,0,0)) { + for (std::vector::iterator iter = directions.begin(); + iter != directions.end(); + iter ++) { + if(*iter == retdir) { + DEBUG_OUT("Pathfinder: removing return direction" << std::endl); + directions.erase(iter); + break; + } + } + } + else { + DEBUG_OUT("Pathfinder: didn't find any valid direction clearing" + << std::endl); + directions.clear(); + } + DEBUG_OUT("Pathfinder: remaining dirs at end:" << directions.size() + << std::endl); + return retdir; +} + +/******************************************************************************/ +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 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); + continue; + } + + path_gridnode& g_pos2 = getIndexElement(ipos2); + + if (!g_pos2.valid) { + VERBOSE_TARGET << LVL "Pathfinder: no data for new position: " + << PPOS(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)) { + 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)) { + int old_cost = g_pos2.totalcost; + DEBUG_OUT(LVL "Pathfinder: updating path at: "<< + PPOS(ipos2) << " from: " << old_cost << " 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; +} + +/******************************************************************************/ +void pathfinder::build_path(std::vector& path,v3s16 pos, int level) { + level ++; + if (level > 1000) { + ERROR_TARGET + << LVL "Pathfinder: path is too long aborting" << std::endl; + return; + } + + path_gridnode& 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; + } + + build_path(path,pos + g_pos.sourcedir,level); + path.push_back(pos); +} + +/******************************************************************************/ +v3f pathfinder::tov3f(v3s16 pos) { + return v3f(BS*pos.X,BS*pos.Y,BS*pos.Z); +} + +#ifdef PATHFINDER_DEBUG + +/******************************************************************************/ +void pathfinder::print_cost() { + print_cost(DIR_XP); + print_cost(DIR_XM); + print_cost(DIR_ZP); + print_cost(DIR_ZM); +} + +/******************************************************************************/ +void pathfinder::print_ydir() { + print_ydir(DIR_XP); + print_ydir(DIR_XM); + print_ydir(DIR_ZP); + print_ydir(DIR_ZM); +} + +/******************************************************************************/ +void pathfinder::print_cost(path_directions dir) { + + std::cout << "Cost in direction: " << dir_to_name(dir) << std::endl; + std::cout << std::setfill('-') << std::setw(80) << "-" << std::endl; + std::cout << std::setfill(' '); + for (int y = 0; y < m_max_index_y; y++) { + + std::cout << "Level: " << y << std::endl; + + std::cout << std::setw(4) << " " << " "; + for (int x = 0; x < m_max_index_x; x++) { + std::cout << std::setw(4) << x; + } + std::cout << std::endl; + + for (int z = 0; z < m_max_index_z; z++) { + std::cout << std::setw(4) << z <<": "; + for (int x = 0; x < m_max_index_x; x++) { + if (m_data[x][z][y].directions[dir].valid) + std::cout << std::setw(4) + << m_data[x][z][y].directions[dir].value; + else + std::cout << std::setw(4) << "-"; + } + std::cout << std::endl; + } + std::cout << std::endl; + } +} + +/******************************************************************************/ +void pathfinder::print_ydir(path_directions dir) { + + std::cout << "Height difference in direction: " << dir_to_name(dir) << std::endl; + std::cout << std::setfill('-') << std::setw(80) << "-" << std::endl; + std::cout << std::setfill(' '); + for (int y = 0; y < m_max_index_y; y++) { + + std::cout << "Level: " << y << std::endl; + + std::cout << std::setw(4) << " " << " "; + for (int x = 0; x < m_max_index_x; x++) { + std::cout << std::setw(4) << x; + } + std::cout << std::endl; + + for (int z = 0; z < m_max_index_z; z++) { + std::cout << std::setw(4) << z <<": "; + for (int x = 0; x < m_max_index_x; x++) { + if (m_data[x][z][y].directions[dir].valid) + std::cout << std::setw(4) + << m_data[x][z][y].directions[dir].direction; + else + std::cout << std::setw(4) << "-"; + } + std::cout << std::endl; + } + std::cout << std::endl; + } +} + +/******************************************************************************/ +void pathfinder::print_type() { + std::cout << "Type of node:" << std::endl; + std::cout << std::setfill('-') << std::setw(80) << "-" << std::endl; + std::cout << std::setfill(' '); + for (int y = 0; y < m_max_index_y; y++) { + + std::cout << "Level: " << y << std::endl; + + std::cout << std::setw(3) << " " << " "; + for (int x = 0; x < m_max_index_x; x++) { + std::cout << std::setw(3) << x; + } + std::cout << std::endl; + + for (int z = 0; z < m_max_index_z; z++) { + std::cout << std::setw(3) << z <<": "; + for (int x = 0; x < m_max_index_x; x++) { + char toshow = m_data[x][z][y].type; + std::cout << std::setw(3) << toshow; + } + std::cout << std::endl; + } + std::cout << std::endl; + } + std::cout << std::endl; +} + +/******************************************************************************/ +void pathfinder::print_pathlen() { + std::cout << "Pathlen:" << std::endl; + std::cout << std::setfill('-') << std::setw(80) << "-" << std::endl; + std::cout << std::setfill(' '); + for (int y = 0; y < m_max_index_y; y++) { + + std::cout << "Level: " << y << std::endl; + + std::cout << std::setw(3) << " " << " "; + for (int x = 0; x < m_max_index_x; x++) { + std::cout << std::setw(3) << x; + } + std::cout << std::endl; + + for (int z = 0; z < m_max_index_z; z++) { + std::cout << std::setw(3) << z <<": "; + for (int x = 0; x < m_max_index_x; x++) { + std::cout << std::setw(3) << m_data[x][z][y].totalcost; + } + std::cout << std::endl; + } + std::cout << std::endl; + } + std::cout << std::endl; +} + +/******************************************************************************/ +std::string pathfinder::dir_to_name(path_directions dir) { + switch (dir) { + case DIR_XP: + return "XP"; + break; + case DIR_XM: + return "XM"; + break; + case DIR_ZP: + return "ZP"; + break; + case DIR_ZM: + return "ZM"; + break; + default: + return "UKN"; + } +} + +/******************************************************************************/ +void pathfinder::print_path(std::vector path) { + + unsigned int current = 0; + for (std::vector::iterator i = path.begin(); + i != path.end(); i++) { + std::cout << std::setw(3) << current << ":" << PPOS((*i)) << std::endl; + current++; + } +} + +#endif diff --git a/src/pathfinder.h b/src/pathfinder.h new file mode 100644 index 00000000..7caf5844 --- /dev/null +++ b/src/pathfinder.h @@ -0,0 +1,345 @@ +/* +Minetest +Copyright (C) 2013 sapier, sapier at gmx dot net + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU Lesser General Public License as published by +the Free Software Foundation; either version 2.1 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU Lesser General Public License for more details. + +You should have received a copy of the GNU Lesser General Public License along +with this program; if not, write to the Free Software Foundation, Inc., +51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. +*/ + +#ifndef PATHFINDER_H_ +#define PATHFINDER_H_ + +/******************************************************************************/ +/* Includes */ +/******************************************************************************/ +#include + +#include "server.h" +#include "irr_v3d.h" + + +/******************************************************************************/ +/* Typedefs and macros */ +/******************************************************************************/ + +//#define PATHFINDER_DEBUG + +typedef enum { + DIR_XP, + DIR_XM, + DIR_ZP, + DIR_ZM +} path_directions; + +/** List of supported algorithms */ +typedef enum { + DIJKSTRA, /**< Dijkstra shortest path algorithm */ + A_PLAIN, /**< A* algorithm using heuristics to find a path */ + A_PLAIN_NP /**< A* algorithm without prefetching of map data */ +} algorithm; + +/******************************************************************************/ +/* declarations */ +/******************************************************************************/ + +/** c wrapper function to use from scriptapi */ +std::vector get_Path(ServerEnvironment* env, + v3s16 source, + v3s16 destination, + unsigned int searchdistance, + unsigned int max_jump, + unsigned int max_drop, + algorithm algo); + +/** representation of cost in specific direction */ +class path_cost { +public: + + /** default constructor */ + path_cost(); + + /** copy constructor */ + path_cost(const path_cost& b); + + /** assignment operator */ + path_cost& operator= (const path_cost& b); + + bool valid; /**< movement is possible */ + int value; /**< cost of movement */ + int direction; /**< y-direction of movement */ + bool updated; /**< this cost has ben calculated */ + +}; + + +/** representation of a mapnode to be used for pathfinding */ +class path_gridnode { + +public: + /** default constructor */ + path_gridnode(); + + /** copy constructor */ + path_gridnode(const path_gridnode& b); + + /** + * assignment operator + * @param b node to copy + */ + path_gridnode& operator= (const path_gridnode& b); + + /** + * read cost in a specific direction + * @param dir direction of cost to fetch + */ + path_cost get_cost(v3s16 dir); + + /** + * set cost value for movement + * @param dir direction to set cost for + * @cost cost to set + */ + void set_cost(v3s16 dir,path_cost cost); + + bool valid; /**< node is on surface */ + bool target; /**< node is target position */ + bool source; /**< node is stating position */ + int totalcost; /**< cost to move here from starting point */ + v3s16 sourcedir; /**< origin of movement for current cost */ + int surfaces; /**< number of surfaces with same x,z value*/ + v3s16 pos; /**< real position of node */ + path_cost directions[4]; /**< cost in different directions */ + + /* debug values */ + bool is_element; /**< node is element of path detected */ + char type; /**< type of node */ +}; + +/** class doing pathfinding */ +class pathfinder { + +public: + /** + * default constructor + */ + pathfinder(); + + /** + * path evaluation function + * @param env environment to look for path + * @param source origin of path + * @param destination end position of path + * @param searchdistance maximum number of nodes to look in each direction + * @param max_jump maximum number of blocks a path may jump up + * @param max_drop maximum number of blocks a path may drop + * @param algo algorithm to use for finding a path + */ + std::vector get_Path(ServerEnvironment* env, + v3s16 source, + v3s16 destination, + unsigned int searchdistance, + unsigned int max_jump, + unsigned int max_drop, + algorithm algo); + +private: + /** data struct for storing internal information */ + struct limits { + struct limit { + int min; + int max; + }; + + limit X; + limit Y; + limit Z; + }; + + /* helper functions */ + + /** + * transform index pos to mappos + * @param ipos a index position + * @return map position + */ + v3s16 getRealPos(v3s16 ipos); + + /** + * transform mappos to index pos + * @param pos a real pos + * @return index position + */ + v3s16 getIndexPos(v3s16 pos); + + /** + * get gridnode at a specific index position + * @param ipos index position + * @return gridnode for index + */ + path_gridnode& getIndexElement(v3s16 ipos); + + /** + * invert a 3d position + * @param pos 3d position + * @return pos *-1 + */ + v3s16 invert(v3s16 pos); + + /** + * check if a index is within current search area + * @param index position to validate + * @return true/false + */ + bool valid_index(v3s16 index); + + /** + * translate position to float position + * @param pos integer position + * @return float position + */ + v3f tov3f(v3s16 pos); + + + /* algorithm functions */ + + /** + * calculate 2d manahttan distance to target + * @param pos position to calc distance + * @return integer distance + */ + int get_manhattandistance(v3s16 pos); + + /** + * get best direction based uppon heuristics + * @param directions list of unchecked directions + * @param g_pos mapnode to start from + * @return direction to check + */ + v3s16 get_dir_heuristic(std::vector& directions,path_gridnode& g_pos); + + /** + * build internal data representation of search area + * @return true/false if costmap creation was successfull + */ + bool build_costmap(); + + /** + * calculate cost of movement + * @param pos real world position to start movement + * @param dir direction to move to + * @return cost information + */ + path_cost calc_cost(v3s16 pos,v3s16 dir); + + /** + * recursive update whole search areas total cost information + * @param ipos position to check next + * @param srcdir positionc checked last time + * @param total_cost cost of moving to ipos + * @param level current recursion depth + * @return true/false path to destination has been found + */ + bool update_all_costs(v3s16 ipos,v3s16 srcdir,int total_cost,int level); + + /** + * recursive try to find a patrh to destionation + * @param ipos position to check next + * @param srcdir positionc checked last time + * @param total_cost cost of moving to ipos + * @param level current recursion depth + * @return true/false path to destination has been found + */ + bool update_cost_heuristic(v3s16 ipos,v3s16 srcdir,int current_cost,int level); + + /** + * recursive build a vector containing all nodes from source to destination + * @param path vector to add nodes to + * @param pos pos to check next + * @param level recursion depth + */ + void build_path(std::vector& path,v3s16 pos, int level); + + /* variables */ + int m_max_index_x; /**< max index of search area in x direction */ + int m_max_index_y; /**< max index of search area in y direction */ + int m_max_index_z; /**< max index of search area in z direction */ + + + int m_searchdistance; /**< max distance to search in each direction */ + int m_maxdrop; /**< maximum number of blocks a path may drop */ + int m_maxjump; /**< maximum number of blocks a path may jump */ + int m_min_target_distance; /**< current smalest path to target */ + + bool m_prefetch; /**< prefetch cost data */ + + v3s16 m_start; /**< source position */ + v3s16 m_destination; /**< destination position */ + + limits m_limits; /**< position limits in real map coordinates */ + + /** 3d grid containing all map data already collected and analyzed */ + std::vector > > m_data; + + ServerEnvironment* m_env; /**< minetest environment pointer */ + +#ifdef PATHFINDER_DEBUG + + /** + * print collected cost information + */ + void print_cost(); + + /** + * print collected cost information in a specific direction + * @param dir direction to print + */ + void print_cost(path_directions dir); + + /** + * print type of node as evaluated + */ + void print_type(); + + /** + * print pathlenght for all nodes in search area + */ + void print_pathlen(); + + /** + * print a path + * @param path path to show + */ + void print_path(std::vector path); + + /** + * print y direction for all movements + */ + void print_ydir(); + + /** + * print y direction for moving in a specific direction + * @param dir direction to show data + */ + void print_ydir(path_directions dir); + + /** + * helper function to translate a direction to speaking text + * @param dir direction to translate + * @return textual name of direction + */ + std::string dir_to_name(path_directions dir); +#endif +}; + +#endif /* PATHFINDER_H_ */ diff --git a/src/scriptapi_env.cpp b/src/scriptapi_env.cpp index 4e068e37..9bf7f0b5 100644 --- a/src/scriptapi_env.cpp +++ b/src/scriptapi_env.cpp @@ -26,6 +26,7 @@ with this program; if not, write to the Free Software Foundation, Inc., #include "content_sao.h" #include "script.h" #include "treegen.h" +#include "pathfinder.h" #include "util/pointedthing.h" #include "scriptapi_types.h" #include "scriptapi_noise.h" @@ -647,6 +648,69 @@ int EnvRef::l_clear_objects(lua_State *L) return 0; } +int EnvRef::l_line_of_sight(lua_State *L) { + float stepsize = 1.0; + + //infostream<<"EnvRef::l_get_node()"<m_env; + if(env == NULL) return 0; + + // read position 1 from lua + v3f pos1 = checkFloatPos(L, 2); + // read position 2 from lua + v3f pos2 = checkFloatPos(L, 2); + //read step size from lua + if(lua_isnumber(L, 3)) + stepsize = lua_tonumber(L, 3); + + return (env->line_of_sight(pos1,pos2,stepsize)); +} + +int EnvRef::l_find_path(lua_State *L) +{ + EnvRef *o = checkobject(L, 1); + ServerEnvironment *env = o->m_env; + + if(env == NULL) return 0; + + v3s16 pos1 = read_v3s16(L, 2); + v3s16 pos2 = read_v3s16(L, 3); + unsigned int searchdistance = luaL_checkint(L, 4); + unsigned int max_jump = luaL_checkint(L, 5); + unsigned int max_drop = luaL_checkint(L, 6); + algorithm algo = A_PLAIN_NP; + if(! lua_isnil(L, 7)) { + std::string algorithm = luaL_checkstring(L,7); + + if (algorithm == "A*") + algo = A_PLAIN; + + if (algorithm == "Dijkstra") + algo = DIJKSTRA; + } + + std::vector path = + get_Path(env,pos1,pos2,searchdistance,max_jump,max_drop,algo); + + if (path.size() > 0) + { + lua_newtable(L); + int top = lua_gettop(L); + unsigned int index = 1; + for (std::vector::iterator i = path.begin(); i != path.end();i++) + { + lua_pushnumber(L,index); + push_v3s16(L, *i); + lua_settable(L, top); + index++; + } + return 1; + } + + return 0; +} + int EnvRef::l_spawn_tree(lua_State *L) { EnvRef *o = checkobject(L, 1); @@ -780,6 +844,8 @@ const luaL_reg EnvRef::methods[] = { luamethod(EnvRef, get_perlin_map), luamethod(EnvRef, clear_objects), luamethod(EnvRef, spawn_tree), + luamethod(EnvRef, line_of_sight), + luamethod(EnvRef, find_path), {0,0} }; diff --git a/src/scriptapi_env.h b/src/scriptapi_env.h index 1599969a..2b7ea957 100644 --- a/src/scriptapi_env.h +++ b/src/scriptapi_env.h @@ -137,6 +137,12 @@ private: static int l_spawn_tree(lua_State *L); + + static int l_line_of_sight(lua_State *L); + + //find a path between two positions + static int l_find_path(lua_State *L); + public: EnvRef(ServerEnvironment *env); -- 2.30.2