diff --git a/binaries/data/mods/public/art/textures/terrain/types/special/grid_subdiv.xml b/binaries/data/mods/public/art/textures/terrain/types/special/grid_subdiv.xml
index f04a3df..381e02a 100644
--- a/binaries/data/mods/public/art/textures/terrain/types/special/grid_subdiv.xml
+++ b/binaries/data/mods/public/art/textures/terrain/types/special/grid_subdiv.xml
@@ -1,5 +1,5 @@
-<?xml version="1.0" encoding="utf-8"?>
-
-<Terrains>
-	<Terrain angle="0.0" size="4.0"/>
-</Terrains>
+<?xml version="1.0" encoding="utf-8"?>
+
+<Terrains>
+	<Terrain angle="0.0" size="4.0"/>
+</Terrains>
diff --git a/binaries/data/mods/public/art/textures/terrain/types/special/textures.xml b/binaries/data/mods/public/art/textures/terrain/types/special/textures.xml
index f267a1a..61d9267 100644
--- a/binaries/data/mods/public/art/textures/terrain/types/special/textures.xml
+++ b/binaries/data/mods/public/art/textures/terrain/types/special/textures.xml
@@ -1,4 +1,4 @@
-<?xml version="1.0" encoding="utf-8"?>
-<Textures>
-  <File pattern="grid_subdiv.png" format="rgba"/>
-</Textures>
+<?xml version="1.0" encoding="utf-8"?>
+<Textures>
+  <File pattern="grid_subdiv.png" format="rgba"/>
+</Textures>
diff --git a/binaries/data/mods/public/art/textures/ui/pregame/shell/logo/wfg_logo_white.png b/binaries/data/mods/public/art/textures/ui/pregame/shell/logo/wfg_logo_white.png
old mode 100755
new mode 100644
diff --git a/binaries/data/mods/public/maps/scenarios/Combat_demo.pmp b/binaries/data/mods/public/maps/scenarios/Combat_demo.pmp
old mode 100755
new mode 100644
diff --git a/binaries/data/mods/public/maps/scenarios/Combat_demo.xml b/binaries/data/mods/public/maps/scenarios/Combat_demo.xml
old mode 100755
new mode 100644
diff --git a/binaries/data/mods/public/maps/scenarios/Combat_demo_(huge).xml b/binaries/data/mods/public/maps/scenarios/Combat_demo_(huge).xml
old mode 100755
new mode 100644
diff --git a/binaries/data/mods/public/simulation/ai/qbot/military.js b/binaries/data/mods/public/simulation/ai/qbot/military.js
old mode 100755
new mode 100644
diff --git a/binaries/data/mods/public/simulation/data/pathfinder.xml b/binaries/data/mods/public/simulation/data/pathfinder.xml
index 47eb0eb..58c7b1a 100644
--- a/binaries/data/mods/public/simulation/data/pathfinder.xml
+++ b/binaries/data/mods/public/simulation/data/pathfinder.xml
@@ -6,15 +6,22 @@
   
   <PassabilityClasses>
 
-    <!-- Unit pathfinding classes: -->
     <unrestricted/>
+
+    <!-- Unit pathfinding classes: -->
     <default>
       <MaxWaterDepth>2</MaxWaterDepth>
       <MaxTerrainSlope>1.0</MaxTerrainSlope>
+      <Clearance>2.0</Clearance>
     </default>
     <ship>
       <MinWaterDepth>1</MinWaterDepth>
+      <Clearance>12.0</Clearance>
     </ship>
+    <ship-small>
+      <MinWaterDepth>1</MinWaterDepth>
+      <Clearance>4.0</Clearance>
+    </ship-small>
     <!-- Building construction classes:
     * Land is used for most buildings, which must be away
         from water and not on cliffs or mountains.
@@ -30,6 +37,11 @@
       <MaxShoreDistance>2.0</MaxShoreDistance>
       <MaxTerrainSlope>1.25</MaxTerrainSlope>
     </building-shore>
+	<!-- Territory growth influences: -->
+    <territory>
+      <MaxWaterDepth>2</MaxWaterDepth>
+      <MaxTerrainSlope>1.0</MaxTerrainSlope>
+    </territory>
 
   </PassabilityClasses>
   
diff --git a/binaries/data/mods/public/simulation/helpers/Entity.js b/binaries/data/mods/public/simulation/helpers/Entity.js
old mode 100755
new mode 100644
diff --git a/binaries/data/mods/public/simulation/templates/template_unit_mechanical_ship_fishing.xml b/binaries/data/mods/public/simulation/templates/template_unit_mechanical_ship_fishing.xml
index 7067e80..79985a5 100644
--- a/binaries/data/mods/public/simulation/templates/template_unit_mechanical_ship_fishing.xml
+++ b/binaries/data/mods/public/simulation/templates/template_unit_mechanical_ship_fishing.xml
@@ -40,6 +40,7 @@
     </Rates>
   </ResourceGatherer>
   <UnitMotion>
+    <PassabilityClass>ship-small</PassabilityClass>
     <WalkSpeed>8.5</WalkSpeed>
   </UnitMotion>
   <Vision>
diff --git a/build/bin/cxxtestgen.pl b/build/bin/cxxtestgen.pl
old mode 100755
new mode 100644
diff --git a/build/coverage/gen.sh b/build/coverage/gen.sh
old mode 100755
new mode 100644
diff --git a/build/errorlist/errorlist.pl b/build/errorlist/errorlist.pl
old mode 100755
new mode 100644
diff --git a/build/premake/premake4/tests/test b/build/premake/premake4/tests/test
old mode 100755
new mode 100644
diff --git a/build/workspaces/clean-workspaces.sh b/build/workspaces/clean-workspaces.sh
old mode 100755
new mode 100644
diff --git a/build/workspaces/update-workspaces.sh b/build/workspaces/update-workspaces.sh
old mode 100755
new mode 100644
diff --git a/libraries/enet/build.sh b/libraries/enet/build.sh
old mode 100755
new mode 100644
diff --git a/libraries/enet/src/config.guess b/libraries/enet/src/config.guess
old mode 100755
new mode 100644
diff --git a/libraries/enet/src/config.sub b/libraries/enet/src/config.sub
old mode 100755
new mode 100644
diff --git a/libraries/enet/src/configure b/libraries/enet/src/configure
old mode 100755
new mode 100644
diff --git a/libraries/enet/src/depcomp b/libraries/enet/src/depcomp
old mode 100755
new mode 100644
diff --git a/libraries/enet/src/install-sh b/libraries/enet/src/install-sh
old mode 100755
new mode 100644
diff --git a/libraries/enet/src/ltmain.sh b/libraries/enet/src/ltmain.sh
old mode 100755
new mode 100644
diff --git a/libraries/enet/src/missing b/libraries/enet/src/missing
old mode 100755
new mode 100644
diff --git a/libraries/nvtt/build.sh b/libraries/nvtt/build.sh
old mode 100755
new mode 100644
diff --git a/libraries/nvtt/src/configure b/libraries/nvtt/src/configure
old mode 100755
new mode 100644
diff --git a/libraries/spidermonkey/build.sh b/libraries/spidermonkey/build.sh
old mode 100755
new mode 100644
diff --git a/source/graphics/Terrain.cpp b/source/graphics/Terrain.cpp
index e38d2a6..fa90cfd 100644
--- a/source/graphics/Terrain.cpp
+++ b/source/graphics/Terrain.cpp
@@ -330,6 +330,58 @@ fixed CTerrain::GetSlopeFixed(ssize_t i, ssize_t j) const
 	return fixed::FromInt(delta / TERRAIN_TILE_SIZE) / (int)HEIGHT_UNITS_PER_METRE;
 }
 
+fixed CTerrain::GetExactSlopeFixed(fixed x, fixed z) const
+{
+	// Clamp to size-2 so we can use the tiles (xi,zi)-(xi+1,zi+1)
+	const ssize_t xi = clamp((ssize_t)(x / (int)TERRAIN_TILE_SIZE).ToInt_RoundToZero(), (ssize_t)0, m_MapSize-2);
+	const ssize_t zi = clamp((ssize_t)(z / (int)TERRAIN_TILE_SIZE).ToInt_RoundToZero(), (ssize_t)0, m_MapSize-2);
+
+	const fixed one = fixed::FromInt(1);
+
+	const fixed xf = clamp((x / (int)TERRAIN_TILE_SIZE) - fixed::FromInt(xi), fixed::Zero(), one);
+	const fixed zf = clamp((z / (int)TERRAIN_TILE_SIZE) - fixed::FromInt(zi), fixed::Zero(), one);
+
+	u16 h00 = m_Heightmap[zi*m_MapSize + xi];
+	u16 h01 = m_Heightmap[(zi+1)*m_MapSize + xi];
+	u16 h10 = m_Heightmap[zi*m_MapSize + (xi+1)];
+	u16 h11 = m_Heightmap[(zi+1)*m_MapSize + (xi+1)];
+
+	u16 delta;
+	if (GetTriangulationDir(xi, zi))
+	{
+		if (xf + zf <= one)
+		{
+			// Lower-left triangle (don't use h11)
+ 			delta = std::max(std::max(h00, h01), h10) -
+ 			        std::min(std::min(h00, h01), h10);
+		}
+		else
+		{
+			// Upper-right triangle (don't use h00)
+			delta = std::max(std::max(h01, h10), h11) -
+			        std::min(std::min(h01, h10), h11);
+		}
+	}
+	else
+	{
+		if (xf <= zf)
+		{
+			// Upper-left triangle (don't use h10)
+			delta = std::max(std::max(h00, h01), h11) -
+			        std::min(std::min(h00, h01), h11);
+		}
+		else
+		{
+			// Lower-right triangle (don't use h01)
+			delta = std::max(std::max(h00, h10), h11) -
+			        std::min(std::min(h00, h10), h11);
+		}
+	}
+
+	// Compute fractional slope (being careful to avoid intermediate overflows)
+	return fixed::FromInt(delta / TERRAIN_TILE_SIZE) / (int)HEIGHT_UNITS_PER_METRE;
+}
+
 float CTerrain::GetExactGroundLevel(float x, float z) const
 {
 	// Clamp to size-2 so we can use the tiles (xi,zi)-(xi+1,zi+1)
diff --git a/source/graphics/Terrain.h b/source/graphics/Terrain.h
index 4002eb5..b059879 100644
--- a/source/graphics/Terrain.h
+++ b/source/graphics/Terrain.h
@@ -88,6 +88,8 @@ public:
 	// get the approximate slope (0 = horizontal, 0.5 = 30 degrees, 1.0 = 45 degrees, etc)
 	fixed GetSlopeFixed(ssize_t i, ssize_t j) const;
 
+	fixed GetExactSlopeFixed(fixed x, fixed z) const;
+
 	// Returns true if the triangulation diagonal for tile (i, j)
 	// should be in the direction (1,-1); false if it should be (1,1)
 	bool GetTriangulationDir(ssize_t i, ssize_t j) const;
diff --git a/source/graphics/TerritoryBoundary.cpp b/source/graphics/TerritoryBoundary.cpp
index da0fcd2..d9cbfb1 100644
--- a/source/graphics/TerritoryBoundary.cpp
+++ b/source/graphics/TerritoryBoundary.cpp
@@ -21,6 +21,7 @@
 #include <algorithm> // for reverse
 
 #include "graphics/Terrain.h"
+#include "simulation2/components/ICmpObstructionManager.h"
 #include "simulation2/components/ICmpTerritoryManager.h"
 
 std::vector<STerritoryBoundary> CTerritoryBoundaryCalculator::ComputeBoundaries(const Grid<u8>* territory)
@@ -125,7 +126,7 @@ std::vector<STerritoryBoundary> CTerritoryBoundaryCalculator::ComputeBoundaries(
 
 			while (true)
 			{
-				points.push_back((CVector2D(ci, cj) + edgeOffsets[cdir]) * TERRAIN_TILE_SIZE);
+				points.push_back((CVector2D(ci, cj) + edgeOffsets[cdir]) * (ICmpObstructionManager::NAVCELL_SIZE * ICmpTerritoryManager::NAVCELLS_PER_TERRITORY_TILE).ToFloat());
 
 				// Given that we're on an edge on a continuous boundary and aiming anticlockwise,
 				// we can either carry on straight or turn left or turn right, so examine each
diff --git a/source/graphics/TerritoryTexture.cpp b/source/graphics/TerritoryTexture.cpp
index 336be12..c420906 100644
--- a/source/graphics/TerritoryTexture.cpp
+++ b/source/graphics/TerritoryTexture.cpp
@@ -25,6 +25,7 @@
 #include "ps/Profile.h"
 #include "renderer/Renderer.h"
 #include "simulation2/Simulation2.h"
+#include "simulation2/components/ICmpObstructionManager.h"
 #include "simulation2/components/ICmpPlayer.h"
 #include "simulation2/components/ICmpPlayerManager.h"
 #include "simulation2/components/ICmpTerrain.h"
@@ -92,7 +93,7 @@ void CTerritoryTexture::ConstructTexture(int unit)
 	if (cmpTerrain.null())
 		return;
 
-	m_MapSize = cmpTerrain->GetVerticesPerSide() - 1;
+	m_MapSize = cmpTerrain->GetTilesPerSide() * ICmpObstructionManager::NAVCELLS_PER_TILE / ICmpTerritoryManager::NAVCELLS_PER_TERRITORY_TILE;
 
 	m_TextureSize = (GLsizei)round_up_to_pow2((size_t)m_MapSize);
 
diff --git a/source/gui/GUIManager.h b/source/gui/GUIManager.h
index 0edfd14..828e4c0 100644
--- a/source/gui/GUIManager.h
+++ b/source/gui/GUIManager.h
@@ -145,7 +145,6 @@ private:
 		CStrW name;
 		boost::unordered_set<VfsPath> inputs; // for hotloading
 
-		JSContext* cx;
 		CScriptValRooted initData; // data to be passed to the init() function
 
 		shared_ptr<CGUI> gui; // the actual GUI page
diff --git a/source/renderer/OverlayRenderer.cpp b/source/renderer/OverlayRenderer.cpp
index 53d6b59..5e225ff 100644
--- a/source/renderer/OverlayRenderer.cpp
+++ b/source/renderer/OverlayRenderer.cpp
@@ -166,6 +166,10 @@ void OverlayRenderer::RenderOverlaysBeforeWater()
 	glDisable(GL_TEXTURE_2D);
 	glEnable(GL_BLEND);
 
+	// Ignore z to draw behind terrain (but don't disable GL_DEPTH_TEST
+	// since we still need to write to the z buffer)
+	glDepthFunc(GL_ALWAYS);
+
 	for (size_t i = 0; i < m->lines.size(); ++i)
 	{
 		SOverlayLine* line = m->lines[i];
@@ -184,6 +188,7 @@ void OverlayRenderer::RenderOverlaysBeforeWater()
 	glDisableClientState(GL_VERTEX_ARRAY);
 
 	glLineWidth(1.f);
+	glDepthFunc(GL_LEQUAL);
 	glDisable(GL_BLEND);
 }
 
@@ -321,7 +326,7 @@ void OverlayRenderer::RenderForegroundOverlays(const CCamera& viewCamera)
 
 	glEnable(GL_TEXTURE_2D);
 	glEnable(GL_BLEND);
-	glDisable(GL_DEPTH_TEST);
+ 	glDisable(GL_DEPTH_TEST);
 
 	CVector3D right = -viewCamera.m_Orientation.GetLeft();
 	CVector3D up = viewCamera.m_Orientation.GetUp();
@@ -354,7 +359,7 @@ void OverlayRenderer::RenderForegroundOverlays(const CCamera& viewCamera)
 	glDisableClientState(GL_VERTEX_ARRAY);
 	glDisableClientState(GL_TEXTURE_COORD_ARRAY);
 
-	glEnable(GL_DEPTH_TEST);
+ 	glEnable(GL_DEPTH_TEST);
 	glDisable(GL_BLEND);
 	glDisable(GL_TEXTURE_2D);
 }
diff --git a/source/renderer/PatchRData.cpp b/source/renderer/PatchRData.cpp
index d6b3bb3..ab3450a 100644
--- a/source/renderer/PatchRData.cpp
+++ b/source/renderer/PatchRData.cpp
@@ -1026,7 +1026,7 @@ void CPatchRData::RenderStreams(const std::vector<CPatchRData*>& patches, int st
 
  	PROFILE_END("compute batches");
 
-	ENSURE(!(streamflags & ~(STREAM_POS|STREAM_COLOR|STREAM_POSTOUV1)));
+	ENSURE(!(streamflags & ~(STREAM_POS|STREAM_COLOR|STREAM_POSTOUV0|STREAM_POSTOUV1)));
 
  	// Render each batch
  	for (VertexBufferBatches::iterator itv = batches.begin(); itv != batches.end(); ++itv)
diff --git a/source/renderer/Renderer.cpp b/source/renderer/Renderer.cpp
index 2e2dcdf..d3d7578 100644
--- a/source/renderer/Renderer.cpp
+++ b/source/renderer/Renderer.cpp
@@ -1575,7 +1575,7 @@ void CRenderer::RenderSubmissions()
 	}
 
 	// render debug-related terrain overlays
-	TerrainOverlay::RenderOverlays();
+	ITerrainOverlay::RenderOverlaysBeforeWater();
 	ogl_WarnIfError();
 
 	// render other debug-related overlays before water (so they can be seen when underwater)
@@ -1606,6 +1606,10 @@ void CRenderer::RenderSubmissions()
 		ogl_WarnIfError();
 	}
 
+	// render debug-related terrain overlays
+	ITerrainOverlay::RenderOverlaysAfterWater();
+	ogl_WarnIfError();
+
 	// render some other overlays after water (so they can be displayed on top of water)
 	m->overlayRenderer.RenderOverlaysAfterWater();
 	ogl_WarnIfError();
@@ -2128,3 +2132,8 @@ CParticleManager& CRenderer::GetParticleManager()
 {
 	return m->particleManager;
 }
+
+TerrainRenderer& CRenderer::GetTerrainRenderer()
+{
+	return *m->terrainRenderer;
+}
diff --git a/source/renderer/Renderer.h b/source/renderer/Renderer.h
index 98d8e48..476f586 100644
--- a/source/renderer/Renderer.h
+++ b/source/renderer/Renderer.h
@@ -44,6 +44,7 @@ class SkyManager;
 class CTextureManager;
 class CShaderManager;
 class CParticleManager;
+class TerrainRenderer;
 
 // rendering modes
 enum ERenderMode { WIREFRAME, SOLID, EDGED_FACES };
@@ -265,6 +266,8 @@ public:
 
 	CParticleManager& GetParticleManager();
 
+	TerrainRenderer& GetTerrainRenderer();
+
 	/**
 	 * SetFastPlayerColor: Tell the renderer which path to take for
 	 * player colored models. Both paths should provide the same visual
diff --git a/source/renderer/TerrainOverlay.cpp b/source/renderer/TerrainOverlay.cpp
index 0e1e3d9..b1c1135 100644
--- a/source/renderer/TerrainOverlay.cpp
+++ b/source/renderer/TerrainOverlay.cpp
@@ -1,4 +1,4 @@
-/* Copyright (C) 2009 Wildfire Games.
+/* Copyright (C) 2012 Wildfire Games.
  * This file is part of 0 A.D.
  *
  * 0 A.D. is free software: you can redistribute it and/or modify
@@ -20,13 +20,16 @@
 #include "TerrainOverlay.h"
 
 #include "graphics/Terrain.h"
-#include "simulation2/system/SimContext.h"
+#include "lib/bits.h"
 #include "lib/ogl.h"
 #include "maths/MathUtil.h"
 #include "ps/Game.h"
 #include "ps/Overlay.h"
 #include "ps/Profile.h"
 #include "ps/World.h"
+#include "renderer/Renderer.h"
+#include "renderer/TerrainRenderer.h"
+#include "simulation2/system/SimContext.h"
 
 #include <algorithm>
 
@@ -54,23 +57,13 @@ private:
 	const equal1st& operator=(const equal1st& rhs);
 };
 
-/// Functor for calling ->Render on pairs' firsts.
-struct render1st
-{
-	template<typename S, typename T> void operator()(const std::pair<S, T>& a) const
-	{
-		a.first->Render();
-	}
-};
-
 //////////////////////////////////////////////////////////////////////////
 
 // Global overlay list management:
 
-static std::vector<std::pair<TerrainOverlay*, int> > g_TerrainOverlayList;
+static std::vector<std::pair<ITerrainOverlay*, int> > g_TerrainOverlayList;
 
-TerrainOverlay::TerrainOverlay(const CSimContext& simContext, int priority /* = 100 */)
-	: m_Terrain(&simContext.GetTerrain())
+ITerrainOverlay::ITerrainOverlay(int priority)
 {
 	// Add to global list of overlays
 	g_TerrainOverlayList.push_back(std::make_pair(this, priority));
@@ -81,48 +74,44 @@ TerrainOverlay::TerrainOverlay(const CSimContext& simContext, int priority /* =
 		compare2nd());
 }
 
-TerrainOverlay::~TerrainOverlay()
+ITerrainOverlay::~ITerrainOverlay()
 {
-	std::vector<std::pair<TerrainOverlay*, int> >::iterator newEnd =
+	std::vector<std::pair<ITerrainOverlay*, int> >::iterator newEnd =
 		std::remove_if(g_TerrainOverlayList.begin(), g_TerrainOverlayList.end(),
-			equal1st<TerrainOverlay*>(this));
+			equal1st<ITerrainOverlay*>(this));
 	g_TerrainOverlayList.erase(newEnd, g_TerrainOverlayList.end());
 }
 
 
-void TerrainOverlay::RenderOverlays()
+void ITerrainOverlay::RenderOverlaysBeforeWater()
 {
-	if (g_TerrainOverlayList.size() == 0)
+	if (g_TerrainOverlayList.empty())
 		return;
 
-	PROFILE3_GPU("terrain overlays");
+	PROFILE3_GPU("terrain overlays (before)");
 
-	glEnable(GL_BLEND);
-	glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
-	glDepthMask(GL_FALSE);
-	// To ensure that outlines are drawn on top of the terrain correctly (and
-	// don't Z-fight and flicker nastily), draw them as QUADS with the LINE
-	// PolygonMode, and use PolygonOffset to pull them towards the camera.
-	// (See e.g. http://www.opengl.org/resources/faq/technical/polygonoffset.htm)
-	glPolygonOffset(-1.f, -1.f);
-	glEnable(GL_POLYGON_OFFSET_LINE);
+	for (size_t i = 0; i < g_TerrainOverlayList.size(); ++i)
+		g_TerrainOverlayList[i].first->RenderBeforeWater();
+}
 
-	glDisable(GL_TEXTURE_2D);
+void ITerrainOverlay::RenderOverlaysAfterWater()
+{
+	if (g_TerrainOverlayList.empty())
+		return;
 
-	std::for_each(g_TerrainOverlayList.begin(), g_TerrainOverlayList.end(),
-		render1st());
+	PROFILE3_GPU("terrain overlays (after)");
 
-	// Clean up state changes
-	glEnable(GL_CULL_FACE);
-	glEnable(GL_DEPTH_TEST);
-	glDisable(GL_POLYGON_OFFSET_LINE);
-	glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
-	glDepthMask(GL_TRUE);
-	glDisable(GL_BLEND);
+	for (size_t i = 0; i < g_TerrainOverlayList.size(); ++i)
+		g_TerrainOverlayList[i].first->RenderAfterWater();
 }
 
 //////////////////////////////////////////////////////////////////////////
 
+TerrainOverlay::TerrainOverlay(const CSimContext& simContext, int priority /* = 100 */)
+	: ITerrainOverlay(priority), m_Terrain(&simContext.GetTerrain())
+{
+}
+
 void TerrainOverlay::StartRender()
 {
 }
@@ -140,11 +129,23 @@ void TerrainOverlay::GetTileExtents(
 	max_i_inclusive = max_j_inclusive = m_Terrain->GetTilesPerSide()-1;
 }
 
-void TerrainOverlay::Render()
+void TerrainOverlay::RenderBeforeWater()
 {
 	if (!m_Terrain)
 		return; // should never happen, but let's play it safe
 
+	glEnable(GL_BLEND);
+	glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+	glDepthMask(GL_FALSE);
+	// To ensure that outlines are drawn on top of the terrain correctly (and
+	// don't Z-fight and flicker nastily), draw them as QUADS with the LINE
+	// PolygonMode, and use PolygonOffset to pull them towards the camera.
+	// (See e.g. http://www.opengl.org/resources/faq/technical/polygonoffset.htm)
+	glPolygonOffset(-1.f, -1.f);
+	glEnable(GL_POLYGON_OFFSET_LINE);
+
+	glDisable(GL_TEXTURE_2D);
+
 	StartRender();
 
 	ssize_t min_i, min_j, max_i, max_j;
@@ -162,6 +163,14 @@ void TerrainOverlay::Render()
 			ProcessTile(m_i, m_j);
 
 	EndRender();
+
+	// Clean up state changes
+	glEnable(GL_CULL_FACE);
+	glEnable(GL_DEPTH_TEST);
+	glDisable(GL_POLYGON_OFFSET_LINE);
+	glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
+	glDepthMask(GL_TRUE);
+	glDisable(GL_BLEND);
 }
 
 void TerrainOverlay::RenderTile(const CColor& colour, bool draw_hidden)
@@ -249,3 +258,59 @@ void TerrainOverlay::RenderTileOutline(const CColor& colour, int line_width, boo
 	if (line_width != 1)
 		glLineWidth(1.0f);
 }
+
+//////////////////////////////////////////////////////////////////////////
+
+TerrainTextureOverlay::TerrainTextureOverlay(float texelsPerTile, int priority) :
+	ITerrainOverlay(priority), m_TexelsPerTile(texelsPerTile), m_Texture(0), m_TextureW(0), m_TextureH(0)
+{
+	glGenTextures(1, &m_Texture);
+}
+
+TerrainTextureOverlay::~TerrainTextureOverlay()
+{
+	glDeleteTextures(1, &m_Texture);
+}
+
+void TerrainTextureOverlay::RenderAfterWater()
+{
+	CTerrain* terrain = g_Game->GetWorld()->GetTerrain();
+
+	ssize_t w = (ssize_t)(terrain->GetTilesPerSide() * m_TexelsPerTile);
+	ssize_t h = (ssize_t)(terrain->GetTilesPerSide() * m_TexelsPerTile);
+
+	pglActiveTextureARB(GL_TEXTURE0);
+
+	// Recreate the texture with new size if necessary
+	if (round_up_to_pow2(w) != m_TextureW || round_up_to_pow2(h) != m_TextureH)
+	{
+		m_TextureW = round_up_to_pow2(w);
+		m_TextureH = round_up_to_pow2(h);
+
+		glBindTexture(GL_TEXTURE_2D, m_Texture);
+
+		glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA8, m_TextureW, m_TextureH, 0, GL_RGBA, GL_UNSIGNED_BYTE, NULL);
+
+		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_BORDER);
+		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_BORDER);
+	}
+
+	u8* data = (u8*)calloc(w * h, 4);
+	BuildTextureRGBA(data, w, h);
+
+	glBindTexture(GL_TEXTURE_2D, m_Texture);
+
+	glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, w, h, GL_RGBA, GL_UNSIGNED_BYTE, data);
+
+	free(data);
+
+	CMatrix3D matrix;
+	matrix.SetZero();
+	matrix._11 = m_TexelsPerTile / (m_TextureW * TERRAIN_TILE_SIZE);
+	matrix._23 = m_TexelsPerTile / (m_TextureH * TERRAIN_TILE_SIZE);
+	matrix._44 = 1;
+
+	g_Renderer.GetTerrainRenderer().RenderTerrainOverlayTexture(matrix);
+}
diff --git a/source/renderer/TerrainOverlay.h b/source/renderer/TerrainOverlay.h
index 7d35581..45c6835 100644
--- a/source/renderer/TerrainOverlay.h
+++ b/source/renderer/TerrainOverlay.h
@@ -1,4 +1,4 @@
-/* Copyright (C) 2009 Wildfire Games.
+/* Copyright (C) 2012 Wildfire Games.
  * This file is part of 0 A.D.
  *
  * 0 A.D. is free software: you can redistribute it and/or modify
@@ -23,10 +23,39 @@
 #ifndef INCLUDED_TERRAINOVERLAY
 #define INCLUDED_TERRAINOVERLAY
 
+#include "lib/ogl.h"
+
 struct CColor;
 class CTerrain;
 class CSimContext;
 
+class ITerrainOverlay
+{
+	NONCOPYABLE(ITerrainOverlay);
+
+public:
+	virtual ~ITerrainOverlay();
+
+	virtual void RenderBeforeWater() { }
+
+	virtual void RenderAfterWater() { }
+
+	/**
+	 * Draw all ITerrainOverlay objects that exist
+	 * and that should be drawn before water.
+	 */
+	static void RenderOverlaysBeforeWater();
+
+	/**
+	 * Draw all ITerrainOverlay objects that exist
+	 * and that should be drawn after water.
+	 */
+	static void RenderOverlaysAfterWater();
+
+protected:
+	ITerrainOverlay(int priority);
+};
+
 /**
  * Base class for (relatively) simple drawing of
  * data onto terrain tiles, intended for debugging purposes and for the Atlas
@@ -42,13 +71,8 @@ class CSimContext;
  * A TerrainOverlay object will be rendered for as long as it exists.
  *
  */
