BLADERUNNER: Cleanup of pathfinding code

2 files changed, 65 insertions, 67 deletions

diff --git a/engines/bladerunner/obstacles.cpp b/engines/bladerunner/obstacles.cpp
index f933187796..933a01ebe3 100644
--- a/engines/bladerunner/obstacles.cpp
+++ b/engines/bladerunner/obstacles.cpp
@@ -345,10 +345,10 @@ bool Obstacles::findNextWaypoint(const Vector3 &from, const Vector3 &to, Vector3
 	}
 
 	int     polyIndex = -1;
-	int     polyNearVertIndex;
+	int     polyNearVertIndex = -1;
 	float   polyNearDist = 0.0f;
 	Vector2 polyNearPos;
-	int     polyFarVertIndex;
+	int     polyFarVertIndex = -1;
 	float   polyFarDist = 0.0f;
 	Vector2 polyFarPos;
 
@@ -675,120 +675,118 @@ bool Obstacles::verticesCanIntersect(int lineType0, int lineType1, float x0, flo
 	return false;
 }
 
-bool Obstacles::findFarthestAvailablePathVertex(Vector2 *path, int pathSize, Vector3 from, Vector3 *next) const {
-	int polygonIndex1 = -1;
-	int vertexIndex1 = -1;
-	int polygonIndex2 = -1;
-	int vertexIndex2 = -1;
-	int polygonIndex3 = -1;
-	int vertexIndex3 = -1;
-
-	int vertexType1 = -1;
-	int vertexType1Prev = -1;
-
+bool Obstacles::findFarthestAvailablePathVertex(Vector2 *path, int pathSize, Vector3 start, Vector3 *next) const {
 	if (pathSize == 0) {
-		*next = from;
+		*next = start;
 		return false;
 	}
 
-	signed int farthestPathIndex = -1;
-
-	vertexType1 = -1;
-	bool fromOnPolygon = findPolygonVerticeByXZWithinTolerance(from.x, from.z, &polygonIndex1, &vertexIndex1);
-	if (fromOnPolygon) {
-		int vertexIndex1Prev = (vertexIndex1 - 1 + _polygons[polygonIndex1].verticeCount) % _polygons[polygonIndex1].verticeCount;
+	int vertexTypeStart = -1;
+	int vertexTypeStartPrev = -1;
+	int polygonIndexStart = -1;
+	int vertexIndexStart = -1;
+	bool startOnPolygon = findPolygonVerticeByXZWithinTolerance(start.x, start.z, &polygonIndexStart, &vertexIndexStart);
+	if (startOnPolygon) {
+		int vertexIndexStartPrev = (vertexIndexStart - 1 + _polygons[polygonIndexStart].verticeCount) % _polygons[polygonIndexStart].verticeCount;
 
-		vertexType1	 = _polygons[polygonIndex1].vertexType[vertexIndex1];
-		vertexType1Prev = _polygons[polygonIndex1].vertexType[vertexIndex1Prev];
+		vertexTypeStart     = _polygons[polygonIndexStart].vertexType[vertexIndexStart];
+		vertexTypeStartPrev = _polygons[polygonIndexStart].vertexType[vertexIndexStartPrev];
 	}
 
+	signed int farthestPathIndex = -1;
 	for (int pathVertexIdx = 0; pathVertexIdx < pathSize; ++pathVertexIdx) {
-		// bool foundVertexNeighbor = false;
-		// bool pathVertexOnPolygon =
-		findPolygonVerticeByXZWithinTolerance(path[pathVertexIdx].x, path[pathVertexIdx].y, &polygonIndex2, &vertexIndex2);
+		bool foundVertexNeighbor = false;
+		int polygonIndexPath = -1;
+		int vertexIndexPath = -1;
+		bool pathVertexOnPolygon = findPolygonVerticeByXZWithinTolerance(path[pathVertexIdx].x, path[pathVertexIdx].y, &polygonIndexPath, &vertexIndexPath) == 1;
 
 		//start and current path vertices are on same polygon and are next to each other
-		// if (pathVertexOnPolygon && polygonIndex1 == polygonIndex2) {
-		// 	int vertexIndex1Prev = (vertexIndex1 - 1 + _polygons[polygonIndex2].verticeCount) % _polygons[polygonIndex2].verticeCount;
-		// 	int vertexIndex1Next = (vertexIndex1 + 1										) % _polygons[polygonIndex2].verticeCount;
+		if (pathVertexOnPolygon && polygonIndexStart == polygonIndexPath) {
+			int vertexIndexStartPrev = (vertexIndexStart - 1 + _polygons[polygonIndexPath].verticeCount) % _polygons[polygonIndexPath].verticeCount;
+			int vertexIndexStartNext = (vertexIndexStart + 1                                           ) % _polygons[polygonIndexPath].verticeCount;
 
-		// 	if (vertexIndex2 == vertexIndex1Next || vertexIndex2 == vertexIndex1Prev || vertexIndex2 == vertexIndex1) {
-		// 		foundVertexNeighbor = true;
-		// 	}
-		// }
+			if (vertexIndexPath == vertexIndexStartNext || vertexIndexPath == vertexIndexStartPrev || vertexIndexPath == vertexIndexStart) {
+				foundVertexNeighbor = true;
+			}
+		}
 
-		// if (!foundVertexNeighbor){
-		// 	 break;
-		// }
+		// neighboring vertices are always available
+		if (foundVertexNeighbor){
+			farthestPathIndex = pathVertexIdx;
+			continue;
+		}
 
-		bool keepSearching = true;
-		for (int currentPolygonIdx = 0; currentPolygonIdx < kPolygonCount && keepSearching; ++currentPolygonIdx) {
+		bool pathVertexAvailable = true;
+		for (int currentPolygonIdx = 0; currentPolygonIdx < kPolygonCount && pathVertexAvailable; ++currentPolygonIdx) {
 			Polygon *polygon = &_polygons[currentPolygonIdx];
 
 			if (!polygon->isPresent || polygon->verticeCount == 0) {
 				continue;
 			}
 
-			for (int polygonVertexIdx = 0; polygonVertexIdx < polygon->verticeCount && keepSearching; ++polygonVertexIdx) {
+			for (int polygonVertexIdx = 0; polygonVertexIdx < polygon->verticeCount && pathVertexAvailable; ++polygonVertexIdx) {
 				int polygonVertexNextIdx = (polygonVertexIdx + 1) % polygon->verticeCount;
 
 				// check intersection between start -> path and polygon edge
 				Vector2 intersection;
-				if (!lineIntersection(Vector2(from.x, from.z), path[pathVertexIdx], polygon->vertices[polygonVertexIdx], polygon->vertices[polygonVertexNextIdx], &intersection)) {
+				if (!lineIntersection(Vector2(start.x, start.z), path[pathVertexIdx], polygon->vertices[polygonVertexIdx], polygon->vertices[polygonVertexNextIdx], &intersection)) {
 					continue;
 				}
 
-				// intersection has to be either on this polygon or on the path or at start
+				// intersection has to be at end of one of these points (either on this polygon or on the path or at start)
 				if (!(
-				    (WITHIN_TOLERANCE(intersection.x, from.x)                                    && WITHIN_TOLERANCE(intersection.y, from.z)                                   )
+					(WITHIN_TOLERANCE(intersection.x, start.x)                                   && WITHIN_TOLERANCE(intersection.y, start.z)                                  )
 				 || (WITHIN_TOLERANCE(intersection.x, path[pathVertexIdx].x)                     && WITHIN_TOLERANCE(intersection.y, path[pathVertexIdx].y)                    )
 				 || (WITHIN_TOLERANCE(intersection.x, polygon->vertices[polygonVertexIdx].x)     && WITHIN_TOLERANCE(intersection.y, polygon->vertices[polygonVertexIdx].y)    )
 				 || (WITHIN_TOLERANCE(intersection.x, polygon->vertices[polygonVertexNextIdx].x) && WITHIN_TOLERANCE(intersection.y, polygon->vertices[polygonVertexNextIdx].y))
 				)) {
-					keepSearching = false;
+					pathVertexAvailable = false;
 					break;
 				}
 
-				if (findPolygonVerticeByXZWithinTolerance(intersection.x, intersection.y, &polygonIndex3, &vertexIndex3)) {
-					// point has to be on current polygon
-					assert(polygonIndex3 == currentPolygonIdx);
+				int polygonIndexIntersection = -1;
+				int vertexIndexIntersection = -1;
+				if (findPolygonVerticeByXZWithinTolerance(intersection.x, intersection.y, &polygonIndexIntersection, &vertexIndexIntersection)) {
+					// hntersection has to be vertex only on current polygon
+					assert(polygonIndexIntersection == currentPolygonIdx);
 
-					if (verticesCanIntersect(vertexType1Prev, vertexType1, from.x, from.z, path[pathVertexIdx].x, path[pathVertexIdx].y)) {
-						keepSearching = false;
+					if (verticesCanIntersect(vertexTypeStartPrev, vertexTypeStart, start.x, start.z, path[pathVertexIdx].x, path[pathVertexIdx].y)) {
+						pathVertexAvailable = false;
 						break;
 					}
 
-					if ((currentPolygonIdx == polygonIndex2 && vertexIndex3 == vertexIndex2)
-					 || (currentPolygonIdx == polygonIndex1 && vertexIndex3 == vertexIndex1)
+					if ((currentPolygonIdx == polygonIndexPath  && vertexIndexIntersection == vertexIndexPath)
+					|| (currentPolygonIdx == polygonIndexStart && vertexIndexIntersection == vertexIndexStart)
 					) {
 						continue;
 					}
 
-					int vertexIndex3prev = (vertexIndex3 - 1 + _polygons[polygonIndex3].verticeCount ) %  _polygons[polygonIndex3].verticeCount;
-					if (verticesCanIntersect(_polygons[polygonIndex3].vertexType[vertexIndex3prev], _polygons[polygonIndex3].vertexType[vertexIndex3], intersection.x, intersection.y, path[pathVertexIdx].x, path[pathVertexIdx].y)) {
-						keepSearching = false;
+					int vertexIndexIntersectionprev = (vertexIndexIntersection - 1 + _polygons[polygonIndexIntersection].verticeCount ) % _polygons[polygonIndexIntersection].verticeCount;
+					if (verticesCanIntersect(_polygons[polygonIndexIntersection].vertexType[vertexIndexIntersectionprev], _polygons[polygonIndexIntersection].vertexType[vertexIndexIntersection], intersection.x, intersection.y, path[pathVertexIdx].x, path[pathVertexIdx].y)) {
+						pathVertexAvailable = false;
 						break;
 					}
 				} else {
-					bool fromIntersectionWithinTolerance = false;
-					if (WITHIN_TOLERANCE(intersection.x, from.x)
-					 && WITHIN_TOLERANCE(intersection.y, from.z)
+					bool startIntersectionWithinTolerance = false;
+					if (WITHIN_TOLERANCE(intersection.x, start.x)
+					 && WITHIN_TOLERANCE(intersection.y, start.z)
 					) {
-						fromIntersectionWithinTolerance = true;
+						startIntersectionWithinTolerance = true;
 					}
-					if (currentPolygonIdx == polygonIndex1 || fromIntersectionWithinTolerance) {
-						if (polygonIndex1 >= 0 || !fromIntersectionWithinTolerance) { // always?
-							keepSearching = false;
+
+					if (currentPolygonIdx == polygonIndexStart || startIntersectionWithinTolerance) {
+						if (polygonIndexStart >= 0 || !startIntersectionWithinTolerance) {
+							pathVertexAvailable = false;
 							break;
 						}
 
 						int polygonVertexType =  polygon->vertexType[polygonVertexIdx];
 						if ((polygonVertexType == TOP_LEFT     && intersection.y < path[pathVertexIdx].y)
-						 || (polygonVertexType == TOP_RIGHT    && intersection.x > path[pathVertexIdx].x)
-						 || (polygonVertexType == BOTTOM_RIGHT && intersection.y > path[pathVertexIdx].y)
-						 || (polygonVertexType == BOTTOM_LEFT  && intersection.x < path[pathVertexIdx].x)
+						|| (polygonVertexType == TOP_RIGHT    && intersection.x > path[pathVertexIdx].x)
+						|| (polygonVertexType == BOTTOM_RIGHT && intersection.y > path[pathVertexIdx].y)
+						|| (polygonVertexType == BOTTOM_LEFT  && intersection.x < path[pathVertexIdx].x)
 						) {
-							keepSearching = false;
+							pathVertexAvailable = false;
 							break;
 						}
 					}
@@ -796,13 +794,13 @@ bool Obstacles::findFarthestAvailablePathVertex(Vector2 *path, int pathSize, Vec
 			}
 		}
 
-		if (keepSearching) {
+		if (pathVertexAvailable) {
 			farthestPathIndex = pathVertexIdx;
 		}
 	}
 
 	if (farthestPathIndex == -1) {
-		*next = from;
+		*next = start;
 		return false;
 	}
 
@@ -816,7 +814,7 @@ bool Obstacles::findFarthestAvailablePathVertex(Vector2 *path, int pathSize, Vec
 		next->y = walkboxAltitude;
 		return true;
 	} else {
-		next->y = from.y;
+		next->y = start.y;
 		return false;
 	}
 }
diff --git a/engines/bladerunner/obstacles.h b/engines/bladerunner/obstacles.h
index 05ee518a64..f07f3909f6 100644
--- a/engines/bladerunner/obstacles.h
+++ b/engines/bladerunner/obstacles.h
@@ -100,7 +100,7 @@ public:
 	int  buildPositivePath(int polyIndex, int vertStartIndex, Vector2 startPos, int vertEndIndex, Vector2 endPos, Vector2 *path, int pathCapacity, bool *pathBlocked);
 
 	bool verticesCanIntersect(int lineType0, int lineType1, float x0, float y0, float x1, float y1) const;
-	bool findFarthestAvailablePathVertex(Vector2 *path, int pathSize, Vector3 from, Vector3 *next) const;
+	bool findFarthestAvailablePathVertex(Vector2 *path, int pathSize, Vector3 start, Vector3 *next) const;
 
 	void backup();
 	void restore();