Work towards gridless difficult terrain pathfinding

rust/src/geometry.rs

@@ -38,6 +38,11 @@ impl Point {
 		let e = 0.000001;
 		(self.x - other.x).abs() < e && (self.y - other.y).abs() < e
 	}
+
+	pub fn is_in_rectangle(&self, rect: &Rectangle) -> bool {
+		between(self.x, rect.left.p1.x, rect.right.p1.x)
+			&& between(self.y, rect.top.p1.y, rect.bottom.p1.y)
+	}
 }
 
 impl Eq for Point {}
@@ -179,9 +184,70 @@ impl LineSegment {
 		}
 		between(intersection.x, self.p1.x, self.p2.x)
 	}
+
+	pub fn intersects_rect(&self, rect: &Rectangle) -> bool {
+		if self.p1.is_in_rectangle(rect) {
+			return true;
+		}
+		[rect.left, rect.top, rect.right, rect.bottom]
+			.iter()
+			.any(|edge| self.intersection(edge).is_some())
+	}
 }
 
 pub fn between<T: Copy + PartialOrd>(num: T, a: T, b: T) -> bool {
 	let (min, max) = if a < b { (a, b) } else { (b, a) };
 	num >= min && num <= max
 }
+
+#[wasm_bindgen]
+extern "C" {
+	pub type JsRectangle;
+
+	#[wasm_bindgen(method, getter)]
+	fn left(this: &JsRectangle) -> f64;
+
+	#[wasm_bindgen(method, getter)]
+	fn top(this: &JsRectangle) -> f64;
+
+	#[wasm_bindgen(method, getter)]
+	fn right(this: &JsRectangle) -> f64;
+
+	#[wasm_bindgen(method, getter)]
+	fn bottom(this: &JsRectangle) -> f64;
+}
+
+#[derive(Debug, Clone, Copy)]
+pub struct Rectangle {
+	pub left: LineSegment,
+	pub top: LineSegment,
+	pub right: LineSegment,
+	pub bottom: LineSegment,
+}
+
+impl Rectangle {
+	pub fn new(left: f64, top: f64, right: f64, bottom: f64) -> Self {
+		Self {
+			left: LineSegment::new(Point::new(left, top), Point::new(left, bottom)),
+			top: LineSegment::new(Point::new(left, top), Point::new(right, top)),
+			right: LineSegment::new(Point::new(right, top), Point::new(right, bottom)),
+			bottom: LineSegment::new(Point::new(left, bottom), Point::new(right, bottom)),
+		}
+	}
+}
+
+impl From<&JsRectangle> for Rectangle {
+	fn from(rect: &JsRectangle) -> Self {
+		let left = rect.left();
+		let top = rect.top();
+		let right = rect.right();
+		let bottom = rect.bottom();
+		Self::new(left, top, right, bottom)
+	}
+}
+
+#[derive(Debug, Copy, Clone)]
+pub struct Circle {
+	pub center: Point,
+	pub radius: f64,
+}

rust/src/js_api.rs

@@ -1,10 +1,11 @@
 use js_sys::Array;
-use sha1::{Sha1, Digest};
+use sha1::{Digest, Sha1};
 use wasm_bindgen::prelude::*;
 
 use crate::{
-	geometry::Point,
+	geometry::{Circle, LineSegment, Point, Rectangle},
 	pathfinder::{DiscoveredNodePtr, Pathfinder},
+	util::Windows,
 };
 
 #[allow(unused)]
@@ -18,6 +19,9 @@ macro_rules! log {
 extern "C" {
 	#[wasm_bindgen(js_namespace = console, js_name=warn)]
 	pub fn log(s: &str);
+
+	#[wasm_bindgen(js_namespace = ["dragRuler", "private"], js_name=terrainRulerWrapper)]
+	pub fn distance_with_terrain(a: Point, b: Point) -> f64;
 }
 
 #[wasm_bindgen]
@@ -75,6 +79,112 @@ impl From<JsPoint> for Point {
 	}
 }
 
