diff --git a/src/foundry_fixes.js b/src/foundry_fixes.js
index 7098aeb..9cafa59 100644
--- a/src/foundry_fixes.js
+++ b/src/foundry_fixes.js
@@ -16,3 +16,21 @@ export function getGridPositionFromPixels(xPixel, yPixel) {
 		return [y, x]
 	return [x, y]
 }
+
+export function getGridPositionFromPixelsObj(o) {
+	const r = {};
+	[r.x, r.y] = getGridPositionFromPixels(o.x, o.y);
+	return r;
+}
+
+export function getPixelsFromGridPositionObj(o) {
+	const r = {};
+	[r.x, r.y] = getPixelsFromGridPosition(o.x, o.y);
+	return r;
+}
+
+export function getCenterFromGridPositionObj(o) {
+	const r = getPixelsFromGridPositionObj(o);
+	[r.x, r.y] = canvas.grid.getCenter(r.x, r.y);
+	return r;
+}
diff --git a/src/foundry_imports.js b/src/foundry_imports.js
index 8fd1002..9452ea8 100644
--- a/src/foundry_imports.js
+++ b/src/foundry_imports.js
@@ -1,8 +1,9 @@
 import {highlightMeasurementTerrainRuler, measureDistances} from "./compatibility.js";
-import {getGridPositionFromPixels} from "./foundry_fixes.js";
+import {getCenterFromGridPositionObj, getGridPositionFromPixels, getGridPositionFromPixelsObj} from "./foundry_fixes.js";
 import {Line} from "./geometry.js";
 import {disableSnap, moveWithoutAnimation} from "./keybindings.js";
 import {trackRays} from "./movement_tracking.js"
+import {find_path} from "./pathfinding.js";
 import {recalculate} from "./socket.js";
 import {applyTokenSizeOffset, enumeratedZip, getSnapPointForEntity, getSnapPointForToken, getTokenShape, highlightTokenShape, sum} from "./util.js";
 
