diff --git a/src/MultilevelAStarEx.cpp b/src/MultilevelAStarEx.cpp
index 3d682cd..3649944 100644
--- a/src/MultilevelAStarEx.cpp
+++ b/src/MultilevelAStarEx.cpp
@@ -14,6 +14,9 @@
 
 using namespace godot;
 
+static const int STRAIGHT_DISTANCE = 10;
+static const int DIAGONAL_DISTANCE = 14;
+
 Node::Node() { }
 
 Node::Node(int x, int y)
@@ -31,11 +34,23 @@ void Node::init(Node *parent, int distanceFromStart, const Vector2i &end)
 
 	int dx = abs(x - end.x);
 	int dy = abs(y - end.y);
-	this->distanceToEnd = (dx + dy) * 10;
+
+	int max, min;
+	if (dx > dy)
+	{
+		max = dx;
+		min = dy;
+	}
+	else
+	{
+		max = dy;
+		min = dx;
+	}
+	this->distanceToEnd = min * 14 + (max - min) * 10;
 
 	if ((dx == 0 || dx == 1) && (dy == 0 || dy == 1))
 	{
-		this->distanceToEndForClosest = 10;
+		this->distanceToEndForClosest = STRAIGHT_DISTANCE;
 	}
 	else
 	{
@@ -189,7 +204,7 @@ TypedArray<Vector2i> MultilevelAStarEx::generate_path(const Node *current) const
 	return arr;
 }
 
-Variant MultilevelAStarEx::find_path(const Vector2i &from, const Vector2i &to, const bool return_closest)
+Variant MultilevelAStarEx::find_path(const Vector2i &from, const Vector2i &to, bool return_closest)
 {
 	DEV_ASSERT(_init);
 
@@ -270,56 +285,56 @@ Variant MultilevelAStarEx::find_path(const Vector2i &from, const Vector2i &to, c
 		{
 			if (can_move(current, current->x - 1, current->y))
 			{
-				process(current, current->x - 1, current->y, 10);
+				process(current, current->x - 1, current->y, STRAIGHT_DISTANCE);
 			}
 		}
 		if (current->x + 1 < _width) // right
 		{
 			if (can_move(current, current->x + 1, current->y))
 			{
-				process(current, current->x + 1, current->y, 10);
+				process(current, current->x + 1, current->y, STRAIGHT_DISTANCE);
 			}
 		}
 		if (current->y - 1 >= 0) // up
 		{
 			if (can_move(current, current->x, current->y - 1))
 			{
-				process(current, current->x, current->y - 1, 10);
+				process(current, current->x, current->y - 1, STRAIGHT_DISTANCE);
 			}
 		}
 		if (current->y + 1 < _height) // down
 		{
 			if (can_move(current, current->x, current->y + 1))
 			{
-				process(current, current->x, current->y + 1, 10);
+				process(current, current->x, current->y + 1, STRAIGHT_DISTANCE);
 			}
 		}
 		if ((current->x - 1 >= 0) && (current->y - 1 >= 0)) // top left
 		{
 			if (can_move(current, current->x - 1, current->y - 1))
 			{
-				process(current, current->x - 1, current->y - 1, 14);
+				process(current, current->x - 1, current->y - 1, DIAGONAL_DISTANCE);
 			}
 		}
 		if ((current->x + 1 < _width) && (current->y - 1 >= 0)) // top right
 		{
 			if (can_move(current, current->x + 1, current->y - 1))
 			{
-				process(current, current->x + 1, current->y - 1, 14);
+				process(current, current->x + 1, current->y - 1, DIAGONAL_DISTANCE);
 			}
 		}
 		if ((current->x - 1 >= 0) && (current->y + 1 < _height)) // bottom left
 		{
 			if (can_move(current, current->x - 1, current->y + 1))
 			{
-				process(current, current->x - 1, current->y + 1, 14);
+				process(current, current->x - 1, current->y + 1, DIAGONAL_DISTANCE);
 			}
 		}
 		if ((current->x + 1 < _width) && (current->y + 1 < _height)) // bottom right
 		{
 			if (can_move(current, current->x + 1, current->y + 1))
 			{
-				process(current, current->x + 1, current->y + 1, 14);
+				process(current, current->x + 1, current->y + 1, DIAGONAL_DISTANCE);
 			}
 		}
 	}
diff --git a/src/MultilevelAStarEx.h b/src/MultilevelAStarEx.h
index f74ebb0..f2e9bd9 100644
--- a/src/MultilevelAStarEx.h
+++ b/src/MultilevelAStarEx.h
@@ -75,7 +75,7 @@ public:
 	TerrainType get_terrain(const Vector2i &cell) const;
 	void set_unit(const Vector2i &cell, bool blocked);
 	bool get_unit(const Vector2i &cell) const;
-	Variant find_path(const Vector2i &from, const Vector2i &to, const bool return_closest);
+	Variant find_path(const Vector2i &from, const Vector2i &to, bool return_closest);
 };
 
 }