Set acceleration only once in falling node

[minetest.git] / src / pathfinder.cpp

diff --git a/src/pathfinder.cpp b/src/pathfinder.cpp
index 43e1e287f6d72a94b3901a4b386c2ec2880d86cd..673d5077eb01f9c53c94ec26ad5a086628133573 100644 (file)
--- a/src/pathfinder.cpp
+++ b/src/pathfinder.cpp
@@ -51,9 +51,9 @@ with this program; if not, write to the Free Software Foundation, Inc.,
 #define ERROR_TARGET     std::cout
 #else
 #define DEBUG_OUT(a)     while(0)
-#define INFO_TARGET      infostream
-#define VERBOSE_TARGET   verbosestream
-#define ERROR_TARGET     errorstream
+#define INFO_TARGET      infostream << "pathfinder: "
+#define VERBOSE_TARGET   verbosestream << "pathfinder: "
+#define ERROR_TARGET     errorstream << "pathfinder: "
 #endif
 
 /******************************************************************************/
@@ -207,7 +207,7 @@ std::vector<v3s16> pathfinder::get_Path(ServerEnvironment* env,
 
        //check parameters
        if (env == 0) {
-               std::cout << "missing environment pointer" << std::endl;
+               ERROR_TARGET << "missing environment pointer" << std::endl;
                return retval;
        }
 
@@ -246,7 +246,7 @@ std::vector<v3s16> pathfinder::get_Path(ServerEnvironment* env,
 
        //build data map
        if (!build_costmap()) {
-               std::cout << "failed to build costmap" << std::endl;
+               ERROR_TARGET << "failed to build costmap" << std::endl;
                return retval;
        }
 #ifdef PATHFINDER_DEBUG
@@ -263,13 +263,13 @@ std::vector<v3s16> pathfinder::get_Path(ServerEnvironment* env,
        path_gridnode& endpos   = getIndexElement(EndIndex);
 
        if (!startpos.valid) {
-               std::cout << "invalid startpos" <<
+               VERBOSE_TARGET << "invalid startpos" <<
                                "Index: " << PPOS(StartIndex) <<
                                "Realpos: " << PPOS(getRealPos(StartIndex)) << std::endl;
                return retval;
        }
        if (!endpos.valid) {
-               std::cout << "invalid stoppos" <<
+               VERBOSE_TARGET << "invalid stoppos" <<
                                "Index: " << PPOS(EndIndex) <<
                                "Realpos: " << PPOS(getRealPos(EndIndex)) << std::endl;
                return retval;
@@ -290,7 +290,7 @@ std::vector<v3s16> pathfinder::get_Path(ServerEnvironment* env,
                        update_cost_retval = update_cost_heuristic(StartIndex,v3s16(0,0,0),0,0);
                        break;
                default:
-                       std::cout << "missing algorithm"<< std::endl;
+                       ERROR_TARGET << "missing algorithm"<< std::endl;
                        break;
        }
 
@@ -310,27 +310,16 @@ std::vector<v3s16> pathfinder::get_Path(ServerEnvironment* env,
                print_path(path);
 #endif
 
-               //optimize path
-               std::vector<v3s16> optimized_path;
-
-               std::vector<v3s16>::iterator startpos = path.begin();
-               optimized_path.push_back(source);
-
+               //finalize path
+               std::vector<v3s16> full_path;
                for (std::vector<v3s16>::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);
-                       }
+                                       i != path.end(); ++i) {
+                       full_path.push_back(getIndexElement(*i).pos);
                }
 
-               optimized_path.push_back(destination);
-
 #ifdef PATHFINDER_DEBUG
-               std::cout << "Optimized path:" << std::endl;
-               print_path(optimized_path);
+               std::cout << "full path:" << std::endl;
+               print_path(full_path);
 #endif
 #ifdef PATHFINDER_CALC_TIME
                timespec ts2;
@@ -344,13 +333,13 @@ std::vector<v3s16> pathfinder::get_Path(ServerEnvironment* env,
                std::cout << "Calculating path took: " << (ts2.tv_sec - ts.tv_sec) <<
                                "s " << ms << "ms " << us << "us " << ns << "ns " << std::endl;
 #endif
-               return optimized_path;
+               return full_path;
        }
        else {
 #ifdef PATHFINDER_DEBUG
                print_pathlen();
 #endif
-               std::cout << "failed to update cost map"<< std::endl;
+               ERROR_TARGET << "failed to update cost map"<< std::endl;
        }
 
 
@@ -532,7 +521,7 @@ path_cost pathfinder::calc_cost(v3s16 pos,v3s16 dir) {
                        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) {
+                               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)
@@ -735,7 +724,7 @@ v3s16 pathfinder::get_dir_heuristic(std::vector<v3s16>& directions,path_gridnode
 
        for (std::vector<v3s16>::iterator iter = directions.begin();
                        iter != directions.end();
-                       iter ++) {
+                       ++iter) {
 
                v3s16 pos1 =  v3s16(srcpos.X + iter->X,0,srcpos.Z+iter->Z);
 
@@ -760,7 +749,7 @@ v3s16 pathfinder::get_dir_heuristic(std::vector<v3s16>& directions,path_gridnode
        if (retdir != v3s16(0,0,0)) {
                for (std::vector<v3s16>::iterator iter = directions.begin();
                                        iter != directions.end();
-                                       iter ++) {
+                                       ++iter) {
                        if(*iter == retdir) {
                                DEBUG_OUT("Pathfinder: removing return direction" << std::endl);
                                directions.erase(iter);
@@ -823,6 +812,7 @@ bool pathfinder::update_cost_heuristic(     v3s16 ipos,
                                                        " 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;
                                }
 
@@ -831,6 +821,7 @@ bool pathfinder::update_cost_heuristic(     v3s16 ipos,
                                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;
                                }
 
@@ -1073,7 +1064,7 @@ void pathfinder::print_path(std::vector<v3s16> path) {
 
        unsigned int current = 0;
        for (std::vector<v3s16>::iterator i = path.begin();
-                       i != path.end(); i++) {
+                       i != path.end(); ++i) {
                std::cout << std::setw(3) << current << ":" << PPOS((*i)) << std::endl;
                current++;
        }