path_fix.diff

gistfile1.cpp

Index: source/simulation2/components/CCmpPathfinder_Tile.cpp
===================================================================
--- source/simulation2/components/CCmpPathfinder_Tile.cpp	(revision 16407)
+++ source/simulation2/components/CCmpPathfinder_Tile.cpp	(working copy)
@@ -232,23 +232,14 @@
 static u32 CalculateHeuristic(u16 i, u16 j, u16 iGoal, u16 jGoal, u16 rGoal)
 {
 #if USE_DIAGONAL_MOVEMENT
-	CFixedVector2D pos (fixed::FromInt(i), fixed::FromInt(j));
-	CFixedVector2D goal (fixed::FromInt(iGoal), fixed::FromInt(jGoal));
-	fixed dist = (pos - goal).Length();
-	// TODO: the heuristic could match the costs better - it's not really Euclidean movement
+	double di = i - iGoal;
+	double dj = j - jGoal;
 
-	fixed rdist = dist - fixed::FromInt(rGoal);
-	rdist = rdist.Absolute();
+	double distSquared = (di * di + dj * dj);
 
-	// To avoid overflows on large distances we have to convert to int before multiplying
-	// by the full tile cost, which means we lose some accuracy over short distances,
-	// so do a partial multiplication here.
-	// (This will overflow if sqrt(2)*tilesPerSide*premul >= 32768, so
-	// premul=32 means max tilesPerSide=724)
-	const int premul = 32;
-	cassert(g_CostPerTile % premul == 0);
-	return (rdist * premul).ToInt_RoundToZero() * (g_CostPerTile / premul);
+	double rDistSquared = abs(distSquared - rGoal * rGoal);
 
+	return rDistSquared * g_CostPerTile;
 #else
 	return (abs((int)i - (int)iGoal) + abs((int)j - (int)jGoal)) * g_CostPerTile;
 #endif