-class TerrainOverlay
+class TerrainOverlay : public ITerrainOverlay
 {
-public:
-	virtual ~TerrainOverlay();
-private:
-	TerrainOverlay(){} // private default ctor (must be subclassed)
-
 protected:
 	/**
 	 * Construct the object and register it with the global
@@ -128,20 +152,9 @@ protected:
 	 */
 	void RenderTileOutline(const CColor& colour, int line_width, bool draw_hidden, ssize_t i, ssize_t j);
 	
-public:
-	/**
-	 * Draw all TerrainOverlay objects that exist.
-	 */
-	static void RenderOverlays();
-
 private:
-	/// Copying not allowed.
-	TerrainOverlay(const TerrainOverlay&);
-
-	friend struct render1st;
-
 	// Process all tiles
-	void Render();
+	virtual void RenderBeforeWater();
 
 	// Temporary storage of tile coordinates, so ProcessTile doesn't need to
 	// pass it to RenderTile/etc (and doesn't have a chance to get it wrong)
@@ -150,6 +163,35 @@ private:
 	CTerrain* m_Terrain;
 };
 
+/**
+ * Base class for texture-based terrain overlays, with an arbitrary number of
+ * texels per terrain tile, intended for debugging purposes.
+ * Subclasses must implement BuildTextureRGBA which will be called each frame.
+ */
+class TerrainTextureOverlay : public ITerrainOverlay
+{
+	NONCOPYABLE(TerrainTextureOverlay);
+public:
+	TerrainTextureOverlay(float texelsPerTile, int priority = 100);
+
+	virtual ~TerrainTextureOverlay();
+
+protected:
+	/**
+	 * Called each frame to generate the texture to render on the terrain.
+	 * @p data is w*h*4 bytes, where w and h are the terrain size multiplied
+	 * by texelsPerTile. @p data defaults to fully transparent, and should
+	 * be filled with data in RGBA order.
+	 */
+	virtual void BuildTextureRGBA(u8* data, size_t w, size_t h) = 0;
+
+private:
+	void RenderAfterWater();
+
+	float m_TexelsPerTile;
+	GLuint m_Texture;
+	GLsizei m_TextureW, m_TextureH;
+};
 
 /* Example usage:
 
diff --git a/source/renderer/TerrainRenderer.cpp b/source/renderer/TerrainRenderer.cpp
index 18703cd..f084794 100644
--- a/source/renderer/TerrainRenderer.cpp
+++ b/source/renderer/TerrainRenderer.cpp
@@ -243,7 +243,7 @@ void TerrainRenderer::RenderTerrain(bool filtered)
 
 	// switch on blending
 	glEnable(GL_BLEND);
-	glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA);
+	glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
 
 	// no need to write to the depth buffer a second time
 	glDepthMask(0);
@@ -372,6 +372,63 @@ void TerrainRenderer::RenderTerrain(bool filtered)
 }
 
 
+void TerrainRenderer::RenderTerrainOverlayTexture(CMatrix3D& textureMatrix)
+{
+	ENSURE(m->phase == Phase_Render);
+
+	std::vector<CPatchRData*>& visiblePatches = m->visiblePatches;
+
+	glEnableClientState(GL_VERTEX_ARRAY);
+	glEnableClientState(GL_TEXTURE_COORD_ARRAY);
+
+	glEnable(GL_TEXTURE_2D);
+	glEnable(GL_BLEND);
+	glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+	glDepthMask(0);
+
+	glTexEnvi(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_REPLACE);
+
+	glMatrixMode(GL_TEXTURE);
+	glLoadMatrixf(&textureMatrix._11);
+	glMatrixMode(GL_MODELVIEW);
+
+	CPatchRData::RenderStreams(visiblePatches, STREAM_POS|STREAM_POSTOUV0);
+
+	// To make the overlay visible over water, render an additional map-sized
+	// water-height patch
+	// TODO: should we use CPatchRData::RenderWater instead?
+	CBoundingBoxAligned waterBounds;
+	for (size_t i = 0; i < m->visiblePatches.size(); ++i)
+	{
+		CPatchRData* data = m->visiblePatches[i];
+		waterBounds += data->GetWaterBounds();
+	}
+	if (!waterBounds.IsEmpty())
+	{
+		float h = g_Renderer.GetWaterManager()->m_WaterHeight + 0.05f; // add a delta to avoid z-fighting
+		float waterPos[] = {
+			waterBounds[0].X, h, waterBounds[0].Z,
+			waterBounds[1].X, h, waterBounds[0].Z,
+			waterBounds[0].X, h, waterBounds[1].Z,
+			waterBounds[1].X, h, waterBounds[1].Z
+		};
+		glVertexPointer(3, GL_FLOAT, 3*sizeof(float), waterPos);
+		glTexCoordPointer(3, GL_FLOAT, 3*sizeof(float), waterPos);
+		glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);
+	}
+
+	glMatrixMode(GL_TEXTURE);
+	glLoadIdentity();
+	glMatrixMode(GL_MODELVIEW);
+
+	glDepthMask(1);
+	glDisable(GL_BLEND);
+	glDisableClientState(GL_COLOR_ARRAY);
+	glDisableClientState(GL_VERTEX_ARRAY);
+	glDisableClientState(GL_TEXTURE_COORD_ARRAY);
+}
+
+
 ///////////////////////////////////////////////////////////////////
 
 /**
diff --git a/source/renderer/TerrainRenderer.h b/source/renderer/TerrainRenderer.h
index 48bb53e..34ae7a5 100644
--- a/source/renderer/TerrainRenderer.h
+++ b/source/renderer/TerrainRenderer.h
@@ -137,6 +137,13 @@ public:
 	 */
 	void RenderPriorities();
 
+	/**
+	 * Render texture unit 0 over the terrain mesh, with UV coords calculated
+	 * by the given texture matrix.
+	 * Intended for use by TerrainTextureOverlay.
+	 */
+	void RenderTerrainOverlayTexture(CMatrix3D& textureMatrix);
+
 private:
 	TerrainRendererInternals* m;
 
diff --git a/source/simulation2/components/CCmpObstructionManager.cpp b/source/simulation2/components/CCmpObstructionManager.cpp
index e1a8d6a..4e3dafa 100644
--- a/source/simulation2/components/CCmpObstructionManager.cpp
+++ b/source/simulation2/components/CCmpObstructionManager.cpp
@@ -1,4 +1,4 @@
-/* Copyright (C) 2011 Wildfire Games.
+/* Copyright (C) 2012 Wildfire Games.
  * This file is part of 0 A.D.
  *
  * 0 A.D. is free software: you can redistribute it and/or modify
@@ -418,7 +418,8 @@ public:
 	virtual bool TestStaticShape(const IObstructionTestFilter& filter, entity_pos_t x, entity_pos_t z, entity_pos_t a, entity_pos_t w, entity_pos_t h, std::vector<entity_id_t>* out);
 	virtual bool TestUnitShape(const IObstructionTestFilter& filter, entity_pos_t x, entity_pos_t z, entity_pos_t r, std::vector<entity_id_t>* out);
 
-	virtual bool Rasterise(Grid<u8>& grid);
+	virtual void Rasterise(Grid<u16>& grid, entity_pos_t expand, ICmpObstructionManager::flags_t requireMask, u8 setMask);
+	virtual bool RasteriseOld(Grid<u8>& grid);
 	virtual void GetObstructionsInRange(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, std::vector<ObstructionSquare>& squares);
 	virtual bool FindMostImportantObstruction(const IObstructionTestFilter& filter, entity_pos_t x, entity_pos_t z, entity_pos_t r, ObstructionSquare& square);
 
@@ -497,6 +498,16 @@ private:
 		return grid.m_DirtyID < m_DirtyID;
 	}
 
+	virtual bool NeedUpdate(size_t* dirtyID)
+	{
+		if (*dirtyID != m_DirtyID)
+		{
+			*dirtyID = m_DirtyID;
+			return true;
+		}
+		return false;
+	}
+
 	/**
 	 * Return whether the given point is within the world bounds by at least r
 	 */
@@ -686,24 +697,77 @@ bool CCmpObstructionManager::TestUnitShape(const IObstructionTestFilter& filter,
 }
 
 /**
- * Compute the tile indexes on the grid nearest to a given point
+ * Compute the navcell indexes on the grid nearest to a given point
  */
-static void NearestTile(entity_pos_t x, entity_pos_t z, u16& i, u16& j, u16 w, u16 h)
+static void NearestNavcell(entity_pos_t x, entity_pos_t z, u16& i, u16& j, u16 w, u16 h)
 {
-	i = (u16)clamp((x / (int)TERRAIN_TILE_SIZE).ToInt_RoundToZero(), 0, w-1);
-	j = (u16)clamp((z / (int)TERRAIN_TILE_SIZE).ToInt_RoundToZero(), 0, h-1);
+	i = (u16)clamp((x / ICmpObstructionManager::NAVCELL_SIZE).ToInt_RoundToZero(), 0, w-1);
+	j = (u16)clamp((z / ICmpObstructionManager::NAVCELL_SIZE).ToInt_RoundToZero(), 0, h-1);
 }
 
 /**
- * Returns the position of the center of the given tile
+ * Returns the position of the center of the given navcell
  */
-static void TileCenter(u16 i, u16 j, entity_pos_t& x, entity_pos_t& z)
+static void NavcellCenter(u16 i, u16 j, entity_pos_t& x, entity_pos_t& z)
+{
+	x = entity_pos_t::FromInt(i*2 + 1).Multiply(ICmpObstructionManager::NAVCELL_SIZE / 2);
+	z = entity_pos_t::FromInt(j*2 + 1).Multiply(ICmpObstructionManager::NAVCELL_SIZE / 2);
+}
+
+void CCmpObstructionManager::Rasterise(Grid<u16>& grid, entity_pos_t expand, ICmpObstructionManager::flags_t requireMask, u8 setMask)
 {
-	x = entity_pos_t::FromInt(i*(int)TERRAIN_TILE_SIZE + (int)TERRAIN_TILE_SIZE/2);
-	z = entity_pos_t::FromInt(j*(int)TERRAIN_TILE_SIZE + (int)TERRAIN_TILE_SIZE/2);
+	PROFILE("Rasterise");
+
+	// Cells are only marked as blocked if the whole cell is strictly inside the shape.
+	// (That ensures the shape's geometric border is always reachable.)
+
+	for (std::map<u32, StaticShape>::iterator it = m_StaticShapes.begin(); it != m_StaticShapes.end(); ++it)
+	{
+		CFixedVector2D center(it->second.x, it->second.z);
+
+		if (it->second.flags & requireMask)
+		{
+			CFixedVector2D halfSize(it->second.hw + expand, it->second.hh + expand);
+			CFixedVector2D halfBound = Geometry::GetHalfBoundingBox(it->second.u, it->second.v, halfSize);
+
+			u16 i0, j0, i1, j1;
+			NearestNavcell(center.X - halfBound.X, center.Y - halfBound.Y, i0, j0, grid.m_W, grid.m_H);
+			NearestNavcell(center.X + halfBound.X, center.Y + halfBound.Y, i1, j1, grid.m_W, grid.m_H);
+			for (u16 j = j0; j <= j1; ++j)
+			{
+				for (u16 i = i0; i <= i1; ++i)
+				{
+					entity_pos_t x, z;
+					NavcellCenter(i, j, x, z);
+					if (Geometry::PointIsInSquare(CFixedVector2D(x - NAVCELL_SIZE/2, z - NAVCELL_SIZE/2) - center, it->second.u, it->second.v, halfSize) &&
+					    Geometry::PointIsInSquare(CFixedVector2D(x + NAVCELL_SIZE/2, z - NAVCELL_SIZE/2) - center, it->second.u, it->second.v, halfSize) &&
+					    Geometry::PointIsInSquare(CFixedVector2D(x + NAVCELL_SIZE/2, z + NAVCELL_SIZE/2) - center, it->second.u, it->second.v, halfSize) &&
+					    Geometry::PointIsInSquare(CFixedVector2D(x - NAVCELL_SIZE/2, z + NAVCELL_SIZE/2) - center, it->second.u, it->second.v, halfSize))
+						grid.set(i, j, grid.get(i, j) | setMask);
+				}
+			}
+		}
+	}
+
+	for (std::map<u32, UnitShape>::iterator it = m_UnitShapes.begin(); it != m_UnitShapes.end(); ++it)
+	{
+		CFixedVector2D center(it->second.x, it->second.z);
+
+		if (it->second.flags & requireMask)
+		{
+			entity_pos_t r = it->second.r + expand;
+
+			u16 i0, j0, i1, j1;
+ 			NearestNavcell(center.X - r, center.Y - r, i0, j0, grid.m_W, grid.m_H);
+ 			NearestNavcell(center.X + r, center.Y + r, i1, j1, grid.m_W, grid.m_H);
+			for (u16 j = j0+1; j < j1; ++j)
+				for (u16 i = i0+1; i < i1; ++i)
+					grid.set(i, j, grid.get(i, j) | setMask);
+		}
+	}
 }
 
