diff --git a/js/main.js b/js/main.js
index d3fd1f9..924afc1 100644
--- a/js/main.js
+++ b/js/main.js
@@ -13,12 +13,12 @@ import {recalculate} from "./socket.js";
 import {SpeedProvider} from "./speed_provider.js"
 import {setSnapParameterOnOptions} from "./util.js";
 
-import * as GridlessPathfinding from "../wasm/gridless_pathfinding.js"
+import initGridlessPathfinding, * as GridlessPathfinding from "../wasm/gridless_pathfinding.js"
 
 CONFIG.debug.dragRuler = false;
 export let debugGraphics = undefined;
 
-GridlessPathfinding.init();
+initGridlessPathfinding();
 
 Hooks.once("init", () => {
 	registerSettings()
diff --git a/rust/Cargo.lock b/rust/Cargo.lock
index b5debf6..ced0395 100644
--- a/rust/Cargo.lock
+++ b/rust/Cargo.lock
@@ -29,6 +29,17 @@ name = "gridless-pathfinding"
 version = "0.1.0"
 dependencies = [
  "console_error_panic_hook",
+ "js-sys",
+ "rustc-hash",
+ "wasm-bindgen",
+]
+
+[[package]]
+name = "js-sys"
+version = "0.3.56"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "a38fc24e30fd564ce974c02bf1d337caddff65be6cc4735a1f7eab22a7440f04"
+dependencies = [
  "wasm-bindgen",
 ]
 
@@ -65,6 +76,12 @@ dependencies = [
  "proc-macro2",
 ]
 
+[[package]]
+name = "rustc-hash"
+version = "1.1.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "08d43f7aa6b08d49f382cde6a7982047c3426db949b1424bc4b7ec9ae12c6ce2"
+
 [[package]]
 name = "syn"
 version = "1.0.86"
diff --git a/rust/Cargo.toml b/rust/Cargo.toml
index 5dc3ffa..3e90b97 100644
--- a/rust/Cargo.toml
+++ b/rust/Cargo.toml
@@ -14,4 +14,6 @@ lto = true
 
 [dependencies]
 console_error_panic_hook = "0.1.7"
+js-sys = "0.3.56"
+rustc-hash = "1.1.0"
 wasm-bindgen = "0.2.79"
diff --git a/rust/src/geometry.rs b/rust/src/geometry.rs
new file mode 100644
index 0000000..711cefa
--- /dev/null
+++ b/rust/src/geometry.rs
@@ -0,0 +1,51 @@
+use std::hash::{Hash, Hasher};
+
+use wasm_bindgen::prelude::*;
+
+#[wasm_bindgen]
+extern "C" {
+	pub type JsPoint;
+
+	#[wasm_bindgen(method, getter)]
+	fn x(this: &JsPoint) -> f64;
+
+	#[wasm_bindgen(method, getter)]
+	fn y(this: &JsPoint) -> f64;
+}
+
+#[wasm_bindgen]
+#[derive(Debug, Copy, Clone, PartialEq)]
+pub struct Point {
+	pub x: f64,
+	pub y: f64,
+}
+
+impl Point {
+	pub fn new(x: f64, y: f64) -> Self {
+		Self { x, y }
+	}
+
+	pub fn distance_to(&self, to: Point) -> f64 {
+		(self.y - to.y).hypot(self.x - to.x)
+	}
+
+	pub fn is_same_as(&self, other: &Self) -> bool {
+		let e = 0.000001;
+		(self.x - other.x).abs() < e && (self.y - other.y).abs() < e
+	}
+}
+
+impl Eq for Point {}
+
+impl Hash for Point {
+	fn hash<H: Hasher>(&self, hasher: &mut H) {
+		self.x.to_bits().hash(hasher);
+		self.y.to_bits().hash(hasher);
+	}
+}
+
+impl From<&JsPoint> for Point {
+	fn from(point: &JsPoint) -> Self {
+		Self::new(point.x(), point.y())
+	}
+}
diff --git a/rust/src/js_api.rs b/rust/src/js_api.rs
new file mode 100644
index 0000000..d65037e
--- /dev/null
+++ b/rust/src/js_api.rs
@@ -0,0 +1,118 @@
+use js_sys::Array;
+use wasm_bindgen::prelude::*;
+
+use crate::{
+	geometry::Point,
+	pathfinder::{DiscoveredNodePtr, Pathfinder},
+};
+
+#[allow(unused)]
+macro_rules! log {
+	( $( $t:tt )* ) => {
+		log(&format!( $( $t )* ));
+	};
+}
+
+#[wasm_bindgen]
+extern "C" {
+	#[wasm_bindgen(js_namespace = console, js_name=warn)]
+	pub fn log(s: &str);
+}
+
+#[wasm_bindgen(
+	inline_js = "export function collidesWithWall(p1, p2) { return canvas.walls.checkCollision(new Ray(p1, p2)); }"
+)]
+extern "C" {
+	#[wasm_bindgen(js_name=collidesWithWall)]
+	pub fn collides_with_wall(p1: Point, p2: Point) -> bool;
+}
+
+#[wasm_bindgen]
+extern "C" {
+	pub type JsWall;
+	pub type JsWallData;
+
+	#[wasm_bindgen(method, getter)]
+	fn data(this: &JsWall) -> JsWallData;
+
+	#[wasm_bindgen(method, getter)]
+	fn c(this: &JsWallData) -> Vec<f64>;
+}
+
+#[derive(Debug, Clone, Copy)]
+pub struct Wall {
+	pub p1: Point,
+	pub p2: Point,
+}
+
+impl Wall {
+	pub fn new(p1: Point, p2: Point) -> Self {
+		Self { p1, p2 }
+	}
+}
+
+impl Wall {
+	fn from_js(wall: &JsWall) -> Self {
+		let data = wall.data();
+		let mut c = data.c();
+		c.iter_mut().for_each(|val| *val = val.round());
+		Self::new(Point::new(c[0], c[1]), Point::new(c[2], c[3]))
+	}
+}
+
+#[allow(dead_code)]
+#[wasm_bindgen(js_name=buildCache)]
+pub fn initialize(js_walls: Vec<JsValue>) -> Pathfinder {
+	let mut walls = Vec::with_capacity(js_walls.len());
+	for wall in js_walls {
+		let wall = JsWall::from(wall);
+		walls.push(Wall::from_js(&wall));
+	}
+	Pathfinder::initialize(walls)
+}
+
+#[allow(dead_code)]
+#[wasm_bindgen]
+pub fn free(pathfinder: Pathfinder) {
+	drop(pathfinder);
+}
+
+#[allow(dead_code)]
+#[wasm_bindgen(js_name=findPath)]
+pub fn find_path(pathfinder: &mut Pathfinder, from: Point, to: Point) -> Option<Array> {
+	if let Some(first_node) = pathfinder.find_path(from, to) {
+		Some(first_node.iter_path().map(JsValue::from).collect())
+	} else {
+		None
+	}
+}
+
+trait IteratePath {
+	fn iter_path(&self) -> PathIterator;
+}
+
+impl IteratePath for DiscoveredNodePtr {
+	fn iter_path(&self) -> PathIterator {
+		PathIterator {
+			current_node: Some(self.clone()),
+		}
+	}
+}
+
+struct PathIterator {
+	current_node: Option<DiscoveredNodePtr>,
+}
+
+impl Iterator for PathIterator {
+	type Item = Point;
+
+	fn next(&mut self) -> Option<Self::Item> {
+		if let Some(node) = self.current_node.clone() {
+			let point = node.borrow().node.borrow().point;
+			self.current_node = node.borrow().previous.clone();
+			Some(point)
+		} else {
+			None
+		}
+	}
+}
diff --git a/rust/src/lib.rs b/rust/src/lib.rs
index b1dbf7e..9ee8ba0 100644
--- a/rust/src/lib.rs
+++ b/rust/src/lib.rs
@@ -1,6 +1,11 @@
+mod geometry;
+mod js_api;
+mod pathfinder;
+mod ptr_indexed_hash_set;
+
 use wasm_bindgen::prelude::*;
 
 #[wasm_bindgen(start)]
 pub fn main() {
-    std::panic::set_hook(Box::new(console_error_panic_hook::hook))
+	std::panic::set_hook(Box::new(console_error_panic_hook::hook));
 }
