/* Minetest Copyright (C) 2013 sapier, sapier at gmx dot net Copyright (C) 2016 est31, 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" #include "map.h" #include "nodedef.h" //#define PATHFINDER_DEBUG //#define PATHFINDER_CALC_TIME #ifdef PATHFINDER_DEBUG #include #endif #ifdef PATHFINDER_DEBUG #include #endif #ifdef PATHFINDER_CALC_TIME #include #endif /******************************************************************************/ /* Typedefs and macros */ /******************************************************************************/ #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 << "Pathfinder: " #define VERBOSE_TARGET verbosestream << "Pathfinder: " #define ERROR_TARGET warningstream << "Pathfinder: " #endif #define PATHFINDER_MAX_WAYPOINTS 700 /******************************************************************************/ /* Class definitions */ /******************************************************************************/ /** representation of cost in specific direction */ class PathCost { public: /** default constructor */ PathCost() = default; /** copy constructor */ PathCost(const PathCost &b); /** assignment operator */ PathCost &operator= (const PathCost &b); bool valid = false; /**< movement is possible */ int value = 0; /**< cost of movement */ int y_change = 0; /**< change of y position of movement */ bool updated = false; /**< this cost has ben calculated */ }; /** representation of a mapnode to be used for pathfinding */ class PathGridnode { public: /** default constructor */ PathGridnode() = default; /** copy constructor */ PathGridnode(const PathGridnode &b); /** * assignment operator * @param b node to copy */ PathGridnode &operator= (const PathGridnode &b); /** * read cost in a specific direction * @param dir direction of cost to fetch */ PathCost getCost(v3s16 dir); /** * set cost value for movement * @param dir direction to set cost for * @cost cost to set */ void setCost(v3s16 dir, const PathCost &cost); bool valid = false; /**< node is on surface */ bool target = false; /**< node is target position */ bool source = false; /**< node is stating position */ int totalcost = -1; /**< cost to move here from starting point */ int estimated_cost = -1; /**< totalcost + heuristic cost to end */ v3s16 sourcedir; /**< origin of movement for current cost */ v3s16 pos; /**< real position of node */ PathCost directions[4]; /**< cost in different directions */ bool is_closed = false; /**< for A* search: if true, is in closed list */ bool is_open = false; /**< for A* search: if true, is in open list */ /* debug values */ bool is_element = false; /**< node is element of path detected */ char type = 'u'; /**< Type of pathfinding node. * u = unknown * i = invalid * s = surface (walkable node) * - = non-walkable node (e.g. air) above surface * g = other non-walkable node */ }; class Pathfinder; class PathfinderCompareHeuristic; /** Abstract class to manage the map data */ class GridNodeContainer { public: virtual PathGridnode &access(v3s16 p)=0; virtual ~GridNodeContainer() = default; protected: Pathfinder *m_pathf; void initNode(v3s16 ipos, PathGridnode *p_node); }; class ArrayGridNodeContainer : public GridNodeContainer { public: virtual ~ArrayGridNodeContainer() = default; ArrayGridNodeContainer(Pathfinder *pathf, v3s16 dimensions); virtual PathGridnode &access(v3s16 p); private: int m_x_stride; int m_y_stride; std::vector m_nodes_array; }; class MapGridNodeContainer : public GridNodeContainer { public: virtual ~MapGridNodeContainer() = default; MapGridNodeContainer(Pathfinder *pathf); virtual PathGridnode &access(v3s16 p); private: std::map m_nodes; }; /** class doing pathfinding */ class Pathfinder { public: Pathfinder() = delete; Pathfinder(Map *map, const NodeDefManager *ndef) : m_map(map), m_ndef(ndef) {} ~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 getPath(v3s16 source, v3s16 destination, unsigned int searchdistance, unsigned int max_jump, unsigned int max_drop, PathAlgorithm algo); private: /* 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 */ PathGridnode &getIndexElement(v3s16 ipos); /** * Get gridnode at a specific index position * @return gridnode for index */ PathGridnode &getIdxElem(s16 x, s16 y, s16 z); /** * invert a 3D position (change sign of coordinates) * @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 isValidIndex(v3s16 index); /* algorithm functions */ /** * calculate 2D Manhattan distance to target * @param pos position to calc distance * @return integer distance */ int getXZManhattanDist(v3s16 pos); /** * calculate cost of movement * @param pos real world position to start movement * @param dir direction to move to * @return cost information */ PathCost calcCost(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 updateAllCosts(v3s16 ipos, v3s16 srcdir, int current_cost, int level); /** * try to find a path to destination using a heuristic function * to estimate distance to target (A* search algorithm) * @param isource start position (index pos) * @param idestination end position (index pos) * @return true/false path to destination has been found */ bool updateCostHeuristic(v3s16 isource, v3s16 idestination); /** * build a vector containing all nodes from destination to source; * to be called after the node costs have been processed * @param path vector to add nodes to * @param ipos initial pos to check (index pos) * @return true/false path has been fully built */ bool buildPath(std::vector &path, v3s16 ipos); /** * go downwards from a position until some barrier * is hit. * @param pos position from which to go downwards * @param max_down maximum distance to go downwards * @return new position after movement; if too far down, * pos is returned */ v3s16 walkDownwards(v3s16 pos, unsigned int max_down); /* variables */ int m_max_index_x = 0; /**< max index of search area in x direction */ int m_max_index_y = 0; /**< max index of search area in y direction */ int m_max_index_z = 0; /**< max index of search area in z direction */ int m_maxdrop = 0; /**< maximum number of blocks a path may drop */ int m_maxjump = 0; /**< maximum number of blocks a path may jump */ int m_min_target_distance = 0; /**< current smalest path to target */ bool m_prefetch = true; /**< prefetch cost data */ v3s16 m_start; /**< source position */ v3s16 m_destination; /**< destination position */ core::aabbox3d m_limits; /**< position limits in real map coordinates */ /** contains all map data already collected and analyzed. Access it via the getIndexElement/getIdxElem methods. */ friend class GridNodeContainer; GridNodeContainer *m_nodes_container = nullptr; Map *m_map = nullptr; const NodeDefManager *m_ndef = nullptr; friend class PathfinderCompareHeuristic; #ifdef PATHFINDER_DEBUG /** * print collected cost information */ void printCost(); /** * print collected cost information in a specific direction * @param dir direction to print */ void printCost(PathDirections dir); /** * print type of node as evaluated */ void printType(); /** * print pathlenght for all nodes in search area */ void printPathLen(); /** * print a path * @param path path to show */ void printPath(std::vector path); /** * print y direction for all movements */ void printYdir(); /** * print y direction for moving in a specific direction * @param dir direction to show data */ void printYdir(PathDirections dir); /** * helper functio/* Minetest Copyright (C) 2010-2013 celeron55, Perttu Ahola <celeron55@gmail.com> 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. */ #include "numeric.h" #include "mathconstants.h" #include "log.h" #include "../constants.h" // BS, MAP_BLOCKSIZE #include "../noise.h" // PseudoRandom, PcgRandom #include <string.h> #include <iostream> std::map<u16, std::vector<v3s16> > FacePositionCache::m_cache; // Calculate the borders of a "d-radius" cube std::vector<v3s16> FacePositionCache::getFacePositions(u16 d) { if (m_cache.find(d) != m_cache.end()) return m_cache[d]; generateFacePosition(d); return m_cache[d]; } void FacePositionCache::generateFacePosition(u16 d) { m_cache[d] = std::vector<v3s16>(); if(d == 0) { m_cache[d].push_back(v3s16(0,0,0)); return; } if(d == 1) { /* This is an optimized sequence of coordinates. */ m_cache[d].push_back(v3s16( 0, 1, 0)); // top m_cache[d].push_back(v3s16( 0, 0, 1)); // back m_cache[d].push_back(v3s16(-1, 0, 0)); // left m_cache[d].push_back(v3s16( 1, 0, 0)); // right m_cache[d].push_back(v3s16( 0, 0,-1)); // front m_cache[d].push_back(v3s16( 0,-1, 0)); // bottom // 6 m_cache[d].push_back(v3s16(-1, 0, 1)); // back left m_cache[d].push_back(v3s16( 1, 0, 1)); // back right m_cache[d].push_back(v3s16(-1, 0,-1)); // front left m_cache[d].push_back(v3s16( 1, 0,-1)); // front right m_cache[d].push_back(v3s16(-1,-1, 0)); // bottom left m_cache[d].push_back(v3s16( 1,-1, 0)); // bottom right m_cache[d].push_back(v3s16( 0,-1, 1)); // bottom back m_cache[d].push_back(v3s16( 0,-1,-1)); // bottom front m_cache[d].push_back(v3s16(-1, 1, 0)); // top left m_cache[d].push_back(v3s16( 1, 1, 0)); // top right m_cache[d].push_back(v3s16( 0, 1, 1)); // top back m_cache[d].push_back(v3s16( 0, 1,-1)); // top front // 18 m_cache[d].push_back(v3s16(-1, 1, 1)); // top back-left m_cache[d].push_back(v3s16( 1, 1, 1)); // top back-right m_cache[d].push_back(v3s16(-1, 1,-1)); // top front-left m_cache[d].push_back(v3s16( 1, 1,-1)); // top front-right m_cache[d].push_back(v3s16(-1,-1, 1)); // bottom back-left m_cache[d].push_back(v3s16( 1,-1, 1)); // bottom back-right m_cache[d].push_back(v3s16(-1,-1,-1)); // bottom front-left m_cache[d].push_back(v3s16( 1,-1,-1)); // bottom front-right // 26 return; } // Take blocks in all sides, starting from y=0 and going +-y for(s16 y=0; y<=d-1; y++) { // Left and right side, including borders for(s16 z=-d; z<=d; z++) { m_cache[d].push_back(v3s16(d,y,z)); m_cache[d].push_back(v3s16(-d,y,z)); if(y != 0) { m_cache[d].push_back(v3s16(d,-y,z)); m_cache[d].push_back(v3s16(-d,-y,z)); } } // Back and front side, excluding borders for(s16 x=-d+1; x<=d-1; x++) { m_cache[d].push_back(v3s16(x,y,d)); m_cache[d].push_back(v3s16(x,y,-d)); if(y != 0) { m_cache[d].push_back(v3s16(x,-y,d)); m_cache[d].push_back(v3s16(x,-y,-d)); } } } // Take the bottom and top face with borders // -d<x<d, y=+-d, -d<z<d for(s16 x=-d; x<=d; x++) for(s16 z=-d; z<=d; z++) { m_cache[d].push_back(v3s16(x,-d,z)); m_cache[d].push_back(v3s16(x,d,z)); } } /* myrand */ PcgRandom g_pcgrand; u32 myrand() { return g_pcgrand.next(); } void mysrand(unsigned int seed) { g_pcgrand.seed(seed); } void myrand_bytes(void *out, size_t len) { g_pcgrand.bytes(out, len); } int myrand_range(int min, int max) { return g_pcgrand.range(min, max); } /* 64-bit unaligned version of MurmurHash */ u64 murmur_hash_64_ua(const void *key, int len, unsigned int seed) { const u64 m = 0xc6a4a7935bd1e995ULL; const int r = 47; u64 h = seed ^ (len * m); const u64 *data = (const u64 *)key; const u64 *end = data + (len / 8); while (data != end) { u64 k; memcpy(&k, data, sizeof(u64)); data++; k *= m; k ^= k >> r; k *= m; h ^= k; h *= m; } const unsigned char *data2 = (const unsigned char *)data; switch (len & 7) { case 7: h ^= (u64)data2[6] << 48; case 6: h ^= (u64)data2[5] << 40; case 5: h ^= (u64)data2[4] << 32; case 4: h ^= (u64)data2[3] << 24; case 3: h ^= (u64)data2[2] << 16; case 2: h ^= (u64)data2[1] << 8; case 1: h ^= (u64)data2[0]; h *= m; } h ^= h >> r; h *= m; h ^= h >> r; return h; } /* blockpos: position of block in block coordinates camera_pos: position of camera in nodes camera_dir: an unit vector pointing to camera direction range: viewing range */ bool isBlockInSight(v3s16 blockpos_b, v3f camera_pos, v3f camera_dir, f32 camera_fov, f32 range, f32 *distance_ptr) { v3s16 blockpos_nodes = blockpos_b * MAP_BLOCKSIZE;