FULLPIPE: Complete MovGraph::findClosestLink() implementation

1 files changed, 32 insertions, 37 deletions

diff --git a/engines/fullpipe/motion.cpp b/engines/fullpipe/motion.cpp
index 00e4d856ba..a4c5b0ba64 100644
--- a/engines/fullpipe/motion.cpp
+++ b/engines/fullpipe/motion.cpp
@@ -688,60 +688,55 @@ void MovGraph::calcNodeDistancesAndAngles() {
 }
 
 bool MovGraph::findClosestLink(int unusedArg, Common::Point *p, MovArr *movarr) {
-#if 0
-	v4 = (double)p->x;
-	v5 = (double)(signed int)p->y;
-	link = 0;
-	v28 = 1.0e20;
+	MovGraphLink *link = 0;
+	double mindist = 1.0e20;
+	int resx = 0, resy = 0;
 
 	for (ObList::iterator i = _links.begin(); i != _links.end(); ++i) {
 		MovGraphLink *lnk = (MovGraphLink *)*i;
 
-		v8 = v6->data;
-
 		if ((lnk->_flags & 0x10000000) && !(lnk->_flags & 0x20000000) ) {
-			v12 = lnk->_movGraphNode1->_x;
-			v13 = lnk->_movGraphNode1->_y;
-			v23 = v12;
-			v14 = lnk->_movGraphNode2->_x;
-			v15 = (double)v12;
-			v27 = v14;
-			v16 = v14 - v12;
-			v17 = lnk->_movGraphNode2->_y;
-			v29 = v15;
-			v18 = v4 - v15;
-			v32 = (double)v13 - v5;
-			v33 = (double)v16;
-			v19 = sqrt(v32 * v32 + v18 * v18);
-			v20 = ((double)(v13 - v17) * v32 + v33 * v18) / lnk->_distance / v19;
-			v21 = v20 * v19;
-			v26 = sqrt(1.0 - v20 * v20) * v19;
-			if ( v20 * v19 < 0.0 ) {
-				v21 = 0.0;
-				v26 = sqrt((v29 - v4) * (v29 - v4) + v32 * v32);
+			double dx1 = lnk->_movGraphNode1->_x - p->x;
+			double dy1 = lnk->_movGraphNode1->_y - p->y;
+			double dx2 = lnk->_movGraphNode2->_x - p->x;
+			double dy2 = lnk->_movGraphNode2->_y - p->y;
+			double dx3 = lnk->_movGraphNode2->_x - lnk->_movGraphNode1->_x;
+			double dy3 = lnk->_movGraphNode2->_y - lnk->_movGraphNode1->_y;
+			double sq1 = sqrt(dy1 * dy1 + dx1 * dx1);
+			double sdist = (dy3 * dy1 + dx3 * dx1) / lnk->_distance / sq1;
+			double ldist = sdist * sq1;
+			double dist = sqrt(1.0 - sdist * sdist) * sq1;
+
+			if (ldist < 0.0) {
+				ldist = 0.0;
+				dist = sqrt(dx1 * dx1 + dy1 * dy1);
 			}
-			if (v21 > lnk->_distance) {
-				v21 = lnk->_distance;
-				v26 = sqrt(((double)v27 - v4) * ((double)v27 - v4) + ((double)lnk->_movGraphNode2->_y - v5) * ((double)lnk->_movGraphNode2->_y - v5));
+
+			if (ldist > lnk->_distance) {
+				ldist = lnk->_distance;
+				dist = sqrt(dx2 * dx2 + dy2 * dy2);
 			}
-			if ( v21 >= 0.0 && v21 <= lnk->_distance && v26 < v28 ) {
-				v30 = v23 + (unsigned __int64)(signed __int64)(v33 * v21 / lnk->_distance);
-				v7 = v13 + (unsigned __int64)(signed __int64)((double)(v17 - v13) * v21 / lnk->_distance);
-				v28 = v26;
+
+			if (ldist >= 0.0 && ldist <= lnk->_distance && dist < mindist) {
+				resx = lnk->_movGraphNode1->_x + (int)(dx3 * ldist / lnk->_distance);
+				resy = lnk->_movGraphNode1->_y + (int)(dy3 * ldist / lnk->_distance);
+
+				mindist = dist;
 				link = lnk;
 			}
 		}
 	}
 
-	if (v28 < 1.0e20) {
+	if (mindist < 1.0e20) {
 		if (movarr)
 			movarr->_link = link;
-		p->x = v30;
-		p->y = v7;
+
+		p->x = resx;
+		p->y = resy;
 
 		return true;
 	}
-#endif
+
 	return false;
 }