Skip to content

Commit

Permalink
Overengineered better grouping algorithm and reducing number of inter…
Browse files Browse the repository at this point in the history
…section points #19
  • Loading branch information
aeplay committed Dec 3, 2016
1 parent 488d118 commit b241b22
Show file tree
Hide file tree
Showing 5 changed files with 191 additions and 90 deletions.
152 changes: 152 additions & 0 deletions src/core/disjoint_sets.rs
@@ -0,0 +1,152 @@
pub struct DisjointSets<T> {
elements: Vec<T>,
parent_indices: Vec<usize>,
ranks: Vec<usize>,
is_sorted: bool
}

impl<T> DisjointSets<T> {
pub fn from_individuals(individuals: Vec<T>) -> Self {
DisjointSets{
parent_indices: (0..individuals.len()).into_iter().collect(),
ranks: vec![0; individuals.len()],
elements: individuals,
is_sorted: true
}
}

fn find_root(&mut self, idx: usize) -> usize {
if self.parent_indices[idx] != idx {
let parent = self.parent_indices[idx];
self.parent_indices[idx] = self.find_root(parent);
}
self.parent_indices[idx]
}

fn union(&mut self, idx_a: usize, idx_b: usize) {
let root_a = self.find_root(idx_a);
let root_b = self.find_root(idx_b);

if root_a != root_b {
if self.ranks[root_a] < self.ranks[root_b] {
self.parent_indices[root_a] = root_b;
} else if self.ranks[root_a] > self.ranks[root_b] {
self.parent_indices[root_b] = root_a;
} else {
self.parent_indices[root_b] = root_a;
self.ranks[root_a] += 1;
}
}

self.is_sorted = false;
}

pub fn union_all_with<F: Fn(&T, &T) -> bool>(&mut self, should_union: F) {
let len = self.elements.len();
for idx_a in 0..len {
for idx_b in (idx_a + 1)..len {
if should_union(&self.elements[idx_a], &self.elements[idx_b]) {
self.union(idx_a, idx_b);
}
}
}
}

fn ensure_sorted(&mut self) {
if !self.is_sorted {
// counting sort
let mut root_occurences = vec![0; self.elements.len()];

for idx in 0..self.elements.len() {
root_occurences[self.find_root(idx)] += 1;
};

let mut root_start_index = root_occurences;

let mut current_start_index = 0;
#[allow(needless_range_loop)]
for root in 0..self.elements.len() {
// still occurence count
let next_start_index = current_start_index + root_start_index[root];
// now start index
root_start_index[root] = current_start_index;
current_start_index = next_start_index;
}

let mut new_elements = Vec::with_capacity(self.elements.len());
let mut new_ranks = Vec::with_capacity(self.elements.len());
let mut old_to_new_idx_map = vec![0; self.elements.len()];
let mut new_to_old_idx_map = vec![0; self.elements.len()];

#[allow(needless_range_loop)]
for idx in 0..self.elements.len() {
let root = self.find_root(idx);
let new_idx = root_start_index[root];
root_start_index[root] += 1;
old_to_new_idx_map[idx] = new_idx;
new_to_old_idx_map[new_idx] = idx;

unsafe {
::std::ptr::copy_nonoverlapping(&self.elements[idx], new_elements.as_mut_ptr().offset(new_idx as isize), 1);
::std::ptr::copy_nonoverlapping(&self.ranks[idx], new_ranks.as_mut_ptr().offset(new_idx as isize), 1);
}
}

unsafe {
new_elements.set_len(self.elements.len());
new_ranks.set_len(self.elements.len());
// prevents items to be dropped, since they live on in new_elements
self.elements.set_len(0);
}

self.elements = new_elements;
self.ranks = new_ranks;
self.parent_indices = new_to_old_idx_map.iter().map(|&old_idx| old_to_new_idx_map[self.parent_indices[old_idx]]).collect();

self.is_sorted = true;
}
}

pub fn sets(&mut self) -> SetsIterator<T> {
self.ensure_sorted();
SetsIterator{
elements: &self.elements,
input_iter: self.parent_indices.iter().enumerate().peekable()
}
}
}

pub struct SetsIterator<'a, T: 'a> {
elements: &'a Vec<T>,
input_iter: ::std::iter::Peekable<::std::iter::Enumerate<::std::slice::Iter<'a, usize>>>
}

