Add support for gridless pathfinding

2022-02-15 17:56:08 +01:00
parent ad3fdf4d18
commit fb702cd850
31 changed files with 1146 additions and 24 deletions
@@ -0,0 +1,334 @@
+use std::{cell::RefCell, f64::consts::PI, rc::Rc};
+
+use wasm_bindgen::prelude::*;
+
+use rustc_hash::FxHashMap;
+
+use crate::{
+	geometry::{LineSegment, Point},
+	js_api::{Wall, WallSenseType},
+	ptr_indexed_hash_set::PtrIndexedHashSet,
+};
+
+pub struct Edge {
+	target: NodePtr,
+	cost: f64,
+}
+
+pub struct Node {
+	pub point: Point,
+	edges: Option<Vec<Edge>>,
+	final_edge: Option<Option<Edge>>,
+}
+
+impl Node {
+	pub fn new(point: Point) -> Self {
+		Self {
+			point,
+			edges: None,
+			final_edge: None,
+		}
+	}
+
+	fn iter_edges(
+		&self,
+	) -> std::iter::Chain<std::slice::Iter<'_, Edge>, std::option::Iter<'_, Edge>> {
+		self.edges
+			.as_ref()
+			.unwrap()
+			.iter()
+			.chain(self.final_edge.as_ref().unwrap().iter())
+	}
+}
+
+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 {
+	regular_nodes: Vec<NodePtr>,
+	final_node: Option<NodePtr>,
+}
+
+pub type NodeStorageIterator<'a> = std::iter::Chain<
+	std::slice::Iter<'a, Rc<RefCell<Node>>>,
+	std::option::Iter<'a, Rc<RefCell<Node>>>,
+>;
+
+impl NodeStorage {
+	fn new() -> Self {
+		Self::default()
+	}
+
+	fn push(&mut self, node: NodePtr) {
+		self.regular_nodes.push(node);
+	}
+
+	fn initialize_edges(&mut self, node: &NodePtr, walls: &[LineSegment]) {
+		if node.borrow().final_edge.is_none() {
+			let final_edge = self
+				.final_node
+				.as_ref()
+				.filter(|neighbor| {
+					!self.collides_with_wall(
+						&LineSegment::new(node.borrow().point, neighbor.borrow().point),
+						walls,
+					)
+				})
+				.map(|neighbor| Edge {
+					target: neighbor.clone(),
+					cost: node.borrow().point.distance_to(neighbor.borrow().point),
+				});
+			node.borrow_mut().final_edge = Some(final_edge);
+		}
+
+		if node.borrow().edges.is_some() {
+			return;
+		}
+
+		let point = node.borrow().point;
+		let mut edges = Vec::new();
+		for neighbor in &self.regular_nodes {
+			if Rc::ptr_eq(neighbor, node) {
+				continue;
+			}
+			let neighbor_point = neighbor.borrow().point;
+			if !self.collides_with_wall(&LineSegment::new(point, neighbor_point), walls) {
+				let cost = point.distance_to(neighbor_point);
+				edges.push(Edge {
+					target: neighbor.clone(),
+					cost,
+				});
+			}
+		}
+		node.borrow_mut().edges = Some(edges);
+	}
+
+	fn collides_with_wall(&self, line: &LineSegment, walls: &[LineSegment]) -> bool {
+		walls.iter().any(|wall| line.intersection(wall).is_some())
+	}
+
+	pub fn cleanup_final_edges(&mut self) {
+		for node in &self.regular_nodes {
+			node.borrow_mut().final_edge = None;
+		}
+	}
+
+	pub fn iter(&self) -> NodeStorageIterator {
+		self.regular_nodes.iter().chain(self.final_node.iter())
+	}
+}
+
+#[wasm_bindgen]
+pub struct Pathfinder {
+	#[wasm_bindgen(skip)]
+	pub nodes: NodeStorage,
+	#[wasm_bindgen(skip)]
+	pub walls: Vec<LineSegment>,
+}
+
+impl Pathfinder {
+	pub fn initialize<I>(walls: I, token_size: f64) -> Self
+	where
+		I: IntoIterator<Item = Wall>,
+	{
+		let distance_from_walls = token_size / 2.0;
+		let mut endpoints = FxHashMap::<Point, Vec<f64>>::default();
+		let mut line_segments = Vec::new();
+		for wall in walls {
+			if wall.move_type == WallSenseType::NONE {
+				continue;
+			}
+			if wall.is_door() && wall.is_open() {
+				continue;
+			}
+			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).rem_euclid(2.0 * PI);
+			let p2_angle = (p1_angle + PI).rem_euclid(2.0 * PI);
+			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);
+			}
+			line_segments.push(LineSegment::new(wall.p1, wall.p2));
+		}
+		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.is_empty());
+			for i in 1..angles.len() {
+				let angle1 = angles[i - 1];
+				let angle2 = angles[i];
+				if angle1 == angle2 {
+					continue;
+				}
+				let angle_diff = angle2 - angle1;
+				if angle_diff <= PI {
+					continue;
+				}
+				{
+					let angle_between = angle_diff / 2.0 + angle1;
+					let pathfinding_node = calc_pathfinding_node(
+						point,
+						angle_between,
+						distance_from_walls,
+						&mut line_segments,
+					);
+					if angle_diff > 1.5 * PI {
+						nodes.push(pathfinding_node);
+					}
+				}
+				nodes.push(calc_pathfinding_node(
+					point,
+					angle1 + 0.5 * PI,
+					distance_from_walls,
+					&mut line_segments,
+				));
+				nodes.push(calc_pathfinding_node(
+					point,
+					angle2 - 0.5 * PI,
+					distance_from_walls,
+					&mut line_segments,
+				));
+			}
+			let angle1 = angles.last().unwrap();
+			let angle2 = angles.first().unwrap() + 2.0 * PI;
+			let angle_diff = angle2 - angle1;
+			if angle_diff <= PI {
+				continue;
+			}
+			if angle_diff > 1.5 * PI {
+				let angle_between = angle_diff / 2.0 + angle1;
+				nodes.push(calc_pathfinding_node(
+					point,
+					angle_between,
+					distance_from_walls,
+					&mut line_segments,
+				));
+			}
+			nodes.push(calc_pathfinding_node(
+				point,
+				angle1 + 0.5 * PI,
+				distance_from_walls,
+				&mut line_segments,
+			));
+			nodes.push(calc_pathfinding_node(
+				point,
+				angle2 - 0.5 * PI,
+				distance_from_walls,
+				&mut line_segments,
+			));
+		}
+		// TODO Eliminating nodes close to each other may improve performance
+		Self {
+			nodes,
+			walls: line_segments,
+		}
+	}
+
+	pub fn find_path(&mut self, from: Point, to: Point) -> Option<DiscoveredNodePtr> {
+		self.nodes.cleanup_final_edges();
+		let mut nodes = self.nodes.clone();
+		nodes.final_node = Some(NodePtr::from(Node::new(from)));
+		let to_node = NodePtr::from(Node::new(to));
+		nodes.initialize_edges(&to_node, &self.walls);
+		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.is_empty() {
+			// 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().iter_edges() {
+				let neighbor = &edge.target;
+				if previous_nodes.contains(neighbor) {
+					continue;
+				}
+				nodes.initialize_edges(neighbor, &self.walls);
+				// Add a flat 0.00001 cost per node to discurage creation of unnecessary waypoints
+				let cost = current_node.borrow().cost + edge.cost + 0.00001;
+				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,
+	distance_from_walls: f64,
+	line_segments: &mut Vec<LineSegment>,
+) -> NodePtr {
+	let offset_x = angle.cos() * distance_from_walls;
+	let offset_y = angle.sin() * distance_from_walls;
+	line_segments.push(LineSegment::new(
+		p,
+		Point {
+			x: p.x + offset_x * 0.99,
+			y: p.y + offset_y * 0.99,
+		},
+	));
+	NodePtr::from(Node::new(Point {
+		x: p.x + offset_x,
+		y: p.y + offset_y,
+	}))
+}