Initial commit

engines/ultima/nuvie/pathfinder/astar_path.cpp

@@ -0,0 +1,218 @@
+/* ScummVM - Graphic Adventure Engine
+ *
+ * ScummVM is the legal property of its developers, whose names
+ * are too numerous to list here. Please refer to the COPYRIGHT
+ * file distributed with this source distribution.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "ultima/shared/std/containers.h"
+#include "ultima/nuvie/core/nuvie_defs.h"
+#include "ultima/nuvie/pathfinder/dir_finder.h"
+#include "ultima/nuvie/pathfinder/astar_path.h"
+
+namespace Ultima {
+namespace Nuvie {
+
+AStarPath::AStarPath() : final_node(0) {
+}
+
+void AStarPath::create_path() {
+	astar_node *i = final_node; // iterator through steps, from back
+	delete_path();
+	Std::vector<astar_node *> reverse_list;
+	while (i) {
+		reverse_list.push_back(i);
+		i = i->parent;
+	}
+	while (!reverse_list.empty()) {
+		i = reverse_list.back();
+		add_step(i->loc);
+		reverse_list.pop_back();
+	}
+	set_path_size(step_count);
+}/* Get a new neighbor to nnode and score it, returning true if it's usable. */
+bool AStarPath::score_to_neighbor(sint8 dir, astar_node *nnode, astar_node *neighbor,
+								  sint32 &nnode_to_neighbor) {
+	sint8 sx = -1, sy = -1;
+	DirFinder::get_adjacent_dir(sx, sy, dir); // sx,sy = neighbor -1,-1 + dir
+	// get neighbor of nnode towards sx,sy, and cost to that neighbor
+	neighbor->loc = nnode->loc.abs_coords(sx, sy);
+	nnode_to_neighbor = step_cost(nnode->loc, neighbor->loc);
+	if (nnode_to_neighbor == -1) {
+		delete neighbor; // this neighbor is blocked
+		return false;
+	}
+	return true;
+}/* Compare a node's score to the start node to already scored neighbors. */
+bool AStarPath::compare_neighbors(astar_node *nnode, astar_node *neighbor,
+								  sint32 nnode_to_neighbor, astar_node *in_open,
+								  astar_node *in_closed) {
+	neighbor->to_start = nnode->to_start + nnode_to_neighbor;
+	// ignore this neighbor if already checked and closer to start
+	if ((in_open && in_open->to_start <= neighbor->to_start)
+	        || (in_closed && in_closed->to_start <= neighbor->to_start)) {
+		delete neighbor;
+		return false;
+	}
+	return true;
+}/* Check all neighbors of a node (location) and save them to the "seen" list. */
+bool AStarPath::search_node_neighbors(astar_node *nnode, const MapCoord &goal,
+									  const uint32 max_score) {
+	for (uint32 dir = 1; dir < 8; dir += 2) {
+		astar_node *neighbor = new astar_node;
+		sint32 nnode_to_neighbor = -1;
+		if (!score_to_neighbor(dir, nnode, neighbor, nnode_to_neighbor))
+			continue; // this neighbor is blocked
+		astar_node *in_open = find_open_node(neighbor),
+		            *in_closed = find_closed_node(neighbor);
+		if (!compare_neighbors(nnode, neighbor, nnode_to_neighbor, in_open, in_closed))
+			continue;
+		neighbor->parent = nnode;
+		neighbor->to_goal = path_cost_est(neighbor->loc, goal);
+		neighbor->score = neighbor->to_start + neighbor->to_goal;
+		neighbor->len = nnode->len + 1;
+		if (neighbor->score > max_score) {
+			delete neighbor; // too far away
+			continue;
+		}
+		// take neighbor out of closed list and put into open list
+		if (in_closed)
+			remove_closed_node(in_closed);
+		if (!in_open)
+			push_open_node(neighbor);
+	}
+	return true;
+}
+
+/* Do A* search of tiles to create a path from `start' to `goal'.
+ * Don't search past nodes with a score over the max. score.
+ * Create a partial path to low-score nodes with a distance-to-start over the
+ * max_steps count, defined here. Actor may perform another search when needed.
+ * Returns true if a path is created
+ */
+bool AStarPath::path_search(const MapCoord &start, const MapCoord &goal) {
+	//DEBUG(0,LEVEL_DEBUGGING,"SEARCH: %d: %d,%d -> %d,%d\n",actor->get_actor_num(),start.x,start.y,goal.x,goal.y);
+	astar_node *start_node = new astar_node;
+	start_node->loc = start;
+	start_node->to_start = 0;
+	start_node->to_goal = path_cost_est(start, goal);
+	start_node->score = start_node->to_start + start_node->to_goal;
+	start_node->len = 0;
+	push_open_node(start_node);
+	const uint32 max_score = get_max_score(start_node->to_goal);
+	const uint32 max_steps = 8 * 2 * 4; // walk up to four screen lengths before searching again
+	while (!open_nodes.empty()) {
+		astar_node *nnode = pop_open_node(); // next closest
+		if (nnode->loc == goal || nnode->len >= max_steps) {
+			if (nnode->loc != goal)
+				DEBUG(0, LEVEL_DEBUGGING, "out of steps, making partial path (nnode->len=%d)\n", nnode->len);
+//DEBUG(0,LEVEL_DEBUGGING,"GOAL\n");
+			final_node = nnode;
+			create_path();
+			delete_nodes();
+			return true; // reached goal - success
+		}
+		// check cardinal neighbors (starting at top going clockwise)
+		search_node_neighbors(nnode, goal, max_score);
+		// node and neighbors checked, put into closed
+		closed_nodes.push_back(nnode);
+	}
+//DEBUG(0,LEVEL_DEBUGGING,"FAIL\n");
+	delete_nodes();
+	return false; // out of open nodes - failure
+}
+
+/* Return the cost of moving one step from `c1' to `c2', which is always 1. This
+ * isn't very helpful, so subclasses should provide their own function.
+ * Returns -1 if c2 is blocked. */
+sint32 AStarPath::step_cost(const MapCoord &c1, const MapCoord &c2) {
+	if (!pf->check_loc(c2.x, c2.y, c2.z)
+	        || c2.distance(c1) > 1)
+		return -1;
+	return 1;
+}
+
+/* Return an item in the list of closed nodes whose location matches `ncmp'.
+ */
+astar_node *AStarPath::find_closed_node(astar_node *ncmp) {
+	for (astar_node *n : closed_nodes)
+		if (n->loc == ncmp->loc)
+			return n;
+	return nullptr;
+}
+
+/* Return an item in the list of open nodes whose location matches `ncmp'.
+ */
+astar_node *AStarPath::find_open_node(astar_node *ncmp) {
+	for (astar_node *n : open_nodes)
+		if (n->loc == ncmp->loc)
+			return n;
+	return nullptr;
+}
+
+/* Add new node pointer to the list of open nodes (sorting by score).
+ */
+void AStarPath::push_open_node(astar_node *node) {
+	if (open_nodes.empty()) {
+		open_nodes.push_front(node);
+		return;
+	}
+
+	Std::list<astar_node *>::iterator n = open_nodes.begin();
+	// get to end of list or to a node with equal or greater score
+	while (n != open_nodes.end() && (*n++)->score < node->score);
+	open_nodes.insert(n, node); // and add before that location
+}
+
+/* Return pointer to the highest priority node from the list of open nodes, and
+ * remove it.
+ */
+astar_node *AStarPath::pop_open_node() {
+	astar_node *best = open_nodes.front();
+	open_nodes.pop_front(); // remove it
+	return best;
+}
+
+/* Find item in the list of closed nodes whose location matched `ncmp', and
+ * remove it from the list.
+ */
+void AStarPath::remove_closed_node(astar_node *ncmp) {
+	Std::list<astar_node *>::iterator n;
+	for (n = closed_nodes.begin(); n != closed_nodes.end(); n++)
+		if ((*n)->loc == ncmp->loc) {
+			closed_nodes.erase(n);
+			return;
+		}
+}
+
+/* Delete nodes dereferenced from pointers in the lists.
+ */
+void AStarPath::delete_nodes() {
+	while (!open_nodes.empty()) {
+		astar_node *delnode = open_nodes.front();
+		open_nodes.pop_front();
+		delete delnode;
+	}
+	while (!closed_nodes.empty()) {
+		astar_node *delnode = closed_nodes.front();
+		closed_nodes.pop_front();
+		delete delnode;
+	}
+}
+
+} // End of namespace Nuvie
+} // End of namespace Ultima