@@ -131,7 +132,7 @@ function scheduleMeasurement(destination, event) {
 	const mt = event._measureTime || 0;
 	const originalEvent = event.data.originalEvent;
 	if (Date.now() - mt > measurementInterval) {
-		this.measure(destination, {snap: !disableSnap});
+		this.measure(destination, {snap: !disableSnap, pathfinding: game.keyboard.isDown("y")});
 		event._measureTime = Date.now();
 		this._state = Ruler.STATES.MEASURING;
 		cancelScheduledMeasurement.call(this);
@@ -153,14 +154,32 @@ export function cancelScheduledMeasurement() {
 
 // This is a modified version of Ruler.measure form foundry 0.7.9
 export function measure(destination, options={}) {
+
 	const isToken = this.draggedEntity instanceof Token;
 	if (isToken && !this.draggedEntity.isVisible)
 		return []
 
+	options.pathfinding = options.pathfinding ?? false;
 	if (options.snap) {
 		destination = getSnapPointForEntity(destination.x, destination.y, this.draggedEntity);
 	}
 
+	// Remove waypoints generated by pathfinding
+	this.waypoints = this.waypoints.filter(waypoint => !waypoint.isPathfinding);
+
+	if (isToken && options.pathfinding) {
+		let path = find_path(getGridPositionFromPixelsObj(this.waypoints[this.waypoints.length - 1]), getGridPositionFromPixelsObj(destination));
+		if (path) {
+			path = path.map(point => getCenterFromGridPositionObj(point));
+			path.forEach(point => point.isPathfinding = true);
+			this.waypoints = this.waypoints.concat(path);
+		}
+		else {
+			// Don't show a path if the pathfinding yields no result to show the user that the destination is unreachable
+			destination = this.waypoints[this.waypoints.length - 1];
+		}
+	}
+
 	if(options.gridSpaces === undefined) {
 		options.gridSpaces = canvas.grid.type !== CONST.GRID_TYPES.GRIDLESS;
 	}
diff --git a/src/pathfinding.js b/src/pathfinding.js
new file mode 100644
index 0000000..340126b
--- /dev/null
+++ b/src/pathfinding.js
@@ -0,0 +1,91 @@
+import {getCenterFromGridPositionObj} from "./foundry_fixes.js";
+
+// TODO Wipe cache if walls layer is being modified
+let cached_nodes = undefined;
+
+function get_node(pos, initialize=true) {
+	if (!cached_nodes)
+		// TODO Check if ceil is the right thing to do here
+		cached_nodes = new Array(Math.ceil(canvas.dimensions.sceneHeight / canvas.dimensions.size));
+	if (!cached_nodes[pos.y])
+		cached_nodes[pos.y] = new Array(Math.ceil(canvas.dimensions.sceneWidth / canvas.dimensions.size));
+	if (!cached_nodes[pos.y][pos.x]) {
+		cached_nodes[pos.y][pos.x] = {x: pos.x, y: pos.y};
+	}
+
+	const node = cached_nodes[pos.y][pos.x];
+	if (initialize && !node.edges) {
+		node.edges = [];
+		for (const neighborPos of neighbors(pos)) {
+			// TODO Work with pixels instead of grid locations
+			if (!canvas.walls.checkCollision(new Ray(getCenterFromGridPositionObj(pos), getCenterFromGridPositionObj(neighborPos)))) {
+				const neighbor = get_node(neighborPos, false);
+				node.edges.push({target: neighbor, cost: 1});
+			}
+		}
+	}
+	return node;
+}
+
+function* neighbors(pos) {
+	for (let y = -1;y < 2;y++) {
+		for (let x = -1;x < 2;x++) {
+			if (x != 0 || y != 0)
+				yield {x: pos.x + x, y: pos.y + y};
+		}
+	}
+}
+
+function calculate_path(from, to) {
+	const nextNodes = [{node: get_node(to), cost: 0, estimated: estimate_cost(to, from), previous: null}];
+	const previousNodes = new Set();
+	while (nextNodes.length > 0) {
+		// Sort by estimated cost, high to low
+		// TODO Re-sorting every iteration is expensive. Think of something better
+		nextNodes.sort((a, b) => b.estimated - a.estimated);
+		// Get node with cheapest estimate
+		const currentNode = nextNodes.pop();
+		if (currentNode.node.x === from.x && currentNode.node.y === from.y)
+			return currentNode;
+		previousNodes.add(currentNode.node);
+		for (const edge of currentNode.node.edges) {
+			const neighborNode = get_node(edge.target);
+			if (previousNodes.has(neighborNode))
+				continue;
+			// TODO We currently assume a cost of one for all transitions. Change this for 5/10/5 or difficult terrain support
+			const neighbor = {node: neighborNode, cost: currentNode.cost + 1, estimated: currentNode.cost + 1 + estimate_cost(neighborNode, from), previous: currentNode};
+			const neighborIndex = nextNodes.findIndex(node => node.node === neighbor.node);
+			if (neighborIndex >= 0) {
+				// If the neighbor is cheaper to reach via the current route than through previously discovered routes, replace it
+				if (nextNodes[neighborIndex].cost > neighbor.cost) {
+					nextNodes[neighborIndex] = neighbor;
+				}
+			}
+			else {
+				nextNodes.push(neighbor);
+			}
+		}
+	}
+}
+
+function estimate_cost(pos, target) {
+	return Math.max(Math.abs(pos.x - target.x), Math.abs(pos.y - target.y));
+}
+
+export function find_path(from, to) {
+	const lastNode = calculate_path(from, to);
+	if (!lastNode)
+		return null;
+	const path = [];
+	let currentNode = lastNode;
+	while (currentNode) {
+		// TODO Check if the distance doesn't change
+		if (path.length > 2 && !canvas.walls.checkCollision(new Ray(getCenterFromGridPositionObj(currentNode.node), getCenterFromGridPositionObj(path[path.length - 2]))))
+			// Replace last waypoint if the current waypoint leads to a valid path
+			path[path.length - 1] = {x: currentNode.node.x, y: currentNode.node.y};
+		else
+			path.push({x: currentNode.node.x, y: currentNode.node.y});
+		currentNode = currentNode.previous;
+	}
+	return path;
+}