Restore weight scale for `AStarGrid2D` (partially)

3 files changed, 41 insertions, 1 deletions

diff --git a/core/math/a_star_grid_2d.cpp b/core/math/a_star_grid_2d.cpp
index c30acf32bb..8befda28e4 100644
--- a/core/math/a_star_grid_2d.cpp
+++ b/core/math/a_star_grid_2d.cpp
@@ -155,6 +155,19 @@ bool AStarGrid2D::is_point_solid(const Vector2i &p_id) const {
 	return points[p_id.y][p_id.x].solid;
 }
 
+void AStarGrid2D::set_point_weight_scale(const Vector2i &p_id, real_t p_weight_scale) {
+	ERR_FAIL_COND_MSG(dirty, "Grid is not initialized. Call the update method.");
+	ERR_FAIL_COND_MSG(!is_in_boundsv(p_id), vformat("Can't set point's weight scale. Point out of bounds (%s/%s, %s/%s).", p_id.x, size.width, p_id.y, size.height));
+	ERR_FAIL_COND_MSG(p_weight_scale < 0.0, vformat("Can't set point's weight scale less than 0.0: %f.", p_weight_scale));
+	points[p_id.y][p_id.x].weight_scale = p_weight_scale;
+}
+
+real_t AStarGrid2D::get_point_weight_scale(const Vector2i &p_id) const {
+	ERR_FAIL_COND_V_MSG(dirty, 0, "Grid is not initialized. Call the update method.");
+	ERR_FAIL_COND_V_MSG(!is_in_boundsv(p_id), 0, vformat("Can't get point's weight scale. Point out of bounds (%s/%s, %s/%s).", p_id.x, size.width, p_id.y, size.height));
+	return points[p_id.y][p_id.x].weight_scale;
+}
+
 AStarGrid2D::Point *AStarGrid2D::_jump(Point *p_from, Point *p_to) {
 	if (!p_to || p_to->solid) {
 		return nullptr;
@@ -388,7 +401,10 @@ bool AStarGrid2D::_solve(Point *p_begin_point, Point *p_end_point) {
 		_get_nbors(p, nbors);
 		for (List<Point *>::Element *E = nbors.front(); E; E = E->next()) {
 			Point *e = E->get(); // The neighbour point.
+			real_t weight_scale = 1.0;
+
 			if (jumping_enabled) {
+				// TODO: Make it works with weight_scale.
 				e = _jump(p, e);
 				if (!e || e->closed_pass == pass) {
 					continue;
@@ -397,9 +413,10 @@ bool AStarGrid2D::_solve(Point *p_begin_point, Point *p_end_point) {
 				if (e->solid || e->closed_pass == pass) {
 					continue;
 				}
+				weight_scale = e->weight_scale;
 			}
 
-			real_t tentative_g_score = p->g_score + _compute_cost(p->id, e->id);
+			real_t tentative_g_score = p->g_score + _compute_cost(p->id, e->id) * weight_scale;
 			bool new_point = false;
 
 			if (e->open_pass != pass) { // The point wasn't inside the open list.
@@ -559,6 +576,8 @@ void AStarGrid2D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("get_default_heuristic"), &AStarGrid2D::get_default_heuristic);
 	ClassDB::bind_method(D_METHOD("set_point_solid", "id", "solid"), &AStarGrid2D::set_point_solid, DEFVAL(true));
 	ClassDB::bind_method(D_METHOD("is_point_solid", "id"), &AStarGrid2D::is_point_solid);
+	ClassDB::bind_method(D_METHOD("set_point_weight_scale", "id", "weight_scale"), &AStarGrid2D::set_point_weight_scale);
+	ClassDB::bind_method(D_METHOD("get_point_weight_scale", "id"), &AStarGrid2D::get_point_weight_scale);
 	ClassDB::bind_method(D_METHOD("clear"), &AStarGrid2D::clear);
 
 	ClassDB::bind_method(D_METHOD("get_point_path", "from_id", "to_id"), &AStarGrid2D::get_point_path);
diff --git a/core/math/a_star_grid_2d.h b/core/math/a_star_grid_2d.h
index 1002f18738..2b81f47e12 100644
--- a/core/math/a_star_grid_2d.h
+++ b/core/math/a_star_grid_2d.h
@@ -72,6 +72,7 @@ private:
 
 		bool solid = false;
 		Vector2 pos;
+		real_t weight_scale = 1.0;
 
 		// Used for pathfinding.
 		Point *prev_point = nullptr;
@@ -166,6 +167,9 @@ public:
 	void set_point_solid(const Vector2i &p_id, bool p_solid = true);
 	bool is_point_solid(const Vector2i &p_id) const;
 
+	void set_point_weight_scale(const Vector2i &p_id, real_t p_weight_scale);
+	real_t get_point_weight_scale(const Vector2i &p_id) const;
+
 	void clear();
 
 	Vector<Vector2> get_point_path(const Vector2i &p_from, const Vector2i &p_to);
diff --git a/doc/classes/AStarGrid2D.xml b/doc/classes/AStarGrid2D.xml
index bffa395770..916946775b 100644
--- a/doc/classes/AStarGrid2D.xml
+++ b/doc/classes/AStarGrid2D.xml
@@ -69,6 +69,13 @@
 				[b]Note:[/b] This method is not thread-safe. If called from a [Thread], it will return an empty [PackedVector3Array] and will print an error message.
 			</description>
 		</method>
+		<method name="get_point_weight_scale" qualifiers="const">
+			<return type="float" />
+			<param index="0" name="id" type="Vector2i" />
+			<description>
+				Returns the weight scale of the point associated with the given [param id].
+			</description>
+		</method>
 		<method name="is_dirty" qualifiers="const">
 			<return type="bool" />
 			<description>
@@ -106,6 +113,15 @@
 				[b]Note:[/b] Calling [method update] is not needed after the call of this function.
 			</description>
 		</method>
+		<method name="set_point_weight_scale">
+			<return type="void" />
+			<param index="0" name="id" type="Vector2i" />
+			<param index="1" name="weight_scale" type="float" />
+			<description>
+				Sets the [param weight_scale] for the point with the given [param id]. The [param weight_scale] is multiplied by the result of [method _compute_cost] when determining the overall cost of traveling across a segment from a neighboring point to this point.
+				[b]Note:[/b] Calling [method update] is not needed after the call of this function.
+			</description>
+		</method>
 		<method name="update">
 			<return type="void" />
 			<description>
@@ -125,6 +141,7 @@
 		</member>
 		<member name="jumping_enabled" type="bool" setter="set_jumping_enabled" getter="is_jumping_enabled" default="false">
 			Enables or disables jumping to skip up the intermediate points and speeds up the searching algorithm.
+			[b]Note:[/b] Currently, toggling it on disables the consideration of weight scaling in pathfinding.
 		</member>
 		<member name="offset" type="Vector2" setter="set_offset" getter="get_offset" default="Vector2(0, 0)">
 			The offset of the grid which will be applied to calculate the resulting point position returned by [method get_point_path]. If changed, [method update] needs to be called before finding the next path.