Skip to content

Commit

Permalink
New intersection/cutting implementation #6 #14
Browse files Browse the repository at this point in the history
  • Loading branch information
@aeplay
aeplay committed Nov 20, 2016
1 parent b62e698 commit 4549198
Show file tree
Hide file tree
Showing 4 changed files with 109 additions and 55 deletions.
8 changes: 8 additions & 0 deletions lib/kay/src/compact_dict.rs
View file
Expand Up @@ -15,6 +15,14 @@ impl <K: Eq + Copy, V: Compact + Clone, A: Allocator> CompactDict<K, V, A> {
}
}

pub fn len(&self) -> usize {
self.keys.len()
}

pub fn is_empty(&self) -> bool {
self.keys.is_empty()
}

pub fn get(&self, query: K) -> Option<&V> {
for i in 0..self.keys.len() {
if self.keys[i] == query {
Expand Down
5 changes: 1 addition & 4 deletions src/core/merge_groups.rs
View file
Expand Up @@ -8,10 +8,7 @@ pub trait MergeGroupsVecLike<T: Sized> : Index<usize, Output=T> + IndexMut<usize
}

pub trait MergeGroups<T, C1: MergeGroupsVecLike<T> + Clone> : MergeGroupsVecLike<C1> {
fn merge_groups<F: Fn(
&MergeGroupsVecLike<T, Output=T, Target=[T]>,
&MergeGroupsVecLike<T, Output=T, Target=[T]>
) -> bool>(&mut self, merge_f: F) {
fn merge_groups<F: Fn(&C1, &C1) -> bool>(&mut self, merge_f: F) {
let mut old_len = 0;
while self.len() != old_len {
old_len = self.len();
Expand Down
42 changes: 25 additions & 17 deletions src/game/lanes_and_cars/mod.rs
View file
Expand Up @@ -5,7 +5,7 @@ mod intelligent_acceleration;
use self::intelligent_acceleration::{intelligent_acceleration, COMFORTABLE_BREAKING_DECELERATION};
use core::geometry::CPath;
use kay::{ID, Actor, CVec, Swarm, CreateWith, Recipient, ActorSystem, Fate};
use descartes::{FiniteCurve, RoughlyComparable, Band, Intersect};
use descartes::{FiniteCurve, RoughlyComparable, Band, Intersect, Curve};
use ordered_float::OrderedFloat;
use itertools::Itertools;
use ::std::f32::INFINITY;
Expand Down Expand Up @@ -34,20 +34,6 @@ impl Lane {
in_construction: 0.0
}
}

fn add_next_lane(&mut self, next_lane: ID) {
self.interactions.push(Interaction{
partner_lane: next_lane,
kind: Next{partner_start: 0.0}
});
}

fn add_previous_lane(&mut self, previous_lane: ID, previous_lane_length: f32) {
self.interactions.push(Interaction{
partner_lane: previous_lane,
kind: Previous{start: 0.0, partner_length: previous_lane_length}
});
}
}

#[derive(Compact, Actor, Clone)]
Expand Down Expand Up @@ -150,8 +136,9 @@ impl Recipient<Tick> for Lane {

loop {
let should_pop = self.cars.iter().rev().find(|car| *car.position > self.length).map(|car_over_end| {
if let Some(next_overlap) = self.interactions.iter().find(|overlap| match overlap.kind {Next{..} => true, _ => false}) {
next_overlap.partner_lane << Add::Car(car_over_end.offset_by(-self.length));
let first_next_interaction = self.interactions.iter().find(|interaction| match interaction.kind {Next{..} => true, _ => false});
if let Some(&Interaction{partner_lane, kind: Next{partner_start}, ..}) = first_next_interaction {
partner_lane << Add::Car(car_over_end.offset_by(-self.length + partner_start));
};
car_over_end
}).is_some();
Expand Down Expand Up @@ -325,7 +312,17 @@ impl Recipient<Connect> for Lane {
partner_start: 0.0
}
})
} else if let Some(self_end_on_other) = other_path.project(self.path.end()) {
if other_path.along(self_end_on_other).is_roughly_within(self.path.end(), CONNECTION_TOLERANCE) {
self.interactions.push(Interaction{
partner_lane: other_id,
kind: InteractionKind::Next{
partner_start: self_end_on_other
}
})
}
}

if other_path.end().is_roughly_within(self.path.start(), CONNECTION_TOLERANCE) {
self.interactions.push(Interaction{
partner_lane: other_id,
Expand All @@ -334,7 +331,18 @@ impl Recipient<Connect> for Lane {
partner_length: other_path.length()
}
})
} else if let Some(other_end_on_self) = self.path.project(other_path.end()) {
if self.path.along(other_end_on_self).is_roughly_within(other_path.end(), CONNECTION_TOLERANCE) {
self.interactions.push(Interaction{
partner_lane: other_id,
kind: InteractionKind::Previous{
start: other_end_on_self,
partner_length: other_path.length()
}
})
}
}

let self_band = Band::new(self.path.clone(), 5.0);
let other_band = Band::new(other_path.clone(), 5.0);
let intersections = (&self_band.outline(), &other_band.outline()).intersect();
Expand Down
109 changes: 75 additions & 34 deletions src/game/lanes_and_cars/planning/plan.rs
View file
@@ -1,4 +1,4 @@
use descartes::{N, Path, Norm, Band, Intersect, convex_hull, Curve, FiniteCurve, RoughlyComparable};
use descartes::{N, Path, Norm, Band, Intersect, convex_hull, Curve, FiniteCurve, RoughlyComparable, Dot, WithUniqueOrthogonal};
use kay::{CVec, CDict};
use core::geometry::{CPath};
use core::merge_groups::MergeGroups;
Expand Down Expand Up @@ -33,20 +33,20 @@ impl Default for PlanDelta{
#[derive(Compact, Clone)]
pub struct Intersection{
pub shape: CPath,
incoming: CVec<RoadStrokeNode>,
outgoing: CVec<RoadStrokeNode>,
incoming: CDict<RoadStrokeRef, RoadStrokeNode>,
outgoing: CDict<RoadStrokeRef, RoadStrokeNode>,
pub strokes: CVec<RoadStroke>
}

impl<'a> RoughlyComparable for &'a Intersection{
fn is_roughly_within(&self, other: &Intersection, tolerance: N) -> bool {
(&self.shape).is_roughly_within(&other.shape, tolerance) &&
self.incoming.len() == other.incoming.len() &&
self.incoming.iter().all(|self_incoming| other.incoming.iter().any(|other_incoming|
self.incoming.values().all(|self_incoming| other.incoming.values().any(|other_incoming|
self_incoming.is_roughly_within(other_incoming, tolerance)
)) &&
self.outgoing.len() == other.outgoing.len() &&
self.outgoing.iter().all(|self_outgoing| other.outgoing.iter().any(|other_outgoing|
self.outgoing.values().all(|self_outgoing| other.outgoing.values().any(|other_outgoing|
self_outgoing.is_roughly_within(other_outgoing, tolerance)
)) &&
self.strokes.len() == other.strokes.len() &&
Expand Down Expand Up @@ -208,50 +208,49 @@ impl Plan{
if group.len() >= 2 {
Some(Intersection{
shape: convex_hull::<CPath>(group),
incoming: CVec::new(),
outgoing: CVec::new(),
incoming: CDict::new(),
outgoing: CDict::new(),
strokes: CVec::new()
})
} else {None}
).collect::<CVec<_>>();

// Cut strokes at intersections

let mut strokes_todo = self.strokes.iter().cloned().collect::<Vec<_>>();
let mut strokes_todo = self.strokes.iter().cloned().enumerate().map(|(i, stroke)| (RoadStrokeRef(i), stroke)).collect::<CDict<_,_>>();
let mut cutting_ongoing = true;
let mut iters = 0;

while cutting_ongoing {
cutting_ongoing = false;
let new_strokes = strokes_todo.iter().flat_map(|stroke| {
let new_strokes = strokes_todo.pairs().map(|(stroke_ref, stroke)| {
let stroke_path = stroke.path();
let mut maybe_cut_strokes = intersections.iter_mut().filter_map(|intersection| {
let intersection_points = (&stroke_path, &intersection.shape).intersect();
if intersection_points.len() >= 2 {
let entry_distance = intersection_points.iter().map(|p| OrderedFloat(p.along_a)).min().unwrap();
let exit_distance = intersection_points.iter().map(|p| OrderedFloat(p.along_a)).max().unwrap();
let mut cut_strokes = Vec::with_capacity(2);
if let Some(before_intersection) = stroke.cut_before(*entry_distance - 1.0) {
intersection.incoming.push(*before_intersection.nodes.last().unwrap());
cut_strokes.push(before_intersection);
}
if let Some(after_intersection) = stroke.cut_after(*exit_distance + 1.0) {
intersection.outgoing.push(after_intersection.nodes[0]);
cut_strokes.push(after_intersection)
}
if cut_strokes.is_empty() {None} else {Some(cut_strokes)}
intersection.incoming.insert(*stroke_ref, RoadStrokeNode{
position: stroke_path.along(*entry_distance),
direction: stroke_path.direction_along(*entry_distance)
});
intersection.outgoing.insert(*stroke_ref, RoadStrokeNode{
position: stroke_path.along(*exit_distance),
direction: stroke_path.direction_along(*exit_distance)
});
None
} else if intersection_points.len() == 1 {
if intersection.shape.contains(stroke.nodes[0].position) {
let exit_distance = intersection_points[0].along_a;
if let Some(after_intersection) = stroke.cut_after(exit_distance + 1.0) {
intersection.outgoing.push(after_intersection.nodes[0]);
Some(vec![after_intersection])
intersection.outgoing.insert(*stroke_ref, after_intersection.nodes[0]);
Some(after_intersection)
} else {None}
} else if intersection.shape.contains(stroke.nodes.last().unwrap().position) {
let entry_distance = intersection_points[0].along_a;
if let Some(before_intersection) = stroke.cut_before(entry_distance - 1.0) {
intersection.incoming.push(*before_intersection.nodes.last().unwrap());
Some(vec![before_intersection])
intersection.incoming.insert(*stroke_ref, *before_intersection.nodes.last().unwrap());
Some(before_intersection)
} else {None}
} else {None}
} else {None}
Expand All @@ -260,11 +259,11 @@ impl Plan{
match maybe_cut_strokes.next() {
Some(cut_strokes) => {
cutting_ongoing = true;
cut_strokes
(*stroke_ref, cut_strokes)
},
None => vec![stroke.clone()]
None => (*stroke_ref, stroke.clone())
}
}).collect::<Vec<_>>();
}).collect::<CDict<_, _>>();

strokes_todo = new_strokes;
iters += 1;
Expand All @@ -278,18 +277,60 @@ impl Plan{
// Create connecting strokes on intersections

for intersection in intersections.iter_mut() {
intersection.strokes = intersection.incoming.iter().flat_map(|incoming|
intersection.outgoing.iter().filter_map(|outgoing|
if (incoming.position - outgoing.position).norm() > MIN_NODE_DISTANCE {
Some(RoadStroke::new(vec![*incoming, *outgoing].into()))
} else {None}
).collect::<Vec<_>>()
).collect::<CVec<_>>()
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);
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);
for outgoing_group in &mut outgoing_groups {
let base_position = outgoing_group[0].1.position;
let direction_right = -outgoing_group[0].1.direction.orthogonal();
outgoing_group.sort_by_key(|group| OrderedFloat((group.1.position - base_position).dot(&direction_right)));
}

intersection.strokes = incoming_groups.iter().flat_map(|incoming_group| {
outgoing_groups.iter().filter_map(|outgoing_group| {
if groups_already_connected(incoming_group, outgoing_group) {
None
} 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()))
} else {None}
).min_by_key(|stroke| OrderedFloat(stroke.path().length()))
}
}).collect::<Vec<_>>()
}).collect::<CVec<_>>();
}

PlanResult{
intersections: intersections.into_iter().enumerate().map(|(i, intersection)| (IntersectionRef(i), intersection)).collect(),
inbetween_strokes: inbetween_strokes.into_iter().enumerate().map(|(i, stroke)| (InbetweenStrokeRef(i), stroke)).collect()
inbetween_strokes: inbetween_strokes.values().cloned().enumerate().map(|(i, stroke)| (InbetweenStrokeRef(i), stroke)).collect()
}
}
}

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(any_incoming_2.direction)
&& (any_incoming_1.position - any_incoming_2.position).dot(&any_incoming_1.direction).is_roughly_within(0.0, MAX_PARALLEL_INTERSECTION_NODES_OFFSET)
}

#[allow(ptr_arg)]
fn groups_already_connected(incoming_group: &InOrOutGroup, outgoing_group: &InOrOutGroup) -> bool {
incoming_group.iter().all(|&(incoming_ref, _)|
outgoing_group.iter().any(|&(outgoing_ref, _)| incoming_ref == outgoing_ref)
)
}

0 comments on commit 4549198

Please sign in to comment.