diff --git a/core/src/gdx/diablo/map/BinaryHeap.java b/core/src/gdx/diablo/map/BinaryHeap.java
new file mode 100644
index 00000000..31d0a53a
--- /dev/null
+++ b/core/src/gdx/diablo/map/BinaryHeap.java
@@ -0,0 +1,222 @@
+/*******************************************************************************
+ * Copyright 2011 See AUTHORS file.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *   http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ ******************************************************************************/
+
+package gdx.diablo.map;
+
+/** @author Nathan Sweet */
+public class BinaryHeap<T extends BinaryHeap.Node> {
+	public int size;
+
+	private Node[] nodes;
+	private final boolean isMaxHeap;
+
+	public BinaryHeap () {
+		this(16, false);
+	}
+
+	public BinaryHeap (int capacity, boolean isMaxHeap) {
+		this.isMaxHeap = isMaxHeap;
+		nodes = new Node[capacity];
+	}
+
+	public T add (T node) {
+		// Expand if necessary.
+		if (size == nodes.length) {
+			Node[] newNodes = new Node[size << 1];
+			System.arraycopy(nodes, 0, newNodes, 0, size);
+			nodes = newNodes;
+		}
+		// Insert at end and bubble up.
+		node.index = size;
+		nodes[size] = node;
+		up(size++);
+		return node;
+	}
+
+	public T add (T node, float value) {
+		node.value = value;
+		return add(node);
+	}
+
+	/** Returns if binary heap contains the provided node.
+	 * @param node May be null.
+	 * @param identity If true, == comparison will be used. If false, .equals() comparison will be used. */
+	public boolean contains (T node, boolean identity) {
+		if (identity || node == null) {
+			for (Node n : nodes)
+				if (n == node) return true;
+		} else {
+			for (Node n : nodes)
+				if (n != null && n.equals(node)) return true;
+		}
+		return false;
+	}
+
+	public T peek () {
+		if (size == 0) throw new IllegalStateException("The heap is empty.");
+		return (T)nodes[0];
+	}
+
+	public T pop () {
+		return remove(0);
+	}
+
+	public T remove (T node) {
+		return remove(node.index);
+	}
+
+	private T remove (int index) {
+		Node[] nodes = this.nodes;
+		Node removed = nodes[index];
+		nodes[index] = nodes[--size];
+		nodes[size] = null;
+		if (size > 0 && index < size) down(index);
+		return (T)removed;
+	}
+
+	/** Returns true if the heap is empty. */
+	public boolean isEmpty () {
+		return size == 0;
+	}
+
+	public void clear () {
+		Node[] nodes = this.nodes;
+		for (int i = 0, n = size; i < n; i++)
+			nodes[i] = null;
+		size = 0;
+	}
+
+	public void setValue (T node, float value) {
+		float oldValue = node.value;
+		node.value = value;
+		if (value < oldValue ^ isMaxHeap)
+			up(node.index);
+		else
+			down(node.index);
+	}
+
+	private void up (int index) {
+		Node[] nodes = this.nodes;
+		Node node = nodes[index];
+		float value = node.value;
+		while (index > 0) {
+			int parentIndex = (index - 1) >> 1;
+			Node parent = nodes[parentIndex];
+			if (value < parent.value ^ isMaxHeap) {
+				nodes[index] = parent;
+				parent.index = index;
+				index = parentIndex;
+			} else
+				break;
+		}
+		nodes[index] = node;
+		node.index = index;
+	}
+
+	private void down (int index) {
+		Node[] nodes = this.nodes;
+		int size = this.size;
+
+		Node node = nodes[index];
+		float value = node.value;
+
+		while (true) {
+			int leftIndex = 1 + (index << 1);
+			if (leftIndex >= size) break;
+			int rightIndex = leftIndex + 1;
+
+			// Always have a left child.
+			Node leftNode = nodes[leftIndex];
+			float leftValue = leftNode.value;
+
+			// May have a right child.
+			Node rightNode;
+			float rightValue;
+			if (rightIndex >= size) {
+				rightNode = null;
+				rightValue = isMaxHeap ? Float.MIN_VALUE : Float.MAX_VALUE;
+			} else {
+				rightNode = nodes[rightIndex];
+				rightValue = rightNode.value;
+			}
+
+			// The smallest of the three values is the parent.
+			if (leftValue < rightValue ^ isMaxHeap) {
+				if (leftValue == value || (leftValue > value ^ isMaxHeap)) break;
+				nodes[index] = leftNode;
+				leftNode.index = index;
+				index = leftIndex;
+			} else {
+				if (rightValue == value || (rightValue > value ^ isMaxHeap)) break;
+				nodes[index] = rightNode;
+				rightNode.index = index;
+				index = rightIndex;
+			}
+		}
+
+		nodes[index] = node;
+		node.index = index;
+	}
+
+	@Override
+	public boolean equals (Object obj) {
+		if (!(obj instanceof BinaryHeap)) return false;
+		BinaryHeap other = (BinaryHeap)obj;
+		if (other.size != size) return false;
+		for (int i = 0, n = size; i < n; i++)
+			if (other.nodes[i].value != nodes[i].value) return false;
+		return true;
+	}
+
+	public int hashCode () {
+		int h = 1;
+		for (int i = 0, n = size; i < n; i++)
+			h = h * 31 + Float.floatToIntBits(nodes[i].value);
+		return h;
+	}
+
+	public String toString () {
+		if (size == 0) return "[]";
+		Node[] nodes = this.nodes;
+		StringBuilder buffer = new StringBuilder(32);
+		buffer.append('[');
+		buffer.append(nodes[0].value);
+		for (int i = 1; i < size; i++) {
+			buffer.append(", ");
+			buffer.append(nodes[i].value);
+		}
+		buffer.append(']');
+		return buffer.toString();
+	}
+
+	/** @author Nathan Sweet */
+	static public class Node {
+		float value;
+		int index;
+
+		public Node (float value) {
+			this.value = value;
+		}
+
+		public float getValue () {
+			return value;
+		}
+
+		public String toString () {
+			return Float.toString(value);
+		}
+	}
+}
\ No newline at end of file
diff --git a/core/src/gdx/diablo/map/Map.java b/core/src/gdx/diablo/map/Map.java
index 960094f4..010bb246 100644
--- a/core/src/gdx/diablo/map/Map.java
+++ b/core/src/gdx/diablo/map/Map.java
@@ -546,9 +546,14 @@ public class Map implements Disposable {
     }
   }
 