diff --git a/rust/src/pathfinder.rs b/rust/src/pathfinder.rs
new file mode 100644
index 0000000..8a26efb
--- /dev/null
+++ b/rust/src/pathfinder.rs
@@ -0,0 +1,200 @@
+use std::{cell::RefCell, rc::Rc};
+
+use wasm_bindgen::prelude::*;
+
+use rustc_hash::FxHashMap;
+
+use crate::{
+	geometry::Point,
+	js_api::{collides_with_wall, Wall},
+	ptr_indexed_hash_set::PtrIndexedHashSet,
+};
+
+pub struct Edge {
+	target: NodePtr,
+	cost: f64,
+}
+
+pub struct Node {
+	pub point: Point,
+	edges: Option<Vec<Edge>>,
+}
+
+impl Node {
+	pub fn new(point: Point) -> Self {
+		Self { point, edges: None }
+	}
+}
+
+type NodePtr = Rc<RefCell<Node>>;
+
+impl From<Node> for NodePtr {
+	fn from(node: Node) -> Self {
+		Rc::new(RefCell::new(node))
+	}
+}
+
+pub struct DiscoveredNode {
+	pub node: NodePtr,
+	cost: f64,
+	estimated: f64,
+	pub previous: Option<DiscoveredNodePtr>,
+}
+
+pub type DiscoveredNodePtr = Rc<RefCell<DiscoveredNode>>;
+
+impl From<DiscoveredNode> for DiscoveredNodePtr {
+	fn from(node: DiscoveredNode) -> Self {
+		Rc::new(RefCell::new(node))
+	}
+}
+
+#[derive(Default, Clone)]
+pub struct NodeStorage(Vec<NodePtr>);
+
+impl NodeStorage {
+	fn new() -> Self {
+		Self::default()
+	}
+
+	fn push(&mut self, node: NodePtr) {
+		self.0.push(node);
+	}
+
+	fn initialize_edges(&self, node: &NodePtr) {
+		if node.borrow().edges.is_some() {
+			return;
+		}
+		let point = node.borrow().point;
+		let mut edges = Vec::new();
+		for neighbor in &self.0 {
+			if Rc::ptr_eq(neighbor, &node) {
+				continue;
+			}
+			let neighbor_point = neighbor.borrow().point;
+			if !collides_with_wall(point, neighbor_point) {
+				let cost = point.distance_to(neighbor_point);
+				edges.push(Edge {
+					target: neighbor.clone(),
+					cost,
+				});
+			}
+		}
+
+		node.borrow_mut().edges = Some(edges);
+	}
+}
+
+#[wasm_bindgen]
+pub struct Pathfinder {
+	#[wasm_bindgen(skip)]
+	pub nodes: NodeStorage,
+}
+
+impl Pathfinder {
+	pub fn initialize<I>(walls: I) -> Self
+	where
+		I: IntoIterator<Item = Wall>,
+	{
+		let mut endpoints = FxHashMap::<Point, Vec<f64>>::default();
+		for wall in walls {
+			let x_diff = wall.p2.x - wall.p1.x;
+			let y_diff = wall.p2.y - wall.p1.y;
+			let p1_angle = y_diff.atan2(x_diff);
+			let p2_angle = (y_diff + 180.0).rem_euclid(360.0);
+			for (point, angle) in [(wall.p1, p1_angle), (wall.p2, p2_angle)] {
+				let angles = endpoints.entry(point).or_insert_with(|| Vec::new());
+				angles.push(angle);
+			}
+		}
+		endpoints
+			.values_mut()
+			.for_each(|angles| angles.sort_by(|a, b| a.partial_cmp(b).unwrap()));
+		let mut nodes = NodeStorage::new();
+		for (point, angles) in endpoints {
+			assert!(angles.len() > 0);
+			for i in 1..angles.len() {
+				let angle1 = angles[i - 1];
+				let angle2 = angles[i - 1];
+				let angle_between = (angle2 - angle1) / 2.0 + angle1;
+				nodes.push(calc_pathfinding_node(point, angle_between));
+			}
+			let angle1 = angles.last().unwrap();
+			let angle2 = angles.first().unwrap() + 360.0;
+			let angle_between = (angle2 - angle1) / 2.0 + angle1;
+			let angle_between = angle_between.rem_euclid(360.0);
+			nodes.push(calc_pathfinding_node(point, angle_between));
+		}
+		// TODO Eliminating nodes close to each other may improve performance
+		Self { nodes }
+	}
+
+	pub fn find_path(&mut self, from: Point, to: Point) -> Option<DiscoveredNodePtr> {
+		let mut nodes = self.nodes.clone();
+		nodes.push(NodePtr::from(Node::new(from)));
+		let nodes = nodes;
+		let to_node = NodePtr::from(Node::new(to));
+		nodes.initialize_edges(&to_node);
+		let to = DiscoveredNode {
+			node: to_node,
+			cost: 0.0,
+			estimated: to.distance_to(from),
+			previous: None,
+		};
+		// TODO Use a sorted set for next_nodes for better performance
+		let mut next_nodes = vec![DiscoveredNodePtr::from(to)];
+		let mut previous_nodes = PtrIndexedHashSet::new();
+		while next_nodes.len() > 0 {
+			// Sort by estimated cost, high to low
+			// TODO Maybe tere's a faster way to do this than re-sorting every iteration?
+			next_nodes.sort_by(|a, b| {
+				b.borrow()
+					.estimated
+					.partial_cmp(&a.borrow().estimated)
+					.unwrap()
+			});
+
+			// Get node with cheapest estimate
+			let current_node = next_nodes.pop().unwrap();
+			if current_node.borrow().node.borrow().point.x == from.x
+				&& current_node.borrow().node.borrow().point.y == from.y
+			{
+				return Some(current_node);
+			}
+			previous_nodes.insert(current_node.borrow().node.clone());
+			for edge in current_node.borrow().node.borrow().edges.as_ref().unwrap() {
+				let neighbor = &edge.target;
+				if previous_nodes.contains(neighbor) {
+					continue;
+				}
+				nodes.initialize_edges(neighbor);
+				let cost = current_node.borrow().cost + edge.cost;
+				let discovered_neighbor = DiscoveredNode {
+					node: neighbor.clone(),
+					cost,
+					estimated: cost + neighbor.borrow().point.distance_to(from),
+					previous: Some(current_node.clone()),
+				};
+				let neighbor_entry = next_nodes
+					.iter()
+					.find(|node| Rc::ptr_eq(&node.borrow().node, neighbor));
+				if let Some(entry) = neighbor_entry {
+					// If the neighbor is cheaper to reach via the current route than through previously discovered routes, replace it
+					if entry.borrow().cost > cost {
+						*entry.borrow_mut() = discovered_neighbor;
+					}
+				} else {
+					next_nodes.push(discovered_neighbor.into());
+				}
+			}
+		}
+		None
+	}
+}
+
+fn calc_pathfinding_node(p: Point, angle: f64) -> NodePtr {
+	let diatance_from_walls = 10.0;
+	let x = p.x + angle.cos() * diatance_from_walls;
+	let y = p.y + angle.sin() * diatance_from_walls;
+	NodePtr::from(Node::new(Point { x, y }))
+}
diff --git a/rust/src/ptr_indexed_hash_set.rs b/rust/src/ptr_indexed_hash_set.rs
new file mode 100644
index 0000000..d70717b
--- /dev/null
+++ b/rust/src/ptr_indexed_hash_set.rs
@@ -0,0 +1,67 @@
+use rustc_hash::FxHashSet;
+use std::collections::hash_set;
+use std::hash::{Hash, Hasher};
+use std::rc::Rc;
+
+pub struct PtrIndexedHashSet<T>(FxHashSet<PtrIndexedRc<T>>);
+
+impl<T> PtrIndexedHashSet<T> {
+	pub fn new() -> Self {
+		PtrIndexedHashSet(FxHashSet::default())
+	}
+
+	pub fn insert(&mut self, value: Rc<T>) -> bool {
+		self.0.insert(PtrIndexedRc(value))
+	}
+
+	pub fn remove(&mut self, value: &Rc<T>) -> bool {
+		self.0.remove(&PtrIndexedRc(Rc::clone(value)))
+	}
+
+	pub fn contains(&mut self, value: &Rc<T>) -> bool {
+		self.0.contains(&PtrIndexedRc(Rc::clone(value)))
+	}
+}
+
+impl<T: std::fmt::Debug> std::fmt::Debug for PtrIndexedHashSet<T> {
+	fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
+		f.debug_set().entries(self.0.iter().map(|e| &e.0)).finish()
+	}
+}
+
+struct PtrIndexedRc<T>(Rc<T>);
+
+impl<T> Hash for PtrIndexedRc<T> {
+	fn hash<H: Hasher>(&self, state: &mut H) {
+		Rc::as_ptr(&self.0).hash(state)
+	}
+}
+
+impl<T> PartialEq for PtrIndexedRc<T> {
+	fn eq(&self, other: &Self) -> bool {
+		Rc::ptr_eq(&self.0, &other.0)
+	}
+}
+
+impl<T> Eq for PtrIndexedRc<T> {}
+
+pub struct PtrIndexedHashSetIterator<'a, T>(hash_set::Iter<'a, PtrIndexedRc<T>>);
+
+impl<'a, T> Iterator for PtrIndexedHashSetIterator<'a, T> {
+	type Item = &'a Rc<T>;
+
+	fn next(&mut self) -> Option<Self::Item> {
+		match self.0.next() {
+			Some(item) => Some(&item.0),
+			None => None,
+		}
+	}
+}
+
+impl<'a, T> IntoIterator for &'a PtrIndexedHashSet<T> {
+	type Item = &'a Rc<T>;
+	type IntoIter = PtrIndexedHashSetIterator<'a, T>;
+	fn into_iter(self) -> Self::IntoIter {
+		PtrIndexedHashSetIterator::<T>((&self.0).into_iter())
+	}
+}