From b5414024179635f19a16d206d1f016e349dc3d66 Mon Sep 17 00:00:00 2001
From: Fabian Mathews <supagu@gmail.com>
Date: Sat, 25 Mar 2017 20:14:41 +1030
Subject: [PATCH] Added ability to change A-star cost function

---
 core/math/a_star.cpp         | 25 ++++++++++++++++++++++---
 core/math/a_star.h           |  3 +++
 scene/scene_string_names.cpp |  3 +++
 scene/scene_string_names.h   |  3 +++
 4 files changed, 31 insertions(+), 3 deletions(-)

diff --git a/core/math/a_star.cpp b/core/math/a_star.cpp
index 110185c2d23..c82a40f30d7 100644
--- a/core/math/a_star.cpp
+++ b/core/math/a_star.cpp
@@ -28,6 +28,8 @@
 /*************************************************************************/
 #include "a_star.h"
 #include "geometry.h"
+#include "scene/scene_string_names.h"
+#include "script_language.h"
 
 int AStar::get_available_point_id() const {
 
@@ -187,7 +189,7 @@ bool AStar::_solve(Point *begin_point, Point *end_point) {
 
 		Point *n = begin_point->neighbours[i];
 		n->prev_point = begin_point;
-		n->distance = n->pos.distance_to(begin_point->pos);
+		n->distance = _compute_cost(n->id, begin_point->id);
 		n->distance *= n->weight_scale;
 		n->last_pass = pass;
 		open_list.add(&n->list);
@@ -215,7 +217,7 @@ bool AStar::_solve(Point *begin_point, Point *end_point) {
 			Point *p = E->self();
 
 			real_t cost = p->distance;
-			cost += p->pos.distance_to(end_point->pos);
+			cost += _estimate_cost(p->id, end_point->id);
 			cost *= p->weight_scale;
 
 			if (cost < least_cost) {
@@ -233,7 +235,7 @@ bool AStar::_solve(Point *begin_point, Point *end_point) {
 
 			Point *e = p->neighbours[i];
 
-			real_t distance = p->pos.distance_to(e->pos) + p->distance;
+			real_t distance = _compute_cost(p->id, e->id) + p->distance;
 			distance *= e->weight_scale;
 
 			if (e->last_pass == pass) {
@@ -274,6 +276,20 @@ bool AStar::_solve(Point *begin_point, Point *end_point) {
 	return found_route;
 }
 
+float AStar::_estimate_cost(int p_from_id, int p_to_id) {
+	if (get_script_instance() && get_script_instance()->has_method(SceneStringNames::get_singleton()->_estimate_cost))
+		return get_script_instance()->call(SceneStringNames::get_singleton()->_estimate_cost, p_from_id, p_to_id);
+
+	return points[p_from_id]->pos.distance_to(points[p_to_id]->pos);
+}
+
+float AStar::_compute_cost(int p_from_id, int p_to_id) {
+	if (get_script_instance() && get_script_instance()->has_method(SceneStringNames::get_singleton()->_compute_cost))
+		return get_script_instance()->call(SceneStringNames::get_singleton()->_compute_cost, p_from_id, p_to_id);
+
+	return points[p_from_id]->pos.distance_to(points[p_to_id]->pos);
+}
+
 PoolVector<Vector3> AStar::get_point_path(int p_from_id, int p_to_id) {
 
 	ERR_FAIL_COND_V(!points.has(p_from_id), PoolVector<Vector3>());
@@ -395,6 +411,9 @@ void AStar::_bind_methods() {
 
 	ClassDB::bind_method(D_METHOD("get_point_path", "from_id", "to_id"), &AStar::get_point_path);
 	ClassDB::bind_method(D_METHOD("get_id_path", "from_id", "to_id"), &AStar::get_id_path);
+
+	BIND_VMETHOD(MethodInfo("_estimate_cost", PropertyInfo(Variant::INT, "from_id"), PropertyInfo(Variant::INT, "to_id")));
+	BIND_VMETHOD(MethodInfo("_compute_cost", PropertyInfo(Variant::INT, "from_id"), PropertyInfo(Variant::INT, "to_id")));
 }
 
 AStar::AStar() {
diff --git a/core/math/a_star.h b/core/math/a_star.h
index 2ac855737ce..43c9c4457ae 100644
--- a/core/math/a_star.h
+++ b/core/math/a_star.h
@@ -93,6 +93,9 @@ class AStar : public Reference {
 protected:
 	static void _bind_methods();
 
+	virtual float _estimate_cost(int p_from_id, int p_to_id);
+	virtual float _compute_cost(int p_from_id, int p_to_id);
+
 public:
 	int get_available_point_id() const;
 
diff --git a/scene/scene_string_names.cpp b/scene/scene_string_names.cpp
index f0a33e0d3b9..d4a5429c02a 100644
--- a/scene/scene_string_names.cpp
+++ b/scene/scene_string_names.cpp
@@ -32,6 +32,9 @@ SceneStringNames *SceneStringNames::singleton = NULL;
 
 SceneStringNames::SceneStringNames() {
 
+	_estimate_cost = StaticCString::create("_estimate_cost");
+	_compute_cost = StaticCString::create("_compute_cost");
+
 	resized = StaticCString::create("resized");
 	dot = StaticCString::create(".");
 	doubledot = StaticCString::create("..");
diff --git a/scene/scene_string_names.h b/scene/scene_string_names.h
index 8900bbe1d99..3ca006daba3 100644
--- a/scene/scene_string_names.h
+++ b/scene/scene_string_names.h
@@ -49,6 +49,9 @@ class SceneStringNames {
 public:
 	_FORCE_INLINE_ static SceneStringNames *get_singleton() { return singleton; }
 
+	StringName _estimate_cost;
+	StringName _compute_cost;
+
 	StringName resized;
 	StringName dot;
 	StringName doubledot;