From 4d6543174aff6322b6473a708c7ae2102abac0c7 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Manuel=20V=C3=B6gele?= <develop@manuel-voegele.de>
Date: Wed, 2 Feb 2022 10:36:03 +0100
Subject: [PATCH] Always keep tokenSize/2 distance from walls

---
 js/pathfinding.js      |  4 ++-
 rust/src/js_api.rs     |  4 +--
 rust/src/pathfinder.rs | 74 +++++++++++++++++++++++++++++++++++-------
 3 files changed, 68 insertions(+), 14 deletions(-)

diff --git a/js/pathfinding.js b/js/pathfinding.js
index f8182d9..a961e59 100644
--- a/js/pathfinding.js
+++ b/js/pathfinding.js
@@ -18,8 +18,10 @@ export function isPathfindingEnabled() {
 
 export function findPath(from, to, token, previousWaypoints) {
 	if (canvas.grid.type === CONST.GRID_TYPES.GRIDLESS) {
+		// TODO Store multiple pathfinders for different token sizes
+		let tokenSize = Math.max(token.data.width, token.data.height) * canvas.dimensions.size;
 		if (!gridlessPathfinder)
-			gridlessPathfinder = GridlessPathfinding.initialize(canvas.walls.placeables);
+			gridlessPathfinder = GridlessPathfinding.initialize(canvas.walls.placeables, tokenSize);
 		paintGridlessPathfindingDebug(gridlessPathfinder);
 		return GridlessPathfinding.findPath(gridlessPathfinder, from, to);
 	}
diff --git a/rust/src/js_api.rs b/rust/src/js_api.rs
index 02daabb..a6fad42 100644
--- a/rust/src/js_api.rs
+++ b/rust/src/js_api.rs
@@ -74,13 +74,13 @@ impl Wall {
 
 #[allow(dead_code)]
 #[wasm_bindgen]
-pub fn initialize(js_walls: Vec<JsValue>) -> Pathfinder {
+pub fn initialize(js_walls: Vec<JsValue>, token_size: f64) -> 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)
+	Pathfinder::initialize(walls, token_size)
 }
 
 #[allow(dead_code)]
diff --git a/rust/src/pathfinder.rs b/rust/src/pathfinder.rs
index e4b6be7..42a7526 100644
--- a/rust/src/pathfinder.rs
+++ b/rust/src/pathfinder.rs
@@ -147,10 +147,11 @@ pub struct Pathfinder {
 }
 
 impl Pathfinder {
-	pub fn initialize<I>(walls: I) -> Self
+	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 {
@@ -180,8 +181,27 @@ impl Pathfinder {
 				if angle_diff <= PI {
 					continue;
 				}
-				let angle_between = angle_diff / 2.0 + angle1;
-				nodes.push(calc_pathfinding_node(point, angle_between));
+				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,
+				));
 			}
 			let angle1 = angles.last().unwrap();
 			let angle2 = angles.first().unwrap() + 2.0 * PI;
@@ -189,9 +209,27 @@ impl Pathfinder {
 			if angle_diff <= PI {
 				continue;
 			}
-			let angle_between = angle_diff / 2.0 + angle1;
-			let angle_between = angle_between.rem_euclid(2.0 * PI);
-			nodes.push(calc_pathfinding_node(point, angle_between));
+			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 {
@@ -263,9 +301,23 @@ impl Pathfinder {
 	}
 }
 
-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 }))
+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,
+	}))
 }