a-b-street/abstreet
1
1
Fork 0
mirror of https://github.com/a-b-street/abstreet.git synced 2024-11-25 22:13:27 +03:00
Code Issues Projects Releases Wiki Activity

new, simple implementation of trace_route and lots of deleted old code

Browse Source
This commit is contained in:
Dustin Carlino 2018-11-04 07:51:37 -08:00
parent 029879828e
commit 376f9bd3c5
7 changed files with 148 additions and 286 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
map_model/Cargo.toml
View File

@ -6,6 +6,7 @@ authors = ["Dustin Carlino <dabreegster@gmail.com>"]
[dependencies]
aabb-quadtree = "0.1.0"
abstutil = { path = "../abstutil" }
derivative = "1.0.0"
dimensioned = { git = "https://github.com/paholg/dimensioned", rev = "0e1076ebfa5128d1ee544bdc9754c948987b6fe3", features = ["serde"] }
geo = "0.10.2"
geom = { path = "../geom" }

6
map_model/src/lib.rs
View File

@ -1,5 +1,7 @@
extern crate aabb_quadtree;
extern crate abstutil;
#[macro_use]
extern crate derivative;
extern crate dimensioned;
extern crate geo;
extern crate geom;
@ -40,9 +42,9 @@ pub use intersection::{Intersection, IntersectionID};
pub use lane::{Lane, LaneID, LaneType, PARKING_SPOT_LENGTH};
pub use map::Map;
pub use parcel::{Parcel, ParcelID};
pub use pathfind::{Path, PathStep, Pathfinder};
pub use pathfind::{Path, PathStep, Pathfinder, Trace};
pub use road::{Road, RoadID};
pub use traversable::{Trace, TraceGeometry, TraceSegment, Traversable};
pub use traversable::Traversable;
pub use turn::{Turn, TurnID};
pub const LANE_THICKNESS: f64 = 2.5;

148
map_model/src/pathfind.rs
View File

