diff --git a/src/pathfinder.cpp b/src/pathfinder.cpp
index d39bdab3a..8eb52bfd1 100644
--- a/src/pathfinder.cpp
+++ b/src/pathfinder.cpp
@@ -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);
-			}
+			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,7 +333,7 @@ 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
@@ -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)