Skip to content

Commit

Permalink
can get the path with layer change details
Browse files Browse the repository at this point in the history
  • Loading branch information
@mockersf
mockersf committed Jul 21, 2024
1 parent 01477df commit 4e38103
Show file tree
Hide file tree
Showing 4 changed files with 181 additions and 10 deletions.
1 change: 1 addition & 0 deletions src/async_helpers.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,6 +91,7 @@ impl<'m> Future for FuturePath<'m> {
return Poll::Ready(Some(Path {
length: self.from.distance(self.to),
path: vec![self.to],
path_with_layers: vec![(self.to, ending_polygon.layer())],
}));
}

Expand Down
51 changes: 51 additions & 0 deletions src/instance.rs
View file
Original file line number Diff line number Diff line change
@@ -1,3 +1,4 @@
use bvh2d::EPSILON;
use smallvec::SmallVec;
#[cfg(feature = "tracing")]
use tracing::instrument;
Expand Down Expand Up @@ -60,6 +61,7 @@ pub(crate) struct SearchInstance<'m> {
pub(crate) queue: BinaryHeap<SearchNode>,
pub(crate) node_buffer: Vec<SearchNode>,
pub(crate) root_history: HashMap<Root, f32>,
pub(crate) from: Vec2,
pub(crate) to: Vec2,
pub(crate) polygon_to: u32,
pub(crate) mesh: &'m Mesh,
Expand Down Expand Up @@ -126,6 +128,7 @@ impl<'m> SearchInstance<'m> {
queue: BinaryHeap::with_capacity(15),
node_buffer: Vec::with_capacity(10),
root_history: HashMap::with_capacity(10),
from: from.0,
to: to.0,
polygon_to: to.1,
mesh,
Expand All @@ -150,6 +153,7 @@ impl<'m> SearchInstance<'m> {

let empty_node = SearchNode {
path: vec![],
path_with_layers: vec![],
root: from.0,
interval: (Vec2::new(0.0, 0.0), Vec2::new(0.0, 0.0)),
edge: (0, 0),
Expand Down Expand Up @@ -246,16 +250,57 @@ impl<'m> SearchInstance<'m> {
self.mesh.scenarios.set(self.mesh.scenarios.get() + 1);
}
let mut path = next.path;

let mut path_with_layers_end = vec![];
if let Some(turn) = turning_point(next.root, self.to, next.interval) {
path.push(turn);
path_with_layers_end.push((turn, next.polygon_to.layer()));
}
let complete = next.polygon_to == self.polygon_to;
if complete {
path.push(self.to);
path_with_layers_end.push((self.to, next.polygon_to.layer()));
}

let mut path_with_layers = vec![];
let mut from = self.from;
for (index, potential_point) in next.path_with_layers.iter().enumerate() {
if potential_point.0 == potential_point.1 {
from = potential_point.0;
path_with_layers.push((potential_point.0, potential_point.2));
} else {
// look for next fixed point to find the intersection
let to = next
.path_with_layers
.iter()
.skip(index + 1)
.find(|point| point.0 == point.1)
.map(|point| point.0)
.unwrap_or(path_with_layers_end[0].0);
if let Some(intersection) = line_intersect_segment(
(from, to),
(potential_point.0, potential_point.1),
) {
from = intersection;
path_with_layers.push((intersection, potential_point.2));
}
}
}
path_with_layers.extend(path_with_layers_end);
let mut path_with_layers_peekable = path_with_layers.iter().peekable();
let mut path_with_layers = vec![];
while let Some(p) = path_with_layers_peekable.next() {
if let Some(n) = path_with_layers_peekable.peek() {
if p.0.distance_squared(n.0) < EPSILON {
continue;
}
}
path_with_layers.push(*p);
}
return InstanceStep::Found(Path {
path,
length: next.f + next.g,
path_with_layers,
});
}
self.successors(next);
Expand Down Expand Up @@ -456,8 +501,13 @@ impl<'m> SearchInstance<'m> {
}

let mut path = node.path.clone();
let mut path_with_layers = node.path_with_layers.clone();
if root != node.root {
path.push(root);
path_with_layers.push((root, root, node.polygon_to.layer()));
}
if other_side.layer() != node.polygon_to.layer() {
path_with_layers.push((start.0, end.0, other_side.layer()));
}

let heuristic = heuristic(root, self.to, (start.0, end.0));
Expand All @@ -473,6 +523,7 @@ impl<'m> SearchInstance<'m> {

let new_node = SearchNode {
path,
path_with_layers,
root,
interval: (start.0, end.0),
edge: (start.1, end.1),
Expand Down
115 changes: 106 additions & 9 deletions src/layers.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -213,9 +213,12 @@ impl Layer {

#[cfg(test)]
mod tests {
use glam::{vec2, Vec2};
use glam::{vec2, IVec2, Vec2};

use crate::{instance::U32Layer, Coords, Layer, Mesh, Path, Polygon, SearchNode, Vertex};
use crate::{
helpers::line_intersect_segment, instance::U32Layer, Coords, Layer, Mesh, Path, Polygon,
SearchNode, Vertex,
};

fn mesh_u_grid() -> Mesh {
let main_layer = Layer {
Expand Down Expand Up @@ -294,6 +297,7 @@ mod tests {
let to = vec2(1.1, 0.1);
let search_node = SearchNode {
path: vec![],
path_with_layers: vec![],
root: from,
interval: (vec2(0.0, 1.0), vec2(1.0, 1.0)),
edge: (0, 1),
Expand All @@ -310,6 +314,7 @@ mod tests {
Path {
path: vec![to],
length: from.distance(to),
path_with_layers: vec![(to, 0)],
}
);
}
Expand All @@ -322,6 +327,7 @@ mod tests {
let to = vec2(2.1, 1.9);
let search_node = SearchNode {
path: vec![],
path_with_layers: vec![],
root: from,
interval: (vec2(0.0, 1.0), vec2(1.0, 1.0)),
edge: (4, 5),
Expand Down Expand Up @@ -352,6 +358,7 @@ mod tests {
length: from.distance(vec2(1.0, 1.0))
+ vec2(1.0, 1.0).distance(vec2(2.0, 1.0))
+ vec2(2.0, 1.0).distance(to),
path_with_layers: vec![(vec2(1.0, 1.0), 0), (vec2(2.0, 1.0), 2), (to, 2)],
}
);
}
Expand Down Expand Up @@ -410,6 +417,12 @@ mod tests {
mesh
}

fn reduce_path_precision(path: Vec<(Vec2, u8)>) -> Vec<(IVec2, u8)> {
path.into_iter()
.map(|(point, layer)| ((point * 100000.0).as_ivec2(), layer))
.collect()
}

#[test]
fn take_shortcut() {
let mesh = mesh_overlapping_layers();
Expand All @@ -418,6 +431,22 @@ mod tests {
let to = vec2(5.0 - i as f32 / 10.0, 0.9);
let path = dbg!(mesh.path(from, to).unwrap());
assert_eq!(path.path, vec![to]);
assert_eq!(
reduce_path_precision(path.path_with_layers),
reduce_path_precision(vec![
(
line_intersect_segment((from, to), (vec2(0.0, 2.0), vec2(5.0, 2.0)))
.unwrap(),
1
),
(
line_intersect_segment((from, to), (vec2(0.0, 1.0), vec2(5.0, 1.0)))
.unwrap(),
0
),
(to, 0)
]),
);
}
}

Expand All @@ -429,20 +458,54 @@ mod tests {
let to = vec2(i as f32 / 10.0, 2.1);
let path = dbg!(mesh.path(from, to).unwrap());
assert_eq!(path.path, vec![to]);
assert_eq!(
reduce_path_precision(path.path_with_layers),
reduce_path_precision(vec![
(
line_intersect_segment((from, to), (vec2(0.0, 1.0), vec2(5.0, 1.0)))
.unwrap(),
1
),
(
line_intersect_segment((from, to), (vec2(0.0, 2.0), vec2(5.0, 2.0)))
.unwrap(),
0
),
(to, 0)
]),
);
}
}

#[test]
fn shortcut_with_corner() {
let mesh = mesh_overlapping_layers();
for i in 7..15 {
for i in 7..8 {
let from = vec2(i as f32 / 10.0, 2.1);
let to = vec2(5.0 - i as f32 / 10.0, 0.9);
let path = dbg!(mesh.path(from, to).unwrap());
match i {
7 => assert_eq!(path.path, vec![vec2(1.0, 2.0), to]),
_ if i < 11 => assert_eq!(path.path, vec![vec2(1.0, 2.0), vec2(4.0, 1.0), to]),
_ if i < 15 => assert_eq!(path.path, vec![vec2(2.0, 2.0), vec2(3.0, 1.0), to]),
7 => {
assert_eq!(path.path, vec![vec2(1.0, 2.0), to]);
assert_eq!(
path.path_with_layers,
vec![(vec2(1.0, 2.0), 1), (vec2(4.0, 1.0), 0), (to, 0)]
);
}
_ if i < 11 => {
assert_eq!(path.path, vec![vec2(1.0, 2.0), vec2(4.0, 1.0), to]);
assert_eq!(
path.path_with_layers,
vec![(vec2(1.0, 2.0), 1), (vec2(4.0, 1.0), 0), (to, 0)]
);
}
_ if i < 15 => {
assert_eq!(path.path, vec![vec2(2.0, 2.0), vec2(3.0, 1.0), to]);
assert_eq!(
path.path_with_layers,
vec![(vec2(2.0, 2.0), 0), (vec2(3.0, 1.0), 0), (to, 0)]
);
}
_ => unreachable!(),
}
}
Expand All @@ -456,9 +519,27 @@ mod tests {
let to = vec2(i as f32 / 10.0, 2.1);
let path = dbg!(mesh.path(from, to).unwrap());
match i {
7 => assert_eq!(path.path, vec![vec2(4.0, 1.0), to]),
_ if i < 11 => assert_eq!(path.path, vec![vec2(4.0, 1.0), vec2(1.0, 2.0), to]),
_ if i < 15 => assert_eq!(path.path, vec![vec2(3.0, 1.0), vec2(2.0, 2.0), to]),
7 => {
assert_eq!(path.path, vec![vec2(4.0, 1.0), to]);
assert_eq!(
path.path_with_layers,
vec![(vec2(4.0, 1.0), 1), (vec2(0.9999997, 2.0), 0), (to, 0)]
);
}
_ if i < 11 => {
assert_eq!(path.path, vec![vec2(4.0, 1.0), vec2(1.0, 2.0), to]);
assert_eq!(
path.path_with_layers,
vec![(vec2(4.0, 1.0), 1), (vec2(1.0, 2.0), 0), (to, 0)]
);
}
_ if i < 15 => {
assert_eq!(path.path, vec![vec2(3.0, 1.0), vec2(2.0, 2.0), to]);
assert_eq!(
path.path_with_layers,
vec![(vec2(3.0, 1.0), 0), (vec2(2.0, 2.0), 0), (to, 0)]
);
}
_ => unreachable!(),
}
}
Expand All @@ -483,6 +564,14 @@ mod tests {
path.path,
vec![vec2(3.0, 1.0,), vec2(4.0, 1.0,), vec2(2.5, 1.5,),],
);
assert_eq!(
path.path_with_layers,
vec![
(vec2(3.0, 1.0), 0),
(vec2(4.0, 1.0), 1),
(vec2(2.5, 1.5), 1),
],
);

let path_back = dbg!(mesh
.path(
Expand All @@ -500,6 +589,14 @@ mod tests {
path_back.path,
vec![vec2(4.0, 1.0,), vec2(3.0, 1.0,), vec2(2.5, 1.5,),],
);
assert_eq!(
path_back.path_with_layers,
vec![
(vec2(4.0, 1.0), 0),
(vec2(3.0, 1.0), 0),
(vec2(2.5, 1.5), 0),
],
);
}

#[test]
Expand Down
Loading

0 comments on commit 4e38103

Please sign in to comment.