@ -1,5 +1,5 @@
use dimensioned::si;
use geom::{Line, Pt2D};
use geom::{Line, PolyLine, Pt2D};
use ordered_float::NotNaN;
use std::collections::{BinaryHeap, HashMap, VecDeque};
use {LaneID, LaneType, Map, Traversable, TurnID};
@ -36,20 +36,23 @@ impl PathStep {
}
}
#[derive(Debug, PartialEq, Eq, Clone, Serialize, Deserialize)]
#[derive(Debug, Clone, Serialize, Deserialize, Derivative)]
#[derivative(PartialEq, Eq)]
pub struct Path {
// TODO way to encode start/end dist? I think it's needed for trace_route later...
// actually not start dist -- that really changes all the time
steps: VecDeque<PathStep>,
// TODO :(
#[derivative(PartialEq = "ignore")]
end_dist: si::Meter<f64>,
}
// TODO can have a method to verify the path is valid
impl Path {
fn new(map: &Map, steps: Vec<PathStep>) -> Path {
fn new(map: &Map, steps: Vec<PathStep>, end_dist: si::Meter<f64>) -> Path {
// Can disable this after trusting it.
validate(map, &steps);
Path {
steps: VecDeque::from(steps),
end_dist,
}
}
@ -91,6 +94,125 @@ impl Path {
pub fn last_step(&self) -> &PathStep {
&self.steps[self.steps.len() - 1]
}
pub fn trace(
&self,
map: &Map,
start_dist: si::Meter<f64>,
dist_ahead: si::Meter<f64>,
) -> Trace {
let mut pts_so_far: Option<PolyLine> = None;
let mut dist_remaining = dist_ahead;
fn extend(a: &mut Option<PolyLine>, b: PolyLine) {
if let Some(ref mut pts) = a {
pts.extend(b);
} else {
*a = Some(b);
}
}
// Special case the first step.
match self.steps[0] {
PathStep::Lane(id) => {
let (pts, dist) = map
.get_l(id)
.lane_center_pts
.slice(start_dist, start_dist + dist_ahead);
pts_so_far = Some(pts);
dist_remaining = dist;
}
PathStep::ContraflowLane(id) => {
let pts = map.get_l(id).lane_center_pts.reversed();
let reversed_start = pts.length() - start_dist;
let (pts, dist) = pts.slice(reversed_start, reversed_start + dist_ahead);
pts_so_far = Some(pts);
dist_remaining = dist;
}
PathStep::Turn(id) => {
let line = &map.get_t(id).line;
if line.length() == 0.0 * si::M {
// Don't do anything yet!
} else {
let (pts, dist) = PolyLine::new(vec![line.pt1(), line.pt2()])
.slice(start_dist, start_dist + dist_ahead);
pts_so_far = Some(pts);
dist_remaining = dist;
}
}
};
// Crunch through the intermediate steps, as long as we can.
for i in 1..self.steps.len() - 1 {
if dist_remaining <= 0.0 * si::M {
return pts_so_far.unwrap();
}
match self.steps[i] {
PathStep::Lane(id) => {
let (pts, dist) = map
.get_l(id)
.lane_center_pts
.slice(0.0 * si::M, dist_remaining);
extend(&mut pts_so_far, pts);
dist_remaining = dist;
}
PathStep::ContraflowLane(id) => {
let (pts, dist) = map
.get_l(id)
.lane_center_pts
.reversed()
.slice(0.0 * si::M, dist_remaining);
extend(&mut pts_so_far, pts);
dist_remaining = dist;
}
PathStep::Turn(id) => {
let line = &map.get_t(id).line;
if line.length() == 0.0 * si::M {
// Don't do anything
} else {
let (pts, dist) = PolyLine::new(vec![line.pt1(), line.pt2()])
.slice(start_dist, start_dist + dist_remaining);
extend(&mut pts_so_far, pts);
dist_remaining = dist;
}
}
}
}
// If we made it to the last step, use the end_dist.
let last_dist = if dist_remaining < self.end_dist {
dist_remaining
} else {
self.end_dist
};
match self.steps[self.steps.len() - 1] {
PathStep::Lane(id) => {
let (pts, _) = map.get_l(id).lane_center_pts.slice(0.0 * si::M, last_dist);
extend(&mut pts_so_far, pts);
}
PathStep::ContraflowLane(id) => {
let (pts, _) = map
.get_l(id)
.lane_center_pts
.reversed()
.slice(0.0 * si::M, last_dist);
extend(&mut pts_so_far, pts);
}
PathStep::Turn(id) => {
let line = &map.get_t(id).line;
if line.length() == 0.0 * si::M {
// Ignore it
} else {
let (pts, _) = PolyLine::new(vec![line.pt1(), line.pt2()])
.slice(start_dist, start_dist + dist_ahead);
extend(&mut pts_so_far, pts);
}
}
};
return pts_so_far.unwrap();
}
}
pub enum Pathfinder {
@ -169,9 +291,13 @@ impl Pathfinder {
if start == end {
if start_dist > end_dist {
assert_eq!(map.get_l(start).lane_type, LaneType::Sidewalk);
return Some(Path::new(map, vec![PathStep::ContraflowLane(start)]));
return Some(Path::new(
map,
vec![PathStep::ContraflowLane(start)],
end_dist,
));
}
return Some(Path::new(map, vec![PathStep::Lane(start)]));
return Some(Path::new(map, vec![PathStep::Lane(start)], end_dist));
}
// This should be deterministic, since cost ties would be broken by LaneID.
@ -193,7 +319,7 @@ impl Pathfinder {
reversed_lanes.reverse();
assert_eq!(reversed_lanes[0], start);
assert_eq!(*reversed_lanes.last().unwrap(), end);
return Some(lanes_to_path(map, VecDeque::from(reversed_lanes)));
return Some(lanes_to_path(map, VecDeque::from(reversed_lanes), end_dist));
}
lookup = backrefs[&lookup];
}
@ -237,7 +363,7 @@ fn validate(map: &Map, steps: &Vec<PathStep>) {
}
// TODO Tmp hack. Need to rewrite the A* implementation to natively understand PathSteps.
fn lanes_to_path(map: &Map, mut lanes: VecDeque<LaneID>) -> Path {
fn lanes_to_path(map: &Map, mut lanes: VecDeque<LaneID>, end_dist: si::Meter<f64>) -> Path {
assert!(lanes.len() > 1);
let mut steps: Vec<PathStep> = Vec::new();
@ -278,7 +404,7 @@ fn lanes_to_path(map: &Map, mut lanes: VecDeque<LaneID>) -> Path {
} else {
steps.push(PathStep::Lane(current_turn.dst));
}
Path::new(map, steps)
Path::new(map, steps, end_dist)
}
fn pick_turn(from: LaneID, to: LaneID, map: &Map) -> TurnID {
@ -304,3 +430,5 @@ fn is_contraflow(map: &Map, from: LaneID, to: LaneID) -> bool {
fn leads_to_end_of_lane(turn: TurnID, map: &Map) -> bool {
is_contraflow(map, turn.src, turn.dst)
}
pub type Trace = PolyLine;

162
map_model/src/traversable.rs
View File

@ -1,5 +1,5 @@
use dimensioned::si;
use geom::{Angle, PolyLine, Pt2D};
use geom::{Angle, Pt2D};
use {LaneID, Map, TurnID};
// TODO this probably doesn't belong in map model after all.
@ -48,89 +48,6 @@ impl Traversable {
}
}
pub fn slice(
&self,
reverse: bool,
map: &Map,
start: si::Meter<f64>,
end: si::Meter<f64>,
) -> (Trace, si::Meter<f64>) {
if start > end || start < 0.0 * si::M || end < 0.0 * si::M {
panic!(
"Can't do slice [{}, {}] with reverse={} on {:?}",
start, end, reverse, self
);
}
match self {
&Traversable::Lane(id) => if reverse {
let pts = &map.get_l(id).lane_center_pts;
let len = pts.length();
let (polyline, remainder) = pts.reversed().slice(len - start, end);
let actual_len = polyline.length();
(
Trace {
geom: TraceGeometry::PolyLine(polyline),
segments: vec![TraceSegment {
on: *self,
start_dist: len - start,
end_dist: len - (start + actual_len),
}],
},
remainder,
)
} else {
let (polyline, remainder) = map.get_l(id).lane_center_pts.slice(start, end);
let actual_len = polyline.length();
(
Trace {
geom: TraceGeometry::PolyLine(polyline),
segments: vec![TraceSegment {
on: *self,
start_dist: start,
end_dist: start + actual_len,
}],
},
remainder,
)
},
&Traversable::Turn(id) => {
assert!(!reverse);
let t = map.get_t(id);
// Don't do the epsilon comparison here... if we did, the assert_eq's in extend()
// need to also have some buffer.
if t.line.length() == 0.0 * si::M {
(
Trace {
geom: TraceGeometry::Point(t.line.pt1()),
segments: vec![TraceSegment {
on: *self,
start_dist: start,
end_dist: start,
}],
},
end,
)
} else {
let (polyline, remainder) =
PolyLine::new(vec![t.line.pt1(), t.line.pt2()]).slice(start, end);
let actual_len = polyline.length();
(
Trace {
geom: TraceGeometry::PolyLine(polyline),
segments: vec![TraceSegment {
on: *self,
start_dist: start,
end_dist: start + actual_len,
}],
},
remainder,
)
}
}
}
}
pub fn dist_along(&self, dist: si::Meter<f64>, map: &Map) -> (Pt2D, Angle) {
match self {
&Traversable::Lane(id) => map.get_l(id).dist_along(dist),
@ -145,80 +62,3 @@ impl Traversable {
}
}
}
pub struct TraceSegment {
pub on: Traversable,
pub start_dist: si::Meter<f64>,
pub end_dist: si::Meter<f64>,
}
pub enum TraceGeometry {
Point(Pt2D),
PolyLine(PolyLine),
}
pub struct Trace {
// TODO A finalized trace is always a PolyLine... have a private builder type?
pub geom: TraceGeometry,
pub segments: Vec<TraceSegment>,
}
impl Trace {
pub fn get_polyline(&self) -> &PolyLine {
match self.geom {
TraceGeometry::Point(_) => panic!("Trace is a point, not polyline"),
TraceGeometry::PolyLine(ref polyline) => &polyline,
}
}
pub fn endpoints(&self) -> (Pt2D, Pt2D) {
match self.geom {
TraceGeometry::Point(pt) => (pt, pt),
TraceGeometry::PolyLine(ref polyline) => {
(polyline.points()[0], *polyline.points().last().unwrap())
}
}
}
pub fn extend(mut self, other: Trace) -> Trace {
self.geom = match (self.geom, other.geom) {
(TraceGeometry::Point(pt1), TraceGeometry::Point(pt2)) => {
assert_eq!(pt1, pt2);
TraceGeometry::Point(pt1)
}
(TraceGeometry::Point(pt1), TraceGeometry::PolyLine(line2)) => {
assert_eq!(pt1, line2.points()[0]);
TraceGeometry::PolyLine(line2)
}
(TraceGeometry::PolyLine(line1), TraceGeometry::Point(pt2)) => {
assert_eq!(*line1.points().last().unwrap(), pt2);
TraceGeometry::PolyLine(line1)
}
(TraceGeometry::PolyLine(mut line1), TraceGeometry::PolyLine(line2)) => {
line1.extend(line2);
TraceGeometry::PolyLine(line1)
}
};
self.segments.extend(other.segments);
self
}
pub fn debug(&self) {
println!("Trace with {} segments", self.segments.len());
match self.geom {
TraceGeometry::Point(pt) => {
println!(" - Point({})", pt);
}
TraceGeometry::PolyLine(ref polyline) => {
println!(
" - PolyLine({} ... {})",
polyline.points()[0],
polyline.points().last().unwrap()
);
}
}
for s in &self.segments {
println!(" - {:?} [{} to {}]", s.on, s.start_dist, s.end_dist);
}
}
}

38
sim/src/router.rs
View File

@ -171,43 +171,7 @@ impl Router {
}
pub fn trace_route(&self, start_dist: Distance, map: &Map, dist_ahead: Distance) -> Trace {
panic!("TODO");
/*
let (mut result, mut dist_left) =
start.slice(false, map, start_dist, start_dist + dist_along);
let mut last_lane = start.maybe_lane();
let mut idx = 0;
while dist_left > 0.0 * si::M && idx < self.path.len() {
let next_lane = self.path[idx];
if let Some(prev) = last_lane {
let (piece, new_dist_left) = Traversable::Turn(pick_turn(prev, next_lane, map))
.slice(false, map, 0.0 * si::M, dist_left);
result = result.extend(piece);
dist_left = new_dist_left;
if dist_left <= 0.0 * si::M {
break;
}
}
let (piece, new_dist_left) =
Traversable::Lane(next_lane).slice(false, map, 0.0 * si::M, dist_left);
if piece.endpoints().0 != result.endpoints().1 {
println!("so far");
result.debug();
println!("new piece");
piece.debug();
}
result = result.extend(piece);
dist_left = new_dist_left;
last_lane = Some(next_lane);
idx += 1;
}
// Excess dist_left is just ignored
result
*/
self.path.trace(map, start_dist, dist_ahead)
}
}

2
sim/src/spawn.rs
View File

@ -153,7 +153,7 @@ impl Spawner {
}
Command::Walk(_, trip, ped, spot1, spot2) => {
trips.agent_starting_trip_leg(AgentID::Pedestrian(ped), trip);
walking_sim.seed_pedestrian(events, ped, trip, spot1, spot2, map, path);
walking_sim.seed_pedestrian(events, ped, trip, spot1, spot2, path);
spawned_agents += 1;
}
};

77
sim/src/walking.rs
View File

@ -462,7 +462,6 @@ impl WalkingSimState {
trip: TripID,
start: SidewalkSpot,
goal: SidewalkSpot,
map: &Map,
path: Path,
) {
let start_lane = start.sidewalk;
@ -533,80 +532,8 @@ impl WalkingSimState {
}
pub fn trace_route(&self, id: PedestrianID, map: &Map, dist_ahead: Distance) -> Option<Trace> {
panic!("TODO");
/*let p = self.peds.get(&id)?;
let (mut result, mut dist_left) = {
if p.path.is_empty() && p.on.maybe_lane().is_some() {
// More edge cases. :(
if p.contraflow {
p.on.slice(false, map, p.goal.dist_along, p.dist_along)
} else {
p.on.slice(false, map, p.dist_along, p.goal.dist_along)
}
} else {
p.on.slice(p.contraflow, map, p.dist_along, p.dist_along + dist_ahead)
}
};
let mut last_lane = p.on.maybe_lane();
let mut idx = 0;
while dist_left > 0.0 * si::M && idx < p.path.len() {
let next_lane = p.path[idx];
if let Some(prev) = last_lane {
let turn = pick_turn(prev, next_lane, map);
// Never contraflow on turns
let (piece, new_dist_left) =
Traversable::Turn(turn).slice(false, map, 0.0 * si::M, dist_left);
result = result.extend(piece);
dist_left = new_dist_left;
if dist_left <= 0.0 * si::M {
break;
}
}
let (contraflow, last_step) = if idx + 1 < p.path.len() {
(is_contraflow(map, next_lane, p.path[idx + 1]), false)
} else {
dist_left = p.goal.dist_along;
// Value of contraflow doesn't matter
(false, true)
};
// TODO ooh this is _really_ cheating. ;) but sometimes we don't cross a lane either
// direction. urgh.
let l = map.get_l(next_lane);
let (pt1, pt2) = (l.first_pt(), l.last_pt());
let last_pt = result.endpoints().1;
if last_pt == pt1 {
if contraflow {
// Already done!
} else {
let (piece, new_dist_left) =
Traversable::Lane(next_lane).slice(false, map, 0.0 * si::M, dist_left);
result = result.extend(piece);
dist_left = new_dist_left;
}
} else if last_pt == pt2 {
if contraflow || last_step {
let (piece, new_dist_left) =
Traversable::Lane(next_lane).slice(true, map, l.length(), dist_left);
result = result.extend(piece);
dist_left = new_dist_left;
} else {
// Already done!
}
} else {
panic!("trace_route for ped doesn't match up");
}
last_lane = Some(next_lane);
idx += 1;
}
// Excess dist_left is just ignored
Some(result)
*/
let p = self.peds.get(&id)?;
Some(p.path.trace(map, p.dist_along, dist_ahead))
}
pub fn get_peds_waiting_at_stop(&self, stop: BusStopID) -> Vec<PedestrianID> {