impl<'a, T: 'a> Iterator for SetsIterator<'a, T> {
type Item = &'a [T];
fn next(&mut self) -> Option<&'a [T]> {
if let Some((set_start_idx, root)) = self.input_iter.next() {
let mut set_end_idx = set_start_idx + 1;

while self.input_iter.peek().map(|&(_, next_root)| next_root == root).unwrap_or(false) {
self.input_iter.next();
set_end_idx += 1;
}

Some(&self.elements[set_start_idx..set_end_idx])
} else {None}
}
}

#[test]
fn test_disjoint_sets() {
let mut numbers = DisjointSets::from_individuals(vec![112, 44, 32, 66, 52, 74, 176]);
numbers.union_all_with(|a, b| a % 10 == b % 10);
println!("{:?}, {:?}", numbers.elements, numbers.parent_indices);
numbers.ensure_sorted();
println!("{:?}, {:?}", numbers.elements, numbers.parent_indices);
let sets = numbers.sets();
let set1 = [112, 32, 52];
let set2 = [44, 74];
let set3 = [66, 176];
assert!(sets.collect::<Vec<_>>() == vec![&set1[..], &set2[..], &set3[..]]);
}
38 changes: 0 additions & 38 deletions src/core/merge_groups.rs

This file was deleted.

2 changes: 1 addition & 1 deletion src/core/mod.rs
@@ -1,4 +1,4 @@
pub mod ui;
pub mod geometry;
pub mod simulation;
pub mod merge_groups;
pub mod disjoint_sets;
18 changes: 7 additions & 11 deletions src/game/lanes_and_cars/planning/mod.rs
@@ -1,7 +1,6 @@
use descartes::{N, V2, P2, Norm, Segment, FiniteCurve, WithUniqueOrthogonal, RelativeToBasis, RoughlyComparable};
use kay::{Swarm, CVec, Recipient, CreateWith, ActorSystem, Individual, Fate};
use core::merge_groups::MergeGroups;
use itertools::Itertools;
use core::disjoint_sets::DisjointSets;

//TODO: Clean up this whole mess with more submodules

Expand Down Expand Up @@ -249,21 +248,18 @@ impl CurrentPlan{
))
});

