From c08d65ffad9a21428dd09b46d9ae28960704f359 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Manuel=20V=C3=B6gele?= Date: Fri, 10 Jun 2022 16:52:44 +0200 Subject: [PATCH] Work towards gridless difficult terrain pathfinding --- js/main.js | 5 +- js/pathfinding.js | 11 +++- rust/src/geometry.rs | 66 +++++++++++++++++++++ rust/src/js_api.rs | 128 +++++++++++++++++++++++++++++++++++++++-- rust/src/lib.rs | 1 + rust/src/pathfinder.rs | 121 +++++++++++++++++++++++++++----------- rust/src/util.rs | 44 ++++++++++++++ 7 files changed, 333 insertions(+), 43 deletions(-) create mode 100644 rust/src/util.rs diff --git a/js/main.js b/js/main.js index 0e14968..2a4d43e 100644 --- a/js/main.js +++ b/js/main.js @@ -7,7 +7,7 @@ import {disableSnap, registerKeybindings} from "./keybindings.js"; import {libWrapper} from "./libwrapper_shim.js"; import {performMigrations} from "./migration.js" import {removeLastHistoryEntryIfAt, resetMovementHistory} from "./movement_tracking.js"; -import {wipePathfindingCache, initializePathfinding, startBackgroundCaching} from "./pathfinding.js"; +import {wipePathfindingCache, initializePathfinding, startBackgroundCaching, terrainRulerWrapper} from "./pathfinding.js"; import {extendRuler} from "./ruler.js"; import {registerSettings, RightClickAction, settingsKey} from "./settings.js" import {recalculate} from "./socket.js"; @@ -62,6 +62,9 @@ Hooks.once("init", () => { registerSystem, recalculate, resetMovementHistory, + private: { + terrainRulerWrapper + } } }) diff --git a/js/pathfinding.js b/js/pathfinding.js index 46bf512..6ee7c4d 100644 --- a/js/pathfinding.js +++ b/js/pathfinding.js @@ -253,7 +253,10 @@ export function findPath(from, to, token, previousWaypoints) { let pathfinder = gridlessPathfinders.get(tokenSize); if (!pathfinder) { let radiusMultiplier = game.settings.get(settingsKey, "pathfindingRadius"); - pathfinder = GridlessPathfinding.initialize(canvas.walls.placeables, tokenSize * radiusMultiplier, token.data.elevation, Boolean(game.modules.get("wall-height")?.active)); + // TODO Pass proper options to listAllTerrain + // TODO Wipe caches when they become invalid + // TODO Multiple caches, analog to gridded pathfinding + pathfinder = GridlessPathfinding.initialize(canvas.walls.placeables, canvas.terrain.listAllTerrain(), tokenSize * radiusMultiplier, token.data.elevation, Boolean(game.modules.get("wall-height")?.active)); gridlessPathfinders.set(tokenSize, pathfinder); } paintGridlessPathfindingDebug(pathfinder); @@ -304,6 +307,12 @@ export function findPath(from, to, token, previousWaypoints) { } } +export function terrainRulerWrapper(from, to) { + // TODO Send list of terrain to terrain layer + const ray = new Ray(from, to); + return terrainRuler.measureDistances([{ray}])[0] / canvas.dimensions.distance * canvas.dimensions.size; +} + function buildTokenData(token) { // Almost all the information we need is for calculating the snap point const tokenData = buildSnapPointTokenData(token); diff --git a/rust/src/geometry.rs b/rust/src/geometry.rs index 0c219d0..30a6fb9 100644 --- a/rust/src/geometry.rs +++ b/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(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, +} diff --git a/rust/src/js_api.rs b/rust/src/js_api.rs index d2fbc8c..831f141 100644 --- a/rust/src/js_api.rs +++ b/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 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; +} + +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 { + 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), + 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, token_size: f64, token_elevation: f64, enable_height: bool) -> Pathfinder { +pub fn initialize( + js_walls: Vec, + js_terrain: Vec, + 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)] diff --git a/rust/src/lib.rs b/rust/src/lib.rs index 0d6ae2b..7678b12 100644 --- a/rust/src/lib.rs +++ b/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::*; diff --git a/rust/src/pathfinder.rs b/rust/src/pathfinder.rs index 89b37b8..20ae032 100644 --- a/rust/src/pathfinder.rs +++ b/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, + #[wasm_bindgen(skip)] + pub terrain: Vec, } impl Pathfinder { - pub fn initialize(walls: I, token_size: f64, token_elevation: f64) -> Self - where - I: IntoIterator, - { + pub fn initialize( + walls: Vec, + terrain: Vec, + 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::>(); + + // TODO Generate points around difficult terrain let mut endpoints = FxHashMap::>::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, + walls: &mut Vec, ) -> 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, diff --git a/rust/src/util.rs b/rust/src/util.rs new file mode 100644 index 0000000..b32cdc1 --- /dev/null +++ b/rust/src/util.rs @@ -0,0 +1,44 @@ +pub struct WindowIterator { + iterator: I, + previous: Option, +} + +impl Iterator for WindowIterator +where + I::Item: Copy, +{ + type Item = (I::Item, I::Item); + + fn next(&mut self) -> Option { + 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 { + fn windows(self) -> WindowIterator + where + Self: Sized + Iterator; +} + +impl Windows for I { + fn windows(self) -> WindowIterator + where + Self: Sized + Iterator, + { + WindowIterator { + iterator: self, + previous: None, + } + } +}