Add support for gridless pathfinding
This commit is contained in:
Manuel Vögele
@@ -0,0 +1,187 @@
|
||||
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)]
|
||||
pub struct Point {
|
||||
pub x: f64,
|
||||
pub y: f64,
|
||||
}
|
||||
|
||||
impl Point {
|
||||
pub fn new(x: f64, y: f64) -> Self {
|
||||
Self { x, y }
|
||||
}
|
||||
|
||||
pub fn from_line_x(line: &Line, x: f64) -> Self {
|
||||
let y = line.calc_y(x);
|
||||
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 PartialEq for Point {
|
||||
fn eq(&self, other: &Self) -> bool {
|
||||
self.x == other.x && self.y == other.y
|
||||
}
|
||||
}
|
||||
|
||||
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())
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug, Copy, Clone)]
|
||||
pub struct Line {
|
||||
pub m: f64,
|
||||
pub b: f64,
|
||||
pub p1: Point,
|
||||
}
|
||||
|
||||
impl Line {
|
||||
pub fn new(m: f64, b: f64, p1: Point) -> Self {
|
||||
Self { m, b, p1 }
|
||||
}
|
||||
|
||||
pub fn from_points(p1: Point, p2: Point) -> Self {
|
||||
let m = (p1.y - p2.y) / (p1.x - p2.x);
|
||||
let b = p1.y - m * p1.x;
|
||||
Self { m, b, p1 }
|
||||
}
|
||||
|
||||
pub fn from_point_and_angle(p1: Point, angle: f64) -> Self {
|
||||
let p2 = Point {
|
||||
x: p1.x - angle.cos(),
|
||||
y: p1.y - angle.sin(),
|
||||
};
|
||||
Line::from_points(p1, p2)
|
||||
}
|
||||
|
||||
pub fn is_vertical(&self) -> bool {
|
||||
self.m.is_infinite()
|
||||
}
|
||||
|
||||
pub fn is_horizontal(&self) -> bool {
|
||||
self.m == 0.0
|
||||
}
|
||||
|
||||
pub fn calc_x(&self, y: f64) -> f64 {
|
||||
(y - self.b) / self.m
|
||||
}
|
||||
|
||||
pub fn calc_y(&self, x: f64) -> f64 {
|
||||
self.m * x + self.b
|
||||
}
|
||||
|
||||
pub fn intersection(&self, other: &Line) -> Option<Point> {
|
||||
// Are both lines vertical?
|
||||
if self.is_vertical() && other.is_vertical() {
|
||||
return None;
|
||||
}
|
||||
|
||||
// Are the lines paralell?
|
||||
if (self.m - other.m).abs() < 0.00000005 {
|
||||
return None;
|
||||
}
|
||||
|
||||
// Is one of the lines vertical?
|
||||
if self.is_vertical() || other.is_vertical() {
|
||||
let vertical;
|
||||
let regular;
|
||||
if self.is_vertical() {
|
||||
vertical = self;
|
||||
regular = other;
|
||||
} else {
|
||||
vertical = other;
|
||||
regular = self;
|
||||
}
|
||||
return Some(Point::from_line_x(regular, vertical.p1.x));
|
||||
}
|
||||
|
||||
// Calculate x coordinate of intersection point between both lines
|
||||
// Find intersection point: x * m1 + b1 = x * m2 + b2
|
||||
// Solve for x: x = (b1 - b2) / (m2 - m1)
|
||||
let x = (self.b - other.b) / (other.m - self.m);
|
||||
if self.m.abs() < other.m.abs() {
|
||||
Some(Point::from_line_x(self, x))
|
||||
} else {
|
||||
Some(Point::from_line_x(other, x))
|
||||
}
|
||||
}
|
||||
|
||||
pub fn get_perpendicular_through_point(&self, p: Point) -> Self {
|
||||
let m = -1.0 / self.m;
|
||||
let b = p.y - m * p.x;
|
||||
Self { m, b, p1: p }
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug, Clone, Copy)]
|
||||
pub struct LineSegment {
|
||||
pub p1: Point,
|
||||
pub p2: Point,
|
||||
pub line: Line,
|
||||
}
|
||||
|
||||
impl LineSegment {
|
||||
pub fn new(p1: Point, p2: Point) -> Self {
|
||||
Self {
|
||||
p1,
|
||||
p2,
|
||||
line: Line::from_points(p1, p2),
|
||||
}
|
||||
}
|
||||
|
||||
pub fn intersection(&self, other: &LineSegment) -> Option<Point> {
|
||||
let intersection = self.line.intersection(&other.line);
|
||||
intersection.filter(|intersection| {
|
||||
self.is_intersection_on_segment(*intersection)
|
||||
&& other.is_intersection_on_segment(*intersection)
|
||||
})
|
||||
}
|
||||
|
||||
fn is_intersection_on_segment(&self, intersection: Point) -> bool {
|
||||
if intersection.is_same_as(&self.p1) || intersection.is_same_as(&self.p2) {
|
||||
return true;
|
||||
}
|
||||
if self.line.is_vertical() || self.line.m.abs() > 1.0 {
|
||||
return between(intersection.y, self.p1.y, self.p2.y);
|
||||
}
|
||||
between(intersection.x, self.p1.x, self.p2.x)
|
||||
}
|
||||
}
|
||||
|
||||
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
|
||||
}
|
||||
@@ -0,0 +1,237 @@
|
||||
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]
|
||||
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>;
|
||||
|
||||
#[wasm_bindgen(method, getter, js_name = "door")]
|
||||
fn door_type(this: &JsWallData) -> DoorType;
|
||||
|
||||
#[wasm_bindgen(method, getter, js_name = "ds")]
|
||||
fn door_state(this: &JsWallData) -> DoorState;
|
||||
|
||||
#[wasm_bindgen(method, getter, js_name = "move")]
|
||||
fn move_type(this: &JsWallData) -> WallSenseType;
|
||||
}
|
||||
|
||||
#[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;
|
||||
}
|
||||
|
||||
impl From<JsPoint> for Point {
|
||||
fn from(point: JsPoint) -> Self {
|
||||
Point {
|
||||
x: point.x(),
|
||||
y: point.y(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[wasm_bindgen]
|
||||
#[derive(Debug, Copy, Clone, PartialEq)]
|
||||
pub enum DoorState {
|
||||
CLOSED = 0,
|
||||
OPEN = 1,
|
||||
LOCKED = 2,
|
||||
}
|
||||
|
||||
impl TryFrom<usize> for DoorState {
|
||||
type Error = ();
|
||||
fn try_from(value: usize) -> Result<Self, Self::Error> {
|
||||
match value {
|
||||
x if x == Self::CLOSED as usize => Ok(Self::CLOSED),
|
||||
x if x == Self::OPEN as usize => Ok(Self::OPEN),
|
||||
x if x == Self::LOCKED as usize => Ok(Self::LOCKED),
|
||||
_ => Err(()),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[wasm_bindgen]
|
||||
#[derive(Debug, Copy, Clone, PartialEq)]
|
||||
pub enum DoorType {
|
||||
NONE = 0,
|
||||
DOOR = 1,
|
||||
SECRET = 2,
|
||||
}
|
||||
|
||||
impl TryFrom<usize> for DoorType {
|
||||
type Error = ();
|
||||
fn try_from(value: usize) -> Result<Self, Self::Error> {
|
||||
match value {
|
||||
x if x == Self::NONE as usize => Ok(Self::NONE),
|
||||
x if x == Self::DOOR as usize => Ok(Self::DOOR),
|
||||
x if x == Self::SECRET as usize => Ok(Self::SECRET),
|
||||
_ => Err(()),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[wasm_bindgen]
|
||||
#[derive(Debug, Copy, Clone, PartialEq)]
|
||||
pub enum WallSenseType {
|
||||
NONE = 0,
|
||||
LIMITED = 10,
|
||||
NORMAL = 20,
|
||||
}
|
||||
|
||||
impl TryFrom<usize> for WallSenseType {
|
||||
type Error = ();
|
||||
fn try_from(value: usize) -> Result<Self, Self::Error> {
|
||||
match value {
|
||||
x if x == Self::NONE as usize => Ok(Self::NONE),
|
||||
x if x == Self::LIMITED as usize => Ok(Self::LIMITED),
|
||||
x if x == Self::NORMAL as usize => Ok(Self::NORMAL),
|
||||
_ => Err(()),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug, Clone, Copy)]
|
||||
pub struct Wall {
|
||||
pub p1: Point,
|
||||
pub p2: Point,
|
||||
pub door_type: DoorType,
|
||||
pub door_state: DoorState,
|
||||
pub move_type: WallSenseType,
|
||||
}
|
||||
|
||||
impl Wall {
|
||||
pub fn new(
|
||||
p1: Point,
|
||||
p2: Point,
|
||||
door_type: DoorType,
|
||||
door_state: DoorState,
|
||||
move_type: WallSenseType,
|
||||
) -> Self {
|
||||
Self {
|
||||
p1,
|
||||
p2,
|
||||
door_type,
|
||||
door_state,
|
||||
move_type,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn is_door(&self) -> bool {
|
||||
self.door_type != DoorType::NONE
|
||||
}
|
||||
|
||||
pub fn is_open(&self) -> bool {
|
||||
self.door_state == DoorState::OPEN
|
||||
}
|
||||
}
|
||||
|
||||
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]),
|
||||
data.door_type(),
|
||||
data.door_state(),
|
||||
data.move_type(),
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
#[allow(dead_code)]
|
||||
#[wasm_bindgen]
|
||||
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, token_size)
|
||||
}
|
||||
|
||||
#[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: JsPoint, to: JsPoint) -> Option<Array> {
|
||||
pathfinder
|
||||
.find_path(from.into(), to.into())
|
||||
.map(|first_node| first_node.iter_path().map(JsValue::from).collect())
|
||||
}
|
||||
|
||||
#[allow(dead_code)]
|
||||
#[wasm_bindgen(js_name=debugGetPathfindingPoints)]
|
||||
pub fn debug_get_pathfinding_points(pathfinder: &Pathfinder) -> Array {
|
||||
pathfinder
|
||||
.nodes
|
||||
.iter()
|
||||
.map(|node| node.borrow().point)
|
||||
.map(JsValue::from)
|
||||
.collect()
|
||||
}
|
||||
|
||||
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
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,12 @@
|
||||
mod geometry;
|
||||
#[macro_use]
|
||||
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));
|
||||
}
|
||||
@@ -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,
|
||||
}))
|
||||
}
|
||||
@@ -0,0 +1,68 @@
|
||||
use rustc_hash::FxHashSet;
|
||||
use std::collections::hash_set;
|
||||
use std::hash::{Hash, Hasher};
|
||||
use std::rc::Rc;
|
||||
|
||||
#[derive(Default)]
|
||||
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).iter())
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user