-  public GraphPath<MapUtils.Point2> path(Vector3 src, Vector3 dst) {
+  public GraphPath<Point2> path(Vector3 src, Vector3 dst) {
     //return new MapGraph(this).path(src, dst);
-    return MapUtils.path(this, src, dst, new DefaultGraphPath<MapUtils.Point2>());
+    //return MapUtils.path(this, src, dst, new DefaultGraphPath<MapUtils.Point2>());
+    long start = System.currentTimeMillis();
+    GraphPath<Point2> path = new DefaultGraphPath<>();
+    new MapPather(this).path(src, dst, path);
+    System.out.println("time = " + (System.currentTimeMillis() - start) + "ms");
+    return path;
   }
 
   static class Zone {
diff --git a/core/src/gdx/diablo/map/MapPather.java b/core/src/gdx/diablo/map/MapPather.java
new file mode 100644
index 00000000..53caa708
--- /dev/null
+++ b/core/src/gdx/diablo/map/MapPather.java
@@ -0,0 +1,109 @@
+package gdx.diablo.map;
+
+import com.badlogic.gdx.ai.pfa.GraphPath;
+import com.badlogic.gdx.math.Vector3;
+import com.badlogic.gdx.utils.Array;
+import com.badlogic.gdx.utils.ObjectFloatMap;
+import com.badlogic.gdx.utils.ObjectMap;
+
+public class MapPather {
+  Map map;
+  Heuristic heuristic = new EuclideanHeuristic();
+
+  public MapPather(Map map) {
+    this.map = map;
+  }
+
+  public boolean path(Vector3 src, Vector3 dst, GraphPath<Point2> path) {
+    return path(new Point2(src), new Point2(dst), path);
+  }
+
+  public boolean path(Point2 src, Point2 dst, GraphPath<Point2> path) {
+    BinaryHeap<Point2> closedSet = new BinaryHeap<>();
+    BinaryHeap<Point2> openSet   = new BinaryHeap<>();
+    openSet.add(src);
+
+    closedSet.contains(dst, false);
+
+    ObjectMap<Point2, Point2> cameFrom = new ObjectMap<>();
+
+    ObjectFloatMap<Point2> gScore = new ObjectFloatMap<>();
+    gScore.put(src, 0);
+
+    ObjectFloatMap<Point2> fScore = new ObjectFloatMap<>();
+    fScore.put(src, heuristic.estimate(src, dst));
+
+    Array<Point2> neighbors = new Array<>(8);
+
+    while (openSet.size > 0) {
+      Point2 current = openSet.pop();
+      if (current.equals(dst)) {
+        buildPath(current, cameFrom, path);
+        return true;
+      }
+
+      getNeighbors(current, neighbors);
+      for (Point2 neighbor : neighbors) {
+        if (closedSet.contains(neighbor, false)) {
+          continue;
+        }
+
+        float tent_gScore = gScore.get(current, Float.POSITIVE_INFINITY) + Point2.dst(current, neighbor);
+        if (!openSet.contains(neighbor, false)) {
+          openSet.add(neighbor);
+        } else if (tent_gScore >= gScore.get(neighbor, Float.POSITIVE_INFINITY)) {
+          continue;
+        }
+
+        cameFrom.put(neighbor, current);
+        gScore.put(neighbor, tent_gScore);
+        fScore.put(neighbor, gScore.get(neighbor, Float.POSITIVE_INFINITY) + heuristic.estimate(neighbor, dst));
+      }
+
+      closedSet.add(current);
+    }
+
+    return false;
+  }
+
+  private void buildPath(Point2 src, ObjectMap<Point2, Point2> cameFrom, GraphPath<Point2> path) {
+    path.add(src);
+    while (cameFrom.containsKey(src)) {
+      src = cameFrom.get(src);
+      path.add(src);
+    }
+  }
+
+  private void getNeighbors(Point2 src, Array<Point2> dst) {
+    dst.size = 0;
+    addNeighbor(src, src.x - 1, src.y - 1, dst);
+    addNeighbor(src, src.x - 1, src.y    , dst);
+    addNeighbor(src, src.x - 1, src.y + 1, dst);
+    addNeighbor(src, src.x    , src.y - 1, dst);
+    addNeighbor(src, src.x    , src.y + 1, dst);
+    addNeighbor(src, src.x + 1, src.y - 1, dst);
+    addNeighbor(src, src.x + 1, src.y    , dst);
+    addNeighbor(src, src.x + 1, src.y + 1, dst);
+  }
+
+  private void addNeighbor(Point2 src, int x, int y, Array<Point2> dst) {
+    Map.Zone zone = map.getZone(x, y);
+    if (zone == null) return;
+    if (zone.flags(x, y) == 0) {
+      float cost = src.getValue() + ((x != src.x && y != src.y) ? 1.414213562373095f : 1f);
+      Point2 point = new Point2(x, y, cost);
+      dst.add(point);
+    }
+  }
+
+  interface Heuristic {
+	float estimate(Point2 src, Point2 dst);
+  }
+
+  static class EuclideanHeuristic implements Heuristic {
+    @Override
+    public float estimate(Point2 src, Point2 dst) {
+      return Point2.dst(src, dst);
+    }
+  }
+}
diff --git a/core/src/gdx/diablo/map/MapRenderer.java b/core/src/gdx/diablo/map/MapRenderer.java
index 44c7be49..27bc7828 100644
--- a/core/src/gdx/diablo/map/MapRenderer.java
+++ b/core/src/gdx/diablo/map/MapRenderer.java
@@ -937,12 +937,12 @@ public class MapRenderer {
     }
   }
 
-  public void renderDebugPath2(ShapeRenderer shapes, GraphPath<MapUtils.Point2> path) {
+  public void renderDebugPath2(ShapeRenderer shapes, GraphPath<Point2> path) {
     shapes.setColor(Color.TAN);
     shapes.set(ShapeRenderer.ShapeType.Filled);
     final int size = path.getCount();
     for (int i = 0; i < size; i++) {
-      MapUtils.Point2 point = path.get(i);
+      Point2 point = path.get(i);
       float px = +(point.x * Tile.SUBTILE_WIDTH50)  - (point.y * Tile.SUBTILE_WIDTH50)  - Tile.SUBTILE_WIDTH50;
       float py = -(point.x * Tile.SUBTILE_HEIGHT50) - (point.y * Tile.SUBTILE_HEIGHT50) - Tile.SUBTILE_HEIGHT50;
       drawDiamondSolid(shapes, px, py, Tile.SUBTILE_WIDTH, Tile.SUBTILE_HEIGHT);
diff --git a/core/src/gdx/diablo/map/MapUtils.java b/core/src/gdx/diablo/map/MapUtils.java
index c9f02a99..e9379ff4 100644
--- a/core/src/gdx/diablo/map/MapUtils.java
+++ b/core/src/gdx/diablo/map/MapUtils.java
@@ -30,6 +30,7 @@ public class MapUtils {
 
     Point2 next = null;
     Point2 last = coords.removeIndex(coords.size - 1);
+    path.add(last);
     while (true) {
       for (Point2 coord : coords) {
         if (last.adjacent(coord)) {
diff --git a/core/src/gdx/diablo/map/Point2.java b/core/src/gdx/diablo/map/Point2.java
new file mode 100644
index 00000000..3057b2a4
--- /dev/null
+++ b/core/src/gdx/diablo/map/Point2.java
@@ -0,0 +1,48 @@
+package gdx.diablo.map;
+
+import com.badlogic.gdx.math.Vector3;
+
+public class Point2 extends BinaryHeap.Node {
+  final int x;
+  final int y;
+  final int hash;
+
+  Point2(int x, int y, float cost) {
+    super(cost);
+    this.x = x;
+    this.y = y;
+    this.hash = hash();
+  }
+
+  Point2(Vector3 src) {
+    this((int) src.x, (int) src.y, 0);
+  }
+
+  private int hash() {
+    return 31 * x + y;
+  }
+
+  @Override
+  public int hashCode() {
+    return hash;
+  }
+
+  @Override
+  public boolean equals(Object obj) {
+    if (obj == null) return false;
+    if (obj == this) return true;
+    if (!(obj instanceof Point2)) return false;
+    Point2 other = (Point2) obj;
+    return x == other.x && y == other.y;
+  }
+
+  public boolean equals(int x, int y) {
+    return this.x == x && this.y == y;
+  }
+
+  public static float dst(Point2 src, Point2 dst) {
+    final float dx = dst.x - src.x;
+    final float dy = dst.y - src.y;
+    return (float) Math.sqrt(dx * dx + dy * dy);
+  }
+}
diff --git a/mapbuilder/src/gdx/diablo/map/MapViewer.java b/mapbuilder/src/gdx/diablo/map/MapViewer.java
index f2e3e622..bbc93e22 100644
--- a/mapbuilder/src/gdx/diablo/map/MapViewer.java
+++ b/mapbuilder/src/gdx/diablo/map/MapViewer.java
@@ -76,7 +76,7 @@ public class MapViewer extends ApplicationAdapter {
 
   Vector3 src;
   Vector3 dst;
-  GraphPath<MapUtils.Point2> path;
+  GraphPath<Point2> path;
 
   boolean drawCrosshair;
   boolean drawGrid;