+#[wasm_bindgen]
+extern "C" {
+	pub type JsTerrainInfo;
+	pub type JsTerrainObject;
+	pub type JsTerrainShape;
+
+	#[wasm_bindgen(method, getter)]
+	fn object(this: &JsTerrainInfo) -> JsTerrainObject;
+
+	#[wasm_bindgen(method, getter)]
+	fn shape(this: &JsTerrainInfo) -> JsTerrainShape;
+
+	#[wasm_bindgen(method, getter)]
+	fn x(this: &JsTerrainObject) -> f64;
+
+	#[wasm_bindgen(method, getter)]
+	fn y(this: &JsTerrainObject) -> f64;
+
+	#[wasm_bindgen(method, getter)]
+	fn width(this: &JsTerrainObject) -> f64;
+
+	#[wasm_bindgen(method, getter)]
+	fn height(this: &JsTerrainObject) -> f64;
+
+	#[wasm_bindgen(method, getter, js_name = "type")]
+	fn shape_type(this: &JsTerrainShape) -> u32;
+
+	#[wasm_bindgen(method, getter)]
+	fn x(this: &JsTerrainShape) -> f64;
+
+	#[wasm_bindgen(method, getter)]
+	fn y(this: &JsTerrainShape) -> f64;
+
+	#[wasm_bindgen(method, getter)]
+	fn radius(this: &JsTerrainShape) -> f64;
+
+	#[wasm_bindgen(method, getter)]
+	fn points(this: &JsTerrainShape) -> Vec<f64>;
+}
+
+impl JsTerrainObject {
+	fn to_bounding_rect(&self) -> Rectangle {
+		let left = self.x();
+		let top = self.y();
+		let right = left + self.width();
+		let bottom = top + self.height();
+		Rectangle::new(left, top, right, bottom)
+	}
+}
+
+impl JsTerrainShape {
+	fn to_segments(&self, x: f64, y: f64) -> Vec<LineSegment> {
+		let points = self.points();
+		assert!(points.len() % 2 == 0);
+		points
+			.chunks(2)
+			.map(|coordinates| Point::new(coordinates[0] + x, coordinates[1] + y))
+			.windows()
+			.map(|(p1, p2)| LineSegment::new(p1, p2))
+			.collect()
+	}
+
+	fn to_circle(&self, x: f64, y: f64) -> Circle {
+		let center = Point::new(self.x() + x, self.y() + y);
+		let radius = self.radius();
+		Circle { center, radius }
+	}
+}
+
+impl From<&JsTerrainInfo> for TerrainShape {
+	fn from(terrain: &JsTerrainInfo) -> Self {
+		let shape = terrain.shape();
+		let object = terrain.object();
+		let x = object.x();
+		let y = object.y();
+		match shape.shape_type() {
+			0 => TerrainShape::Polygon(shape.to_segments(x, y)),
+			2 => TerrainShape::Circle(shape.to_circle(x, y)),
+			_ => unimplemented!(),
+		}
+	}
+}
+
+#[derive(Debug, Clone)]
+pub enum TerrainShape {
+	Polygon(Vec<LineSegment>),
+	Circle(Circle),
+}
+
+#[derive(Debug, Clone)]
+pub struct Terrain {
+	pub shape: TerrainShape,
+	pub bounding_box: Rectangle,
+}
+
+impl From<&JsTerrainInfo> for Terrain {
+	fn from(terrain: &JsTerrainInfo) -> Self {
+		let bounding_box = terrain.object().to_bounding_rect();
+		let shape = terrain.into();
+		Self {
+			bounding_box,
+			shape,
+		}
+	}
+}
+
 #[wasm_bindgen]
 #[derive(Debug, Copy, Clone, PartialEq)]
 pub enum DoorState {
@@ -212,8 +322,7 @@ impl Wall {
 		c.iter_mut().for_each(|val| *val = val.round());
 		let height = if enable_height {
 			data.flags().wall_height().into()
-		}
-		else {
+		} else {
 			WallHeight::default()
 		};
 		Self::new(
@@ -229,13 +338,20 @@ impl Wall {
 
 #[allow(dead_code)]
 #[wasm_bindgen]
-pub fn initialize(js_walls: Vec<JsValue>, token_size: f64, token_elevation: f64, enable_height: bool) -> Pathfinder {
+pub fn initialize(
+	js_walls: Vec<JsValue>,
+	js_terrain: Vec<JsTerrainInfo>,
+	token_size: f64,
+	token_elevation: f64,
+	enable_height: bool,
+) -> 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, enable_height));
 	}
-	Pathfinder::initialize(walls, token_size, token_elevation)
+	let terrain = js_terrain.iter().map(|terrain| terrain.into()).collect();
+	Pathfinder::initialize(walls, terrain, token_size, token_elevation)
 }
 
 #[allow(dead_code)]

rust/src/lib.rs

@@ -3,6 +3,7 @@ mod geometry;
 mod js_api;
 mod pathfinder;
 mod ptr_indexed_hash_set;
+mod util;
 
 use wasm_bindgen::prelude::*;
 

rust/src/pathfinder.rs

@@ -5,8 +5,8 @@ use wasm_bindgen::prelude::*;
 use rustc_hash::FxHashMap;
 
 use crate::{
-	geometry::{LineSegment, Point},
-	js_api::{Wall, WallSenseType},
+	geometry::{LineSegment, Point, Rectangle},
+	js_api::{distance_with_terrain, Terrain, TerrainShape, Wall, WallSenseType, log},
 	ptr_indexed_hash_set::PtrIndexedHashSet,
 };
 
@@ -84,7 +84,7 @@ impl NodeStorage {
 		self.regular_nodes.push(node);
 	}
 
-	fn initialize_edges(&mut self, node: &NodePtr, walls: &[LineSegment]) {
+	fn initialize_edges(&mut self, node: &NodePtr, walls: &[LineSegment], terrain: &[Rectangle]) {
 		if node.borrow().final_edge.is_none() {
 			let final_edge = self
 				.final_node
@@ -97,7 +97,11 @@ impl NodeStorage {
 				})
 				.map(|neighbor| Edge {
 					target: neighbor.clone(),
-					cost: node.borrow().point.distance_to(neighbor.borrow().point),
+					cost: Self::measure_distance(
+						node.borrow().point,
+						neighbor.borrow().point,
+						terrain,
+					),
 				});
 			node.borrow_mut().final_edge = Some(final_edge);
 		}
@@ -114,7 +118,8 @@ impl NodeStorage {
 			}
 			let neighbor_point = neighbor.borrow().point;
 			if !self.collides_with_wall(&LineSegment::new(point, neighbor_point), walls) {
-				let cost = point.distance_to(neighbor_point);
+				let cost =
+					Self::measure_distance(node.borrow().point, neighbor.borrow().point, terrain);
 				edges.push(Edge {
 					target: neighbor.clone(),
 					cost,
@@ -128,6 +133,15 @@ impl NodeStorage {
 		walls.iter().any(|wall| line.intersection(wall).is_some())
 	}
 
+	fn measure_distance(a: Point, b: Point, terrain: &[Rectangle]) -> f64 {
+		let segment = LineSegment::new(a, b);
+		if terrain.iter().any(|rect| segment.intersects_rect(rect)) {
+			distance_with_terrain(a, b)
+		} else {
+			a.distance_to(b)
+		}
+	}
+
 	pub fn cleanup_final_edges(&mut self) {
 		for node in &self.regular_nodes {
 			node.borrow_mut().final_edge = None;
@@ -145,35 +159,51 @@ pub struct Pathfinder {
 	pub nodes: NodeStorage,
 	#[wasm_bindgen(skip)]
 	pub walls: Vec<LineSegment>,
+	#[wasm_bindgen(skip)]
+	pub terrain: Vec<Rectangle>,
 }
 
 impl Pathfinder {
-	pub fn initialize<I>(walls: I, token_size: f64, token_elevation: f64) -> Self
-	where
-		I: IntoIterator<Item = Wall>,
-	{
+	pub fn initialize(
+		walls: Vec<Wall>,
+		terrain: Vec<Terrain>,
+		token_size: f64,
+		token_elevation: f64,
+	) -> Self {
+		log!("{:#?}", terrain);
 		let distance_from_walls = token_size / 2.0;
+
+		// TODO Place pathfinding nodes around terrain edges
+		let mut polygon_terrain = Vec::new();
+		let mut circle_terrain = Vec::new();
+
+		for terrain in &terrain {
+			match &terrain.shape {
+				TerrainShape::Polygon(polygon) => polygon_terrain.append(&mut polygon.clone()),
+				TerrainShape::Circle(circle) => circle_terrain.push(circle),
+			}
+		}
+
+		let mut walls = walls
+			.into_iter()
+			.filter(|wall| wall.move_type != WallSenseType::NONE)
+			.filter(|wall| !(wall.is_door() && wall.is_open()))
+			.filter(|wall| wall.height.contains(token_elevation))
+			.map(|wall| LineSegment::new(wall.p1, wall.p2))
+			.collect::<Vec<_>>();
+
+		// TODO Generate points around difficult terrain
 		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;
-			}
-			if !wall.height.contains(token_elevation) {
-				continue;
-			}
-			let x_diff = wall.p2.x - wall.p1.x;
-			let y_diff = wall.p2.y - wall.p1.y;
+
+		for segments in walls.iter().chain(polygon_terrain.iter()) {
+			let x_diff = segments.p2.x - segments.p1.x;
+			let y_diff = segments.p2.y - segments.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)] {
+			for (point, angle) in [(segments.p1, p1_angle), (segments.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()
@@ -196,19 +226,19 @@ impl Pathfinder {
 					point,
 					angle_between,
 					distance_from_walls,
-					&mut line_segments,
+					&mut walls,
 				));
 				nodes.push(calc_pathfinding_node(
 					point,
 					angle1 + 0.5 * PI,
 					distance_from_walls,
-					&mut line_segments,
+					&mut walls,
 				));
 				nodes.push(calc_pathfinding_node(
 					point,
 					angle2 - 0.5 * PI,
 					distance_from_walls,
-					&mut line_segments,
+					&mut walls,
 				));
 			}
 			let angle1 = angles.last().unwrap();
@@ -222,25 +252,46 @@ impl Pathfinder {
 				point,
 				angle_between,
 				distance_from_walls,
-				&mut line_segments,
+				&mut walls,
 			));
 			nodes.push(calc_pathfinding_node(
 				point,
 				angle1 + 0.5 * PI,
 				distance_from_walls,
-				&mut line_segments,
+				&mut walls,
 			));
 			nodes.push(calc_pathfinding_node(
 				point,
 				angle2 - 0.5 * PI,
 				distance_from_walls,
-				&mut line_segments,
+				&mut walls,
 			));
 		}
+
+		// TODO Check bounding box of circle terrain
+		for circle in circle_terrain {
+			let angle_step = f64::asin(token_size / 2.0 / circle.radius);
+			let mut angle = 0.0;
+			while angle < 2.0 * PI {
+				let point = Point {
+					x: circle.center.x + angle.cos() * circle.radius,
+					y: circle.center.y + angle.sin() * circle.radius,
+				};
+				nodes.push(calc_pathfinding_node(
+					point,
+					angle,
+					distance_from_walls,
+					&mut walls,
+				));
+				angle += angle_step;
+			}
+		}
+
 		// TODO Eliminating nodes close to each other may improve performance
 		Self {
 			nodes,
-			walls: line_segments,
+			walls,
+			terrain: terrain.iter().map(|terrain| terrain.bounding_box).collect(),
 		}
 	}
 
@@ -249,7 +300,7 @@ impl Pathfinder {
 		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);
+		nodes.initialize_edges(&to_node, &self.walls, &self.terrain);
 		let to = DiscoveredNode {
 			node: to_node,
 			cost: 0.0,
@@ -282,7 +333,7 @@ impl Pathfinder {
 				if previous_nodes.contains(neighbor) {
 					continue;
 				}
-				nodes.initialize_edges(neighbor, &self.walls);
+				nodes.initialize_edges(neighbor, &self.walls, &self.terrain);
 				// 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 {
@@ -312,11 +363,11 @@ fn calc_pathfinding_node(
 	p: Point,
 	angle: f64,
 	distance_from_walls: f64,
-	line_segments: &mut Vec<LineSegment>,
+	walls: &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(
+	walls.push(LineSegment::new(
 		p,
 		Point {
 			x: p.x + offset_x * 0.99,

rust/src/util.rs

@@ -0,0 +1,44 @@
+pub struct WindowIterator<I: Iterator> {
+	iterator: I,
+	previous: Option<I::Item>,
+}
+
+impl<I: Iterator> Iterator for WindowIterator<I>
+where
+	I::Item: Copy,
+{
+	type Item = (I::Item, I::Item);
+
+	fn next(&mut self) -> Option<Self::Item> {
+		if self.previous.is_none() {
+			self.previous = self.iterator.next();
+		}
+		let current = self.iterator.next();
+		if current.is_none() {
+			return None;
+		}
+		let current = current.unwrap();
+		let previous = self.previous.unwrap();
+		let result = (current, previous);
+		self.previous = Some(current);
+		Some(result)
+	}
+}
+
+pub trait Windows<T> {
+	fn windows(self) -> WindowIterator<Self>
+	where
+		Self: Sized + Iterator;
+}
+
+impl<I: Iterator> Windows<I> for I {
+	fn windows(self) -> WindowIterator<Self>
+	where
+		Self: Sized + Iterator,
+	{
+		WindowIterator {
+			iterator: self,
+			previous: None,
+		}
+	}
+}