let mut interactable_groups = new_interactables.clone().into_iter().map(
|interactable| vec![interactable]).collect::<Vec<_>>();
interactable_groups.merge_groups(|group_1, group_2|
group_1.iter().cartesian_product(group_2.iter()).any(|(interactable_1, interactable_2)|
(interactable_1.position - interactable_2.position).norm() < 20.0
&& (interactable_1.direction.is_roughly_within(interactable_2.direction, 0.1)
|| interactable_1.direction.is_roughly_within(-interactable_2.direction, 0.1))
)
let mut interactable_groups = DisjointSets::from_individuals(new_interactables.clone());
interactable_groups.union_all_with(|interactable_1, interactable_2|
(interactable_1.position - interactable_2.position).norm() < 20.0
&& (interactable_1.direction.is_roughly_within(interactable_2.direction, 0.1)
|| interactable_1.direction.is_roughly_within(-interactable_2.direction, 0.1))
);

for interactable in new_interactables.into_iter().chain(old_interactables) {
Swarm::<RoadStrokeNodeInteractable>::all() << CreateWith(interactable, AddToUI);
}

for interactable_group in interactable_groups {
for interactable_group in interactable_groups.sets() {
if interactable_group.len() > 1 {
let position = (interactable_group.iter().fold(V2::new(0.0, 0.0),
|sum, interactable| sum + interactable.position.to_vector()
Expand Down
71 changes: 31 additions & 40 deletions src/game/lanes_and_cars/planning/plan_result_steps.rs
@@ -1,24 +1,25 @@
use descartes::{N, P2, Path, Norm, Band, Intersect, convex_hull, Curve, FiniteCurve, RoughlyComparable, Dot, WithUniqueOrthogonal, Segment};
use kay::{CVec, CDict};
use core::geometry::{CPath};
use core::merge_groups::MergeGroups;
use core::disjoint_sets::DisjointSets;
use ordered_float::OrderedFloat;
use itertools::{Itertools};
use super::{RoadStroke, RoadStrokeNode, MIN_NODE_DISTANCE, Intersection, RoadStrokeRef};

const STROKE_INTERSECTION_WIDTH : N = 4.0;
const INTERSECTION_GROUPING_RADIUS : N = 30.0;

#[inline(never)]
pub fn find_intersections(strokes: &CVec<RoadStroke>) -> CVec<Intersection> {
let mut intersection_point_groups = find_intersection_points(strokes)
.into_iter().map(|point| vec![point]).collect::<Vec<_>>();
let mut intersection_point_groups = DisjointSets::from_individuals(find_intersection_points(strokes));

intersection_point_groups.merge_groups(|group_1, group_2|
group_1.iter().cartesian_product(group_2.iter())
.any(|(point_i, point_j)|
(*point_i - *point_j).norm() < INTERSECTION_GROUPING_RADIUS));
intersection_point_groups.union_all_with(|point_i, point_j|
(point_i.x - point_j.x).abs() < INTERSECTION_GROUPING_RADIUS
&& (point_i.y - point_j.y).abs() < INTERSECTION_GROUPING_RADIUS
&& (*point_i - *point_j).norm() < INTERSECTION_GROUPING_RADIUS
);

intersection_point_groups.iter().filter_map(|group|
intersection_point_groups.sets().filter_map(|group|
if group.len() >= 2 {
Some(Intersection{
shape: convex_hull::<CPath>(group),
Expand All @@ -30,40 +31,20 @@ pub fn find_intersections(strokes: &CVec<RoadStroke>) -> CVec<Intersection> {
).collect::<CVec<_>>()
}

#[inline(never)]
fn find_intersection_points(strokes: &CVec<RoadStroke>) -> Vec<P2> {
strokes.iter().enumerate().flat_map(|(i, stroke_1)| {
let path_1 = stroke_1.path();
let band_1 = Band::new(path_1.clone(), STROKE_INTERSECTION_WIDTH).outline();
strokes[i+1..].iter().flat_map(|stroke_2| {
let path_2 = stroke_2.path();
let band_2 = Band::new(path_2.clone(), STROKE_INTERSECTION_WIDTH).outline();
(&band_1, &band_2).intersect().iter().flat_map(|intersection| {
let point_1_distance = path_1.project(intersection.position);
let mirrored_point_1 = point_1_distance.map(|distance|
path_1.along(distance) + (path_1.along(distance) - intersection.position)
);
let point_2_distance = path_2.project(intersection.position);
let mirrored_point_2 = point_2_distance.map(|distance|
path_2.along(distance) + (path_2.along(distance) - intersection.position)
);
vec![intersection.position].into_iter()
.chain(mirrored_point_1.into_iter()).chain(mirrored_point_2.into_iter())
}).collect::<Vec<_>>()
(&band_1, &band_2).intersect().iter().map(|intersection| intersection.position).collect::<Vec<_>>()
}).collect::<Vec<_>>()
}).collect::<Vec<_>>()
}

const MAX_PARALLEL_INTERSECTION_NODES_OFFSET : f32 = 10.0;

type InOrOutGroup = Vec<(RoadStrokeRef, RoadStrokeNode)>;
#[allow(ptr_arg)]
fn merge_incoming_or_outgoing_group(group_1: &InOrOutGroup, group_2: &InOrOutGroup) -> bool {
let any_incoming_1 = group_1[0].1;
let any_incoming_2 = group_2[0].1;
any_incoming_1.direction.is_roughly_within(any_incoming_2.direction, 0.05)
&& (any_incoming_1.position - any_incoming_2.position).dot(&any_incoming_1.direction).is_roughly_within(0.0, MAX_PARALLEL_INTERSECTION_NODES_OFFSET)
}

#[inline(never)]
pub fn trim_strokes_and_add_incoming_outgoing(strokes: &CVec<RoadStroke>, intersections: &mut CVec<Intersection>) -> CVec<RoadStroke> {
let mut strokes_todo = strokes.iter().cloned().enumerate().map(|(i, stroke)| (RoadStrokeRef(i), stroke)).collect::<Vec<_>>();
let mut trimming_ongoing = true;
Expand Down Expand Up @@ -131,6 +112,7 @@ pub fn trim_strokes_and_add_incoming_outgoing(strokes: &CVec<RoadStroke>, inters
strokes_todo.into_iter().map(|(_, stroke)| stroke).collect()
}

#[inline(never)]
pub fn find_transfer_strokes(trimmed_strokes: &CVec<RoadStroke>) -> Vec<RoadStroke> {
trimmed_strokes.iter().enumerate().flat_map(|(i, stroke_1)| {
let path_1 = stroke_1.path();
Expand Down Expand Up @@ -214,20 +196,29 @@ pub fn find_transfer_strokes(trimmed_strokes: &CVec<RoadStroke>) -> Vec<RoadStro
}).collect::<Vec<_>>()
}

const MAX_PARALLEL_INTERSECTION_NODES_OFFSET : f32 = 10.0;

#[inline(never)]
pub fn create_connecting_strokes(intersections: &mut CVec<Intersection>) {
for intersection in intersections.iter_mut() {
let mut incoming_groups = intersection.incoming.pairs().map(
|(incoming_ref, incoming)| vec![(*incoming_ref, *incoming)]).collect::<Vec<_>>();
incoming_groups.merge_groups(merge_incoming_or_outgoing_group);
let mut incoming_groups_sets = DisjointSets::from_individuals(intersection.incoming.pairs().collect());
incoming_groups_sets.union_all_with(|&(_, incoming_1), &(_, incoming_2)|
incoming_1.direction.is_roughly_within(incoming_2.direction, 0.05)
&& (incoming_1.position - incoming_2.position).dot(&incoming_1.direction).is_roughly_within(0.0, MAX_PARALLEL_INTERSECTION_NODES_OFFSET)
);
let mut incoming_groups = incoming_groups_sets.sets().map(|set| set.to_vec()).collect::<Vec<_>>();
for incoming_group in &mut incoming_groups {
let base_position = incoming_group[0].1.position;
let direction_right = -incoming_group[0].1.direction.orthogonal();
incoming_group.sort_by_key(|group| OrderedFloat((group.1.position - base_position).dot(&direction_right)));
}

let mut outgoing_groups = intersection.outgoing.pairs().map(
|(outgoing_ref, outgoing)| vec![(*outgoing_ref, *outgoing)]).collect::<Vec<_>>();
outgoing_groups.merge_groups(merge_incoming_or_outgoing_group);
let mut outgoing_groups_sets = DisjointSets::from_individuals(intersection.outgoing.pairs().collect());
outgoing_groups_sets.union_all_with(|&(_, outgoing_1), &(_, outgoing_2)|
outgoing_1.direction.is_roughly_within(outgoing_2.direction, 0.05)
&& (outgoing_1.position - outgoing_2.position).dot(&outgoing_1.direction).is_roughly_within(0.0, MAX_PARALLEL_INTERSECTION_NODES_OFFSET)
);
let mut outgoing_groups = outgoing_groups_sets.sets().map(|set| set.to_vec()).collect::<Vec<_>>();
for outgoing_group in &mut outgoing_groups {
let base_position = outgoing_group[0].1.position;
let direction_right = -outgoing_group[0].1.direction.orthogonal();
Expand All @@ -241,13 +232,13 @@ pub fn create_connecting_strokes(intersections: &mut CVec<Intersection>) {
incoming_group.iter().cartesian_product(outgoing_group.iter()).filter_map(
|(&(incoming_ref, incoming), &(outgoing_ref, outgoing))|
if incoming_ref == outgoing_ref {
Some(RoadStroke::new(vec![incoming, outgoing].into()))
Some(RoadStroke::new(vec![*incoming, *outgoing].into()))
} else {None}
).into_iter().collect::<Vec<_>>()
} else {
incoming_group.iter().zip(outgoing_group.iter()).filter_map(|(&(_, incoming), &(_, outgoing))|
if (incoming.position - outgoing.position).norm() > MIN_NODE_DISTANCE {
Some(RoadStroke::new(vec![incoming, outgoing].into()))
Some(RoadStroke::new(vec![*incoming, *outgoing].into()))
} else {None}
).min_by_key(|stroke| OrderedFloat(stroke.path().length())).into_iter().collect::<Vec<_>>()
}
Expand All @@ -257,7 +248,7 @@ pub fn create_connecting_strokes(intersections: &mut CVec<Intersection>) {
}

#[allow(ptr_arg)]
fn groups_correspond(incoming_group: &InOrOutGroup, outgoing_group: &InOrOutGroup) -> bool {
fn groups_correspond(incoming_group: &Vec<(&RoadStrokeRef, &RoadStrokeNode)>, outgoing_group: &Vec<(&RoadStrokeRef, &RoadStrokeNode)>) -> bool {
incoming_group.iter().all(|&(incoming_ref, _)|
outgoing_group.iter().any(|&(outgoing_ref, _)| incoming_ref == outgoing_ref)
)
Expand Down

0 comments on commit b241b22

Please sign in to comment.