Implemented path-finding logic for accurate player movement

svn-id: r50936

4 files changed, 132 insertions, 10 deletions

diff --git a/engines/m4/mads_player.cpp b/engines/m4/mads_player.cpp
index ff5f3464bf..5106d2210d 100644
--- a/engines/m4/mads_player.cpp
+++ b/engines/m4/mads_player.cpp
@@ -747,9 +747,44 @@ void MadsPlayer::turnToDestFacing() {
 }
 
 void MadsPlayer::setupRoute(bool bitFlag) {
-	// TODO: Properly Implement route setup
-	_routeIndexes[0] = _madsVm->scene()->getSceneResources()._nodes.size() - 1;
-	_routeCount = 1;
+	// Reset the flag set of nodes in use
+	SceneNodeList &nodes = _madsVm->scene()->getSceneResources()._nodes;
+	for (uint i = 0; i < nodes.size(); ++i)
+		nodes[i].active = false;
+
+	// Start constructing route node list
+	_routeLength = 0x3FFF;
+	_routeCount = 0;
+
+	setupRouteNode(_tempRoute, nodes.size() - 1, bitFlag ? 0xC000 : 0x8000, 0);
+}
+
+void MadsPlayer::setupRouteNode(int *routeIndexP, int nodeIndex, int flags, int routeLength) {
+	SceneNodeList &nodes = _madsVm->scene()->getSceneResources()._nodes;
+	SceneNode &currentNode = nodes[nodeIndex];
+	currentNode.active = true;
+
+	*routeIndexP++ = nodeIndex;
+
+	int subIndex = nodes.size() - 2;
+	int indexVal = nodes[nodeIndex].indexes[subIndex];
+	if (indexVal & flags) {
+		routeLength += indexVal & 0x3FFF;
+		if (routeLength < _routeLength) {
+			// Found a new shorter route to destination, so set up the route with the found one
+			Common::copy(_tempRoute, routeIndexP, _routeIndexes);
+			_routeCount = routeIndexP - _tempRoute;
+			_routeLength = indexVal & 0x3FFF;
+		}
+	} else {
+		for (int idx = nodes.size() - 2; idx > 0; --idx) {
+			int nodePos = idx - 1;
+			if (!nodes[nodePos].active && ((currentNode.indexes[nodePos] & flags) != 0))
+				setupRouteNode(routeIndexP, nodePos, 0x8000, indexVal & 0x3fff);
+		}
+	}
+
+	currentNode.active = false;
 }
 
 } // End of namespace M4
diff --git a/engines/m4/mads_player.h b/engines/m4/mads_player.h
index 2fbba70664..6a9b7b4ca1 100644
--- a/engines/m4/mads_player.h
+++ b/engines/m4/mads_player.h
@@ -27,11 +27,11 @@
 #define M4_MADS_PLAYER_H
 
 #include "common/scummsys.h"