-bool CCmpObstructionManager::Rasterise(Grid<u8>& grid)
+bool CCmpObstructionManager::RasteriseOld(Grid<u8>& grid)
 {
 	if (!IsDirty(grid))
 		return false;
@@ -743,14 +807,14 @@ bool CCmpObstructionManager::Rasterise(Grid<u8>& grid)
 			CFixedVector2D halfBound = Geometry::GetHalfBoundingBox(it->second.u, it->second.v, halfSize);
 
 			u16 i0, j0, i1, j1;
-			NearestTile(center.X - halfBound.X, center.Y - halfBound.Y, i0, j0, grid.m_W, grid.m_H);
-			NearestTile(center.X + halfBound.X, center.Y + halfBound.Y, i1, j1, grid.m_W, grid.m_H);
+			NearestNavcell(center.X - halfBound.X, center.Y - halfBound.Y, i0, j0, grid.m_W, grid.m_H);
+			NearestNavcell(center.X + halfBound.X, center.Y + halfBound.Y, i1, j1, grid.m_W, grid.m_H);
 			for (u16 j = j0; j <= j1; ++j)
 			{
 				for (u16 i = i0; i <= i1; ++i)
 				{
 					entity_pos_t x, z;
-					TileCenter(i, j, x, z);
+					NavcellCenter(i, j, x, z);
 					if (Geometry::PointIsInSquare(CFixedVector2D(x, z) - center, it->second.u, it->second.v, halfSize))
 						grid.set(i, j, grid.get(i, j) | TILE_OBSTRUCTED_PATHFINDING);
 				}
@@ -763,14 +827,14 @@ bool CCmpObstructionManager::Rasterise(Grid<u8>& grid)
 			CFixedVector2D halfBound = Geometry::GetHalfBoundingBox(it->second.u, it->second.v, halfSize);
 
 			u16 i0, j0, i1, j1;
-			NearestTile(center.X - halfBound.X, center.Y - halfBound.Y, i0, j0, grid.m_W, grid.m_H);
-			NearestTile(center.X + halfBound.X, center.Y + halfBound.Y, i1, j1, grid.m_W, grid.m_H);
+			NearestNavcell(center.X - halfBound.X, center.Y - halfBound.Y, i0, j0, grid.m_W, grid.m_H);
+			NearestNavcell(center.X + halfBound.X, center.Y + halfBound.Y, i1, j1, grid.m_W, grid.m_H);
 			for (u16 j = j0; j <= j1; ++j)
 			{
 				for (u16 i = i0; i <= i1; ++i)
 				{
 					entity_pos_t x, z;
-					TileCenter(i, j, x, z);
+					NavcellCenter(i, j, x, z);
 					if (Geometry::PointIsInSquare(CFixedVector2D(x, z) - center, it->second.u, it->second.v, halfSize))
 						grid.set(i, j, grid.get(i, j) | TILE_OBSTRUCTED_FOUNDATION);
 				}
@@ -787,8 +851,8 @@ bool CCmpObstructionManager::Rasterise(Grid<u8>& grid)
 			entity_pos_t r = it->second.r + expandPathfinding;
 
 			u16 i0, j0, i1, j1;
-			NearestTile(center.X - r, center.Y - r, i0, j0, grid.m_W, grid.m_H);
-			NearestTile(center.X + r, center.Y + r, i1, j1, grid.m_W, grid.m_H);
+			NearestNavcell(center.X - r, center.Y - r, i0, j0, grid.m_W, grid.m_H);
+			NearestNavcell(center.X + r, center.Y + r, i1, j1, grid.m_W, grid.m_H);
 			for (u16 j = j0; j <= j1; ++j)
 				for (u16 i = i0; i <= i1; ++i)
 					grid.set(i, j, grid.get(i, j) | TILE_OBSTRUCTED_PATHFINDING);
@@ -799,8 +863,8 @@ bool CCmpObstructionManager::Rasterise(Grid<u8>& grid)
 			entity_pos_t r = it->second.r + expandFoundation;
 
 			u16 i0, j0, i1, j1;
-			NearestTile(center.X - r, center.Y - r, i0, j0, grid.m_W, grid.m_H);
-			NearestTile(center.X + r, center.Y + r, i1, j1, grid.m_W, grid.m_H);
+			NearestNavcell(center.X - r, center.Y - r, i0, j0, grid.m_W, grid.m_H);
+			NearestNavcell(center.X + r, center.Y + r, i1, j1, grid.m_W, grid.m_H);
 			for (u16 j = j0; j <= j1; ++j)
 				for (u16 i = i0; i <= i1; ++i)
 					grid.set(i, j, grid.get(i, j) | TILE_OBSTRUCTED_FOUNDATION);
@@ -810,7 +874,7 @@ bool CCmpObstructionManager::Rasterise(Grid<u8>& grid)
 	// Any tiles outside or very near the edge of the map are impassable
 
 	// WARNING: CCmpRangeManager::LosIsOffWorld needs to be kept in sync with this
-	const u16 edgeSize = 3; // number of tiles around the edge that will be off-world
+	const u16 edgeSize = 3 * ICmpObstructionManager::NAVCELLS_PER_TILE; // number of tiles around the edge that will be off-world
 
 	u8 edgeFlags = TILE_OBSTRUCTED_PATHFINDING | TILE_OBSTRUCTED_FOUNDATION | TILE_OUTOFBOUNDS;
 
@@ -837,8 +901,8 @@ bool CCmpObstructionManager::Rasterise(Grid<u8>& grid)
 	else
 	{
 		u16 i0, j0, i1, j1;
-		NearestTile(m_WorldX0, m_WorldZ0, i0, j0, grid.m_W, grid.m_H);
-		NearestTile(m_WorldX1, m_WorldZ1, i1, j1, grid.m_W, grid.m_H);
+		NearestNavcell(m_WorldX0, m_WorldZ0, i0, j0, grid.m_W, grid.m_H);
+		NearestNavcell(m_WorldX1, m_WorldZ1, i1, j1, grid.m_W, grid.m_H);
 
 		for (u16 j = 0; j < grid.m_H; ++j)
 			for (u16 i = 0; i < i0+edgeSize; ++i)
diff --git a/source/simulation2/components/CCmpPathfinder.cpp b/source/simulation2/components/CCmpPathfinder.cpp
index 4ebdf08..9ea08e7 100644
--- a/source/simulation2/components/CCmpPathfinder.cpp
+++ b/source/simulation2/components/CCmpPathfinder.cpp
@@ -1,4 +1,4 @@
-/* Copyright (C) 2011 Wildfire Games.
+/* Copyright (C) 2012 Wildfire Games.
  * This file is part of 0 A.D.
  *
  * 0 A.D. is free software: you can redistribute it and/or modify
@@ -47,13 +47,19 @@ void CCmpPathfinder::Init(const CParamNode& UNUSED(paramNode))
 	m_MapSize = 0;
 	m_Grid = NULL;
 	m_ObstructionGrid = NULL;
+	m_ObstructionGridDirtyID = 0;
 	m_TerrainDirty = true;
 	m_NextAsyncTicket = 1;
 
 	m_DebugOverlay = NULL;
 	m_DebugGrid = NULL;
+	m_DebugGridJPS = NULL;
 	m_DebugPath = NULL;
 
+	PathfinderHierInit();
+
+//	SetDebugOverlay(true); // XXX
+
 	m_SameTurnMovesCount = 0;
 
 	// Since this is used as a system component (not loaded from an entity template),
@@ -152,6 +158,8 @@ void CCmpPathfinder::Deinit()
 	SetDebugOverlay(false); // cleans up memory
 	ResetDebugPath();
 
+	PathfinderHierDeinit();
+
 	delete m_Grid;
 	delete m_ObstructionGrid;
 }
@@ -235,6 +243,8 @@ void CCmpPathfinder::RenderSubmit(SceneCollector& collector)
 {
 	for (size_t i = 0; i < m_DebugOverlayShortPathLines.size(); ++i)
 		collector.Submit(&m_DebugOverlayShortPathLines[i]);
+
+	PathfinderHierRenderSubmit(collector);
 }
 
 
@@ -322,14 +332,16 @@ void CCmpPathfinder::UpdateGrid()
 	if (!m_Grid)
 	{
 		m_MapSize = cmpTerrain->GetTilesPerSide();
-		m_Grid = new Grid<TerrainTile>(m_MapSize, m_MapSize);
-		m_ObstructionGrid = new Grid<u8>(m_MapSize, m_MapSize);
+		m_Grid = new Grid<TerrainTile>(m_MapSize * ICmpObstructionManager::NAVCELLS_PER_TILE, m_MapSize * ICmpObstructionManager::NAVCELLS_PER_TILE);
+		m_ObstructionGrid = new Grid<u8>(m_MapSize * ICmpObstructionManager::NAVCELLS_PER_TILE, m_MapSize * ICmpObstructionManager::NAVCELLS_PER_TILE);
+		m_ObstructionGridDirtyID = 0;
 	}
 
 	CmpPtr<ICmpObstructionManager> cmpObstructionManager(GetSimContext(), SYSTEM_ENTITY);
 
-	bool obstructionsDirty = cmpObstructionManager->Rasterise(*m_ObstructionGrid);
+	bool obstructionsDirty = cmpObstructionManager->NeedUpdate(&m_ObstructionGridDirtyID);
 
+#if 0
 	if (obstructionsDirty && !m_TerrainDirty)
 	{
 		PROFILE("UpdateGrid obstructions");
@@ -343,11 +355,11 @@ void CCmpPathfinder::UpdateGrid()
 		// then TILE_OUTOFBOUNDS will change and we can't use this fast path, but
 		// currently it'll just set obstructionsDirty and we won't notice
 
-		for (u16 j = 0; j < m_MapSize; ++j)
+		for (u16 j = 0; j < m_MapSize * ICmpObstructionManager::NAVCELLS_PER_TILE; ++j)
 		{
-			for (u16 i = 0; i < m_MapSize; ++i)
+			for (u16 i = 0; i < m_MapSize * ICmpObstructionManager::NAVCELLS_PER_TILE; ++i)
 			{
-				TerrainTile& t = m_Grid->get(i, j);
+				TerrainTile& t = m_Grid->get(i/ICmpObstructionManager::NAVCELLS_PER_TILE, j/ICmpObstructionManager::NAVCELLS_PER_TILE);
 
 				u8 obstruct = m_ObstructionGrid->get(i, j);
 
@@ -365,7 +377,9 @@ void CCmpPathfinder::UpdateGrid()
 
 		++m_Grid->m_DirtyID;
 	}
-	else if (obstructionsDirty || m_TerrainDirty)
+	else
+#endif
+	if (obstructionsDirty || m_TerrainDirty)
 	{
 		PROFILE("UpdateGrid full");
 
@@ -479,17 +493,18 @@ void CCmpPathfinder::UpdateGrid()
 			}
 		}
 
-		// Apply passability classes to terrain
-		for (u16 j = 0; j < m_MapSize; ++j)
+		// Compute initial terrain-dependent passability
+		// TODO: expand by unit radius
+		for (u16 j = 0; j < m_MapSize * ICmpObstructionManager::NAVCELLS_PER_TILE; ++j)
 		{
-			for (u16 i = 0; i < m_MapSize; ++i)
+			for (u16 i = 0; i < m_MapSize * ICmpObstructionManager::NAVCELLS_PER_TILE; ++i)
 			{
 				fixed x, z;
-				TileCenter(i, j, x, z);
+				NavcellCenter(i, j, x, z);
 
 				TerrainTile t = 0;
 
-				u8 obstruct = m_ObstructionGrid->get(i, j);
+// 				u8 obstruct = m_ObstructionGrid->get(i, j);
 
 				fixed height = terrain.GetExactGroundLevelFixed(x, z);
 
@@ -499,23 +514,24 @@ void CCmpPathfinder::UpdateGrid()
 
 				fixed depth = water - height;
 
-				fixed slope = terrain.GetSlopeFixed(i, j);
-
-				fixed shoredist = fixed::FromInt(shoreGrid.get(i, j));
-
-				if (obstruct & ICmpObstructionManager::TILE_OBSTRUCTED_PATHFINDING)
-					t |= 1;
-
-				if (obstruct & ICmpObstructionManager::TILE_OBSTRUCTED_FOUNDATION)
-					t |= 2;
-
-				if (obstruct & ICmpObstructionManager::TILE_OUTOFBOUNDS)
-				{
-					// If out of bounds, nobody is allowed to pass
-					for (size_t n = 0; n < m_PassClasses.size(); ++n)
-						t |= m_PassClasses[n].m_Mask;
-				}
-				else
+// 				fixed slope = terrain.GetExactSlopeFixed(x, z);
+				fixed slope = terrain.GetSlopeFixed(i / ICmpObstructionManager::NAVCELLS_PER_TILE, j / ICmpObstructionManager::NAVCELLS_PER_TILE);
+
+				fixed shoredist = fixed::FromInt(shoreGrid.get(i / ICmpObstructionManager::NAVCELLS_PER_TILE, j / ICmpObstructionManager::NAVCELLS_PER_TILE));
+
+// 				if (obstruct & ICmpObstructionManager::TILE_OBSTRUCTED_PATHFINDING)
+// 					t |= 1;
+// 
+// 				if (obstruct & ICmpObstructionManager::TILE_OBSTRUCTED_FOUNDATION)
+// 					t |= 2;
+// 
+// 				if (obstruct & ICmpObstructionManager::TILE_OUTOFBOUNDS)
+// 				{
+// 					// If out of bounds, nobody is allowed to pass
+// 					for (size_t n = 0; n < m_PassClasses.size(); ++n)
+// 						t |= m_PassClasses[n].m_Mask;
+// 				}
+// 				else
 				{
 					for (size_t n = 0; n < m_PassClasses.size(); ++n)
 					{
@@ -524,7 +540,7 @@ void CCmpPathfinder::UpdateGrid()
 					}
 				}
 
-				std::string moveClass = terrain.GetMovementClass(i, j);
+				std::string moveClass = terrain.GetMovementClass(i / ICmpObstructionManager::NAVCELLS_PER_TILE, j / ICmpObstructionManager::NAVCELLS_PER_TILE);
 				if (m_TerrainCostClassTags.find(moveClass) != m_TerrainCostClassTags.end())
 					t |= COST_CLASS_MASK(m_TerrainCostClassTags[moveClass]);
 
@@ -532,9 +548,47 @@ void CCmpPathfinder::UpdateGrid()
 			}
 		}
 
+		for (size_t n = 0; n < m_PassClasses.size(); ++n)
+		{
+			if (m_PassClasses[n].m_HasClearance)
+				cmpObstructionManager->Rasterise(*m_Grid, m_PassClasses[n].m_Clearance, ICmpObstructionManager::FLAG_BLOCK_PATHFINDING, m_PassClasses[n].m_Mask);
+		}
+
+		if (1) // XXX: if circular
+		{
+			// WARNING: CCmpRangeManager::LosIsOffWorld needs to be kept in sync with this
+			const u16 edgeSize = 3 * ICmpObstructionManager::NAVCELLS_PER_TILE; // number of tiles around the edge that will be off-world
+
+			TerrainTile edgeMask = 0;
+			for (size_t n = 0; n < m_PassClasses.size(); ++n)
+				edgeMask |= m_PassClasses[n].m_Mask;
+
+			u16 w = m_Grid->m_W;
+			u16 h = m_Grid->m_H;
+			for (u16 j = 0; j < h; ++j)
+			{
+				for (u16 i = 0; i < w; ++i)
+				{
+					// Based on CCmpRangeManager::LosIsOffWorld
+					// but tweaked since it's tile-based instead.
+					// (We double all the values so we can handle half-tile coordinates.)
+					// This needs to be slightly tighter than the LOS circle,
+					// else units might get themselves lost in the SoD around the edge.
+
+					ssize_t dist2 = (i*2 + 1 - w)*(i*2 + 1 - w)
+						+ (j*2 + 1 - h)*(j*2 + 1 - h);
+
+					if (dist2 >= (w - 2*edgeSize) * (h - 2*edgeSize))
+						m_Grid->set(i, j, m_Grid->get(i, j) | edgeMask);
+				}
+			}
+		}
+
 		m_TerrainDirty = false;
 
 		++m_Grid->m_DirtyID;
+
+		PathfinderHierReload();
 	}
 }
 
diff --git a/source/simulation2/components/CCmpPathfinder_Common.h b/source/simulation2/components/CCmpPathfinder_Common.h
index f025ab9..964127f 100644
--- a/source/simulation2/components/CCmpPathfinder_Common.h
+++ b/source/simulation2/components/CCmpPathfinder_Common.h
@@ -34,12 +34,16 @@
 #include "graphics/Overlay.h"
 #include "graphics/Terrain.h"
 #include "maths/MathUtil.h"
+#include "simulation2/components/ICmpObstructionManager.h"
 #include "simulation2/helpers/Geometry.h"
 #include "simulation2/helpers/Grid.h"
 
 class PathfinderOverlay;
 class SceneCollector;
 struct PathfindTile;
+struct PathfindTileJPS;
+
+class CCmpPathfinder_Hier;
 
 #ifdef NDEBUG
 #define PATHFIND_DEBUG 0
@@ -107,6 +111,16 @@ public:
 		else
 			m_MaxShore = std::numeric_limits<fixed>::max();
 
+		if (node.GetChild("Clearance").IsOk())
+		{
+			m_HasClearance = true;
+			m_Clearance = node.GetChild("Clearance").ToFixed();
+		}
+		else
+		{
+			m_HasClearance = false;
+			m_Clearance = fixed::Zero();
+		}
 	}
 
 	bool IsPassable(fixed waterdepth, fixed steepness, fixed shoredist)
@@ -115,6 +129,10 @@ public:
 	}
 
 	ICmpPathfinder::pass_class_t m_Mask;
+
+	bool m_HasClearance; // whether static obstructions are impassable
+	fixed m_Clearance; // min distance from static obstructions
+
 private:
 	fixed m_MinDepth;
 	fixed m_MaxDepth;
@@ -137,6 +155,84 @@ const int COST_CLASS_BITS = 16 - (PASS_CLASS_BITS + 2);
 #define COST_CLASS_MASK(id) ( (TerrainTile) ((id) << (PASS_CLASS_BITS + 2)) )
 
 typedef SparseGrid<PathfindTile> PathfindTileGrid;
+typedef SparseGrid<PathfindTileJPS> PathfindTileJPSGrid;
+
+/**
+ * Represents the 2D coordinates of a tile.
+ * The i/j components are packed into a single u32, since we usually use these
+ * objects for comparisons and the VC2010 optimiser doesn't seem to automatically
+ * compare two u16s in a single operation.
+ */
+struct TileID
+{
+	TileID() { }
+
+	TileID(u16 i, u16 j) : data((i << 16) | j) { }
+
+	bool operator==(const TileID& b) const
+	{
+		return data == b.data;
+	}
+
+	/// Returns lexicographic order over (i,j)
+	bool operator<(const TileID& b) const
+	{
+		return data < b.data;
+	}
+
+	u16 i() const { return data >> 16; }
+	u16 j() const { return data & 0xFFFF; }
+
+private:
+	u32 data;
+};
+
+
+/**
+ * Represents the cost of a path consisting of horizontal/vertical and
+ * diagonal movements over a uniform-cost grid.
+ * Maximum path length before overflow is about 45K steps.
+ */
+struct PathCost
+{
+	PathCost() : data(0) { }
+
+	PathCost(u16 hv, u16 d) : data(hv*65536 + d*92682) { }
+
+	/// Construct for horizontal/vertical movement of given number of steps
+	static PathCost horizvert(u16 n)
+	{
+		return PathCost(n, 0);
+	}
+
+	/// Construct for diagonal movement of given number of steps
+	static PathCost diag(u16 n)
+	{
+		return PathCost(0, n);
+	}
+
+	PathCost operator+(const PathCost& a) const
+	{
+		PathCost c;
+		c.data = data + a.data;
+		return c;
+	}
+
+	bool operator<=(const PathCost& b) const { return data <= b.data; }
+	bool operator< (const PathCost& b) const { return data <  b.data; }
+	bool operator>=(const PathCost& b) const { return data >= b.data; }
+	bool operator> (const PathCost& b) const { return data >  b.data; }
+
+	u32 ToInt()
+	{
+		return data;
+	}
+
+private:
+	u32 data;
+};
+
+
 
 struct AsyncLongPathRequest
 {
@@ -201,8 +297,16 @@ public:
 	u16 m_MapSize; // tiles per side
 	Grid<TerrainTile>* m_Grid; // terrain/passability information
 	Grid<u8>* m_ObstructionGrid; // cached obstruction information (TODO: we shouldn't bother storing this, it's redundant with LSBs of m_Grid)
+	size_t m_ObstructionGridDirtyID;
 	bool m_TerrainDirty; // indicates if m_Grid has been updated since terrain changed
 	
+	CCmpPathfinder_Hier* m_PathfinderHier;
+	void PathfinderHierInit();
+	void PathfinderHierDeinit();
+	void PathfinderHierReload();
+	void PathfinderHierRenderSubmit(SceneCollector& collector);
+	bool PathfinderHierMakeGoalReachable(u16 i0, u16 j0, ICmpPathfinder::Goal& goal, pass_class_t passClass);
+
 	// For responsiveness we will process some moves in the same turn they were generated in
 	
 	u16 m_MaxSameTurnMoves; // max number of moves that can be created and processed in the same turn
@@ -210,7 +314,10 @@ public:
 	// Debugging - output from last pathfind operation:
 
 	PathfindTileGrid* m_DebugGrid;
+	PathfindTileJPSGrid* m_DebugGridJPS;
 	u32 m_DebugSteps;
+	double m_DebugTime;
+	Goal m_DebugGoal;
 	Path* m_DebugPath;
 	PathfinderOverlay* m_DebugOverlay;
 	pass_class_t m_DebugPassClass;
@@ -242,6 +349,8 @@ public:
 
 	virtual void ComputePath(entity_pos_t x0, entity_pos_t z0, const Goal& goal, pass_class_t passClass, cost_class_t costClass, Path& ret);
 
+	virtual void ComputePathJPS(entity_pos_t x0, entity_pos_t z0, const Goal& goal, pass_class_t passClass, cost_class_t costClass, Path& ret);
+
 	virtual u32 ComputePathAsync(entity_pos_t x0, entity_pos_t z0, const Goal& goal, pass_class_t passClass, cost_class_t costClass, entity_id_t notify);
 
 	virtual void ComputeShortPath(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t r, entity_pos_t range, const Goal& goal, pass_class_t passClass, Path& ret);
@@ -254,6 +363,10 @@ public:
 
 	virtual void SetDebugOverlay(bool enabled);
 
+	virtual void GetDebugData(u32& steps, double& time, Grid<u8>& grid);
+
+	virtual void GetDebugDataJPS(u32& steps, double& time, Grid<u8>& grid);
+
 	virtual fixed GetMovementSpeed(entity_pos_t x0, entity_pos_t z0, cost_class_t costClass);
 
 	virtual CFixedVector2D GetNearestPointOnGoal(CFixedVector2D pos, const Goal& goal);
@@ -277,8 +390,14 @@ public:
 	 */
 	void NearestTile(entity_pos_t x, entity_pos_t z, u16& i, u16& j)
 	{
-		i = (u16)clamp((x / (int)TERRAIN_TILE_SIZE).ToInt_RoundToZero(), 0, m_MapSize-1);
-		j = (u16)clamp((z / (int)TERRAIN_TILE_SIZE).ToInt_RoundToZero(), 0, m_MapSize-1);
+		i = (u16)clamp((x / (int)TERRAIN_TILE_SIZE).ToInt_RoundToNegInfinity(), 0, m_MapSize-1);
+		j = (u16)clamp((z / (int)TERRAIN_TILE_SIZE).ToInt_RoundToNegInfinity(), 0, m_MapSize-1);
+	}
+
+	void NearestNavcell(entity_pos_t x, entity_pos_t z, u16& i, u16& j)
+	{
+		i = (u16)clamp((x / ICmpObstructionManager::NAVCELL_SIZE).ToInt_RoundToNegInfinity(), 0, m_MapSize*ICmpObstructionManager::NAVCELLS_PER_TILE - 1);
+		j = (u16)clamp((z / ICmpObstructionManager::NAVCELL_SIZE).ToInt_RoundToNegInfinity(), 0, m_MapSize*ICmpObstructionManager::NAVCELLS_PER_TILE - 1);
 	}
 
 	/**
@@ -290,6 +409,12 @@ public:
 		z = entity_pos_t::FromInt(j*(int)TERRAIN_TILE_SIZE + (int)TERRAIN_TILE_SIZE/2);
 	}
 
+	static void NavcellCenter(u16 i, u16 j, entity_pos_t& x, entity_pos_t& z)
+	{
+		x = entity_pos_t::FromInt(i*2 + 1).Multiply(ICmpObstructionManager::NAVCELL_SIZE / 2);
+		z = entity_pos_t::FromInt(j*2 + 1).Multiply(ICmpObstructionManager::NAVCELL_SIZE / 2);
+	}
+
 	static fixed DistanceToGoal(CFixedVector2D pos, const CCmpPathfinder::Goal& goal);
 
 	/**
diff --git a/source/simulation2/components/CCmpPathfinder_Hier.cpp b/source/simulation2/components/CCmpPathfinder_Hier.cpp
new file mode 100644
index 0000000..5f8c471
--- /dev/null
+++ b/source/simulation2/components/CCmpPathfinder_Hier.cpp
@@ -0,0 +1,600 @@
+/* Copyright (C) 2012 Wildfire Games.
+ * This file is part of 0 A.D.
+ *
+ * 0 A.D. 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 2 of the License, or
+ * (at your option) any later version.
+ *
+ * 0 A.D. 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 0 A.D.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "precompiled.h"
+
+#include "CCmpPathfinder_Common.h"
+
+#include "renderer/Scene.h"
+#include "renderer/TerrainOverlay.h"
+#include "simulation2/helpers/Render.h"
+
+class PathfinderHierOverlay;
+
+class CCmpPathfinder_Hier
+{
+	typedef ICmpPathfinder::pass_class_t pass_class_t;
+
+	NONCOPYABLE(CCmpPathfinder_Hier);
+
+public:
+	struct RegionID
+	{
+		u8 ci, cj; // chunk ID
+		u16 r; // unique-per-chunk local region ID
+
+		RegionID(u8 ci, u8 cj, u16 r) : ci(ci), cj(cj), r(r) { }
+
+		bool operator<(RegionID b) const
+		{
+			if (ci < b.ci)
+				return true;
+			if (b.ci < ci)
+				return false;
+			if (cj < b.cj)
+				return true;
+			if (b.cj < cj)
+				return false;
+			return r < b.r;
+		}
+	};
+
+	CCmpPathfinder_Hier(CCmpPathfinder& pathfinder);
+	~CCmpPathfinder_Hier();
+
+	void Init(const std::vector<PathfinderPassability>& passClasses, Grid<TerrainTile>* grid);
+
+	RegionID Get(u16 i, u16 j, pass_class_t passClass);
+
+	bool MakeGoalReachable(u16 i0, u16 j0, ICmpPathfinder::Goal& goal, pass_class_t passClass);
+
+private:
+	static const u8 CHUNK_SIZE = 64; // number of navcells per side
+
+	class Chunk
+	{
+	public:
+		void InitRegions(u8 ci, u8 cj, Grid<TerrainTile>* grid, pass_class_t passClass);
+
+		RegionID Get(u16 i, u16 j);
+
+		void RegionCenter(u16 r, u16& i, u16& j) const;
+
+		bool RegionContainsGoal(u16 r, const PathGoal& goal) const;
+
+		void RegionNavcellNearest(u16 r, u16 iGoal, u16 jGoal, u16& iBest, u16& jBest, u32& dist2Best) const;
+
+	private:
+		void FloodFill(u8 i0, u8 j0, u16 r);
+
+		u8 m_ChunkI, m_ChunkJ; // chunk ID
+		u16 m_Regions[CHUNK_SIZE][CHUNK_SIZE]; // local region ID per navcell
+		u16 m_NumRegions; // number of local region IDs (starting from 1)
+	};
+
+	typedef std::map<RegionID, std::set<RegionID> > EdgesMap;
+
+	void FindEdges(u8 ci, u8 cj, pass_class_t passClass, EdgesMap& edges);
+
+	void AddDebugEdges(pass_class_t passClass);
+
+	void FindReachableRegions(RegionID from, std::set<RegionID>& reachable, pass_class_t passClass);
+
+	Chunk& GetChunk(u8 ci, u8 cj, pass_class_t passClass)
+	{
+		return m_Chunks[passClass].at(cj * m_ChunksW + ci);
+	}
+
+	CCmpPathfinder& m_Pathfinder;
+
+	PathfinderHierOverlay* m_DebugOverlay;
+
+	u16 m_ChunksW, m_ChunksH;
+	std::map<pass_class_t, std::vector<Chunk> > m_Chunks;
+	
+	std::map<pass_class_t, EdgesMap> m_Edges;
+
+public:
+	std::vector<SOverlayLine> m_DebugOverlayLines;
+};
+
+class PathfinderHierOverlay : public TerrainTextureOverlay
+{
+public:
+	CCmpPathfinder_Hier& m_PathfinderHier;
+
+	PathfinderHierOverlay(CCmpPathfinder_Hier& pathfinderHier) :
+		TerrainTextureOverlay(ICmpObstructionManager::NAVCELLS_PER_TILE), m_PathfinderHier(pathfinderHier)
+	{
+	}
+
+	virtual void BuildTextureRGBA(u8* data, size_t w, size_t h)
+	{
+		ICmpPathfinder::pass_class_t passClass = (ICmpPathfinder::pass_class_t)(1u << 4); // XXX
+
+		static u8 colors[][3] = {
+			{ 255, 0, 0 },
+			{ 0, 255, 0 },
+			{ 0, 0, 255 },
+			{ 255, 255, 0 },
+			{ 255, 0, 255 },
+			{ 0, 255, 255 },
+			{ 255, 255, 255 },
+
+			{ 127, 0, 0 },
+			{ 0, 127, 0 },
+			{ 0, 0, 127 },
+			{ 127, 127, 0 },
+			{ 127, 0, 127 },
+			{ 0, 127, 127 },
+			{ 127, 127, 127},
+
+			{ 255, 127, 0 },
+			{ 127, 255, 0 },
+			{ 255, 0, 127 },
+			{ 127, 0, 255},
+			{ 0, 255, 127 },
+			{ 0, 127, 255},
+			{ 255, 127, 127},
+			{ 127, 255, 127},
+			{ 127, 127, 255},
+
+			{ 127, 255, 255 },
+			{ 255, 127, 255 },
+			{ 255, 255, 127 },
+		};
+
+		for (size_t j = 0; j < h; ++j)
+		{
+			for (size_t i = 0; i < w; ++i)
+			{
+				SColor4ub color;
+
+				CCmpPathfinder_Hier::RegionID rid = m_PathfinderHier.Get(i, j, passClass);
+				if (rid.r == 0)
+					color = SColor4ub(0, 0, 0, 0);
+				else if (rid.r == 0xFFFF)
+					color = SColor4ub(255, 0, 255, 255);
+				else
+				{
+					int c = (rid.r + rid.ci*5 + rid.cj*7) % ARRAY_SIZE(colors);
+					color = SColor4ub(colors[c][0], colors[c][1], colors[c][2], 127);
+				}
+
+				*data++ = color.R;
+				*data++ = color.G;
+				*data++ = color.B;
+				*data++ = color.A;
+			}
+		}
+	}
+};
+
+void CCmpPathfinder_Hier::Chunk::InitRegions(u8 ci, u8 cj, Grid<TerrainTile>* grid, pass_class_t passClass)
+{
+	m_ChunkI = ci;
+	m_ChunkJ = cj;
+
+	memset(m_Regions, 0, sizeof(m_Regions));
+
+	u16 i0 = ci * CHUNK_SIZE;
+	u16 j0 = cj * CHUNK_SIZE;
+	u16 i1 = std::min((u16)(i0 + CHUNK_SIZE), grid->m_W);
+	u16 j1 = std::min((u16)(j0 + CHUNK_SIZE), grid->m_H);
+
+	// Start by filling the grid with 0 for blocked,
+	// and a 0xFFFF placeholder for unblocked
+	for (u16 j = j0; j < j1; ++j)
+	{
+		for (u16 i = i0; i < i1; ++i)
+		{
+			if (IS_PASSABLE(grid->get(i, j), passClass))
+				m_Regions[j - j0][i - i0] = 0xFFFF;
+			else
+				m_Regions[j - j0][i - i0] = 0;
+		}
+	}
+
+	// Scan for tiles with the 0xFFFF placeholder, and then floodfill
+	// the new region this tile belongs to
+	u16 r = 0;
+	for (u16 j = 0; j < CHUNK_SIZE; ++j)
+	{
+		for (u16 i = 0; i < CHUNK_SIZE; ++i)
+		{
+			if (m_Regions[j][i] == 0xFFFF)
+				FloodFill(i, j, ++r);
+		}
+	}
+
+	m_NumRegions = r;
+}
+
+void CCmpPathfinder_Hier::Chunk::FloodFill(u8 i0, u8 j0, u16 r)
+{
+	std::vector<std::pair<u8, u8> > stack;
+	stack.push_back(std::make_pair(i0, j0));
+
+	while (!stack.empty())
+	{
+		u8 i = stack.back().first;
+		u8 j = stack.back().second;
+		stack.pop_back();
+		m_Regions[j][i] = r;
+
+		if (i > 0 && m_Regions[j][i-1] == 0xFFFF)
+			stack.push_back(std::make_pair(i-1, j));
+		if (j > 0 && m_Regions[j-1][i] == 0xFFFF)
+			stack.push_back(std::make_pair(i, j-1));
+		if (i < CHUNK_SIZE-1 && m_Regions[j][i+1] == 0xFFFF)
+			stack.push_back(std::make_pair(i+1, j));
+		if (j < CHUNK_SIZE-1 && m_Regions[j+1][i] == 0xFFFF)
+			stack.push_back(std::make_pair(i, j+1));
+	}
+}
+
+
+CCmpPathfinder_Hier::RegionID CCmpPathfinder_Hier::Chunk::Get(u16 i, u16 j)
+{
+	return RegionID(m_ChunkI, m_ChunkJ, m_Regions[j % CHUNK_SIZE][i % CHUNK_SIZE]);
+}
+
+void CCmpPathfinder_Hier::Chunk::RegionCenter(u16 r, u16& i_out, u16& j_out) const
+{
+	u32 si = 0, sj = 0; // sum of i,j coords
+	u32 n = 0;
+
+	for (u16 j = 0; j < CHUNK_SIZE; ++j)
+	{
+		for (u16 i = 0; i < CHUNK_SIZE; ++i)
+		{
+			if (m_Regions[j][i] == r)
+			{
+				si += i;
+				sj += j;
+				n += 1;
+			}
+		}
+	}
+
+	// Avoid divide-by-zero
+	if (n == 0)
+		n = 1;
+
+	i_out = m_ChunkI * CHUNK_SIZE + si / n;
+	j_out = m_ChunkJ * CHUNK_SIZE + sj / n;
+}
+
+bool CCmpPathfinder_Hier::Chunk::RegionContainsGoal(u16 r, const PathGoal& goal) const
+{
+	for (u16 j = 0; j < CHUNK_SIZE; ++j)
+	{
+		for (u16 i = 0; i < CHUNK_SIZE; ++i)
+		{
+			if (m_Regions[j][i] == r)
+			{
+				if (goal.NavcellContainsGoal(m_ChunkI * CHUNK_SIZE + i, m_ChunkJ * CHUNK_SIZE + j))
+					return true;
+			}
+		}
+	}
+
+	return false;
+}
+
+void CCmpPathfinder_Hier::Chunk::RegionNavcellNearest(u16 r, u16 iGoal, u16 jGoal, u16& iBest, u16& jBest, u32& dist2Best) const
+{
+	iBest = 0;
+	jBest = 0;
+	dist2Best = std::numeric_limits<u32>::max();
+
+	for (u16 j = 0; j < CHUNK_SIZE; ++j)
+	{
+		for (u16 i = 0; i < CHUNK_SIZE; ++i)
+		{
+			if (m_Regions[j][i] == r)
+			{
+				u32 dist2 = ((i32)i + m_ChunkI*CHUNK_SIZE - iGoal)*((i32)i + m_ChunkI*CHUNK_SIZE - iGoal)
+				          + ((i32)j + m_ChunkJ*CHUNK_SIZE - jGoal)*((i32)j + m_ChunkJ*CHUNK_SIZE - jGoal);
+
+				if (dist2 < dist2Best)
+				{
+					iBest = i + m_ChunkI*CHUNK_SIZE;
+					jBest = j + m_ChunkJ*CHUNK_SIZE;
+					dist2Best = dist2;
+				}
+			}
+		}
+	}
+}
+
+CCmpPathfinder_Hier::CCmpPathfinder_Hier(CCmpPathfinder& pathfinder) :
+	m_Pathfinder(pathfinder)
+{
+	m_DebugOverlay = NULL;
+//	m_DebugOverlay = new PathfinderHierOverlay(*this);
+}
+
+CCmpPathfinder_Hier::~CCmpPathfinder_Hier()
+{
+	SAFE_DELETE(m_DebugOverlay);
+}
+
+void CCmpPathfinder_Hier::Init(const std::vector<PathfinderPassability>& passClasses, Grid<TerrainTile>* grid)
+{
+	m_ChunksW = (grid->m_W + CHUNK_SIZE - 1) / CHUNK_SIZE;
+	m_ChunksH = (grid->m_H + CHUNK_SIZE - 1) / CHUNK_SIZE;
+
+	m_Chunks.clear();
+	m_Edges.clear();
+
+	for (size_t n = 0; n < passClasses.size(); ++n)
+	{
+		pass_class_t passClass = passClasses[n].m_Mask;
+
+		// Compute the regions within each chunk
+		m_Chunks[passClass].resize(m_ChunksW*m_ChunksH);
+		for (u16 cj = 0; cj < m_ChunksH; ++cj)
+		{
+			for (u16 ci = 0; ci < m_ChunksW; ++ci)
+			{
+				m_Chunks[passClass].at(cj*m_ChunksW + ci).InitRegions(ci, cj, grid, passClass);
+			}
+		}
+
+		// Construct the search graph
+		
+		EdgesMap& edges = m_Edges[passClass];
+
+		for (u16 cj = 0; cj < m_ChunksH; ++cj)
+		{
+			for (u16 ci = 0; ci < m_ChunksW; ++ci)
+			{
+				FindEdges(ci, cj, passClass, edges);
+			}
+		}
+	}
+
+	m_DebugOverlayLines.clear();
+	AddDebugEdges((pass_class_t)(1u << 4));
+}
+
+void CCmpPathfinder_Hier::FindEdges(u8 ci, u8 cj, pass_class_t passClass, EdgesMap& edges)
+{
+	std::vector<Chunk>& chunks = m_Chunks[passClass];
+
+	Chunk& a = chunks.at(cj*m_ChunksW + ci);
+
+	if (ci > 0)
+	{
+		Chunk& b = chunks.at(cj*m_ChunksW + (ci-1));
+		for (u16 j = 0; j < CHUNK_SIZE; ++j)
+		{
+			RegionID ra = a.Get(0, j);
+			RegionID rb = b.Get(CHUNK_SIZE-1, j);
+			if (ra.r && rb.r)
+			{
+				edges[ra].insert(rb);
+				edges[rb].insert(ra);
+			}
+		}
+	}
+
+	if (cj > 0)
+	{
+		Chunk& b = chunks.at((cj-1)*m_ChunksW + ci);
+		for (u16 i = 0; i < CHUNK_SIZE; ++i)
+		{
+			RegionID ra = a.Get(i, 0);
+			RegionID rb = b.Get(i, CHUNK_SIZE-1);
+			if (ra.r && rb.r)
+			{
+				edges[ra].insert(rb);
+				edges[rb].insert(ra);
+			}
+		}
+	}
+
+}
+
+void CCmpPathfinder_Hier::AddDebugEdges(pass_class_t passClass)
+{
+	const EdgesMap& edges = m_Edges[passClass];
+	const std::vector<Chunk>& chunks = m_Chunks[passClass];
+
+	for (EdgesMap::const_iterator it = edges.begin(); it != edges.end(); ++it)
+	{
+		for (std::set<RegionID>::const_iterator rit = it->second.begin(); rit != it->second.end(); ++rit)
+		{
+			u16 i0, j0, i1, j1;
+			chunks[it->first.cj * m_ChunksW + it->first.ci].RegionCenter(it->first.r, i0, j0);
+			chunks[rit->cj * m_ChunksW + rit->ci].RegionCenter(rit->r, i1, j1);
+
+			CFixedVector2D a, b;
+			m_Pathfinder.NavcellCenter(i0, j0, a.X, a.Y);
+			m_Pathfinder.NavcellCenter(i1, j1, b.X, b.Y);
+
+			// Push inwards a little to avoid overlaps
+			CFixedVector2D d = b - a;
+			d.Normalize(entity_pos_t::FromInt(1));
+			a += d;
+			b -= d;
+
+			std::vector<float> xz;
+			xz.push_back(a.X.ToFloat());
+			xz.push_back(a.Y.ToFloat());
+			xz.push_back(b.X.ToFloat());
+			xz.push_back(b.Y.ToFloat());
+
+			m_DebugOverlayLines.push_back(SOverlayLine());
+			m_DebugOverlayLines.back().m_Color = CColor(1.0, 1.0, 1.0, 1.0);
+			SimRender::ConstructLineOnGround(m_Pathfinder.GetSimContext(), xz, m_DebugOverlayLines.back(), true);
+		}
+	}
+}
+
+CCmpPathfinder_Hier::RegionID CCmpPathfinder_Hier::Get(u16 i, u16 j, pass_class_t passClass)
+{
+	u16 ci = i / CHUNK_SIZE;
+	u16 cj = j / CHUNK_SIZE;
+	ENSURE(ci < m_ChunksW && cj < m_ChunksH);
+	return m_Chunks[passClass][cj*m_ChunksW + ci].Get(i, j);
+}
+
+bool CCmpPathfinder_Hier::MakeGoalReachable(u16 i0, u16 j0, ICmpPathfinder::Goal& goal, pass_class_t passClass)
+{
+	RegionID source = Get(i0, j0, passClass);
+
+	std::set<RegionID> reachableRegions;
+	FindReachableRegions(source, reachableRegions, passClass);
+
+// 	debug_printf(L"\nreach: ");
+// 	for (std::set<RegionID>::iterator it = reachableRegions.begin(); it != reachableRegions.end(); ++it)
+// 		debug_printf(L"[%d,%d,%d], ", it->ci, it->cj, it->r);
+// 	debug_printf(L"\n");
+
+	for (std::set<RegionID>::const_iterator it = reachableRegions.begin(); it != reachableRegions.end(); ++it)
+	{
+		// Skip region if its chunk doesn't contain the goal area
+		entity_pos_t x0 = ICmpObstructionManager::NAVCELL_SIZE * (it->ci * CHUNK_SIZE);
+		entity_pos_t z0 = ICmpObstructionManager::NAVCELL_SIZE * (it->cj * CHUNK_SIZE);
+		entity_pos_t x1 = x0 + ICmpObstructionManager::NAVCELL_SIZE * CHUNK_SIZE;
+		entity_pos_t z1 = z0 + ICmpObstructionManager::NAVCELL_SIZE * CHUNK_SIZE;
+		if (!goal.RectContainsGoal(x0, z0, x1, z1))
+			continue;
+
+		// If the region contains the goal area, the goal is reachable
+		// and we don't need to move it
+		if (GetChunk(it->ci, it->cj, passClass).RegionContainsGoal(it->r, goal))
+			return false;
+	}
+
+	// The goal area wasn't reachable
+
+	// Find the navcell that's nearest to the goal's center:
+	// * For each (reachable) region, record the (squared) minimal distance to the goal point
+	// * Sort regions by that underestimated distance
+	// * For each region, find the actual nearest navcell
+	// * Stop when the underestimated distances are worse than the best real distance
+
+	u16 iGoal, jGoal;
+	m_Pathfinder.NearestNavcell(goal.x, goal.z, iGoal, jGoal);
+
+	std::vector<std::pair<u32, RegionID> > regionDistEsts; // pair of (distance^2, region)
+
+	for (std::set<RegionID>::const_iterator it = reachableRegions.begin(); it != reachableRegions.end(); ++it)
+	{
+		u16 i0 = it->ci * CHUNK_SIZE;
+		u16 j0 = it->cj * CHUNK_SIZE;
+		u16 i1 = i0 + CHUNK_SIZE - 1;
+		u16 j1 = j0 + CHUNK_SIZE - 1;
+
+		// Pick the point in the chunk nearest the goal
+		u16 iNear = Clamp(iGoal, i0, i1);
+		u16 jNear = Clamp(jGoal, j0, j1);
+
+		u32 dist2 = ((i32)iNear - iGoal)*((i32)iNear - iGoal)
+		          + ((i32)jNear - jGoal)*((i32)jNear - jGoal);
+
+		regionDistEsts.push_back(std::make_pair(dist2, *it));
+	}
+
+	// Sort by increasing distance (tie-break on RegionID)
+	std::sort(regionDistEsts.begin(), regionDistEsts.end());
+
+	u16 iBest = i0;
+	u16 jBest = j0;
+	u32 dist2Best = std::numeric_limits<u32>::max();
+
+	for (size_t n = 0; n < regionDistEsts.size(); ++n)
+	{
+		RegionID region = regionDistEsts[n].second;
+
+		debug_printf(L"# region [%d,%d,%d] est %d\n", region.ci, region.cj, region.r, regionDistEsts[n].first);
+
+		if (regionDistEsts[n].first >= dist2Best)
+			break;
+
+		u16 i, j;
+		u32 dist2;
+		GetChunk(region.ci, region.cj, passClass).RegionNavcellNearest(region.r, iGoal, jGoal, i, j, dist2);
+
+		debug_printf(L"#   actual dist [%d,%d] = %d\n", i, j, dist2);
+
+		if (dist2 < dist2Best)
+		{
+			iBest = i;
+			jBest = j;
+			dist2Best = dist2;
+		}
+	}
+
+	PathGoal newGoal;
+	newGoal.type = PathGoal::POINT;
+	m_Pathfinder.NavcellCenter(iBest, jBest, newGoal.x, newGoal.z);
+	goal = newGoal;
+
+	return true;
+}
+
+void CCmpPathfinder_Hier::FindReachableRegions(RegionID from, std::set<RegionID>& reachable, pass_class_t passClass)
+{
+	std::vector<RegionID> open;
+	open.push_back(from);
+	reachable.insert(from);
+
+	while (!open.empty())
+	{
+		RegionID curr = open.back();
+		open.pop_back();
+
+		const std::set<RegionID>& neighbours = m_Edges[passClass][curr];
+		for (std::set<RegionID>::const_iterator it = neighbours.begin(); it != neighbours.end(); ++it)
+		{
+			// Add to the reachable set; if this is the first time we added
+			// then also add it to the open set
+			if (reachable.insert(*it).second)
+				open.push_back(*it);
+		}
+	}
+}
+
+void CCmpPathfinder::PathfinderHierInit()
+{
+	m_PathfinderHier = new CCmpPathfinder_Hier(*this);
+}
+
+void CCmpPathfinder::PathfinderHierDeinit()
+{
+	SAFE_DELETE(m_PathfinderHier);
+}
+
+void CCmpPathfinder::PathfinderHierReload()
+{
+	m_PathfinderHier->Init(m_PassClasses, m_Grid);
+}
+
+void CCmpPathfinder::PathfinderHierRenderSubmit(SceneCollector& collector)
+{
+	for (size_t i = 0; i < m_PathfinderHier->m_DebugOverlayLines.size(); ++i)
+		collector.Submit(&m_PathfinderHier->m_DebugOverlayLines[i]);
+}
+
+bool CCmpPathfinder::PathfinderHierMakeGoalReachable(u16 i0, u16 j0, ICmpPathfinder::Goal& goal, pass_class_t passClass)
+{
+	return m_PathfinderHier->MakeGoalReachable(i0, j0, goal, passClass);
+}
\ No newline at end of file
diff --git a/source/simulation2/components/CCmpPathfinder_JPS.cpp b/source/simulation2/components/CCmpPathfinder_JPS.cpp
new file mode 100644
index 0000000..cd9f543
--- /dev/null
+++ b/source/simulation2/components/CCmpPathfinder_JPS.cpp
@@ -0,0 +1,1069 @@
+/* Copyright (C) 2011 Wildfire Games.
+ * This file is part of 0 A.D.
+ *
+ * 0 A.D. 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 2 of the License, or
+ * (at your option) any later version.
+ *
+ * 0 A.D. 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 0 A.D.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "precompiled.h"
+
+#include "CCmpPathfinder_Common.h"
+
+#include "lib/bits.h"
+#include "lib/sysdep/rtl.h"
+#include "ps/Profile.h"
+#include "renderer/TerrainOverlay.h"
+#include "simulation2/helpers/PriorityQueue.h"
+
+#define PATHFIND_STATS 0
+
+#define USE_JUMPPOINT_CACHE 1
+
+namespace
+{
+
+struct Cost
+{
+	Cost() : data(0) { }
+	Cost(u16 hv, u16 d) : data(hv*65536 + d*92682) { }
+
+	static Cost horizvert(u16 n)
+	{
+		return Cost(n, 0);
+	}
+
+	static Cost diag(u16 n)
+	{
+		return Cost(0, n);
+	}
+
+	Cost operator+(const Cost& a) const
+	{
+		Cost c;
+		c.data = data + a.data;
+		return c;
+	}
+
+	bool operator<(const Cost& b) const
+	{
+		return data < b.data;
+	}
+
+	bool operator>=(const Cost& b) const
+	{
+		return data >= b.data;
+	}
+
+	bool operator>(const Cost& b) const
+	{
+		return data > b.data;
+	}
+
+	u32 ToInt()
+	{
+		return data;
+	}
+
+	u32 data;
+};
+
+
+struct CostAndExtra1
+{
+	CostAndExtra1() {}
+
+	Cost GetCost() const
+	{
+		return c;
+	}
+
+	void SetCost(Cost c_)
+	{
+		c = c_;
+	}
+
+	u8 GetStatus() const
+	{
+		return data & 3;
+	}
+
+	void SetStatus(u8 s)
+	{
+		ASSERT(s < 4);
+		data &= ~3;
+		data |= (s & 3);
+	}
+
+	int GetPredDI() const
+	{
+		return (i32)data >> 17;
+	}
+
+	int GetPredDJ() const
+	{
+		return ((i32)data << 15) >> 17;
+	}
+
+	void SetPred(int di, int dj)
+	{
+		ASSERT(-16384 <= di && di < 16384);
+		ASSERT(-16384 <= dj && dj < 16384);
+		data &= 3;
+		data |= (((u32)di & 0x7FFF) << 17) | (((u32)dj & 0x7FFF) << 2);
+	}
+
+private:
+	Cost c;
+	u32 data;
+};
+
+//cassert(sizeof(CostAndExtra1) == 8);
+typedef CostAndExtra1 CostAndExtra;
+
+bool operator>=(const Cost& a, const CostAndExtra& b)
+{
+	return a >= b.GetCost();
+}
+
+typedef PriorityQueueHeap<TileID, Cost> PriorityQueue;
+
+class JumpPointCache
+{
+	struct RowRaw
+	{
+		u16 last;
+
+		RowRaw(u16 length)
+		{
+			data.resize(length);
+			last = 0;
+		}
+
+		void SetRange(u16 x0, u16 x1, u16 val)
+		{
+			ENSURE(last == x0);
+			ENSURE(x0 <= x1);
+			ENSURE(x1 < data.size());
+			last = x1;
+			for (u16 x = x0; x < x1; ++x)
+				data[x] = val;
+		}
+
+		u16 Get(u16 x)
+		{
+			ENSURE(0 <= x && x < data.size());
+			return data[x];
+		}
+
+		void Finish() { }
+
+		std::vector<u16> data;
+	};
+
+	struct RowTree
+	{
+		u16 last;
+
+		struct Interval
+		{
+			u16 x0;
+			u16 x1;
+			u16 val;
+		};
+		struct Interval2
+		{
+			u16 x0;
+			u16 x1;
+		};
+		std::vector<Interval> data;
+		std::vector<Interval2> tree;
+
+		RowTree(u16 length)
+		{
+			last = 0;
+		}
+
+		void SetRange(u16 x0, u16 x1, u16 val)
+		{
+			ENSURE(last == x0);
+			ENSURE(x0 <= x1);
+			ENSURE(val == 0 || val == x1);
+			last = x1;
+
+			// If the last interval was 0 and this is too, extend the last interval;
+			// otherwise push a new interval
+			if (val == 0 && !data.empty() && data.back().val == 0)
+			{
+				data.back().x1 = x1;
+			}
+			else
+			{
+				Interval iv = { x0, x1, val };
+				data.push_back(iv);
+			}
+		}
+
+		void ConstructTree(size_t x0, size_t pivot, size_t x1, size_t idx_tree)
+		{
+			ENSURE(x0 < data.size());
+			ENSURE(x1 <= data.size());
+			ENSURE(x0 <= pivot);
+			ENSURE(pivot < x1);
+			ENSURE(idx_tree < tree.size());
+
+			tree[idx_tree].x0 = data[pivot].x0 << 1;
+			tree[idx_tree].x1 = data[pivot].x1;
+			if (data[pivot].val)
+				tree[idx_tree].x0 |= 1;
+
+			if (x0 < pivot)
+				ConstructTree(x0, (x0 + pivot) / 2, pivot, (idx_tree << 1) + 1);
+			if (pivot+1 < x1)
+				ConstructTree(pivot+1, (pivot + x1) / 2, x1, (idx_tree << 1) + 2);
+		}
+
+		void Finish()
+		{
+			size_t depth = ceil_log2(data.size() + 1);
+			tree.resize((1 << depth) - 1);
+
+			ConstructTree(0, data.size() / 2, data.size(), 0);
+		}
+
+		u16 Get(u16 x)
+		{
+			u32 i = 0;
+			while (true)
+			{
+ 				ASSERT(i < tree.size());
+				Interval2 interval = tree[i];
+				u16 x0 = interval.x0 >> 1;
+				u16 x1 = interval.x1;
+				if (x < x0)
+					i = (i << 1) + 1;
+				else if (x >= x1)
+					i = (i << 1) + 2;
+				else
+					return interval.x0 & 1 ? x1 : 0;
+			}
+		}
+	};
+
+	typedef RowTree Row;
+
+public:
+	u16 m_Width;
+	u16 m_Height;
+	std::vector<Row> m_JumpPointsRight;
+	std::vector<Row> m_JumpPointsLeft;
+	std::vector<Row> m_JumpPointsUp;
+	std::vector<Row> m_JumpPointsDown;
+
+	void reset(const Grid<TerrainTile>* terrain, ICmpPathfinder::pass_class_t passClass, u16 iGoal, u16 jGoal)
+	{
+		u16 w = terrain->m_W;
+		u16 h = terrain->m_H;
+		m_Width = w;
+		m_Height = h;
+
+		m_JumpPointsRight.reserve(h);
+		for (u16 j = 0; j < h; ++j)
+			m_JumpPointsRight.push_back(Row(w));
+
+		m_JumpPointsLeft.reserve(h);
+		for (u16 j = 0; j < h; ++j)
+			m_JumpPointsLeft.push_back(Row(w));
+
+		m_JumpPointsUp.reserve(w);
+		for (u16 i = 0; i < w; ++i)
+			m_JumpPointsUp.push_back(Row(h));
+
+		m_JumpPointsDown.reserve(w);
+		for (u16 i = 0; i < w; ++i)
+			m_JumpPointsDown.push_back(Row(h));
+
+		for (u16 j = 1; j < h - 1; ++j)
+		{
+			for (u16 i = 0; i < w-1; )
+			{
+				u16 i0 = i;
+				i++;
+				while (true)
+				{
+					if (i == w-1)
+					{
+						m_JumpPointsRight[j].SetRange(i0, i, 0);
+						break;
+					}
+
+					if (!IS_PASSABLE(terrain->get(i, j), passClass))
+					{
+						m_JumpPointsRight[j].SetRange(i0, i, 0);
+						break;
+					}
+
+					if ((i == iGoal && j == jGoal) ||
+						(!IS_PASSABLE(terrain->get(i, j-1), passClass)
+						&& IS_PASSABLE(terrain->get(i+1, j-1), passClass)) ||
+						(!IS_PASSABLE(terrain->get(i, j+1), passClass)
+						&& IS_PASSABLE(terrain->get(i+1, j+1), passClass)))
+					{
+						m_JumpPointsRight[j].SetRange(i0, i, i);
+						break;
+					}
+
+					i++;
+				}
+			}
+
+			m_JumpPointsRight[j].Finish();
+		}
+
+		for (u16 j = 1; j < h - 1; ++j)
+		{
+			for (u16 i = 0; i < w-1; )
+			{
+				u16 i0 = i;
+				i++;
+				while (true)
+				{
+					if (i == w-1)
+					{
+						m_JumpPointsLeft[j].SetRange(i0, i, 0);
+						break;
+					}
+
+					if (!IS_PASSABLE(terrain->get(w-1-i, j), passClass))
+					{
+						m_JumpPointsLeft[j].SetRange(i0, i, 0);
+						break;
+					}
+
+					if ((w-1-i == iGoal && j == jGoal) ||
+						(!IS_PASSABLE(terrain->get(w-1-i, j-1), passClass)
+						&& IS_PASSABLE(terrain->get(w-1-(i+1), j-1), passClass)) ||
+						(!IS_PASSABLE(terrain->get(w-1-i, j+1), passClass)
+						&& IS_PASSABLE(terrain->get(w-1-(i+1), j+1), passClass)))
+					{
+						m_JumpPointsLeft[j].SetRange(i0, i, i);
+						break;
+					}
+
+					i++;
+				}
+			}
+
+			m_JumpPointsLeft[j].Finish();
+		}
+
+		for (u16 i = 1; i < w - 1; ++i)
+		{
+			for (u16 j = 0; j < h-1; )
+			{
+				u16 j0 = j;
+				j++;
+				while (true)
+				{
+					if (j == h-1)
+					{
+						m_JumpPointsUp[i].SetRange(j0, j, 0);
+						break;
+					}
+
+					if (!IS_PASSABLE(terrain->get(i, j), passClass))
+					{
+						m_JumpPointsUp[i].SetRange(j0, j, 0);
+						break;
+					}
+
+					if ((i == iGoal && j == jGoal) ||
+						(!IS_PASSABLE(terrain->get(i-1, j), passClass)
+						&& IS_PASSABLE(terrain->get(i-1, j+1), passClass)) ||
+						(!IS_PASSABLE(terrain->get(i+1, j), passClass)
+						&& IS_PASSABLE(terrain->get(i+1, j+1), passClass)))
+					{
+						m_JumpPointsUp[i].SetRange(j0, j, j);
+						break;
+					}
+
+					j++;
+				}
+			}
+
+			m_JumpPointsUp[i].Finish();
+		}
+
+		for (u16 i = 1; i < w - 1; ++i)
+		{
+			for (u16 j = 0; j < h-1; )
+			{
+				u16 j0 = j;
+				j++;
+				while (true)
+				{
+					if (j == h-1)
+					{
+						m_JumpPointsDown[i].SetRange(j0, j, 0);
+						break;
+					}
+
+					if (!IS_PASSABLE(terrain->get(i, h-1-j), passClass))
+					{
+						m_JumpPointsDown[i].SetRange(j0, j, 0);
+						break;
+					}
+
+					if ((i == iGoal && h-1-j == jGoal) ||
+						(!IS_PASSABLE(terrain->get(i-1, h-1-j), passClass)
+						&& IS_PASSABLE(terrain->get(i-1, h-1-(j+1)), passClass)) ||
+						(!IS_PASSABLE(terrain->get(i+1, h-1-j), passClass)
+						&& IS_PASSABLE(terrain->get(i+1, h-1-(j+1)), passClass)))
+					{
+						m_JumpPointsDown[i].SetRange(j0, j, j);
+						break;
+					}
+
+					j++;
+				}
+			}
+
+			m_JumpPointsDown[i].Finish();
+		}
+
+// 		size_t bytes = 0;
+// 		for (u16 i = 1; i < w - 1; ++i)
+// 		{
+// 			bytes += m_JumpPointsUp[i].tree.size();
+// 			bytes += m_JumpPointsDown[i].tree.size();
+// 		}
+// 		for (u16 j = 1; j < h - 1; ++j)
+// 		{
+// 			bytes += m_JumpPointsRight[j].tree.size();
+// 			bytes += m_JumpPointsLeft[j].tree.size();
+// 		}
+// 		debug_printf(L"@@@ bytes = %d\n", bytes*4);
+
+	}
+
+	u16 GetJumpPointRight(u16 i, u16 j, u16 iGoal, u16 jGoal)
+	{
+		u16 jump = m_JumpPointsRight[j].Get(i);
+		return jump;
+	}
+
+	u16 GetJumpPointLeft(u16 i, u16 j, u16 iGoal, u16 jGoal)
+	{
+		u16 mirrorjump = m_JumpPointsLeft[j].Get(m_Width-1 - i);
+		u16 jump = m_Width-1 - mirrorjump;
+		return jump;
+	}
+
+	u16 GetJumpPointUp(u16 i, u16 j, u16 iGoal, u16 jGoal)
+	{
+		u16 jump = m_JumpPointsUp[i].Get(j);
+		return jump;
+	}
+
+	u16 GetJumpPointDown(u16 i, u16 j, u16 iGoal, u16 jGoal)
+	{
+		u16 mirrorjump = m_JumpPointsDown[i].Get(m_Height-1 - j);
+		u16 jump = m_Height-1 - mirrorjump;
+		return jump;
+	}
+};
+
+} // anon namespace
+
+/**
+ * Tile data for A* computation.
+ * (We store an array of one of these per terrain tile, so it ought to be optimised for size)
+ */
+struct PathfindTileJPS
+{
+public:
+	enum {
+		STATUS_UNEXPLORED = 0,
+		STATUS_OPEN = 1,
+		STATUS_CLOSED = 2
+	};
+
+	bool IsUnexplored() { return cost.GetStatus() == STATUS_UNEXPLORED; }
+	bool IsOpen() { return cost.GetStatus() == STATUS_OPEN; }
+	bool IsClosed() { return cost.GetStatus() == STATUS_CLOSED; }
+	void SetStatusOpen() { cost.SetStatus(STATUS_OPEN); }
+	void SetStatusClosed() { cost.SetStatus(STATUS_CLOSED); }
+
+	// Get pi,pj coords of predecessor to this tile on best path, given i,j coords of this tile
+	u16 GetPredI(u16 i) { return (u16)(i + cost.GetPredDI()); }
+	u16 GetPredJ(u16 j) { return (u16)(j + cost.GetPredDJ()); }
+	int GetPredDI() { return cost.GetPredDI(); }
+	int GetPredDJ() { return cost.GetPredDJ(); }
+	// Set the pi,pj coords of predecessor, given i,j coords of this tile
+	void SetPred(u16 pi_, u16 pj_, u16 i, u16 j)
+	{
+		cost.SetPred((int)pi_ - (int)i, (int)pj_ - (int)j);
+	}
+
+public:
+	CostAndExtra cost; // g (cost to this tile)
+};
+
+//////////////////////////////////////////////////////////
+
+struct PathfinderStateJPS
+{
+	u32 steps; // number of algorithm iterations
+
+	u16 iGoal, jGoal; // goal tile
+	u16 rGoal; // radius of goal (around tile center)
+
+	ICmpPathfinder::pass_class_t passClass;
+	std::vector<u32> moveCosts;
+
+	PriorityQueue open;
+	// (there's no explicit closed list; it's encoded in PathfindTile)
+
+	PathfindTileJPSGrid* tiles;
+	Grid<TerrainTile>* terrain;
+
+	bool ignoreImpassable; // allows us to escape if stuck in patches of impassability
+
+	Cost hBest; // heuristic of closest discovered tile to goal
+	u16 iBest, jBest; // closest tile
+
+	JumpPointCache* jpc;
+	BitGrid passGrid;
+	BitGrid jumpPointGrid0;
+	BitGrid jumpPointGrid1;
+	BitGrid jumpPointGrid2;
+	BitGrid jumpPointGrid3;
+
+#if PATHFIND_STATS
+	// Performance debug counters
+	size_t numProcessed;
+	size_t numImproveOpen;
+	size_t numAddToOpen;
+	size_t sumOpenSize;
+#endif
+};
+
+static bool AtGoal(u16 i, u16 j, const ICmpPathfinder::Goal& goal)
+{
+	// Allow tiles slightly more than sqrt(2) from the actual goal,
+	// i.e. adjacent diagonally to the target tile
+	fixed tolerance = entity_pos_t::FromInt(TERRAIN_TILE_SIZE*3/2);
+
+	if (goal.type == ICmpPathfinder::Goal::POINT)
+	{
+		u16 gi, gj;
+		gi = (u16)(goal.x*ICmpObstructionManager::NAVCELLS_PER_TILE / (int)TERRAIN_TILE_SIZE).ToInt_RoundToZero();
+		gj = (u16)(goal.z*ICmpObstructionManager::NAVCELLS_PER_TILE / (int)TERRAIN_TILE_SIZE).ToInt_RoundToZero();
+		return gi == i && gj == j;
+	}
+
+	entity_pos_t x, z;
+	CCmpPathfinder::TileCenter(i, j, x, z);
+	fixed dist = CCmpPathfinder::DistanceToGoal(CFixedVector2D(x, z), goal);
+	return (dist < tolerance);
+}
+
+// Calculate heuristic cost from tile i,j to destination
+// (This ought to be an underestimate for correctness)
+static Cost CalculateHeuristic(u16 i, u16 j, u16 iGoal, u16 jGoal, u16 rGoal)
+{
+	int di = abs((int)i - (int)iGoal);
+	int dj = abs((int)j - (int)jGoal);
+	int diag = std::min(di, dj);
+	return Cost(di-diag + dj-diag, diag);
+}
+
+
+// Do the A* processing for a neighbour tile i,j.
+static void ProcessNeighbour(u16 pi, u16 pj, u16 i, u16 j, Cost pg, PathfinderStateJPS& state)
+{
+#if PATHFIND_STATS
+	state.numProcessed++;
+#endif
+
+	// Reject impassable tiles
+	if (!IS_PASSABLE(state.terrain->get(i, j), state.passClass) && !state.ignoreImpassable)
+		return;
+
+	PathfindTileJPS& n = state.tiles->get(i, j);
+
+ 	if (n.IsClosed())
+ 		return;
+
+	Cost dg;
+	if (pi == i)
+		dg = Cost::horizvert(abs((int)pj - (int)j));
+	else if (pj == j)
+		dg = Cost::horizvert(abs((int)pi - (int)i));
+	else
+	{
+		ASSERT(abs((int)pi - (int)i) == abs((int)pj - (int)j));
+		dg = Cost::diag(abs((int)pi - (int)i));
+	}
+
+	Cost g = pg + dg; // cost to this tile = cost to predecessor + delta from predecessor
+
+	Cost h = CalculateHeuristic(i, j, state.iGoal, state.jGoal, state.rGoal);
+
+	// If this is a new tile, compute the heuristic distance
+	if (n.IsUnexplored())
+	{
+		// Remember the best tile we've seen so far, in case we never actually reach the target
+		if (h < state.hBest)
+		{
+			state.hBest = h;
+			state.iBest = i;
+			state.jBest = j;
+		}
+	}
+	else
+	{
+		// If we've already seen this tile, and the new path to this tile does not have a
+		// better cost, then stop now
+		if (g >= n.cost)
+			return;
+
+		// Otherwise, we have a better path.
+
+		// If we've already added this tile to the open list:
+		if (n.IsOpen())
+		{
+			// This is a better path, so replace the old one with the new cost/parent
+			Cost gprev = n.cost.GetCost();
+			n.cost.SetCost(g);
+			n.SetPred(pi, pj, i, j);
+			state.open.promote(TileID(i, j), gprev + h, g + h);
+#if PATHFIND_STATS
+			state.numImproveOpen++;
+#endif
+			return;
+		}
+	}
+
+	// Add it to the open list:
+	n.SetStatusOpen();
+	n.cost.SetCost(g);
+	n.SetPred(pi, pj, i, j);
+	PriorityQueue::Item t = { TileID(i, j), g + h };
+	state.open.push(t);
+#if PATHFIND_STATS
+	state.numAddToOpen++;
+#endif
+}
+
+#define PASSABLE(i, j) IS_PASSABLE(state.terrain->get(i, j), state.passClass)
+
+#if USE_JUMPPOINT_CACHE
+
+static void AddJumpedHoriz(u16 i, u16 j, i8 di, Cost g, PathfinderStateJPS& state)
+{
+	if (di > 0)
+	{
+		u16 jump = state.jpc->GetJumpPointRight(i, j, state.iGoal, state.jGoal);
+		if (jump != 0)
+			ProcessNeighbour(i, j, jump, j, g, state);
+	}
+	else
+	{
+		u16 jump = state.jpc->GetJumpPointLeft(i, j, state.iGoal, state.jGoal);
+		if (jump != state.terrain->m_W-1)
+			ProcessNeighbour(i, j, jump, j, g, state);
+	}
+}
+
+static bool HasJumpedHoriz(u16 i, u16 j, i8 di, PathfinderStateJPS& state)
+{
+	if (di > 0)
+	{
+		u16 jump = state.jpc->GetJumpPointRight(i, j, state.iGoal, state.jGoal);
+		return (jump != 0);
+	}
+	else
+	{
+		u16 jump = state.jpc->GetJumpPointLeft(i, j, state.iGoal, state.jGoal);
+		return (jump != state.terrain->m_W-1);
+	}
+}
+
+static void AddJumpedVert(u16 i, u16 j, i8 dj, Cost g, PathfinderStateJPS& state)
+{
+	if (dj > 0)
+	{
+		u16 jump = state.jpc->GetJumpPointUp(i, j, state.iGoal, state.jGoal);
+		if (jump != 0)
+			ProcessNeighbour(i, j, i, jump, g, state);
+	}
+	else
+	{
+		u16 jump = state.jpc->GetJumpPointDown(i, j, state.iGoal, state.jGoal);
+		if (jump != state.terrain->m_H-1)
+			ProcessNeighbour(i, j, i, jump, g, state);
+		return;
+	}
+}
+
+static bool HasJumpedVert(u16 i, u16 j, i8 dj, PathfinderStateJPS& state)
+{
+	if (dj > 0)
+	{
+		u16 jump = state.jpc->GetJumpPointUp(i, j, state.iGoal, state.jGoal);
+		return (jump != 0);
+	}
+	else
+	{
+		u16 jump = state.jpc->GetJumpPointDown(i, j, state.iGoal, state.jGoal);
+		return (jump != state.terrain->m_H-1);
+	}
+}
+
+#else // USE_JUMPPOINT_CACHE
+
+static void AddJumpedHoriz(u16 i, u16 j, i8 di, Cost g, PathfinderStateJPS& state)
+{
+	ASSERT(di == 1 || di == -1);
+	u16 ni = i + di;
+	while (true)
+	{
+		if (!PASSABLE(ni, j))
+			break;
+
+		if ((ni == state.iGoal && j == state.jGoal) ||
+			(!PASSABLE(ni, j-1)
+			&& PASSABLE(ni+di, j-1)) ||
+			(!PASSABLE(ni, j+1)
+			&& PASSABLE(ni+di, j+1)))
+		{
+			ProcessNeighbour(i, j, ni, j, g, state);
+			break;
+		}
+
+		ni += di;
+	}
+}
+
+static bool HasJumpedHoriz(u16 i, u16 j, i8 di, PathfinderStateJPS& state)
+{
+	ASSERT(di == 1 || di == -1);
+	u16 ni = i + di;
+	while (true)
+	{
+		if (!PASSABLE(ni, j))
+			return false;
+
+		if ((ni == state.iGoal && j == state.jGoal) ||
+			(!PASSABLE(ni, j-1)
+			&& PASSABLE(ni+di, j-1)) ||
+			(!PASSABLE(ni, j+1)
+			&& PASSABLE(ni+di, j+1)))
+		{
+			return true;
+		}
+
+		ni += di;
+	}
+}
+
+static void AddJumpedVert(u16 i, u16 j, i8 dj, Cost g, PathfinderStateJPS& state)
+{
+	ASSERT(dj == 1 || dj == -1);
+	u16 nj = j + dj;
+	while (true)
+	{
+		if (!PASSABLE(i, nj))
+			break;
+
+		if ((i == state.iGoal && nj == state.jGoal) ||
+			(!PASSABLE(i-1, nj)
+			 && PASSABLE(i-1, nj+dj)) ||
+			(!PASSABLE(i+1, nj)
+			 && PASSABLE(i+1, nj+dj)))
+		{
+			ProcessNeighbour(i, j, i, nj, g, state);
+			break;
+		}
+
+		nj += dj;
+	}
+}
+
+static bool HasJumpedVert(u16 i, u16 j, i8 dj, PathfinderStateJPS& state)
+{
+	ASSERT(dj == 1 || dj == -1);
+	u16 nj = j + dj;
+	while (true)
+	{
+		if (!PASSABLE(i, nj))
+			return false;
+
+		if ((i == state.iGoal && nj == state.jGoal) ||
+			(!PASSABLE(i-1, nj)
+			&& PASSABLE(i-1, nj+dj)) ||
+			(!PASSABLE(i+1, nj)
+			&& PASSABLE(i+1, nj+dj)))
+		{
+			return true;
+		}
+
+		nj += dj;
+	}
+}
+
+#endif // USE_JUMPPOINT_CACHE
+
+
+
+static void AddJumpedDiag(u16 i, u16 j, i8 di, i8 dj, Cost g, PathfinderStateJPS& state)
+{
+// 	ProcessNeighbour(i, j, (u16)(i + di), (u16)(j + dj), g, state);
+// 	return;
+
+	ASSERT(di == 1 || di == -1);
+	ASSERT(dj == 1 || dj == -1);
+
+	u16 ni = i + di;
+	u16 nj = j + dj;
+	while (true)
+	{
+		if (!PASSABLE(ni, nj))
+			return;
+
+		if ((ni == state.iGoal && nj == state.jGoal) ||
+			(!PASSABLE(ni - di, nj)
+			 && PASSABLE(ni - di, nj + dj)) ||
+			(!PASSABLE(ni, nj - dj)
+			 && PASSABLE(ni + di, nj - dj)))
+		{
+			ProcessNeighbour(i, j, ni, nj, g, state);
+			return;
+		}
+
+		if (HasJumpedHoriz(ni, nj, di, state) || HasJumpedVert(ni, nj, dj, state))
+		{
+			ProcessNeighbour(i, j, ni, nj, g, state);
+			return;
+		}
+
+		ni += di;
+		nj += dj;
+	}
+}
+
+
+void CCmpPathfinder::ComputePathJPS(entity_pos_t x0, entity_pos_t z0, const Goal& goal, pass_class_t passClass, cost_class_t costClass, Path& path)
+{
+	UpdateGrid();
+
+	PathfinderStateJPS state = { 0 };
+
+	static JumpPointCache* jpc;
+#if USE_JUMPPOINT_CACHE
+ 	if (!jpc)
+ 	{
+ 		jpc = new JumpPointCache;
+		NearestNavcell(goal.x, goal.z, state.iGoal, state.jGoal);
+ 		jpc->reset(m_Grid, passClass, state.iGoal, state.jGoal);
+ 	}
+#endif
+	state.jpc = jpc;
+
+	PROFILE3("ComputePathJPS");
+	double time = timer_Time();
+
+	// Convert the start/end coordinates to tile indexes
+	u16 i0, j0;
+	NearestNavcell(x0, z0, i0, j0);
+	NearestNavcell(goal.x, goal.z, state.iGoal, state.jGoal);
+
+	// If we're already at the goal tile, then move directly to the exact goal coordinates
+	if (AtGoal(i0, j0, goal))
+	{
+		Waypoint w = { goal.x, goal.z };
+		path.m_Waypoints.push_back(w);
+		return;
+	}
+
+	// If the target is a circle, we want to aim for the edge of it (so e.g. if we're inside
+	// a large circle then the heuristics will aim us directly outwards);
+	// otherwise just aim at the center point. (We'll never try moving outwards to a square shape.)
+	if (goal.type == Goal::CIRCLE)
+		state.rGoal = (u16)(goal.hw / (int)TERRAIN_TILE_SIZE).ToInt_RoundToZero();
+	else
+		state.rGoal = 0;
+
+	state.passClass = passClass;
+	state.moveCosts = m_MoveCosts.at(costClass);
+
+	state.steps = 0;
+
+	state.tiles = new PathfindTileJPSGrid(m_Grid->m_W, m_Grid->m_H);
+	state.terrain = m_Grid;
+
+	state.iBest = i0;
+	state.jBest = j0;
+	state.hBest = CalculateHeuristic(i0, j0, state.iGoal, state.jGoal, state.rGoal);
+
+	PriorityQueue::Item start = { TileID(i0, j0), Cost() };
+	state.open.push(start);
+	state.tiles->get(i0, j0).SetStatusOpen();
+	state.tiles->get(i0, j0).SetPred(i0, j0, i0, j0);
+	state.tiles->get(i0, j0).cost.SetCost(Cost());
+
+	// To prevent units getting very stuck, if they start on an impassable tile
+	// surrounded entirely by impassable tiles, we ignore the impassability
+	state.ignoreImpassable = !IS_PASSABLE(state.terrain->get(i0, j0), state.passClass);
+
+	while (1)
+	{
+		++state.steps;
+
+		// Hack to avoid spending ages computing giant paths, particularly when
+		// the destination is unreachable
+// 		if (state.steps > 40000)
+// 			break;
+
+		// If we ran out of tiles to examine, give up
+		if (state.open.empty())
+			break;
+
+#if PATHFIND_STATS
+		state.sumOpenSize += state.open.size();
+#endif
+
+		// Move best tile from open to closed
+		PriorityQueue::Item curr = state.open.pop();
+		u16 i = curr.id.i();
+		u16 j = curr.id.j();
+		state.tiles->get(i, j).SetStatusClosed();
+
+		// If we've reached the destination, stop
+		if (AtGoal(i, j, goal))
+		{
+			state.iBest = i;
+			state.jBest = j;
+			state.hBest = Cost();
+			break;
+		}
+
+		// As soon as we find an escape route from the impassable terrain,
+		// take it and forbid any further use of impassable tiles
+		if (state.ignoreImpassable)
+		{
+			if (i > 0 && IS_PASSABLE(state.terrain->get(i-1, j), state.passClass))
+				state.ignoreImpassable = false;
+			else if (i < m_MapSize-1 && IS_PASSABLE(state.terrain->get(i+1, j), state.passClass))
+				state.ignoreImpassable = false;
+			else if (j > 0 && IS_PASSABLE(state.terrain->get(i, j-1), state.passClass))
+				state.ignoreImpassable = false;
+			else if (j < m_MapSize-1 && IS_PASSABLE(state.terrain->get(i, j+1), state.passClass))
+				state.ignoreImpassable = false;
+		}
+
+		PathfindTileJPS tile = state.tiles->get(i, j);
+		Cost g = tile.cost.GetCost();
+		int dpi = tile.GetPredDI();
+		int dpj = tile.GetPredDJ();
+		dpi = (dpi < 0 ? -1 : dpi > 0 ? 1 : 0);
+		dpj = (dpj < 0 ? -1 : dpj > 0 ? 1 : 0);
+
+		if (dpi != 0 && dpj == 0)
+		{
+			if (!IS_PASSABLE(state.terrain->get(i, j-1), state.passClass))
+				AddJumpedDiag(i, j, -dpi, -1, g, state);
+			if (!IS_PASSABLE(state.terrain->get(i, j+1), state.passClass))
+				AddJumpedDiag(i, j, -dpi, +1, g, state);
+
+			AddJumpedHoriz(i, j, -dpi, g, state);
+		}
+		else if (dpi == 0 && dpj != 0)
+		{
+			if (!IS_PASSABLE(state.terrain->get(i-1, j), state.passClass))
+				AddJumpedDiag(i, j, -1, -dpj, g, state);
+			if (!IS_PASSABLE(state.terrain->get(i+1, j), state.passClass))
+				AddJumpedDiag(i, j, +1, -dpj, g, state);
+
+			AddJumpedVert(i, j, -dpj, g, state);
+		}
+		else if (dpi != 0 && dpj != 0)
+		{
+			if (!IS_PASSABLE(state.terrain->get(i + dpi, j), state.passClass))
+				AddJumpedDiag(i, j, dpi, -dpj, g, state);
+			if (!IS_PASSABLE(state.terrain->get(i, j + dpj), state.passClass))
+				AddJumpedDiag(i, j, -dpi, dpj, g, state);
+
+			AddJumpedHoriz(i, j, -dpi, g, state);
+			AddJumpedVert(i, j, -dpj, g, state);
+			AddJumpedDiag(i, j, -dpi, -dpj, g, state);
+		}
+		else
+		{
+			ProcessNeighbour(i, j, (u16)(i-1), (u16)(j-1), g, state);
+			ProcessNeighbour(i, j, (u16)(i+1), (u16)(j-1), g, state);
+			ProcessNeighbour(i, j, (u16)(i-1), (u16)(j+1), g, state);
+			ProcessNeighbour(i, j, (u16)(i+1), (u16)(j+1), g, state);
+			ProcessNeighbour(i, j, (u16)(i-1), j, g, state);
+			ProcessNeighbour(i, j, (u16)(i+1), j, g, state);
+			ProcessNeighbour(i, j, i, (u16)(j-1), g, state);
+			ProcessNeighbour(i, j, i, (u16)(j+1), g, state);
+		}
+	}
+
+	// Reconstruct the path (in reverse)
+	u16 ip = state.iBest, jp = state.jBest;
+	while (ip != i0 || jp != j0)
+	{
+		PathfindTileJPS& n = state.tiles->get(ip, jp);
+		entity_pos_t x, z;
+		NavcellCenter(ip, jp, x, z);
+		Waypoint w = { x, z };
+		path.m_Waypoints.push_back(w);
+
+		// Follow the predecessor link
+		ip = n.GetPredI(ip);
+		jp = n.GetPredJ(jp);
+	}
+
+	// Save this grid for debug display
+	m_DebugTime = timer_Time() - time;
+	delete m_DebugGridJPS;
+	m_DebugGridJPS = state.tiles;
+	m_DebugSteps = state.steps;
+
+	PROFILE2_ATTR("from: (%d, %d)", i0, j0);
+	PROFILE2_ATTR("to: (%d, %d)", state.iGoal, state.jGoal);
+	PROFILE2_ATTR("reached: (%d, %d)", state.iBest, state.jBest);
+	PROFILE2_ATTR("steps: %d", state.steps);
+
+#if PATHFIND_STATS
+	debug_printf(L"PATHFINDER: steps=%d avgo=%d proc=%d impo=%d addo=%d\n", state.steps, state.sumOpenSize/state.steps, state.numProcessed, state.numImproveOpen, state.numAddToOpen);
+#endif
+}
+
+void CCmpPathfinder::GetDebugDataJPS(u32& steps, double& time, Grid<u8>& grid)
+{
+	steps = m_DebugSteps;
+	time = m_DebugTime;
+
+	grid = Grid<u8>(m_DebugGridJPS->m_W, m_DebugGridJPS->m_H);
+	for (u16 j = 0; j < grid.m_H; ++j)
+	{
+		for (u16 i = 0; i < grid.m_W; ++i)
+		{
+			PathfindTileJPS t = m_DebugGridJPS->get(i, j);
+			grid.set(i, j, (t.IsOpen() ? 1 : 0) | (t.IsClosed() ? 2 : 0));
+		}
+	}
+}
\ No newline at end of file
diff --git a/source/simulation2/components/CCmpPathfinder_Tile.cpp b/source/simulation2/components/CCmpPathfinder_Tile.cpp
index 21e5463..0402e9a 100644
--- a/source/simulation2/components/CCmpPathfinder_Tile.cpp
+++ b/source/simulation2/components/CCmpPathfinder_Tile.cpp
@@ -1,4 +1,4 @@
-/* Copyright (C) 2010 Wildfire Games.
+/* Copyright (C) 2012 Wildfire Games.
  * This file is part of 0 A.D.
  *
  * 0 A.D. is free software: you can redistribute it and/or modify
@@ -30,15 +30,9 @@
 #include "renderer/TerrainOverlay.h"
 #include "simulation2/helpers/PriorityQueue.h"
 
-typedef PriorityQueueHeap<std::pair<u16, u16>, u32> PriorityQueue;
+#define PATHFIND_STATS 1
 
-#define PATHFIND_STATS 0
-
-#define USE_DIAGONAL_MOVEMENT 1
-
-// Heuristic cost to move between adjacent tiles.
-// This should be similar to DEFAULT_MOVE_COST.
-const u32 g_CostPerTile = 256;
+typedef PriorityQueueHeap<TileID, PathCost> PriorityQueue;
 
 /**
  * Tile data for A* computation.
@@ -63,24 +57,26 @@ public:
 	u16 GetPredI(u16 i) { return (u16)(i + dpi); }
 	u16 GetPredJ(u16 j) { return (u16)(j + dpj); }
 	// Set the pi,pj coords of predecessor, given i,j coords of this tile
-	void SetPred(u16 pi_, u16 pj_, u16 i, u16 j)
+	void SetPred(u16 pi, u16 pj, u16 i, u16 j)
 	{
-		dpi = (i8)((int)pi_ - (int)i);
-		dpj = (i8)((int)pj_ - (int)j);
 #if PATHFIND_DEBUG
 		// predecessor must be adjacent
-		ENSURE(pi_-i == -1 || pi_-i == 0 || pi_-i == 1);
-		ENSURE(pj_-j == -1 || pj_-j == 0 || pj_-j == 1);
+		ENSURE(pi-i == -1 || pi-i == 0 || pi-i == 1);
+		ENSURE(pj-j == -1 || pj-j == 0 || pj-j == 1);
 #endif
+		dpi = (i8)((int)pi - (int)i);
+		dpj = (i8)((int)pj - (int)j);
 	}
 
+	PathCost GetCost() const { return g; }
+	void SetCost(PathCost cost) { g = cost; }
+
 private:
-	u8 status; // this only needs 2 bits
-	i8 dpi, dpj; // these only really need 2 bits in total
-public:
-	u32 cost; // g (cost to this tile)
-	u32 h; // h (heuristic cost to goal) (TODO: is it really better for performance to store this instead of recomputing?)
+	i8 dpi, dpj; // values are in {-1, 0, 1}, pointing to an adjacent tile
+ 	u8 status; // this only needs 2 bits
+	PathCost g; // cost to reach this tile
 
+public:
 #if PATHFIND_DEBUG
 	u32 GetStep() { return step; }
 	void SetStep(u32 s) { step = s; }
@@ -97,52 +93,48 @@ private:
  * Terrain overlay for pathfinder debugging.
  * Renders a representation of the most recent pathfinding operation.
  */
-class PathfinderOverlay : public TerrainOverlay
+class PathfinderOverlay : public TerrainTextureOverlay
 {
-	NONCOPYABLE(PathfinderOverlay);
 public:
 	CCmpPathfinder& m_Pathfinder;
 
-	PathfinderOverlay(CCmpPathfinder& pathfinder)
-		: TerrainOverlay(pathfinder.GetSimContext()), m_Pathfinder(pathfinder)
+	PathfinderOverlay(CCmpPathfinder& pathfinder) :
+		TerrainTextureOverlay(ICmpObstructionManager::NAVCELLS_PER_TILE), m_Pathfinder(pathfinder)
 	{
 	}
 
-	virtual void StartRender()
+	virtual void BuildTextureRGBA(u8* data, size_t w, size_t h)
 	{
 		m_Pathfinder.UpdateGrid();
-	}
-
-	virtual void ProcessTile(ssize_t i, ssize_t j)
-	{
-		if (m_Pathfinder.m_Grid && !IS_PASSABLE(m_Pathfinder.m_Grid->get((int)i, (int)j), m_Pathfinder.m_DebugPassClass))
-			RenderTile(CColor(1, 0, 0, 0.6f), false);
-
-		if (m_Pathfinder.m_DebugGrid)
-		{
-			PathfindTile& n = m_Pathfinder.m_DebugGrid->get((int)i, (int)j);
-
-			float c = clamp((float)n.GetStep() / (float)m_Pathfinder.m_DebugSteps, 0.f, 1.f);
-
-			if (n.IsOpen())
-				RenderTile(CColor(1, 1, c, 0.6f), false);
-			else if (n.IsClosed())
-				RenderTile(CColor(0, 1, c, 0.6f), false);
-		}
-	}
 
-	virtual void EndRender()
-	{
-		if (m_Pathfinder.m_DebugPath)
+		for (size_t j = 0; j < h; ++j)
 		{
-			std::vector<ICmpPathfinder::Waypoint>& wp = m_Pathfinder.m_DebugPath->m_Waypoints;
-			for (size_t n = 0; n < wp.size(); ++n)
+			for (size_t i = 0; i < w; ++i)
 			{
-				u16 i, j;
-				m_Pathfinder.NearestTile(wp[n].x, wp[n].z, i, j);
-				RenderTileOutline(CColor(1, 1, 1, 1), 2, false, i, j);
+				SColor4ub color(0, 0, 0, 0);
+				if (!IS_PASSABLE(m_Pathfinder.m_Grid->get((int)i, (int)j), m_Pathfinder.m_DebugPassClass))
+					color = SColor4ub(255, 0, 0, 127);
+
+				if (m_Pathfinder.m_DebugGrid)
+				{
+					PathfindTile& n = m_Pathfinder.m_DebugGrid->get((int)i, (int)j);
+
+					if (n.IsOpen())
+						color = SColor4ub(255, 255, 0, 127);
+					else if (n.IsClosed())
+						color = SColor4ub(0, 255, 0, 127);
+
+					if (m_Pathfinder.m_DebugGoal.NavcellContainsGoal(i, j))
+						color = SColor4ub(0, 0, 255, 127);
+				}
+
+				*data++ = color.R;
+				*data++ = color.G;
+				*data++ = color.B;
+				*data++ = color.A;
 			}
 		}
+
 	}
 };
 
@@ -164,8 +156,7 @@ void CCmpPathfinder::SetDebugPath(entity_pos_t x0, entity_pos_t z0, const Goal&
 	if (!m_DebugOverlay)
 		return;
 
-	delete m_DebugGrid;
-	m_DebugGrid = NULL;
+	SAFE_DELETE(m_DebugGrid);
 	delete m_DebugPath;
 	m_DebugPath = new Path();
 	ComputePath(x0, z0, goal, passClass, costClass, *m_DebugPath);
@@ -174,16 +165,13 @@ void CCmpPathfinder::SetDebugPath(entity_pos_t x0, entity_pos_t z0, const Goal&
 
 void CCmpPathfinder::ResetDebugPath()
 {
-	delete m_DebugGrid;
-	m_DebugGrid = NULL;
-	delete m_DebugPath;
-	m_DebugPath = NULL;
+	SAFE_DELETE(m_DebugGrid);
+	SAFE_DELETE(m_DebugPath);
 }
 
 
 //////////////////////////////////////////////////////////
 
-
 struct PathfinderState
 {
 	u32 steps; // number of algorithm iterations
@@ -202,7 +190,7 @@ struct PathfinderState
 
 	bool ignoreImpassable; // allows us to escape if stuck in patches of impassability
 
-	u32 hBest; // heuristic of closest discovered tile to goal
+	PathCost hBest; // heuristic of closest discovered tile to goal
 	u16 iBest, jBest; // closest tile
 
 #if PATHFIND_STATS
@@ -215,104 +203,55 @@ struct PathfinderState
 #endif
 };
 
-static bool AtGoal(u16 i, u16 j, const ICmpPathfinder::Goal& goal)
-{
-	// Allow tiles slightly more than sqrt(2) from the actual goal,
-	// i.e. adjacent diagonally to the target tile
-	fixed tolerance = entity_pos_t::FromInt(TERRAIN_TILE_SIZE*3/2);
-
-	entity_pos_t x, z;
-	CCmpPathfinder::TileCenter(i, j, x, z);
-	fixed dist = CCmpPathfinder::DistanceToGoal(CFixedVector2D(x, z), goal);
-	return (dist < tolerance);
-}
-
 // Calculate heuristic cost from tile i,j to destination
 // (This ought to be an underestimate for correctness)
-static u32 CalculateHeuristic(u16 i, u16 j, u16 iGoal, u16 jGoal, u16 rGoal)
+static PathCost 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
-
-	fixed rdist = dist - fixed::FromInt(rGoal);
-	rdist = rdist.Absolute();
-
-	// 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);
-
-#else
-	return (abs((int)i - (int)iGoal) + abs((int)j - (int)jGoal)) * g_CostPerTile;
-#endif
-}
+//	return (abs((int)i - (int)iGoal) + abs((int)j - (int)jGoal)) * g_CostPerTile;
 
-// Calculate movement cost from predecessor tile pi,pj to tile i,j
-static u32 CalculateCostDelta(u16 pi, u16 pj, u16 i, u16 j, PathfindTileGrid* tempGrid, u32 tileCost)
-{
-	u32 dg = tileCost;
-
-#if USE_DIAGONAL_MOVEMENT
-	// XXX: Probably a terrible hack:
-	// For simplicity, we only consider horizontally/vertically adjacent neighbours, but
-	// units can move along arbitrary lines. That results in ugly square paths, so we want
-	// to prefer diagonal paths.
-	// Instead of solving this nicely, I'll just special-case 45-degree and 30-degree lines
-	// by checking the three predecessor tiles (which'll be in the closed set and therefore
-	// likely to be reasonably stable) and reducing the cost, and use a Euclidean heuristic.
-	// At least this makes paths look a bit nicer for now...
-
-	PathfindTile& p = tempGrid->get(pi, pj);
-	u16 ppi = p.GetPredI(pi);
-	u16 ppj = p.GetPredJ(pj);
-	if (ppi != i && ppj != j)
-		dg = (dg << 16) / 92682; // dg*sqrt(2)/2
-	else
-	{
-		PathfindTile& pp = tempGrid->get(ppi, ppj);
-		int di = abs(i - pp.GetPredI(ppi));
-		int dj = abs(j - pp.GetPredJ(ppj));
-		if ((di == 1 && dj == 2) || (di == 2 && dj == 1))
-			dg = (dg << 16) / 79742; // dg*(sqrt(5)-sqrt(2))
-	}
-#endif
+//	return Cost::horizvert(abs((int)i - (int)iGoal) + abs((int)j - (int)jGoal));
 
-	return dg;
+	int di = abs((int)i - (int)iGoal);
+	int dj = abs((int)j - (int)jGoal);
+	int diag = std::min(di, dj);
+	return PathCost(di-diag + dj-diag, diag);
 }
 
 // Do the A* processing for a neighbour tile i,j.
-static void ProcessNeighbour(u16 pi, u16 pj, u16 i, u16 j, u32 pg, PathfinderState& state)
+static void ProcessNeighbour(u16 pi, u16 pj, u16 i, u16 j, PathCost pg, PathfinderState& state)
 {
 #if PATHFIND_STATS
 	state.numProcessed++;
 #endif
 
+	PathfindTile& n = state.tiles->get(i, j);
+
+ 	if (n.IsClosed())
+ 		return;
+
 	// Reject impassable tiles
 	TerrainTile tileTag = state.terrain->get(i, j);
 	if (!IS_PASSABLE(tileTag, state.passClass) && !state.ignoreImpassable)
 		return;
 
-	u32 dg = CalculateCostDelta(pi, pj, i, j, state.tiles, state.moveCosts.at(GET_COST_CLASS(tileTag)));
+	PathCost dg;
+	if (pi == i || pj == j)
+		dg = PathCost::horizvert(1);
+	else
+		dg = PathCost::diag(1);
 
-	u32 g = pg + dg; // cost to this tile = cost to predecessor + delta from predecessor
+	PathCost g = pg + dg; // cost to this tile = cost to predecessor + delta from predecessor
 
-	PathfindTile& n = state.tiles->get(i, j);
+	PathCost h = CalculateHeuristic(i, j, state.iGoal, state.jGoal, state.rGoal);
 
 	// If this is a new tile, compute the heuristic distance
 	if (n.IsUnexplored())
 	{
-		n.h = CalculateHeuristic(i, j, state.iGoal, state.jGoal, state.rGoal);
+// 		n.h = CalculateHeuristic(i, j, state.iGoal, state.jGoal, state.rGoal);
 		// Remember the best tile we've seen so far, in case we never actually reach the target
-		if (n.h < state.hBest)
+		if (h < state.hBest)
 		{
-			state.hBest = n.h;
+			state.hBest = h;
 			state.iBest = i;
 			state.jBest = j;
 		}
@@ -321,7 +260,7 @@ static void ProcessNeighbour(u16 pi, u16 pj, u16 i, u16 j, u32 pg, PathfinderSta
 	{
 		// If we've already seen this tile, and the new path to this tile does not have a
 		// better cost, then stop now
-		if (g >= n.cost)
+		if (g >= n.GetCost())
 			return;
 
 		// Otherwise, we have a better path.
@@ -330,10 +269,11 @@ static void ProcessNeighbour(u16 pi, u16 pj, u16 i, u16 j, u32 pg, PathfinderSta
 		if (n.IsOpen())
 		{
 			// This is a better path, so replace the old one with the new cost/parent
-			n.cost = g;
+			PathCost gprev = n.GetCost();
+			n.SetCost(g);
 			n.SetPred(pi, pj, i, j);
 			n.SetStep(state.steps);
-			state.open.promote(std::make_pair(i, j), g + n.h);
+			state.open.promote(TileID(i, j), gprev + h, g + h);
 #if PATHFIND_STATS
 			state.numImproveOpen++;
 #endif
@@ -341,43 +281,48 @@ static void ProcessNeighbour(u16 pi, u16 pj, u16 i, u16 j, u32 pg, PathfinderSta
 		}
 
 		// If we've already found the 'best' path to this tile:
-		if (n.IsClosed())
-		{
-			// This is a better path (possible when we use inadmissible heuristics), so reopen it
-#if PATHFIND_STATS
-			state.numImproveClosed++;
-#endif
-			// (fall through)
-		}
+// 		if (n.IsClosed())
+// 		{
+// 			// This is a better path (possible when we use inadmissible heuristics), so reopen it
+// #if PATHFIND_STATS
+// 			state.numImproveClosed++;
+// #endif
+// 			// (fall through)
+// 		}
 	}
 
 	// Add it to the open list:
 	n.SetStatusOpen();
-	n.cost = g;
+	n.SetCost(g);
 	n.SetPred(pi, pj, i, j);
 	n.SetStep(state.steps);
-	PriorityQueue::Item t = { std::make_pair(i, j), g + n.h };
+	PriorityQueue::Item t = { TileID(i, j), g + h };
 	state.open.push(t);
 #if PATHFIND_STATS
 	state.numAddToOpen++;
 #endif
 }
 
-void CCmpPathfinder::ComputePath(entity_pos_t x0, entity_pos_t z0, const Goal& goal, pass_class_t passClass, cost_class_t costClass, Path& path)
+void CCmpPathfinder::ComputePath(entity_pos_t x0, entity_pos_t z0, const Goal& origGoal, pass_class_t passClass, cost_class_t costClass, Path& path)
 {
 	UpdateGrid();
 
 	PROFILE3("ComputePath");
+	double time = timer_Time();
 
 	PathfinderState state = { 0 };
 
 	// Convert the start/end coordinates to tile indexes
 	u16 i0, j0;
-	NearestTile(x0, z0, i0, j0);
-	NearestTile(goal.x, goal.z, state.iGoal, state.jGoal);
+	NearestNavcell(x0, z0, i0, j0);
+
+	Goal goal = origGoal;
+	debug_printf(L"## reachable=%d\n", PathfinderHierMakeGoalReachable(i0, j0, goal, passClass));
+
+	NearestNavcell(goal.x, goal.z, state.iGoal, state.jGoal);
 
 	// If we're already at the goal tile, then move directly to the exact goal coordinates
-	if (AtGoal(i0, j0, goal))
+	if (goal.NavcellContainsGoal(i0, j0))
 	{
 		Waypoint w = { goal.x, goal.z };
 		path.m_Waypoints.push_back(w);
@@ -388,7 +333,7 @@ void CCmpPathfinder::ComputePath(entity_pos_t x0, entity_pos_t z0, const Goal& g
 	// a large circle then the heuristics will aim us directly outwards);
 	// otherwise just aim at the center point. (We'll never try moving outwards to a square shape.)
 	if (goal.type == Goal::CIRCLE)
-		state.rGoal = (u16)(goal.hw / (int)TERRAIN_TILE_SIZE).ToInt_RoundToZero();
+		state.rGoal = (u16)(goal.hw / ICmpObstructionManager::NAVCELL_SIZE).ToInt_RoundToZero();
 	else
 		state.rGoal = 0;
 
@@ -397,18 +342,18 @@ void CCmpPathfinder::ComputePath(entity_pos_t x0, entity_pos_t z0, const Goal& g
 
 	state.steps = 0;
 
-	state.tiles = new PathfindTileGrid(m_MapSize, m_MapSize);
+	state.tiles = new PathfindTileGrid(m_Grid->m_W, m_Grid->m_H);
 	state.terrain = m_Grid;
 
 	state.iBest = i0;
 	state.jBest = j0;
 	state.hBest = CalculateHeuristic(i0, j0, state.iGoal, state.jGoal, state.rGoal);
 
-	PriorityQueue::Item start = { std::make_pair(i0, j0), 0 };
+	PriorityQueue::Item start = { TileID(i0, j0), PathCost() };
 	state.open.push(start);
 	state.tiles->get(i0, j0).SetStatusOpen();
 	state.tiles->get(i0, j0).SetPred(i0, j0, i0, j0);
-	state.tiles->get(i0, j0).cost = 0;
+	state.tiles->get(i0, j0).SetCost(PathCost());
 
 	// To prevent units getting very stuck, if they start on an impassable tile
 	// surrounded entirely by impassable tiles, we ignore the impassability
@@ -420,8 +365,8 @@ void CCmpPathfinder::ComputePath(entity_pos_t x0, entity_pos_t z0, const Goal& g
 
 		// Hack to avoid spending ages computing giant paths, particularly when
 		// the destination is unreachable
-		if (state.steps > 40000)
-			break;
+// 		if (state.steps > 40000)
+// 			break;
 
 		// If we ran out of tiles to examine, give up
 		if (state.open.empty())
@@ -433,16 +378,16 @@ void CCmpPathfinder::ComputePath(entity_pos_t x0, entity_pos_t z0, const Goal& g
 
 		// Move best tile from open to closed
 		PriorityQueue::Item curr = state.open.pop();
-		u16 i = curr.id.first;
-		u16 j = curr.id.second;
+		u16 i = curr.id.i();
+		u16 j = curr.id.j();
 		state.tiles->get(i, j).SetStatusClosed();
 
 		// If we've reached the destination, stop
-		if (AtGoal(i, j, goal))
+		if (goal.NavcellContainsGoal(i, j))
 		{
 			state.iBest = i;
 			state.jBest = j;
-			state.hBest = 0;
+			state.hBest = PathCost();
 			break;
 		}
 
@@ -452,23 +397,23 @@ void CCmpPathfinder::ComputePath(entity_pos_t x0, entity_pos_t z0, const Goal& g
 		{
 			if (i > 0 && IS_PASSABLE(state.terrain->get(i-1, j), state.passClass))
 				state.ignoreImpassable = false;
-			else if (i < m_MapSize-1 && IS_PASSABLE(state.terrain->get(i+1, j), state.passClass))
+			else if (i < state.terrain->m_W-1 && IS_PASSABLE(state.terrain->get(i+1, j), state.passClass))
 				state.ignoreImpassable = false;
 			else if (j > 0 && IS_PASSABLE(state.terrain->get(i, j-1), state.passClass))
 				state.ignoreImpassable = false;
-			else if (j < m_MapSize-1 && IS_PASSABLE(state.terrain->get(i, j+1), state.passClass))
+			else if (j < state.terrain->m_H-1 && IS_PASSABLE(state.terrain->get(i, j+1), state.passClass))
 				state.ignoreImpassable = false;
 		}
 
-		u32 g = state.tiles->get(i, j).cost;
-		if (i > 0)
-			ProcessNeighbour(i, j, (u16)(i-1), j, g, state);
-		if (i < m_MapSize-1)
-			ProcessNeighbour(i, j, (u16)(i+1), j, g, state);
-		if (j > 0)
-			ProcessNeighbour(i, j, i, (u16)(j-1), g, state);
-		if (j < m_MapSize-1)
-			ProcessNeighbour(i, j, i, (u16)(j+1), g, state);
+		PathCost g = state.tiles->get(i, j).GetCost();
+		ProcessNeighbour(i, j, (u16)(i-1), (u16)(j-1), g, state);
+		ProcessNeighbour(i, j, (u16)(i+1), (u16)(j-1), g, state);
+		ProcessNeighbour(i, j, (u16)(i-1), (u16)(j+1), g, state);
+		ProcessNeighbour(i, j, (u16)(i+1), (u16)(j+1), g, state);
+		ProcessNeighbour(i, j, (u16)(i-1), j, g, state);
+		ProcessNeighbour(i, j, (u16)(i+1), j, g, state);
+		ProcessNeighbour(i, j, i, (u16)(j-1), g, state);
+		ProcessNeighbour(i, j, i, (u16)(j+1), g, state);
 	}
 
 	// Reconstruct the path (in reverse)
@@ -477,7 +422,7 @@ void CCmpPathfinder::ComputePath(entity_pos_t x0, entity_pos_t z0, const Goal& g
 	{
 		PathfindTile& n = state.tiles->get(ip, jp);
 		entity_pos_t x, z;
-		TileCenter(ip, jp, x, z);
+		NavcellCenter(ip, jp, x, z);
 		Waypoint w = { x, z };
 		path.m_Waypoints.push_back(w);
 
@@ -487,9 +432,11 @@ void CCmpPathfinder::ComputePath(entity_pos_t x0, entity_pos_t z0, const Goal& g
 	}
 
 	// Save this grid for debug display
+	m_DebugTime = timer_Time() - time;
 	delete m_DebugGrid;
 	m_DebugGrid = state.tiles;
 	m_DebugSteps = state.steps;
+	m_DebugGoal = goal;
 
 	PROFILE2_ATTR("from: (%d, %d)", i0, j0);
 	PROFILE2_ATTR("to: (%d, %d)", state.iGoal, state.jGoal);
@@ -497,6 +444,22 @@ void CCmpPathfinder::ComputePath(entity_pos_t x0, entity_pos_t z0, const Goal& g
 	PROFILE2_ATTR("steps: %u", state.steps);
 
 #if PATHFIND_STATS
-	printf("PATHFINDER: steps=%d avgo=%d proc=%d impc=%d impo=%d addo=%d\n", state.steps, state.sumOpenSize/state.steps, state.numProcessed, state.numImproveClosed, state.numImproveOpen, state.numAddToOpen);
+	debug_printf(L"PATHFINDER: steps=%d avgo=%d proc=%d impc=%d impo=%d addo=%d\n", state.steps, state.sumOpenSize/state.steps, state.numProcessed, state.numImproveClosed, state.numImproveOpen, state.numAddToOpen);
 #endif
 }
+
+void CCmpPathfinder::GetDebugData(u32& steps, double& time, Grid<u8>& grid)
+{
+	steps = m_DebugSteps;
+	time = m_DebugTime;
+
+	grid = Grid<u8>(m_DebugGrid->m_W, m_DebugGrid->m_H);
+	for (u16 j = 0; j < grid.m_H; ++j)
+	{
+		for (u16 i = 0; i < grid.m_W; ++i)
+		{
+			PathfindTile t = m_DebugGrid->get(i, j);
+			grid.set(i, j, (t.IsOpen() ? 1 : 0) | (t.IsClosed() ? 2 : 0));
+		}
+	}
+}
diff --git a/source/simulation2/components/CCmpPathfinder_Vertex.cpp b/source/simulation2/components/CCmpPathfinder_Vertex.cpp
index 89d7622..6ac40d8 100644
--- a/source/simulation2/components/CCmpPathfinder_Vertex.cpp
+++ b/source/simulation2/components/CCmpPathfinder_Vertex.cpp
@@ -1,4 +1,4 @@
-/* Copyright (C) 2010 Wildfire Games.
+/* Copyright (C) 2012 Wildfire Games.
  * This file is part of 0 A.D.
  *
  * 0 A.D. is free software: you can redistribute it and/or modify
@@ -333,6 +333,7 @@ struct TileEdge
 	enum { TOP, BOTTOM, LEFT, RIGHT } dir;
 };
 
+#if 0
 static void AddTerrainEdges(std::vector<Edge>& edgesAA, std::vector<Vertex>& vertexes,
 	u16 i0, u16 j0, u16 i1, u16 j1, fixed r,
 	ICmpPathfinder::pass_class_t passClass, const Grid<TerrainTile>& terrain)
@@ -442,9 +443,189 @@ static void AddTerrainEdges(std::vector<Edge>& edgesAA, std::vector<Vertex>& ver
 		}
 	}
 }
+#endif
+static void AddTerrainEdges(std::vector<Edge>& edges, std::vector<Vertex>& vertexes,
+	u16 i0, u16 j0, u16 i1, u16 j1, fixed r,
+	ICmpPathfinder::pass_class_t passClass, const Grid<TerrainTile>& terrain)
+{
+	PROFILE("AddTerrainEdges");
+
+	// XXX: avoid grid overflows
+
+	for (u16 j = j0; j <= j1; ++j)
+	{
+		for (u16 i = i0; i <= i1; ++i)
+		{
+			if (IS_TERRAIN_PASSABLE(terrain.get(i, j), passClass))
+				continue;
+
+			if (IS_TERRAIN_PASSABLE(terrain.get(i+1, j), passClass) && IS_TERRAIN_PASSABLE(terrain.get(i, j+1), passClass) && IS_TERRAIN_PASSABLE(terrain.get(i+1, j+1), passClass))
+			{
+				Vertex vert;
+				vert.status = Vertex::UNEXPLORED;
+				vert.quadOutward = QUADRANT_ALL;
+				vert.quadInward = QUADRANT_BL;
+				vert.p = CFixedVector2D(fixed::FromInt(i+1)+EDGE_EXPAND_DELTA, fixed::FromInt(j+1)+EDGE_EXPAND_DELTA).Multiply(ICmpObstructionManager::NAVCELL_SIZE);
+				vertexes.push_back(vert);
+			}
+
+			if (IS_TERRAIN_PASSABLE(terrain.get(i-1, j), passClass) && IS_TERRAIN_PASSABLE(terrain.get(i, j+1), passClass) && IS_TERRAIN_PASSABLE(terrain.get(i-1, j+1), passClass))
+			{
+				Vertex vert;
+				vert.status = Vertex::UNEXPLORED;
+				vert.quadOutward = QUADRANT_ALL;
+				vert.quadInward = QUADRANT_BR;
+				vert.p = CFixedVector2D(fixed::FromInt(i)-EDGE_EXPAND_DELTA, fixed::FromInt(j+1)+EDGE_EXPAND_DELTA).Multiply(ICmpObstructionManager::NAVCELL_SIZE);
+				vertexes.push_back(vert);
+			}
+
+			if (IS_TERRAIN_PASSABLE(terrain.get(i+1, j), passClass) && IS_TERRAIN_PASSABLE(terrain.get(i, j-1), passClass) && IS_TERRAIN_PASSABLE(terrain.get(i+1, j-1), passClass))
+			{
+				Vertex vert;
+				vert.status = Vertex::UNEXPLORED;
+				vert.quadOutward = QUADRANT_ALL;
+				vert.quadInward = QUADRANT_TL;
+				vert.p = CFixedVector2D(fixed::FromInt(i+1)+EDGE_EXPAND_DELTA, fixed::FromInt(j)-EDGE_EXPAND_DELTA).Multiply(ICmpObstructionManager::NAVCELL_SIZE);
+				vertexes.push_back(vert);
+			}
+
+			if (IS_TERRAIN_PASSABLE(terrain.get(i-1, j), passClass) && IS_TERRAIN_PASSABLE(terrain.get(i, j-1), passClass) && IS_TERRAIN_PASSABLE(terrain.get(i-1, j-1), passClass))
+			{
+				Vertex vert;
+				vert.status = Vertex::UNEXPLORED;
+				vert.quadOutward = QUADRANT_ALL;
+				vert.quadInward = QUADRANT_TR;
+				vert.p = CFixedVector2D(fixed::FromInt(i)-EDGE_EXPAND_DELTA, fixed::FromInt(j)-EDGE_EXPAND_DELTA).Multiply(ICmpObstructionManager::NAVCELL_SIZE);
+				vertexes.push_back(vert);
+			}
+		}
+	}
+
+	for (u16 j = j0; j < j1; ++j)
+	{
+		std::vector<u16> segmentsR;
+		std::vector<u16> segmentsL;
+
+		for (u16 i = i0; i <= i1; ++i)
+		{
+			bool a = IS_TERRAIN_PASSABLE(terrain.get(i, j+1), passClass);
+			bool b = IS_TERRAIN_PASSABLE(terrain.get(i, j), passClass);
+			if (a && !b)
+				segmentsL.push_back(i);
+			if (b && !a)
+				segmentsR.push_back(i);
+		}
+
+		if (!segmentsR.empty())
+		{
+			segmentsR.push_back(0); // sentinel value to simplify the loop
+			u16 ia = segmentsR[0];
+			u16 ib = ia + 1;
+			for (size_t n = 1; n < segmentsR.size(); ++n)
+			{
+				if (segmentsR[n] == ib)
+					++ib;
+				else
+				{
+					CFixedVector2D v0 = CFixedVector2D(fixed::FromInt(ia), fixed::FromInt(j+1)).Multiply(ICmpObstructionManager::NAVCELL_SIZE);
+					CFixedVector2D v1 = CFixedVector2D(fixed::FromInt(ib), fixed::FromInt(j+1)).Multiply(ICmpObstructionManager::NAVCELL_SIZE);
+					Edge e = { v0, v1 };
+					edges.push_back(e);
+
+					ia = segmentsR[n];
+					ib = ia + 1;
+				}
+			}
+		}
+
+		if (!segmentsL.empty())
+		{
+			segmentsL.push_back(0); // sentinel value to simplify the loop
+			u16 ia = segmentsL[0];
+			u16 ib = ia + 1;
+			for (size_t n = 1; n < segmentsL.size(); ++n)
+			{
+				if (segmentsL[n] == ib)
+					++ib;
+				else
+				{
+					CFixedVector2D v0 = CFixedVector2D(fixed::FromInt(ib), fixed::FromInt(j+1)).Multiply(ICmpObstructionManager::NAVCELL_SIZE);
+					CFixedVector2D v1 = CFixedVector2D(fixed::FromInt(ia), fixed::FromInt(j+1)).Multiply(ICmpObstructionManager::NAVCELL_SIZE);
+					Edge e = { v0, v1 };
+					edges.push_back(e);
+
+					ia = segmentsL[n];
+					ib = ia + 1;
+				}
+			}
+		}
+	}
+
+	for (u16 i = i0; i < i1; ++i)
+	{
+		std::vector<u16> segmentsU;
+		std::vector<u16> segmentsD;
+
+		for (u16 j = j0; j <= j1; ++j)
+		{
+			bool a = IS_TERRAIN_PASSABLE(terrain.get(i+1, j), passClass);
+			bool b = IS_TERRAIN_PASSABLE(terrain.get(i, j), passClass);
+			if (a && !b)
+				segmentsU.push_back(j);
+			if (b && !a)
+				segmentsD.push_back(j);
+		}
+
+		if (!segmentsU.empty())
+		{
+			segmentsU.push_back(0); // sentinel value to simplify the loop
+			u16 ja = segmentsU[0];
+			u16 jb = ja + 1;
+			for (size_t n = 1; n < segmentsU.size(); ++n)
+			{
+				if (segmentsU[n] == jb)
+					++jb;
+				else
+				{
+					CFixedVector2D v0 = CFixedVector2D(fixed::FromInt(i+1), fixed::FromInt(ja)).Multiply(ICmpObstructionManager::NAVCELL_SIZE);
+					CFixedVector2D v1 = CFixedVector2D(fixed::FromInt(i+1), fixed::FromInt(jb)).Multiply(ICmpObstructionManager::NAVCELL_SIZE);
+					Edge e = { v0, v1 };
+					edges.push_back(e);
+
+					ja = segmentsU[n];
+					jb = ja + 1;
+				}
+			}
+		}
+
+		if (!segmentsD.empty())
+		{
+			segmentsD.push_back(0); // sentinel value to simplify the loop
+			u16 ja = segmentsD[0];
+			u16 jb = ja + 1;
+			for (size_t n = 1; n < segmentsD.size(); ++n)
+			{
+				if (segmentsD[n] == jb)
+					++jb;
+				else
+				{
+					CFixedVector2D v0 = CFixedVector2D(fixed::FromInt(i+1), fixed::FromInt(jb)).Multiply(ICmpObstructionManager::NAVCELL_SIZE);
+					CFixedVector2D v1 = CFixedVector2D(fixed::FromInt(i+1), fixed::FromInt(ja)).Multiply(ICmpObstructionManager::NAVCELL_SIZE);
+					Edge e = { v0, v1 };
+					edges.push_back(e);
+
+					ja = segmentsD[n];
+					jb = ja + 1;
+				}
+			}
+		}
+	}
+}
 
 static void SplitAAEdges(CFixedVector2D a,
+		const std::vector<Edge>& edges,
 		const std::vector<Edge>& edgesAA,
+		std::vector<Edge>& edgesUnaligned,
 		std::vector<EdgeAA>& edgesLeft, std::vector<EdgeAA>& edgesRight,
 		std::vector<EdgeAA>& edgesBottom, std::vector<EdgeAA>& edgesTop)
 {
@@ -476,6 +657,48 @@ static void SplitAAEdges(CFixedVector2D a,
 			edgesTop.push_back(e);
 		}
 	}
+
+	for (size_t i = 0; i < edges.size(); ++i)
+	{
+		if (edges[i].p0.X == edges[i].p1.X)
+		{
+			if (edges[i].p1.Y < edges[i].p0.Y)
+			{
+				if (!(a.X <= edges[i].p0.X))
+					continue;
+				EdgeAA e = { edges[i].p1, edges[i].p0.Y };
+				edgesLeft.push_back(e);
+			}
+			else
+			{
+				if (!(a.X >= edges[i].p0.X))
+					continue;
+				EdgeAA e = { edges[i].p1, edges[i].p0.Y };
+				edgesRight.push_back(e);
+			}
+		}
+		else if (edges[i].p0.Y == edges[i].p1.Y)
+		{
+			if (edges[i].p0.X < edges[i].p1.X)
+			{
+				if (!(a.Y <= edges[i].p0.Y))
+					continue;
+				EdgeAA e = { edges[i].p0, edges[i].p1.X };
+				edgesBottom.push_back(e);
+			}
+			else
+			{
+				if (!(a.Y >= edges[i].p0.Y))
+					continue;
+				EdgeAA e = { edges[i].p0, edges[i].p1.X };
+				edgesTop.push_back(e);
+			}
+		}
+		else
+		{
+			edgesUnaligned.push_back(edges[i]);
+		}
+	}
 }
 
 /**
@@ -500,7 +723,7 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter,
 	UpdateGrid(); // TODO: only need to bother updating if the terrain changed
 
 	PROFILE3("ComputeShortPath");
-//	ScopeTimer UID__(L"ComputeShortPath");
+	ScopeTimer UID__(L"ComputeShortPath");
 
 	m_DebugOverlayShortPathLines.clear();
 
@@ -574,9 +797,9 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter,
 	// Add terrain obstructions
 	{
 		u16 i0, j0, i1, j1;
-		NearestTile(rangeXMin, rangeZMin, i0, j0);
-		NearestTile(rangeXMax, rangeZMax, i1, j1);
-		AddTerrainEdges(edgesAA, vertexes, i0, j0, i1, j1, r, passClass, *m_Grid);
+		NearestNavcell(rangeXMin, rangeZMin, i0, j0);
+		NearestNavcell(rangeXMax, rangeZMax, i1, j1);
+		AddTerrainEdges(edges, vertexes, i0, j0, i1, j1, r, passClass, *m_Grid);
 	}
 
 	// Find all the obstruction squares that might affect us
@@ -647,18 +870,68 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter,
 
 	if (m_DebugOverlay)
 	{
-		// Render the obstruction edges
+#define PUSH_POINT(p) xz.push_back(p.X.ToFloat()); xz.push_back(p.Y.ToFloat());
+		// Render the vertexes
+		for (size_t i = 0; i < vertexes.size(); ++i)
+		{
+			m_DebugOverlayShortPathLines.push_back(SOverlayLine());
+			m_DebugOverlayShortPathLines.back().m_Color = CColor(1, 1, 0, 1);
+
+			float x = vertexes[i].p.X.ToFloat();
+			float z = vertexes[i].p.Y.ToFloat();
+
+			float a0 = 0, a1 = 0;
+			if (vertexes[i].quadInward == QUADRANT_BL)
+			{
+				a0 = -(float)M_PI/2;
+				a1 = (float)M_PI;
+			}
+			else if (vertexes[i].quadInward == QUADRANT_TR)
+			{
+				a0 = (float)M_PI/2;
+				a1 = 2*(float)M_PI;
+			}
+			else if (vertexes[i].quadInward == QUADRANT_TL)
+			{
+				a0 = -(float)M_PI;
+				a1 = (float)M_PI/2;
+			}
+			else if (vertexes[i].quadInward == QUADRANT_BR)
+			{
+				a0 = 0;
+				a1 = 3*(float)M_PI/2;
+			}
+
+			if (a0 == a1)
+				SimRender::ConstructCircleOnGround(GetSimContext(), x, z, 0.5f,
+					m_DebugOverlayShortPathLines.back(), true);
+			else
+				SimRender::ConstructClosedArcOnGround(GetSimContext(), x, z, 0.5f,
+					a0, a1, m_DebugOverlayShortPathLines.back(), true);
+		}
+		
+		// Render the edges
 		for (size_t i = 0; i < edges.size(); ++i)
 		{
 			m_DebugOverlayShortPathLines.push_back(SOverlayLine());
 			m_DebugOverlayShortPathLines.back().m_Color = CColor(0, 1, 1, 1);
 			std::vector<float> xz;
-			xz.push_back(edges[i].p0.X.ToFloat());
-			xz.push_back(edges[i].p0.Y.ToFloat());
-			xz.push_back(edges[i].p1.X.ToFloat());
-			xz.push_back(edges[i].p1.Y.ToFloat());
+			PUSH_POINT(edges[i].p0);
+			PUSH_POINT(edges[i].p1);
+
+			// Add an arrowhead to indicate the direction
+			CFixedVector2D d = edges[i].p1 - edges[i].p0;
+			d.Normalize(fixed::FromInt(1)/8);
+			CFixedVector2D p2 = edges[i].p1 - d*2;
+			CFixedVector2D p3 = p2 + d.Perpendicular();
+			CFixedVector2D p4 = p2 - d.Perpendicular();
+			PUSH_POINT(p3);
+			PUSH_POINT(p4);
+			PUSH_POINT(edges[i].p1);
+
 			SimRender::ConstructLineOnGround(GetSimContext(), xz, m_DebugOverlayShortPathLines.back(), true);
 		}
+#undef PUSH_POINT
 
 		for (size_t i = 0; i < edgesAA.size(); ++i)
 		{
@@ -716,11 +989,13 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter,
 		// since they're more likely to block the rays
 		std::sort(edgesAA.begin(), edgesAA.end(), EdgeSort(vertexes[curr.id].p));
 
+		std::vector<Edge> edgesUnaligned;
 		std::vector<EdgeAA> edgesLeft;
 		std::vector<EdgeAA> edgesRight;
 		std::vector<EdgeAA> edgesBottom;
 		std::vector<EdgeAA> edgesTop;
-		SplitAAEdges(vertexes[curr.id].p, edgesAA, edgesLeft, edgesRight, edgesBottom, edgesTop);
+		SplitAAEdges(vertexes[curr.id].p, edges, edgesAA, edgesUnaligned, edgesLeft, edgesRight, edgesBottom, edgesTop);
+		//debug_printf(L"edges: e=%d aa=%d; u=%d l=%d r=%d b=%d t=%d\n", edges.size(), edgesAA.size(), edgesUnaligned.size(), edgesLeft.size(), edgesRight.size(), edgesBottom.size(), edgesTop.size());
 
 		// Check the lines to every other vertex
 		for (size_t n = 0; n < vertexes.size(); ++n)
@@ -768,7 +1043,7 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter,
 				CheckVisibilityRight(vertexes[curr.id].p, npos, edgesRight) &&
 				CheckVisibilityBottom(vertexes[curr.id].p, npos, edgesBottom) &&
 				CheckVisibilityTop(vertexes[curr.id].p, npos, edgesTop) &&
-				CheckVisibility(vertexes[curr.id].p, npos, edges);
+				CheckVisibility(vertexes[curr.id].p, npos, edgesUnaligned);
 
 			/*
 			// Render the edges that we examine
@@ -823,6 +1098,7 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter,
 						continue;
 
 					// Otherwise, we have a better path, so replace the old one with the new cost/parent
+					fixed gprev = vertexes[n].g;
 					vertexes[n].g = g;
 					vertexes[n].pred = curr.id;
 
@@ -834,7 +1110,7 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter,
 					if (n == GOAL_VERTEX_ID)
 						vertexes[n].p = npos; // remember the new best goal position
 
-					open.promote((u16)n, g + vertexes[n].h);
+					open.promote((u16)n, gprev + vertexes[n].h, g + vertexes[n].h);
 				}
 			}
 		}
@@ -861,34 +1137,86 @@ bool CCmpPathfinder::CheckMovement(const IObstructionTestFilter& filter,
 	if (cmpObstructionManager->TestLine(filter, x0, z0, x1, z1, r))
 		return false;
 
-	// Test against terrain:
+	// Test against the passability grid.
+	// This should ignore r, and just check that the line (x0,z0)-(x1,z1)
+	// does not intersect any impassable navcells.
+	// We shouldn't allow lines between diagonally-adjacent navcells.
+	// It doesn't matter whether we allow lines precisely along the edge
+	// of an impassable navcell.
 
-	// (TODO: this could probably be a tiny bit faster by not reusing all the vertex computation code)
+	// To rasterise the line:
+	// If the line is (e.g.) aiming up-right, then we start at the navcell
+	// containing the start point and the line must either end in that navcell
+	// or else exit along the top edge or the right edge (or through the top-right corner,
+	// which we'll arbitrary treat as the horizontal edge).
+	// So we jump into the adjacent navcell across that edge, and continue.
 
 	UpdateGrid();
 
-	std::vector<Edge> edgesAA;
-	std::vector<Vertex> vertexes;
-
 	u16 i0, j0, i1, j1;
-	NearestTile(std::min(x0, x1) - r, std::min(z0, z1) - r, i0, j0);
-	NearestTile(std::max(x0, x1) + r, std::max(z0, z1) + r, i1, j1);
-	AddTerrainEdges(edgesAA, vertexes, i0, j0, i1, j1, r, passClass, *m_Grid);
-
-	CFixedVector2D a(x0, z0);
-	CFixedVector2D b(x1, z1);
-
-	std::vector<EdgeAA> edgesLeft;
-	std::vector<EdgeAA> edgesRight;
-	std::vector<EdgeAA> edgesBottom;
-	std::vector<EdgeAA> edgesTop;
-	SplitAAEdges(a, edgesAA, edgesLeft, edgesRight, edgesBottom, edgesTop);
-
-	bool visible =
-		CheckVisibilityLeft(a, b, edgesLeft) &&
-		CheckVisibilityRight(a, b, edgesRight) &&
-		CheckVisibilityBottom(a, b, edgesBottom) &&
-		CheckVisibilityTop(a, b, edgesTop);
-
-	return visible;
+	NearestNavcell(x0, z0, i0, j0);
+	NearestNavcell(x1, z1, i1, j1);
+
+	// Find which direction the line heads in
+	int di = (i0 < i1 ? +1 : i1 < i0 ? -1 : 0);
+	int dj = (j0 < j1 ? +1 : j1 < j0 ? -1 : 0);
+
+	u16 i = i0;
+	u16 j = j0;
+
+// 	debug_printf(L"(%f,%f)..(%f,%f) [%d,%d]..[%d,%d]\n", x0.ToFloat(), z0.ToFloat(), x1.ToFloat(), z1.ToFloat(), i0, j0, i1, j1);
+
+	while (true)
+	{
+		// Fail if we're on an impassable navcell
+		if (!IS_TERRAIN_PASSABLE(m_Grid->get(i, j), passClass))
+			return false;
+
+		// Succeed if we're at the target
+		if (i == i1 && j == j1)
+			return true;
+
+		// If we can only move horizontally/vertically, then just move in that direction
+		if (di == 0)
+		{
+			j += dj;
+			continue;
+		}
+		else if (dj == 0)
+		{
+			i += di;
+			continue;
+		}
+
+		// Otherwise we need to check which cell to move into:
+
+		// Check whether the line intersects the horizontal (top/bottom) edge of
+		// the current navcell.
+		// Horizontal edge is (i, j + (dj>0?1:0)) .. (i + 1, j + (dj>0?1:0))
+		// Since we already know the line is moving from this navcell into a different
+		// navcell, we simply need to test that the edge's endpoints are not both on the
+		// same side of the line.
+
+		entity_pos_t xia = entity_pos_t::FromInt(i).Multiply(ICmpObstructionManager::NAVCELL_SIZE);
+		entity_pos_t xib = entity_pos_t::FromInt(i+1).Multiply(ICmpObstructionManager::NAVCELL_SIZE);
+		entity_pos_t zj = entity_pos_t::FromInt(j + (dj+1)/2).Multiply(ICmpObstructionManager::NAVCELL_SIZE);
+
+		CFixedVector2D perp = CFixedVector2D(x1 - x0, z1 - z0).Perpendicular();
+		entity_pos_t dota = (CFixedVector2D(xia, zj) - CFixedVector2D(x0, z0)).Dot(perp);
+		entity_pos_t dotb = (CFixedVector2D(xib, zj) - CFixedVector2D(x0, z0)).Dot(perp);
+
+// 		debug_printf(L"(%f,%f)-(%f,%f) :: %f %f\n", xia.ToFloat(), zj.ToFloat(), xib.ToFloat(), zj.ToFloat(), dota.ToFloat(), dotb.ToFloat());
+
+		if ((dota < entity_pos_t::Zero() && dotb < entity_pos_t::Zero()) ||
+		    (dota > entity_pos_t::Zero() && dotb > entity_pos_t::Zero()))
+		{
+			// Horizontal edge is fully on one side of the line, so the line doesn't
+			// intersect it, so we should move across the vertical edge instead
+			i += di;
+		}
+		else
+		{
+			j += dj;
+		}
+	}
 }
diff --git a/source/simulation2/components/CCmpRangeManager.cpp b/source/simulation2/components/CCmpRangeManager.cpp
index 4883073..cf71f31 100644
--- a/source/simulation2/components/CCmpRangeManager.cpp
+++ b/source/simulation2/components/CCmpRangeManager.cpp
@@ -22,6 +22,7 @@
 
 #include "simulation2/MessageTypes.h"
 #include "simulation2/components/ICmpPosition.h"
+#include "simulation2/components/ICmpObstructionManager.h"
 #include "simulation2/components/ICmpTerritoryManager.h"
 #include "simulation2/components/ICmpVision.h"
 #include "simulation2/helpers/Render.h"
@@ -955,10 +956,20 @@ public:
 			return;
 
 		const Grid<u8>& grid = cmpTerritoryManager->GetTerritoryGrid();
-		ENSURE(grid.m_W == m_TerrainVerticesPerSide-1 && grid.m_H == m_TerrainVerticesPerSide-1);
 
-		// For each tile, if it is owned by a valid player then update the LOS
-		// for every vertex around that tile, to mark them as explored
+		// Territory data is stored per territory-tile (typically a multiple of terrain tiles).
+		// LOS data is stored per terrain tile vertex.
+
+		// For each territory-tile, if it is owned by a valid player then update the LOS
+		// for every vertex inside/around that tile, to mark them as explored
+
+		// (Currently this code doesn't support territory-tiles smaller than terrain tiles;
+		// it will get scale==0 and break)
+		cassert(ICmpTerritoryManager::NAVCELLS_PER_TERRITORY_TILE > ICmpObstructionManager::NAVCELLS_PER_TILE);
+
+		int scale = ICmpTerritoryManager::NAVCELLS_PER_TERRITORY_TILE / ICmpObstructionManager::NAVCELLS_PER_TILE;
+
+		ENSURE(grid.m_W*scale == m_TerrainVerticesPerSide-1 && grid.m_H*scale == m_TerrainVerticesPerSide-1);
 
 		for (u16 j = 0; j < grid.m_H; ++j)
 		{
@@ -967,10 +978,9 @@ public:
 				u8 p = grid.get(i, j) & ICmpTerritoryManager::TERRITORY_PLAYER_MASK;
 				if (p > 0 && p <= MAX_LOS_PLAYER_ID)
 				{
-					m_LosState[i + j*m_TerrainVerticesPerSide] |= (LOS_EXPLORED << (2*(p-1)));
-					m_LosState[i+1 + j*m_TerrainVerticesPerSide] |= (LOS_EXPLORED << (2*(p-1)));
-					m_LosState[i + (j+1)*m_TerrainVerticesPerSide] |= (LOS_EXPLORED << (2*(p-1)));
-					m_LosState[i+1 + (j+1)*m_TerrainVerticesPerSide] |= (LOS_EXPLORED << (2*(p-1)));
+					for (int dj = 0; dj <= scale; ++dj)
+						for (int di = 0; di <= scale; ++di)
+							m_LosState[(i*scale+di) + (j*scale+dj)*m_TerrainVerticesPerSide] |= (LOS_EXPLORED << (2*(p-1)));
 				}
 			}
 		}
diff --git a/source/simulation2/components/CCmpTerritoryManager.cpp b/source/simulation2/components/CCmpTerritoryManager.cpp
index fe911d4..b43acda 100644
--- a/source/simulation2/components/CCmpTerritoryManager.cpp
+++ b/source/simulation2/components/CCmpTerritoryManager.cpp
@@ -1,4 +1,4 @@
-/* Copyright (C) 2011 Wildfire Games.
+/* Copyright (C) 2012 Wildfire Games.
  * This file is part of 0 A.D.
  *
  * 0 A.D. is free software: you can redistribute it and/or modify
@@ -48,15 +48,14 @@
 
 class CCmpTerritoryManager;
 
-class TerritoryOverlay : public TerrainOverlay
+class TerritoryOverlay : public TerrainTextureOverlay
 {
 	NONCOPYABLE(TerritoryOverlay);
 public:
 	CCmpTerritoryManager& m_TerritoryManager;
 
 	TerritoryOverlay(CCmpTerritoryManager& manager);
-	virtual void StartRender();
-	virtual void ProcessTile(ssize_t i, ssize_t j);
+	virtual void BuildTextureRGBA(u8* data, size_t w, size_t h);
 };
 
 class CCmpTerritoryManager : public ICmpTerritoryManager
@@ -259,6 +258,24 @@ public:
 
 REGISTER_COMPONENT_TYPE(TerritoryManager)
 
+/**
+ * Compute the tile indexes on the grid nearest to a given point
+ */
+static void NearestTerritoryTile(entity_pos_t x, entity_pos_t z, u16& i, u16& j, u16 w, u16 h)
+{
+	i = (u16)clamp((x / (ICmpObstructionManager::NAVCELL_SIZE * ICmpTerritoryManager::NAVCELLS_PER_TERRITORY_TILE)).ToInt_RoundToNegInfinity(), 0, w-1);
+	j = (u16)clamp((z / (ICmpObstructionManager::NAVCELL_SIZE * ICmpTerritoryManager::NAVCELLS_PER_TERRITORY_TILE)).ToInt_RoundToNegInfinity(), 0, h-1);
+}
+
+/**
+ * Returns the position of the center of the given tile
+ */
+static void TerritoryTileCenter(u16 i, u16 j, entity_pos_t& x, entity_pos_t& z)
+{
+	x = entity_pos_t::FromInt(i*2 + 1).Multiply(ICmpObstructionManager::NAVCELL_SIZE * ICmpTerritoryManager::NAVCELLS_PER_TERRITORY_TILE / 2);
+	z = entity_pos_t::FromInt(j*2 + 1).Multiply(ICmpObstructionManager::NAVCELL_SIZE * ICmpTerritoryManager::NAVCELLS_PER_TERRITORY_TILE / 2);
+}
+
 /*
 We compute the territory influence of an entity with a kind of best-first search,
 storing an 'open' list of tiles that have not yet been processed,
@@ -333,31 +350,64 @@ void CCmpTerritoryManager::CalculateTerritories()
 	if (!cmpTerrain->IsLoaded())
 		return;
 
-	u16 tilesW = cmpTerrain->GetTilesPerSide();
-	u16 tilesH = cmpTerrain->GetTilesPerSide();
+	CmpPtr<ICmpPathfinder> cmpPathfinder(GetSimContext(), SYSTEM_ENTITY);
+	ICmpPathfinder::pass_class_t passClassDefault = cmpPathfinder->GetPassabilityClass("territory");
+	ICmpPathfinder::pass_class_t passClassUnrestricted = cmpPathfinder->GetPassabilityClass("unrestricted");
+
+	const Grid<u16>& passGrid = cmpPathfinder->GetPassabilityGrid();
+
+	// Downsample the passability data to count the number of impassable
+	// navcells per territory tile
+
+	u16 tilesW = passGrid.m_W / NAVCELLS_PER_TERRITORY_TILE;
+	u16 tilesH = passGrid.m_H / NAVCELLS_PER_TERRITORY_TILE;
 
 	m_Territories = new Grid<u8>(tilesW, tilesH);
 
-	// Compute terrain-passability-dependent costs per tile
-	Grid<u8> influenceGrid(tilesW, tilesH);
+	// Downsample passability grid horizontally first
+	Grid<u16> tempPassGrid(passGrid.m_W / NAVCELLS_PER_TERRITORY_TILE, passGrid.m_H);
+	cassert(NAVCELLS_PER_TERRITORY_TILE < 16); // else we might overflow the counters
 
-	CmpPtr<ICmpPathfinder> cmpPathfinder(GetSimContext(), SYSTEM_ENTITY);
-	ICmpPathfinder::pass_class_t passClassDefault = cmpPathfinder->GetPassabilityClass("default");
-	ICmpPathfinder::pass_class_t passClassUnrestricted = cmpPathfinder->GetPassabilityClass("unrestricted");
+	for (u16 j = 0; j < tempPassGrid.m_H; ++j)
+	{
+		for (u16 i = 0; i < tempPassGrid.m_W; ++i)
+		{
+			u32 count = 0;
+			for (u16 di = 0; di < NAVCELLS_PER_TERRITORY_TILE; ++di)
+			{
+				u16 g = passGrid.get(i*NAVCELLS_PER_TERRITORY_TILE + di, j);
+				if (g & passClassUnrestricted)
+					count += 65536; // off the world; force maximum cost
+				else if (g & passClassDefault)
+					count += 1;
+			}
+			tempPassGrid.set(i, j, std::min((u32)65535, count)); // clamp to avoid overflow
+		}
+	}
 
-	const Grid<u16>& passGrid = cmpPathfinder->GetPassabilityGrid();
+	// Compute terrain-passability-dependent costs per tile
+
+	Grid<u8> influenceGrid(tilesW, tilesH);
 	for (u16 j = 0; j < tilesH; ++j)
 	{
 		for (u16 i = 0; i < tilesW; ++i)
 		{
-			u16 g = passGrid.get(i, j);
+			// Downsample vertically
+			u32 count = 0;
+			for (u16 dj = 0; dj < NAVCELLS_PER_TERRITORY_TILE; ++dj)
+				count += tempPassGrid.get(i, j*NAVCELLS_PER_TERRITORY_TILE + dj);
+
 			u8 cost;
-			if (g & passClassUnrestricted)
-				cost = 255; // off the world; use maximum cost
-			else if (g & passClassDefault)
-				cost = m_ImpassableCost;
+			if (count >= 65535)
+			{
+				cost = 255; // at least partially off the world; use maximum cost
+			}
 			else
-				cost = 1;
+			{
+				// Compute average cost of the cells within this tile
+				u32 totalCells = NAVCELLS_PER_TERRITORY_TILE*NAVCELLS_PER_TERRITORY_TILE;
+				cost = (m_ImpassableCost*count + 1*(totalCells-count)) / totalCells;
+			}
 			influenceGrid.set(i, j, cost);
 		}
 	}
@@ -420,12 +470,12 @@ void CCmpTerritoryManager::CalculateTerritories()
 
 			CmpPtr<ICmpPosition> cmpPosition(GetSimContext(), *eit);
 			CFixedVector2D pos = cmpPosition->GetPosition2D();
-			u16 i = (u16)clamp((pos.X / (int)TERRAIN_TILE_SIZE).ToInt_RoundToNegInfinity(), 0, tilesW-1);
-			u16 j = (u16)clamp((pos.Y / (int)TERRAIN_TILE_SIZE).ToInt_RoundToNegInfinity(), 0, tilesH-1);
+			u16 i, j;
+			NearestTerritoryTile(pos.X, pos.Y, i, j, tilesW, tilesH);
 
 			CmpPtr<ICmpTerritoryInfluence> cmpTerritoryInfluence(GetSimContext(), *eit);
 			u32 weight = cmpTerritoryInfluence->GetWeight();
-			u32 radius = cmpTerritoryInfluence->GetRadius() / TERRAIN_TILE_SIZE;
+			u32 radius = (fixed::FromInt(cmpTerritoryInfluence->GetRadius()) / (ICmpObstructionManager::NAVCELL_SIZE * NAVCELLS_PER_TERRITORY_TILE)).ToInt_RoundToNegInfinity();
 			u32 falloff = weight / radius; // earlier check for GetRadius() == 0 prevents divide-by-zero
 
 			// TODO: we should have some maximum value on weight, to avoid overflow
@@ -477,8 +527,8 @@ void CCmpTerritoryManager::CalculateTerritories()
 		CmpPtr<ICmpPosition> cmpPosition(GetSimContext(), *it);
 
 		CFixedVector2D pos = cmpPosition->GetPosition2D();
-		u16 i = (u16)clamp((pos.X / (int)TERRAIN_TILE_SIZE).ToInt_RoundToNegInfinity(), 0, tilesW-1);
-		u16 j = (u16)clamp((pos.Y / (int)TERRAIN_TILE_SIZE).ToInt_RoundToNegInfinity(), 0, tilesH-1);
+		u16 i, j;
+		NearestTerritoryTile(pos.X, pos.Y, i, j, tilesW, tilesH);
 
 		u8 owner = (u8)cmpOwnership->GetOwner();
 
@@ -527,27 +577,6 @@ void CCmpTerritoryManager::CalculateTerritories()
 	}
 }
 
-/**
- * Compute the tile indexes on the grid nearest to a given point
- */
-static void NearestTile(entity_pos_t x, entity_pos_t z, u16& i, u16& j, u16 w, u16 h)
-{
-	i = (u16)clamp((x / (int)TERRAIN_TILE_SIZE).ToInt_RoundToZero(), 0, w-1);
-	j = (u16)clamp((z / (int)TERRAIN_TILE_SIZE).ToInt_RoundToZero(), 0, h-1);
-}
-
-/**
- * Returns the position of the center of the given tile
- */
-static void TileCenter(u16 i, u16 j, entity_pos_t& x, entity_pos_t& z)
-{
-	x = entity_pos_t::FromInt(i*(int)TERRAIN_TILE_SIZE + (int)TERRAIN_TILE_SIZE/2);
-	z = entity_pos_t::FromInt(j*(int)TERRAIN_TILE_SIZE + (int)TERRAIN_TILE_SIZE/2);
-}
-
-// TODO: would be nice not to duplicate those two functions from CCmpObstructionManager.cpp
-
-
 void CCmpTerritoryManager::RasteriseInfluences(CComponentManager::InterfaceList& infls, Grid<u8>& grid)
 {
 	for (CComponentManager::InterfaceList::iterator it = infls.begin(); it != infls.end(); ++it)
@@ -570,14 +599,14 @@ void CCmpTerritoryManager::RasteriseInfluences(CComponentManager::InterfaceList&
 		CFixedVector2D halfBound = Geometry::GetHalfBoundingBox(square.u, square.v, halfSize);
 
 		u16 i0, j0, i1, j1;
-		NearestTile(square.x - halfBound.X, square.z - halfBound.Y, i0, j0, grid.m_W, grid.m_H);
-		NearestTile(square.x + halfBound.X, square.z + halfBound.Y, i1, j1, grid.m_W, grid.m_H);
+		NearestTerritoryTile(square.x - halfBound.X, square.z - halfBound.Y, i0, j0, grid.m_W, grid.m_H);
+		NearestTerritoryTile(square.x + halfBound.X, square.z + halfBound.Y, i1, j1, grid.m_W, grid.m_H);
 		for (u16 j = j0; j <= j1; ++j)
 		{
 			for (u16 i = i0; i <= i1; ++i)
 			{
 				entity_pos_t x, z;
-				TileCenter(i, j, x, z);
+				TerritoryTileCenter(i, j, x, z);
 				if (Geometry::PointIsInSquare(CFixedVector2D(x - square.x, z - square.z), square.u, square.v, halfSize))
 					grid.set(i, j, (u8)cost);
 			}
@@ -715,7 +744,7 @@ player_id_t CCmpTerritoryManager::GetOwner(entity_pos_t x, entity_pos_t z)
 	if (!m_Territories)
 		return 0;
 
-	NearestTile(x, z, i, j, m_Territories->m_W, m_Territories->m_H);
+	NearestTerritoryTile(x, z, i, j, m_Territories->m_W, m_Territories->m_H);
 	return m_Territories->get(i, j) & TERRITORY_PLAYER_MASK;
 }
 
@@ -726,36 +755,43 @@ bool CCmpTerritoryManager::IsConnected(entity_pos_t x, entity_pos_t z)
 	if (!m_Territories)
 		return false;
 
-	NearestTile(x, z, i, j, m_Territories->m_W, m_Territories->m_H);
+	NearestTerritoryTile(x, z, i, j, m_Territories->m_W, m_Territories->m_H);
 	return (m_Territories->get(i, j) & TERRITORY_CONNECTED_MASK) != 0;
 }
 
 TerritoryOverlay::TerritoryOverlay(CCmpTerritoryManager& manager)
-	: TerrainOverlay(manager.GetSimContext()), m_TerritoryManager(manager)
+	: TerrainTextureOverlay((float)ICmpObstructionManager::NAVCELLS_PER_TILE / ICmpTerritoryManager::NAVCELLS_PER_TERRITORY_TILE),
+	m_TerritoryManager(manager)
 { }
 
-void TerritoryOverlay::StartRender()
+void TerritoryOverlay::BuildTextureRGBA(u8* data, size_t w, size_t h)
 {
 	m_TerritoryManager.CalculateTerritories();
-}
 
-void TerritoryOverlay::ProcessTile(ssize_t i, ssize_t j)
-{
-	if (!m_TerritoryManager.m_Territories)
-		return;
-
-	u8 id = (m_TerritoryManager.m_Territories->get((int) i, (int) j) & ICmpTerritoryManager::TERRITORY_PLAYER_MASK);
+	static u8 colors[][3] = {
+		{ 255, 0, 0 },
+		{ 0, 255, 0 },
+		{ 0, 0, 255 },
+		{ 255, 255, 0 },
+		{ 255, 0, 255 },
+		{ 0, 255, 255 },
+		{ 255, 255, 255 },
+	};
 
-	float a = 0.2f;
-	switch (id)
+	for (size_t j = 0; j < h; ++j)
 	{
-	case 0:  break;
-	case 1:  RenderTile(CColor(1, 0, 0, a), false); break;
-	case 2:  RenderTile(CColor(0, 1, 0, a), false); break;
-	case 3:  RenderTile(CColor(0, 0, 1, a), false); break;
-	case 4:  RenderTile(CColor(1, 1, 0, a), false); break;
-	case 5:  RenderTile(CColor(0, 1, 1, a), false); break;
-	case 6:  RenderTile(CColor(1, 0, 1, a), false); break;
-	default: RenderTile(CColor(1, 1, 1, a), false); break;
+		for (size_t i = 0; i < w; ++i)
+		{
+			SColor4ub color;
+
+			u8 id = (m_TerritoryManager.m_Territories->get((int)i, (int)j) & ICmpTerritoryManager::TERRITORY_PLAYER_MASK);
+			int c = id % ARRAY_SIZE(colors);
+			color = SColor4ub(colors[c][0], colors[c][1], colors[c][2], 64);
+
+			*data++ = color.R;
+			*data++ = color.G;
+			*data++ = color.B;
+			*data++ = color.A;
+		}
 	}
 }
diff --git a/source/simulation2/components/CCmpUnitMotion.cpp b/source/simulation2/components/CCmpUnitMotion.cpp
index 9309b3e..8fc9755 100644
--- a/source/simulation2/components/CCmpUnitMotion.cpp
+++ b/source/simulation2/components/CCmpUnitMotion.cpp
@@ -38,6 +38,10 @@
 #include "ps/Profile.h"
 #include "renderer/Scene.h"
 
+// For debugging; units will start going straight to the target
+// instead of calling the pathfinder
+#define DISABLE_PATHFINDER 1
+
 /**
  * When advancing along the long path, and picking a new waypoint to move
  * towards, we'll pick one that's up to this far from the unit's current
@@ -1107,6 +1111,19 @@ void CCmpUnitMotion::BeginPathing(CFixedVector2D from, const ICmpPathfinder::Goa
 	if (!cmpObstruction.null())
 		cmpObstruction->SetMovingFlag(true);
 
+#if DISABLE_PATHFINDER
+	{
+		CmpPtr<ICmpPathfinder> cmpPathfinder (GetSimContext(), SYSTEM_ENTITY);
+		CFixedVector2D goalPos = cmpPathfinder->GetNearestPointOnGoal(from, m_FinalGoal);
+		m_LongPath.m_Waypoints.clear();
+		m_ShortPath.m_Waypoints.clear();
+		ICmpPathfinder::Waypoint wp = { goalPos.X, goalPos.Y };
+		m_ShortPath.m_Waypoints.push_back(wp);
+		m_PathState = PATHSTATE_FOLLOWING;
+		return;
+	}
+#endif
+
 	// If we're aiming at a target entity and it's close and we can reach
 	// it in a straight line, then we'll just go along the straight line
 	// instead of computing a path.
diff --git a/source/simulation2/components/ICmpObstructionManager.cpp b/source/simulation2/components/ICmpObstructionManager.cpp
index ba4e21c..b348212 100644
--- a/source/simulation2/components/ICmpObstructionManager.cpp
+++ b/source/simulation2/components/ICmpObstructionManager.cpp
@@ -21,6 +21,10 @@
 
 #include "simulation2/system/InterfaceScripted.h"
 
+#include "graphics/Terrain.h"
+
+const fixed ICmpObstructionManager::NAVCELL_SIZE = fixed::FromInt(TERRAIN_TILE_SIZE) / NAVCELLS_PER_TILE;
+
 BEGIN_INTERFACE_WRAPPER(ObstructionManager)
 DEFINE_INTERFACE_METHOD_1("SetPassabilityCircular", void, ICmpObstructionManager, SetPassabilityCircular, bool)
 DEFINE_INTERFACE_METHOD_1("SetDebugOverlay", void, ICmpObstructionManager, SetDebugOverlay, bool)
diff --git a/source/simulation2/components/ICmpObstructionManager.h b/source/simulation2/components/ICmpObstructionManager.h
index 50f8189..b0b99c5 100644
--- a/source/simulation2/components/ICmpObstructionManager.h
+++ b/source/simulation2/components/ICmpObstructionManager.h
@@ -55,6 +55,23 @@ class ICmpObstructionManager : public IComponent
 {
 public:
 	/**
+	 * The pathfinders operate primarily over a navigation grid (a uniform-cost
+	 * 2D passability grid, with horizontal/vertical (not diagonal) connectivity).
+	 * This is based on the terrain tile passability, plus the rasterised shapes of
+	 * obstructions, all expanded outwards by the radius of the units.
+	 * Since units are much smaller than terrain tiles, the nav grid should be
+	 * higher resolution than the tiles.
+	 * We therefore split each terrain tile into NxN "nav cells" (for some integer N,
+	 * preferably a power of two).
+	 */
+	static const int NAVCELLS_PER_TILE = 4;
+
+	/**
+	 * Size of a navcell in metres ( = TERRAIN_TILE_SIZE / NAVCELLS_PER_TILE)
+	 */
+	static const fixed NAVCELL_SIZE;
+
+	/**
 	 * External identifiers for shapes.
 	 * (This is a struct rather than a raw u32 for type-safety.)
 	 */
@@ -207,7 +224,11 @@ public:
 	 * @param grid the grid to be updated
 	 * @return true if any changes were made to the grid, false if it was already up-to-date
 	 */
-	virtual bool Rasterise(Grid<u8>& grid) = 0;
+	virtual bool RasteriseOld(Grid<u8>& grid) = 0;
+
+	virtual void Rasterise(Grid<u16>& grid, entity_pos_t expand, ICmpObstructionManager::flags_t requireMask, u8 setMask) = 0;
+
+	virtual bool NeedUpdate(size_t* dirtyID) = 0;
 
 	/**
 	 * Standard representation for all types of shapes, for use with geometry processing code.
@@ -325,6 +346,8 @@ public:
 	{
 		if (group == m_Group)
 			return false;
+		if (flags & ICmpObstructionManager::FLAG_BLOCK_PATHFINDING)
+			return false;
 		if (!(flags & ICmpObstructionManager::FLAG_BLOCK_MOVEMENT))
 			return false;
 		if ((flags & ICmpObstructionManager::FLAG_MOVING) && !m_AvoidMoving)
diff --git a/source/simulation2/components/ICmpPathfinder.h b/source/simulation2/components/ICmpPathfinder.h
index ee7a300..9acede9 100644
--- a/source/simulation2/components/ICmpPathfinder.h
+++ b/source/simulation2/components/ICmpPathfinder.h
@@ -1,4 +1,4 @@
-/* Copyright (C) 2011 Wildfire Games.
+/* Copyright (C) 2012 Wildfire Games.
  * This file is part of 0 A.D.
  *
  * 0 A.D. is free software: you can redistribute it and/or modify
@@ -20,6 +20,7 @@
 
 #include "simulation2/system/Interface.h"
 
+#include "simulation2/helpers/PathGoal.h"
 #include "simulation2/helpers/Position.h"
 
 #include "maths/FixedVector2D.h"
@@ -50,17 +51,7 @@ public:
 	typedef u16 pass_class_t;
 	typedef u8 cost_class_t;
 
-	struct Goal
-	{
-		enum Type {
-			POINT,
-			CIRCLE,
-			SQUARE
-		} type;
-		entity_pos_t x, z; // position of center
-		CFixedVector2D u, v; // if SQUARE, then orthogonal unit axes
-		entity_pos_t hw, hh; // if SQUARE, then half width & height; if CIRCLE, then hw is radius
-	};
+	typedef PathGoal Goal; // XXX
 
 	struct Waypoint
 	{
@@ -102,6 +93,8 @@ public:
 	 */
 	virtual void ComputePath(entity_pos_t x0, entity_pos_t z0, const Goal& goal, pass_class_t passClass, cost_class_t costClass, Path& ret) = 0;
 
+	virtual void ComputePathJPS(entity_pos_t x0, entity_pos_t z0, const Goal& goal, pass_class_t passClass, cost_class_t costClass, Path& ret) = 0;
+
 	/**
 	 * Asynchronous version of ComputePath.
 	 * The result will be sent as CMessagePathResult to 'notify'.
@@ -178,6 +171,10 @@ public:
 	 */
 	virtual void ProcessSameTurnMoves() = 0;
 
+	virtual void GetDebugData(u32& steps, double& time, Grid<u8>& grid) = 0;
+
+	virtual void GetDebugDataJPS(u32& steps, double& time, Grid<u8>& grid) = 0;
+
 	DECLARE_INTERFACE_TYPE(Pathfinder)
 };
 
diff --git a/source/simulation2/components/ICmpTerritoryManager.h b/source/simulation2/components/ICmpTerritoryManager.h
index 575755e..f96a630 100644
--- a/source/simulation2/components/ICmpTerritoryManager.h
+++ b/source/simulation2/components/ICmpTerritoryManager.h
@@ -29,6 +29,14 @@ class ICmpTerritoryManager : public IComponent
 public:
 	virtual bool NeedUpdate(size_t* dirtyID) = 0;
 
+	/**
+	 * Number of pathfinder navcells per territory tile.
+	 * Passability data is stored per navcell, but we probably don't need that much
+	 * resolution, and a lower resolution can make the boundary lines look prettier
+	 * and will take less memory, so we downsample the passability data.
+	 */
+	static const int NAVCELLS_PER_TERRITORY_TILE = 8;
+
 	static const int TERRITORY_PLAYER_MASK = 0x3F;
 	static const int TERRITORY_CONNECTED_MASK = 0x40;
 	static const int TERRITORY_PROCESSED_MASK = 0x80; //< For internal use; marks a tile as processed.
diff --git a/source/simulation2/components/tests/test_Pathfinder.h b/source/simulation2/components/tests/test_Pathfinder.h
index 4a6c7aa..5954e4e 100644
--- a/source/simulation2/components/tests/test_Pathfinder.h
+++ b/source/simulation2/components/tests/test_Pathfinder.h
@@ -26,6 +26,7 @@
 #include "lib/timer.h"
 #include "lib/tex/tex.h"
 #include "ps/Loader.h"
+#include "ps/Pyrogenesis.h"
 #include "simulation2/Simulation2.h"
 
 class TestCmpPathfinder : public CxxTest::TestSuite
@@ -139,4 +140,161 @@ public:
 		for (size_t i = 0; i < path.m_Waypoints.size(); ++i)
 			printf("# %d: %f %f\n", (int)i, path.m_Waypoints[i].x.ToFloat(), path.m_Waypoints[i].z.ToFloat());
 	}
+
+	template<typename T>
+	void DumpGrid(std::ostream& stream, const Grid<T>& grid, int mask)
+	{
+		for (u16 j = 0; j < grid.m_H; ++j)
+		{
+			for (u16 i = 0; i < grid.m_W; )
+			{
+				if (!(grid.get(i, j) & mask))
+				{
+					i++;
+					continue;
+				}
+
+				u16 i0 = i;
+				for (i = i0+1; ; ++i)
+				{
+					if (i >= grid.m_W || !(grid.get(i, j) & mask))
+					{
+						stream << "  <rect x='" << i0 << "' y='" << j << "' width='" << (i-i0) << "' height='1'/>\n";
+						break;
+					}
+				}
+			}
+		}
+	}
+
+#define USE_JPS 1
+
+	void test_perf2()
+	{
+		CTerrain terrain;
+
+		CSimulation2 sim2(NULL, &terrain);
+		sim2.LoadDefaultScripts();
+		sim2.ResetState();
+
+		CMapReader* mapReader = new CMapReader(); // it'll call "delete this" itself
+
+		LDR_BeginRegistering();
+		mapReader->LoadMap(L"maps/scenarios/Peloponnese.pmp", &terrain, NULL, NULL, NULL, NULL, NULL, NULL,
+			&sim2, &sim2.GetSimContext(), -1, false);
+		LDR_EndRegistering();
+		TS_ASSERT_OK(LDR_NonprogressiveLoad());
+
+		sim2.Update(0);
+
+		std::ofstream stream(OsString("perf2.html").c_str(), std::ofstream::out | std::ofstream::trunc);
+
+		CmpPtr<ICmpObstructionManager> cmpObstructionManager(sim2, SYSTEM_ENTITY);
+		CmpPtr<ICmpPathfinder> cmpPathfinder(sim2, SYSTEM_ENTITY);
+
+		ICmpPathfinder::pass_class_t obstructionsMask = cmpPathfinder->GetPassabilityClass("default") | ICmpObstructionManager::TILE_OBSTRUCTED_PATHFINDING;
+		const Grid<u16>& obstructions = cmpPathfinder->GetPassabilityGrid();
+
+		int scale = 1;
+		stream << "<!DOCTYPE html>\n";
+		stream << "<style>\n";
+		stream << ".passability rect { fill: black; }\n";
+		stream << ".astar-open rect { fill: rgba(255,255,0,0.75); }\n";
+		stream << ".astar-closed rect { fill: rgba(255,0,0,0.75); }\n";
+//		stream << ".astar-closed rect { fill: rgba(0,255,0,0.75); }\n";
+		stream << ".path { stroke: rgba(0,0,255,0.75); stroke-width: 1; fill: none; }\n";
+		stream << "</style>\n";
+		stream << "<svg width='" << obstructions.m_W*scale << "' height='" << obstructions.m_H*scale << "'>\n";
+		stream << "<g transform='translate(0 " << obstructions.m_H*scale << ") scale(" << scale << " -" << scale << ")'>\n";
+
+		stream << " <g class='passability'>\n";
+		DumpGrid(stream, obstructions, obstructionsMask);
+		stream << " </g>\n";
+
+		DumpPath(stream, 128, 256, 128, 384, cmpPathfinder);
+// 	  	RepeatPath(500, 128, 256, 128, 384, cmpPathfinder);
+// //		RepeatPath(100, 128, 256, 128, 384, cmpPathfinder);
+// 
+// 		DumpPath(stream, 128, 204, 192, 204, cmpPathfinder);
+// 
+// 		DumpPath(stream, 128, 230, 32, 230, cmpPathfinder);
+
+		stream << "</g>\n";
+		stream << "</svg>\n";
+	}
+
+	void DumpPath(std::ostream& stream, int i0, int j0, int i1, int j1, CmpPtr<ICmpPathfinder>& cmpPathfinder)
+	{
+		entity_pos_t x0 = entity_pos_t::FromInt(i0*TERRAIN_TILE_SIZE);
+		entity_pos_t z0 = entity_pos_t::FromInt(j0*TERRAIN_TILE_SIZE);
+		entity_pos_t x1 = entity_pos_t::FromInt(i1*TERRAIN_TILE_SIZE);
+		entity_pos_t z1 = entity_pos_t::FromInt(j1*TERRAIN_TILE_SIZE);
+
+		ICmpPathfinder::Goal goal = { ICmpPathfinder::Goal::POINT, x1, z1 };
+
+		ICmpPathfinder::Path path;
+#if USE_JPS
+		cmpPathfinder->ComputePathJPS(x0, z0, goal, cmpPathfinder->GetPassabilityClass("default"), cmpPathfinder->GetCostClass("default"), path);
+#else
+		cmpPathfinder->ComputePath(x0, z0, goal, cmpPathfinder->GetPassabilityClass("default"), cmpPathfinder->GetCostClass("default"), path);
+#endif
+
+		u32 debugSteps;
+		double debugTime;
+		Grid<u8> debugGrid;
+#if USE_JPS
+		cmpPathfinder->GetDebugDataJPS(debugSteps, debugTime, debugGrid);
+#else
+		cmpPathfinder->GetDebugData(debugSteps, debugTime, debugGrid);
+#endif
+// 		stream << " <g style='visibility:hidden'>\n";
+		stream << " <g>\n";
+		stream << " <g class='astar-open'>\n";
+		DumpGrid(stream, debugGrid, 1);
+		stream << " </g>\n";
+		stream << " <g class='astar-closed'>\n";
+		DumpGrid(stream, debugGrid, 2);
+		stream << " </g>\n";
+		stream << " </g>\n";
+
+		stream << " <polyline";
+		stream << " onmouseover='this.previousElementSibling.style.visibility=\"visible\"' onmouseout='this.previousElementSibling.style.visibility=\"hidden\"'";
+		double length = 0;
+		for (ssize_t i = 0; i < (ssize_t)path.m_Waypoints.size()-1; ++i)
+		{
+			double dx = (path.m_Waypoints[i+1].x - path.m_Waypoints[i].x).ToDouble();
+			double dz = (path.m_Waypoints[i+1].z - path.m_Waypoints[i].z).ToDouble();
+			ENSURE(dx == 0 || dz == 0 || abs(dx) == abs(dz));
+			length += sqrt(dx*dx + dz*dz);
+		}
+		stream << " title='length: " << length << "; tiles explored: " << debugSteps << "; time: " << debugTime*1000 << " msec'";
+		stream << " class='path' points='";
+		for (size_t i = 0; i < path.m_Waypoints.size(); ++i)
+			stream << path.m_Waypoints[i].x.ToDouble()*ICmpObstructionManager::NAVCELLS_PER_TILE/TERRAIN_TILE_SIZE << "," << path.m_Waypoints[i].z.ToDouble()*ICmpObstructionManager::NAVCELLS_PER_TILE/TERRAIN_TILE_SIZE << " ";
+		stream << "'/>\n";
+	}
+
+	void RepeatPath(int n, int i0, int j0, int i1, int j1, CmpPtr<ICmpPathfinder>& cmpPathfinder)
+	{
+		entity_pos_t x0 = entity_pos_t::FromInt(i0*TERRAIN_TILE_SIZE);
+		entity_pos_t z0 = entity_pos_t::FromInt(j0*TERRAIN_TILE_SIZE);
+		entity_pos_t x1 = entity_pos_t::FromInt(i1*TERRAIN_TILE_SIZE);
+		entity_pos_t z1 = entity_pos_t::FromInt(j1*TERRAIN_TILE_SIZE);
+
+		ICmpPathfinder::Goal goal = { ICmpPathfinder::Goal::POINT, x1, z1 };
+
+		double t = timer_Time();
+		for (int i = 0; i < n; ++i)
+		{
+			ICmpPathfinder::Path path;
+#if USE_JPS
+			cmpPathfinder->ComputePathJPS(x0, z0, goal, cmpPathfinder->GetPassabilityClass("default"), cmpPathfinder->GetCostClass("default"), path);
+#else
+			cmpPathfinder->ComputePath(x0, z0, goal, cmpPathfinder->GetPassabilityClass("default"), cmpPathfinder->GetCostClass("default"), path);
+#endif
+		}
+		t = timer_Time() - t;
+		debug_printf(L"### RepeatPath %fms each (%fs total)\n", 1000*t / n, t);
+	}
+
 };
diff --git a/source/simulation2/helpers/Grid.h b/source/simulation2/helpers/Grid.h
index fc087a2..2bbb700 100644
--- a/source/simulation2/helpers/Grid.h
+++ b/source/simulation2/helpers/Grid.h
@@ -48,6 +48,7 @@ public:
 
 	Grid(const Grid& g)
 	{
+		m_Data = NULL;
 		*this = g;
 	}
 
@@ -60,6 +61,7 @@ public:
 			m_DirtyID = g.m_DirtyID;
 			if (g.m_Data)
 			{
+				delete[] m_Data;
 				m_Data = new T[m_W * m_H];
 				memcpy(m_Data, g.m_Data, m_W*m_H*sizeof(T));
 			}
@@ -80,6 +82,13 @@ public:
 			memset(m_Data, 0, m_W*m_H*sizeof(T));
 	}
 
+	void unsetMask(T mask)
+	{
+		T inverse = ~mask;
+		for (int n = 0; n < m_W*m_H; ++n)
+			m_Data[n] &= inverse;
+	}
+
 	void set(int i, int j, const T& value)
 	{
 #if GRID_BOUNDS_DEBUG
@@ -102,6 +111,107 @@ public:
 	size_t m_DirtyID; // if this is < the id maintained by ICmpObstructionManager then it needs to be updated
 };
 
+class BitGrid
+{
+public:
+	BitGrid() : m_W(0), m_H(0), m_RowStride(0), m_Data(NULL)
+	{
+	}
+
+	BitGrid(u16 w, u16 h) : m_W(w), m_H(h), m_RowStride((w+7)/8), m_Data(NULL)
+	{
+		if (m_W || m_H)
+			m_Data = (u8*)malloc(m_RowStride * m_H);
+		reset();
+	}
+
+	BitGrid(const BitGrid& g)
+	{
+		*this = g;
+	}
+
+	BitGrid& operator=(const BitGrid& g)
+	{
+		if (this != &g)
+		{
+			m_W = g.m_W;
+			m_H = g.m_H;
+			m_RowStride = g.m_RowStride;
+			if (g.m_Data)
+			{
+				free(m_Data);
+				m_Data = (u8*)malloc(m_RowStride * m_H);
+				memcpy(m_Data, g.m_Data, m_RowStride * m_H);
+			}
+			else
+				m_Data = NULL;
+		}
+		return *this;
+	}
+
+	~BitGrid()
+	{
+		free(m_Data);
+	}
+
+	void reset()
+	{
+		if (m_Data)
+			memset(m_Data, 0, m_RowStride * m_H);
+	}
+
+	void set(int i, int j, bool value)
+	{
+#if GRID_BOUNDS_DEBUG
+		ENSURE(0 <= i && i < m_W && 0 <= j && j < m_H);
+#endif
+		m_Data[j*m_RowStride + (i >> 3)] &= ~(1 << (i & 7));
+		m_Data[j*m_RowStride + (i >> 3)] |= value << (i & 7);
+	}
+
+	bool get(int i, int j) const
+	{
+#if GRID_BOUNDS_DEBUG
+		ENSURE(0 <= i && i < m_W && 0 <= j && j < m_H);
+#endif
+		return (m_Data[j*m_RowStride + (i >> 3)] & (1 << (i & 7))) != 0;
+	}
+
+	u8 get8(int i, int j) const
+	{
+#if GRID_BOUNDS_DEBUG
+		ENSURE(0 <= i && i < m_W && 0 <= j && j < m_H);
+#endif
+		return m_Data[j*m_RowStride + (i >> 3)];
+	}
+
+	bool getz(int i, int j) const
+	{
+		if ((i & 7) == 7)
+		{
+			u8 a = m_Data[(j-1)*m_RowStride + (i >> 3)];
+			//u8 b = m_Data[j*m_RowStride + (i >> 3)];
+			u8 c = m_Data[(j+1)*m_RowStride + (i >> 3)];
+			u8 a2 = m_Data[(j-1)*m_RowStride + (i >> 3) + 1];
+			u8 c2 = m_Data[(j+1)*m_RowStride + (i >> 3) + 1];
+			return (!(a & 128) && (a2 & 1)) || (!(c & 128) && (c2 & 1));
+		}
+		else
+		{
+			u8 a = m_Data[(j-1)*m_RowStride + (i >> 3)];
+			//u8 b = m_Data[j*m_RowStride + (i >> 3)];
+			u8 c = m_Data[(j+1)*m_RowStride + (i >> 3)];
+			return ((a & (3 << (i & 7))) == (2 << (i & 7)) || (c & (3 << (i & 7))) == (2 << (i & 7)));
+		}
+	}
+
+	u16 m_W, m_H;
+	u16 m_RowStride;
+	u8* m_Data;
+
+	size_t m_DirtyID; // if this is < the id maintained by ICmpObstructionManager then it needs to be updated
+};
+
 /**
  * Similar to Grid, except optimised for sparse usage (the grid is subdivided into
  * buckets whose contents are only initialised on demand, to save on memset cost).
diff --git a/source/simulation2/helpers/PathGoal.cpp b/source/simulation2/helpers/PathGoal.cpp
new file mode 100644
index 0000000..b7cafa9
--- /dev/null
+++ b/source/simulation2/helpers/PathGoal.cpp
@@ -0,0 +1,91 @@
+/* Copyright (C) 2012 Wildfire Games.
+ * This file is part of 0 A.D.
+ *
+ * 0 A.D. 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 2 of the License, or
+ * (at your option) any later version.
+ *
+ * 0 A.D. 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 0 A.D.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "precompiled.h"
+
+#include "PathGoal.h"
+
+#include "graphics/Terrain.h"
+#include "simulation2/components/ICmpObstructionManager.h"
+#include "simulation2/helpers/Geometry.h"
+
+bool PathGoal::NavcellContainsGoal(u16 i, u16 j) const
+{
+	switch (type)
+	{
+	case POINT:
+	{
+		u16 gi, gj;
+		gi = (u16)(x * ICmpObstructionManager::NAVCELLS_PER_TILE / (int)TERRAIN_TILE_SIZE).ToInt_RoundToZero();
+		gj = (u16)(z * ICmpObstructionManager::NAVCELLS_PER_TILE / (int)TERRAIN_TILE_SIZE).ToInt_RoundToZero();
+		return gi == i && gj == j;
+	}
+	case CIRCLE:
+	{
+		entity_pos_t x0 = entity_pos_t::FromInt(i).Multiply(ICmpObstructionManager::NAVCELL_SIZE);
+		entity_pos_t z0 = entity_pos_t::FromInt(j).Multiply(ICmpObstructionManager::NAVCELL_SIZE);
+		entity_pos_t x1 = x0 + ICmpObstructionManager::NAVCELL_SIZE;
+		entity_pos_t z1 = z0 + ICmpObstructionManager::NAVCELL_SIZE;
+		entity_pos_t nx = Clamp(x, x0, x1);
+		entity_pos_t nz = Clamp(z, z0, z1);
+		return (CFixedVector2D(nx, nz) - CFixedVector2D(x, z)).CompareLength(hw) <= 0;
+	}
+	case SQUARE:
+	{
+		entity_pos_t x0 = entity_pos_t::FromInt(i).Multiply(ICmpObstructionManager::NAVCELL_SIZE);
+		entity_pos_t z0 = entity_pos_t::FromInt(j).Multiply(ICmpObstructionManager::NAVCELL_SIZE);
+		entity_pos_t x1 = x0 + ICmpObstructionManager::NAVCELL_SIZE;
+		entity_pos_t z1 = z0 + ICmpObstructionManager::NAVCELL_SIZE;
+		entity_pos_t nx = Clamp(x, x0, x1);
+		entity_pos_t nz = Clamp(z, z0, z1);
+		return Geometry::PointIsInSquare(CFixedVector2D(nx - x, nz - z), u, v, CFixedVector2D(hw, hh));
+	}
+	default:
+	{
+		debug_warn(L"invalid type");
+		return false;
+	}
+	}
+}
+
+bool PathGoal::RectContainsGoal(entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1) const
+{
+	switch (type)
+	{
+	case POINT:
+	{
+		return x0 <= x && x <= x1 && z0 <= z && z <= z1;
+	}
+	case CIRCLE:
+	{
+		entity_pos_t nx = Clamp(x, x0, x1);
+		entity_pos_t nz = Clamp(z, z0, z1);
+		return (CFixedVector2D(nx, nz) - CFixedVector2D(x, z)).CompareLength(hw) <= 0;
+	}
+	case SQUARE:
+	{
+		entity_pos_t nx = Clamp(x, x0, x1);
+		entity_pos_t nz = Clamp(z, z0, z1);
+		return Geometry::PointIsInSquare(CFixedVector2D(nx - x, nz - z), u, v, CFixedVector2D(hw, hh));
+	}
+	default:
+	{
+		debug_warn(L"invalid type");
+		return false;
+	}
+	}
+}
diff --git a/source/simulation2/helpers/PathGoal.h b/source/simulation2/helpers/PathGoal.h
new file mode 100644
index 0000000..353c71d
--- /dev/null
+++ b/source/simulation2/helpers/PathGoal.h
@@ -0,0 +1,48 @@
+/* Copyright (C) 2012 Wildfire Games.
+ * This file is part of 0 A.D.
+ *
+ * 0 A.D. 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 2 of the License, or
+ * (at your option) any later version.
+ *
+ * 0 A.D. 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 0 A.D.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef INCLUDED_PATHGOAL
+#define INCLUDED_PATHGOAL
+
+#include "maths/FixedVector2D.h"
+#include "simulation2/helpers/Position.h"
+
+class PathGoal
+{
+public:
+	enum Type {
+		POINT,
+		CIRCLE,
+		SQUARE
+	} type;
+
+	entity_pos_t x, z; // position of center
+
+	CFixedVector2D u, v; // if SQUARE, then orthogonal unit axes
+
+	entity_pos_t hw, hh; // if SQUARE, then half width & height; if CIRCLE, then hw is radius
+
+	bool NavcellContainsGoal(u16 i, u16 j) const;
+
+	/**
+	 * Returns true if the rectangle defined by (x0,z0)-(x1,z1) (inclusive)
+	 * contains a part of the goal area.
+	 */
+	bool RectContainsGoal(entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1) const;
+};
+
+#endif // INCLUDED_PATHGOAL
diff --git a/source/simulation2/helpers/PriorityQueue.h b/source/simulation2/helpers/PriorityQueue.h
index 9e7c8e3..0d10816 100644
--- a/source/simulation2/helpers/PriorityQueue.h
+++ b/source/simulation2/helpers/PriorityQueue.h
@@ -32,14 +32,13 @@
 template <typename Item, typename CMP>
 struct QueueItemPriority
 {
-	bool operator()(const Item& a, const Item& b)
+	bool operator()(const Item& a, const Item& b) const
 	{
 		if (CMP()(b.rank, a.rank)) // higher costs are lower priority
 			return true;
 		if (CMP()(a.rank, b.rank))
 			return false;
 		// Need to tie-break to get a consistent ordering
-		// TODO: Should probably tie-break on g or h or something, but don't bother for now
 		if (a.id < b.id)
 			return true;
 		if (b.id < a.id)
@@ -73,19 +72,10 @@ public:
 		push_heap(m_Heap.begin(), m_Heap.end(), QueueItemPriority<Item, CMP>());
 	}
 
-	Item* find(ID id)
-	{
-		for (size_t n = 0; n < m_Heap.size(); ++n)
-		{
-			if (m_Heap[n].id == id)
-				return &m_Heap[n];
-		}
-		return NULL;
-	}
-
-	void promote(ID id, R newrank)
+	void promote(ID id, R UNUSED(oldrank), R newrank)
 	{
-		for (size_t n = 0; n < m_Heap.size(); ++n)
+		// Loop backwards since it seems a little faster in practice
+		for (ssize_t n = m_Heap.size() - 1; n >= 0; --n)
 		{
 			if (m_Heap[n].id == id)
 			{
@@ -93,7 +83,7 @@ public:
 				ENSURE(m_Heap[n].rank > newrank);
 #endif
 				m_Heap[n].rank = newrank;
-				push_heap(m_Heap.begin(), m_Heap.begin()+n+1, QueueItemPriority<Item, CMP>());
+ 				push_heap(m_Heap.begin(), m_Heap.begin()+n+1, QueueItemPriority<Item, CMP>());
 				return;
 			}
 		}
@@ -104,8 +94,8 @@ public:
 #if PRIORITYQUEUE_DEBUG
 		ENSURE(m_Heap.size());
 #endif
-		Item r = m_Heap.front();
-		pop_heap(m_Heap.begin(), m_Heap.end(), QueueItemPriority<Item, CMP>());
+		Item r = m_Heap[0];
+ 		pop_heap(m_Heap.begin(), m_Heap.end(), QueueItemPriority<Item, CMP>());
 		m_Heap.pop_back();
 		return r;
 	}
@@ -155,7 +145,7 @@ public:
 		return NULL;
 	}
 
-	void promote(ID id, R newrank)
+	void promote(ID id, R UNUSED(oldrank), R newrank)
 	{
 		find(id)->rank = newrank;
 	}
diff --git a/source/simulation2/helpers/Render.cpp b/source/simulation2/helpers/Render.cpp
index a8d9b3431..7a3c4dd 100644
--- a/source/simulation2/helpers/Render.cpp
+++ b/source/simulation2/helpers/Render.cpp
@@ -100,6 +100,52 @@ void SimRender::ConstructCircleOnGround(const CSimContext& context, float x, flo
 	}
 }
 
+void SimRender::ConstructClosedArcOnGround(const CSimContext& context, float x, float z, float radius,
+	float start, float end,
+	SOverlayLine& overlay, bool floating, float heightOffset)
+{
+	overlay.m_Coords.clear();
+
+	CmpPtr<ICmpTerrain> cmpTerrain(context, SYSTEM_ENTITY);
+	if (cmpTerrain.null())
+		return;
+
+	float water = 0.f;
+	if (floating)
+	{
+		CmpPtr<ICmpWaterManager> cmpWaterMan(context, SYSTEM_ENTITY);
+		if (!cmpWaterMan.null())
+			water = cmpWaterMan->GetExactWaterLevel(x, z);
+	}
+
+	// Adapt the circle resolution to look reasonable for small and largeish radiuses
+	size_t numPoints = clamp((size_t)(radius*4.0f), (size_t)12, (size_t)48);
+
+	overlay.m_Coords.reserve((numPoints + 1 + 2) * 3);
+
+	// Start at the center point
+	float cy = std::max(water, cmpTerrain->GetExactGroundLevel(x, z)) + heightOffset;
+	overlay.m_Coords.push_back(x);
+	overlay.m_Coords.push_back(cy);
+	overlay.m_Coords.push_back(z);
+
+	for (size_t i = 0; i <= numPoints; ++i) // use '<=' so it's a closed loop
+	{
+		float a = start + (float)i * (end - start) / (float)numPoints;
+		float px = x + radius * cosf(a);
+		float pz = z + radius * sinf(a);
+		float py = std::max(water, cmpTerrain->GetExactGroundLevel(px, pz)) + heightOffset;
+		overlay.m_Coords.push_back(px);
+		overlay.m_Coords.push_back(py);
+		overlay.m_Coords.push_back(pz);
+	}
+
+	// Return to the center point
+	overlay.m_Coords.push_back(x);
+	overlay.m_Coords.push_back(cy);
+	overlay.m_Coords.push_back(z);
+}
+
 // This method splits up a straight line into a number of line segments each having a length ~= TERRAIN_TILE_SIZE
 static void SplitLine(std::vector<std::pair<float, float> >& coords, float x1, float y1, float x2, float y2)
 {
diff --git a/source/simulation2/helpers/Render.h b/source/simulation2/helpers/Render.h
index 2ce2837..0283316 100644
--- a/source/simulation2/helpers/Render.h
+++ b/source/simulation2/helpers/Render.h
@@ -72,6 +72,14 @@ void ConstructCircleOnGround(const CSimContext& context, float x, float z, float
 		bool floating, float heightOffset = 0.25f);
 
 /**
+ * Updates @p overlay so that it represents the given circle, flattened on the terrain.
+ */
+void ConstructClosedArcOnGround(const CSimContext& context, float x, float z, float radius,
+		float start, float end,
+		SOverlayLine& overlay,
+		bool floating, float heightOffset = 0.25f);
+
+/**
  * Updates @p overlay so that it represents the given square, flattened on the terrain.
  * @p x and @p z are position of center, @p w and @p h are size of rectangle, @p a is clockwise angle.
  */
diff --git a/source/tools/autobuild2/manage.cgi b/source/tools/autobuild2/manage.cgi
old mode 100755
new mode 100644
diff --git a/source/tools/autobuild2/terminator.pl b/source/tools/autobuild2/terminator.pl
old mode 100755
new mode 100644
diff --git a/source/tools/dist/build.sh b/source/tools/dist/build.sh
old mode 100755
new mode 100644
diff --git a/source/tools/entdocs/build.sh b/source/tools/entdocs/build.sh
old mode 100755
new mode 100644
diff --git a/source/tools/entity/checkrefs.pl b/source/tools/entity/checkrefs.pl
old mode 100755
new mode 100644