+#include "m4/mads_scene.h"
 
 namespace M4 {
 
 #define PLAYER_SEQ_INDEX -2
-#define MAX_ROUTE_NODES 22
 
 class MadsPlayer {
 private:
@@ -46,6 +46,7 @@ private:
 	void reset();
 	int scanPath(M4Surface *depthSurface, const Common::Point &srcPos, const Common::Point &destPos);
 	void startMovement();
+	void setupRouteNode(int *routeIndexP, int nodeIndex, int flags, int routeLength);
 public:
 	char _spritesPrefix[16];
 	int _spriteSetCount;
@@ -92,6 +93,7 @@ public:
 	int _v8452E;
 	int _v8452C;
 	int _v84530;
+	int _routeLength;
 
 	static const int _directionListIndexes[32];
 public:
diff --git a/engines/m4/mads_scene.cpp b/engines/m4/mads_scene.cpp
index 1972e2d989..8ffe6b8c95 100644
--- a/engines/m4/mads_scene.cpp
+++ b/engines/m4/mads_scene.cpp
@@ -50,11 +50,11 @@ static const int SCROLLER_DELAY = 200;
 
 void SceneNode::load(Common::SeekableReadStream *stream) {
 	// Get the next data block
-	uint8 obj[0x30];
-	stream->read(obj, 0x30);
+	pt.x = stream->readUint16LE();
+	pt.y = stream->readUint16LE();
 
-	pt.x = READ_LE_UINT16(&obj[0]);
-	pt.y = READ_LE_UINT16(&obj[2]);
+	for (int i = 0; i < MAX_ROUTE_NODES; ++i)
+		indexes[i] = stream->readUint16LE();
 }
 
 //--------------------------------------------------------------------------
@@ -860,9 +860,82 @@ void MadsSceneResources::load(int sceneNumber, const char *resName, int v0, M4Su
 }
 
 void MadsSceneResources::setRouteNode(int nodeIndex, const Common::Point &pt, M4Surface *depthSurface) {
+	int flags, hypotenuse;
+
 	_nodes[nodeIndex].pt = pt;
 
-	// TODO: Implement the rest of the logic of this method
+	// Recalculate inter-node lengths
+	for (uint idx = 0; idx < _nodes.size(); ++idx) {
+		int entry;
+		if (idx == (uint)nodeIndex) {
+			entry = 0x3FFF;
+		} else {
+			// Process the node
+			flags = getRouteFlags(pt, _nodes[idx].pt, depthSurface);
+
+			int xDiff = ABS(_nodes[idx].pt.x - pt.x);
+			int yDiff = ABS(_nodes[idx].pt.y - pt.y);
+			hypotenuse = SqrtF16(xDiff * xDiff + yDiff * yDiff);
+
+			if (hypotenuse >= 0x3FFF)
+				// Shouldn't ever be this large
+				hypotenuse = 0x3FFF;
+			
+			entry = hypotenuse | flags;
+			_nodes[idx].indexes[nodeIndex] = entry;
+			_nodes[nodeIndex].indexes[idx] = entry;
+		}
+	}
+}
+
+int MadsSceneResources::getRouteFlags(const Common::Point &src, const Common::Point &dest, M4Surface *depthSurface) {
+	int result = 0x8000;
+	bool flag = false;
+
+	int xDiff = ABS(dest.x - src.x);
+	int yDiff = ABS(dest.y - src.y);
+	int xDirection = dest.x >= src.x ? 1 : -1;
+	int yDirection = dest.y >= src.y ? depthSurface->width() : -depthSurface->width();
+	int majorDiff = 0;
+	if (dest.x < src.x)
+		majorDiff = MAX(xDiff, yDiff);
+	++xDiff;
+	++yDiff;
+
+	byte *srcP = depthSurface->getBasePtr(src.x, src.y);
+	
+	int totalCtr = majorDiff;
+	for (int xCtr = 0; xCtr < xDiff; ++xCtr, srcP += xDirection) {
+		totalCtr += yDiff;
+
+		if ((*srcP & 0x80) == 0)
+			flag = false;
+		else if (!flag) {
+			flag = true;
+			result -= 0x4000;
+			if (result == 0)
+				break;
+		}
+
+		while (totalCtr >= xDiff) {
+			totalCtr -= xDiff;
+
+			if ((*srcP & 0x80) == 0)
+				flag = false;
+			else if (!flag) {
+				flag = true;
+				result -= 0x4000;
+				if (result == 0)
+					break;
+			}
+
+			srcP += yDirection;
+		}
+		if (result == 0)
+			break;
+	}
+
+	return result;
 }
 
 /*--------------------------------------------------------------------------*/
diff --git a/engines/m4/mads_scene.h b/engines/m4/mads_scene.h
index 66fefcb0c0..7defe613bd 100644
--- a/engines/m4/mads_scene.h
+++ b/engines/m4/mads_scene.h
@@ -36,22 +36,34 @@ namespace M4 {
 class MadsInterfaceView;
 
 #define DEPTH_BANDS_SIZE 15
+#define MAX_ROUTE_NODES 22
 
 class SceneNode {
 public:
 	Common::Point pt;
+	int indexes[MAX_ROUTE_NODES];
+
+	bool active;
+
+	SceneNode() {
+		active = false;
+	}
 
 	void load(Common::SeekableReadStream *stream);
 };
 
+typedef Common::Array<SceneNode> SceneNodeList;
+
 class MadsSceneResources: public SceneResources {
+private:
+	int getRouteFlags(const Common::Point &src, const Common::Point &dest, M4Surface *depthSurface);
 public:
 	int _sceneId;
 	int _artFileNum;
 	int _depthStyle;
 	int _width;
 	int _height;
-	Common::Array<SceneNode> _nodes;
+	SceneNodeList _nodes;
 	Common::Array<Common::String> _setNames;
 	int _yBandsStart, _yBandsEnd;
 	int _maxScale, _minScale;