a-b-street/abstreet
1
1
Fork 0
mirror of https://github.com/a-b-street/abstreet.git synced 2024-12-24 23:15:24 +03:00
Code Issues Projects Releases Wiki Activity

making a line struct

Browse Source
This commit is contained in:
Dustin Carlino 2018-06-28 15:25:54 -07:00
parent 4bdf70365e
commit 7f09f22bcf
17 changed files with 229 additions and 211 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
convert_osm/src/main.rs
View File

@ -12,7 +12,7 @@ mod osm;
mod srtm;
mod traffic_signals;
use map_model::{raw_data, Pt2D};
use map_model::raw_data;
use ordered_float::NotNaN;
use srtm::Elevation;
use structopt::StructOpt;
@ -73,7 +73,9 @@ fn main() {
for pt in &s.intersections {
if bounds.contains(pt.x(), pt.y()) {
let distance = |i: &raw_data::Intersection| {
pt.gps_dist_meters(&Pt2D::new(i.point.longitude, i.point.latitude))
// TODO weird to use Pt2D at all for GPS, uh oh
raw_data::LonLat::new(pt.x(), pt.y())
.gps_dist_meters(raw_data::LonLat::new(i.point.longitude, i.point.latitude))
};
// TODO use a quadtree or some better way to match signals to the closest

9
docs/lanes.md
View File

@ -121,11 +121,14 @@ wait slow down even more -- before any of this change, lanes on adjacent roads s
- useful to precompute sidewalk paths
- waiting on https://github.com/paholg/dimensioned/issues/31 to release
- cant easily serialize ordered float, so move away from it first?
- small geometry refactorings (like shifting polyline on opposite side, reversing pts)
- MORE CLEANUP: do we really need to hash pt2d's often? Should maybe settle and use Vec2d more
- MORE CLEANUP: Line type that's just a pair of pt2d's with length and fmt display
- polyline too; search for Vec<Pt2D>
- isolate vec2d
- turn's slope() and vecmath::normalized
- split out geometry crate again
- let ezgui stuff depend on it too
- also a polygon struct?

2
editor/src/plugins/warp.rs
View File

@ -59,7 +59,7 @@ fn warp(line: String, map: &Map, canvas: &mut Canvas, window_size: &Size) {
let id = RoadID(idx);
println!("Warping to {}", id);
let pt = map.get_r(id).first_pt();
canvas.center_on_map_pt(pt[0], pt[1], window_size);
canvas.center_on_map_pt(pt.x(), pt.y(), window_size);
}
Err(_) => {
println!("{} isn't a valid ID", line);

3
editor/src/render/building.rs
View File

@ -24,7 +24,8 @@ impl DrawBuilding {
id: bldg.id,
// TODO ideally start the path on a side of the building
front_path: bldg.front_path
.map(|pair| [pair.0.x(), pair.0.y(), pair.1.x(), pair.1.y()]),
.as_ref()
.map(|l| [l.pt1().x(), l.pt1().y(), l.pt2().x(), l.pt2().y()]),
polygon: pts,
}
}

10
editor/src/render/intersection.rs
View File

@ -6,7 +6,7 @@ use graphics;
use graphics::math::Vec2d;
use graphics::types::Color;
use map_model;
use map_model::geometry;
use map_model::{geometry, Pt2D};
use render::DrawRoad;
use std::f64;
@ -35,11 +35,9 @@ impl DrawIntersection {
let center = inter.point;
// Sort points by angle from the center
pts.sort_by_key(|pt| {
let mut angle = (pt[1] - center.y()).atan2(pt[0] - center.x()).to_degrees();
if angle < 0.0 {
angle += 360.0;
}
angle as i64
center
.angle_to(Pt2D::new(pt[0], pt[1]))
.normalized_degrees() as i64
});
let first_pt = pts[0].clone();
pts.push(first_pt);

12
editor/src/render/map.rs
View File

@ -36,17 +36,7 @@ impl DrawMap {
for r in map.all_roads() {
let mut turns = map.get_turns_from_road(r.id);
// Sort the turn icons by angle.
turns.sort_by_key(|t| {
let src_pt = map.get_r(t.src).last_pt();
let dst_pt = map.get_r(t.dst).first_pt();
let mut angle = (dst_pt[1] - src_pt[1])
.atan2(dst_pt[0] - src_pt[0])
.to_degrees();
if angle < 0.0 {
angle += 360.0;
}
angle as i64
});
turns.sort_by_key(|t| t.line.angle().normalized_degrees() as i64);
for (idx, t) in turns.iter().enumerate() {
turn_to_road_offset.insert(t.id, idx);

17
editor/src/render/road.rs
View File

@ -9,7 +9,7 @@ use graphics::math::Vec2d;
use graphics::types::Color;
use map_model;
use map_model::geometry;
use map_model::{Pt2D, RoadID};
use map_model::{Line, Pt2D, RoadID};
use render::PARCEL_BOUNDARY_THICKNESS;
#[derive(Debug)]
@ -29,11 +29,9 @@ pub struct DrawRoad {
impl DrawRoad {
pub fn new(road: &map_model::Road) -> DrawRoad {
let (first1, first2) = road.first_line();
let (start_1, start_2) = perp_line(first1, first2, geometry::LANE_THICKNESS);
let (start_1, start_2) = perp_line(road.first_line(), geometry::LANE_THICKNESS);
let (last1, last2) = road.last_line();
let (end_1, end_2) = perp_line(last2, last1, geometry::LANE_THICKNESS);
let (end_1, end_2) = perp_line(road.last_line().reverse(), geometry::LANE_THICKNESS);
let polygons =
map_model::polygons_for_polyline(&road.lane_center_pts, geometry::LANE_THICKNESS);
@ -173,9 +171,10 @@ impl DrawRoad {
}
// TODO this always does it at pt1
fn perp_line(orig1: Pt2D, orig2: Pt2D, length: f64) -> (Vec2d, Vec2d) {
let (pt1, _) = map_model::shift_line(length / 2.0, orig1, orig2);
let (_, pt2) = map_model::shift_line(length / 2.0, orig2, orig1);
// TODO move to Line or reimplement differently
fn perp_line(l: Line, length: f64) -> (Vec2d, Vec2d) {
let pt1 = l.shift(length / 2.0).pt1();
let pt2 = l.reverse().shift(length / 2.0).pt2();
(pt1.to_vec(), pt2.to_vec())
}
@ -191,7 +190,7 @@ fn calculate_sidewalk_lines(road: &map_model::Road) -> Vec<(Vec2d, Vec2d)> {
let (pt, angle) = road.dist_along(dist_along);
// Reuse perp_line. Project away an arbitrary amount
let pt2 = pt.project_away(1.0, angle);
result.push(perp_line(pt, pt2, geometry::LANE_THICKNESS));
result.push(perp_line(Line::new(pt, pt2), geometry::LANE_THICKNESS));
dist_along += tile_every;
}

31
editor/src/render/turn.rs
View File

@ -2,6 +2,7 @@
use aabb_quadtree::geom::Rect;
use colors::{ColorScheme, Colors};
use dimensioned::si;
use ezgui::GfxCtx;
use graphics;
use graphics::math::Vec2d;
@ -28,32 +29,34 @@ impl DrawTurn {
let offset_along_road = offset_along_road as f64;
let src_pt = map.get_r(turn.src).last_pt();
let dst_pt = map.get_r(turn.dst).first_pt();
let slope = vecmath::vec2_normalized([dst_pt[0] - src_pt[0], dst_pt[1] - src_pt[1]]);
let slope = vecmath::vec2_normalized([dst_pt.x() - src_pt.x(), dst_pt.y() - src_pt.y()]);
let last_line = map.get_r(turn.src).last_line();
let icon_center = geometry::dist_along_line(
// Start the distance from the intersection
(&last_line.1, &last_line.0),
(offset_along_road + 0.5) * TURN_ICON_ARROW_LENGTH,
);
// Start the distance from the intersection
let icon_center = last_line
.reverse()
.unbounded_dist_along((offset_along_road + 0.5) * TURN_ICON_ARROW_LENGTH * si::M);
let icon_src = [
icon_center[0] - (TURN_ICON_ARROW_LENGTH / 2.0) * slope[0],
icon_center[1] - (TURN_ICON_ARROW_LENGTH / 2.0) * slope[1],
icon_center.x() - (TURN_ICON_ARROW_LENGTH / 2.0) * slope[0],
icon_center.y() - (TURN_ICON_ARROW_LENGTH / 2.0) * slope[1],
];
let icon_dst = [
icon_center[0] + (TURN_ICON_ARROW_LENGTH / 2.0) * slope[0],
icon_center[1] + (TURN_ICON_ARROW_LENGTH / 2.0) * slope[1],
icon_center.x() + (TURN_ICON_ARROW_LENGTH / 2.0) * slope[0],
icon_center.y() + (TURN_ICON_ARROW_LENGTH / 2.0) * slope[1],
];
let icon_circle =
geometry::circle(icon_center[0], icon_center[1], TURN_ICON_ARROW_LENGTH / 2.0);
let icon_circle = geometry::circle(
icon_center.x(),
icon_center.y(),
TURN_ICON_ARROW_LENGTH / 2.0,
);
let icon_arrow = [icon_src[0], icon_src[1], icon_dst[0], icon_dst[1]];
DrawTurn {
id: turn.id,
src_pt,
dst_pt,
src_pt: src_pt.to_vec(),
dst_pt: dst_pt.to_vec(),
icon_circle,
icon_arrow,
}

3
map_model/src/building.rs
View File

@ -1,5 +1,6 @@
// Copyright 2018 Google LLC, licensed under http://www.apache.org/licenses/LICENSE-2.0
use Line;
use Pt2D;
use std::collections::HashMap;
@ -14,7 +15,7 @@ pub struct Building {
pub osm_tags: HashMap<String, String>,
pub osm_way_id: i64,
pub front_path: Option<(Pt2D, Pt2D)>,
pub front_path: Option<Line>,
}
impl PartialEq for Building {

72
map_model/src/geometry.rs
View File

@ -2,13 +2,13 @@
use Angle;
use Bounds;
use Line;
use Pt2D;
use aabb_quadtree::geom::{Point, Rect};
use dimensioned::si;
use graphics::math::Vec2d;
use polyline;
use std::f64;
use vecmath;
pub const LANE_THICKNESS: f64 = 2.5;
pub const BIG_ARROW_THICKNESS: f64 = 0.5;
@ -57,68 +57,6 @@ pub fn point_in_circle(x: f64, y: f64, center: Vec2d, radius: f64) -> bool {
(x - center[0]).powi(2) + (y - center[1]).powi(2) < radius.powi(2)
}
/*pub fn interpolate_along_line((pt1, pt2): (&Pt2D, &Pt2D), factor_along: f64) -> Vec2d {
assert!(factor_along >= 0.0 && factor_along <= 1.0);
let x = pt1.x + factor_along * (pt2.x - pt1.x);
let y = pt1.y + factor_along * (pt2.y - pt1.y);
return [x, y];
}*/
// TODO borrow or copy?
// TODO valid to do euclidean distance on screen-space points that're formed from
// Haversine?
pub(crate) fn euclid_dist((pt1, pt2): (Pt2D, Pt2D)) -> si::Meter<f64> {
return ((pt1.x() - pt2.x()).powi(2) + (pt1.y() - pt2.y()).powi(2)).sqrt() * si::M;
}
pub fn line_segments_intersect((pt1, pt2): (&Vec2d, &Vec2d), (pt3, pt4): (&Vec2d, &Vec2d)) -> bool {
// From http://bryceboe.com/2006/10/23/line-segment-intersection-algorithm/
is_counter_clockwise(pt1, pt3, pt4) != is_counter_clockwise(pt2, pt3, pt4)
&& is_counter_clockwise(pt1, pt2, pt3) != is_counter_clockwise(pt1, pt2, pt4)
}
fn is_counter_clockwise(pt1: &Vec2d, pt2: &Vec2d, pt3: &Vec2d) -> bool {
(pt3[1] - pt1[1]) * (pt2[0] - pt1[0]) > (pt2[1] - pt1[1]) * (pt3[0] - pt1[0])
}
pub fn line_segment_intersection(l1: (Pt2D, Pt2D), l2: (Pt2D, Pt2D)) -> Option<Pt2D> {
// TODO shoddy way of implementing this
// TODO doesn't handle nearly parallel lines
if !line_segments_intersect(
(&l1.0.to_vec(), &l1.1.to_vec()),
(&l2.0.to_vec(), &l2.1.to_vec()),
) {
return None;
}
polyline::line_intersection(l1, l2)
}
pub fn dist_along_line((pt1, pt2): (&Pt2D, &Pt2D), dist_along: f64) -> Vec2d {
//assert!(euclid_dist(&pt1, &pt2) <= dist_along);
let vec = vecmath::vec2_normalized([pt2.x() - pt1.x(), pt2.y() - pt1.y()]);
[pt1.x() + dist_along * vec[0], pt1.y() + dist_along * vec[1]]
}
// TODO rm the other one
pub fn safe_dist_along_line((pt1, pt2): (&Pt2D, &Pt2D), dist_along: si::Meter<f64>) -> Vec2d {
let len = euclid_dist((*pt1, *pt2));
if dist_along > len + EPSILON_METERS {
panic!("cant do {} along a line of length {}", dist_along, len);
}
let percent = (dist_along / len).value_unsafe;
[
pt1.x() + percent * (pt2.x() - pt1.x()),
pt1.y() + percent * (pt2.y() - pt1.y()),
]
// TODO unit test
/*
let res_len = euclid_dist((pt1, &Pt2D::new(res[0], res[1])));
if res_len != dist_along {
println!("whats the delta btwn {} and {}?", res_len, dist_along);
}
*/}
pub fn get_bbox_for_polygons(polygons: &[Vec<Vec2d>]) -> Rect {
let mut b = Bounds::new();
for poly in polygons {
@ -163,15 +101,15 @@ pub fn circle_to_bbox(c: &[f64; 4]) -> Rect {
pub fn dist_along(pts: &Vec<Pt2D>, dist_along: si::Meter<f64>) -> (Pt2D, Angle) {
let mut dist_left = dist_along;
for (idx, pair) in pts.windows(2).enumerate() {
let length = euclid_dist((pair[0], pair[1]));
let l = Line(pair[0], pair[1]);
let length = l.length();
let epsilon = if idx == pts.len() - 2 {
EPSILON_METERS
} else {
0.0 * si::M
};
if dist_left <= length + epsilon {
let vec = safe_dist_along_line((&pair[0], &pair[1]), dist_left);
return (Pt2D::new(vec[0], vec[1]), pair[0].angle_to(pair[1]));
return (l.dist_along(dist_left), l.angle());
}
dist_left -= length;
}
@ -180,6 +118,6 @@ pub fn dist_along(pts: &Vec<Pt2D>, dist_along: si::Meter<f64>) -> (Pt2D, Angle)
pub fn polyline_len(pts: &Vec<Pt2D>) -> si::Meter<f64> {
pts.windows(2).fold(0.0 * si::M, |so_far, pair| {
so_far + euclid_dist((pair[0], pair[1]))
so_far + Line(pair[0], pair[1]).length()
})
}

114
map_model/src/lib.rs
View File

@ -24,12 +24,13 @@ mod road;
mod turn;
pub use building::{Building, BuildingID};
use dimensioned::si;
use graphics::math::Vec2d;
pub use intersection::{Intersection, IntersectionID};
pub use map::Map;
use ordered_float::NotNaN;
pub use parcel::{Parcel, ParcelID};
pub use polyline::{polygons_for_polyline, shift_line, shift_polyline};
pub use polyline::{polygons_for_polyline, shift_polyline};
use raw_data::LonLat;
pub use road::{LaneType, Road, RoadID};
use std::f64;
@ -198,14 +199,115 @@ impl Angle {
self.0
}
}
pub fn normalized_degrees(&self) -> f64 {
self.normalized_radians().to_degrees()
}
}
impl fmt::Display for Angle {
fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
write!(
f,
"Angle({} degrees)",
self.normalized_radians().to_degrees()
)
write!(f, "Angle({} degrees)", self.normalized_degrees())
}
}
// Segment, technically
#[derive(Debug)]
pub struct Line(Pt2D, Pt2D);
impl Line {
// TODO only one place outside this crate calls this, try to fix maybe?
pub fn new(pt1: Pt2D, pt2: Pt2D) -> Line {
Line(pt1, pt2)
}
// TODO we call these frequently here; unnecessary copies?
pub fn pt1(&self) -> Pt2D {
self.0
}
pub fn pt2(&self) -> Pt2D {
self.1
}
// TODO valid to do euclidean distance on world-space points that're formed from
// Haversine?
pub fn length(&self) -> si::Meter<f64> {
((self.pt1().x() - self.pt2().x()).powi(2) + (self.pt1().y() - self.pt2().y()).powi(2))
.sqrt() * si::M
}
pub fn intersection(&self, other: &Line) -> Option<Pt2D> {
// TODO shoddy way of implementing this
// TODO doesn't handle nearly parallel lines
if !self.intersects(other) {
None
} else {
polyline::line_intersection(self, other)
}
}
pub fn shift(&self, width: f64) -> Line {
let angle = self.pt1().angle_to(self.pt2()).rotate_degs(90.0);
Line(
self.pt1().project_away(width, angle),
self.pt2().project_away(width, angle),
)
}
pub fn reverse(&self) -> Line {
Line(self.pt2(), self.pt1())
}
pub fn intersects(&self, other: &Line) -> bool {
// From http://bryceboe.com/2006/10/23/line-segment-intersection-algorithm/
is_counter_clockwise(self.pt1(), other.pt1(), other.pt2())
!= is_counter_clockwise(self.pt2(), other.pt1(), other.pt2())
&& is_counter_clockwise(self.pt1(), self.pt2(), other.pt1())
!= is_counter_clockwise(self.pt1(), self.pt2(), other.pt2())
}
pub fn angle(&self) -> Angle {
self.pt1().angle_to(self.pt2())
}
pub fn dist_along(&self, dist: si::Meter<f64>) -> Pt2D {
let len = self.length();
if dist > len + geometry::EPSILON_METERS {
panic!("cant do {} along a line of length {}", dist, len);
}
let percent = (dist / len).value_unsafe;
Pt2D::new(
self.pt1().x() + percent * (self.pt2().x() - self.pt1().x()),
self.pt1().y() + percent * (self.pt2().y() - self.pt1().y()),
)
// TODO unit test
/*
let res_len = euclid_dist((pt1, &Pt2D::new(res[0], res[1])));
if res_len != dist_along {
println!("whats the delta btwn {} and {}?", res_len, dist_along);
}
*/ }
pub fn unbounded_dist_along(&self, dist: si::Meter<f64>) -> Pt2D {
let len = self.length();
let percent = (dist / len).value_unsafe;
Pt2D::new(
self.pt1().x() + percent * (self.pt2().x() - self.pt1().x()),
self.pt1().y() + percent * (self.pt2().y() - self.pt1().y()),
)
// TODO unit test
/*
let res_len = euclid_dist((pt1, &Pt2D::new(res[0], res[1])));
if res_len != dist_along {
println!("whats the delta btwn {} and {}?", res_len, dist_along);
}
*/ }
}
fn is_counter_clockwise(pt1: Pt2D, pt2: Pt2D, pt3: Pt2D) -> bool {
(pt3.y() - pt1.y()) * (pt2.x() - pt1.x()) > (pt2.y() - pt1.y()) * (pt3.x() - pt1.x())
}
pub struct PolyLine(Vec<Pt2D>);

5
map_model/src/make/buildings.rs
View File

@ -2,6 +2,7 @@ use Bounds;
use Building;
use BuildingID;
use LaneType;
use Line;
use Pt2D;
use Road;
use RoadID;
@ -36,7 +37,7 @@ fn find_front_path(
bldg_points: &Vec<Pt2D>,
bldg_osm_tags: &HashMap<String, String>,
roads: &Vec<Road>,
) -> Option<(Pt2D, Pt2D)> {
) -> Option<Line> {
use geo::prelude::{ClosestPoint, EuclideanDistance};
if let Some(street_name) = bldg_osm_tags.get("addr:street") {
@ -65,7 +66,7 @@ fn find_front_path(
.iter()
.min_by_key(|pair| NotNaN::new(pair.1.euclidean_distance(&center_pt)).unwrap())
{
return Some((bldg_center, Pt2D::new(closest.1.x(), closest.1.y())));
return Some(Line(bldg_center, Pt2D::new(closest.1.x(), closest.1.y())));
}
}
None

39
map_model/src/make/trim_lines.rs
View File

@ -1,30 +1,25 @@
use Pt2D;
use Line;
use dimensioned::si;
use geometry;
use intersection::Intersection;
use road::{Road, RoadID};
use std::collections::HashMap;
use std::collections::hash_map::Entry;
pub(crate) fn trim_lines(roads: &mut Vec<Road>, i: &Intersection) {
let mut shortest_first_line: HashMap<RoadID, (Pt2D, Pt2D, si::Meter<f64>)> = HashMap::new();
let mut shortest_last_line: HashMap<RoadID, (Pt2D, Pt2D, si::Meter<f64>)> = HashMap::new();
let mut shortest_first_line: HashMap<RoadID, (Line, si::Meter<f64>)> = HashMap::new();
let mut shortest_last_line: HashMap<RoadID, (Line, si::Meter<f64>)> = HashMap::new();
fn update_shortest(
m: &mut HashMap<RoadID, (Pt2D, Pt2D, si::Meter<f64>)>,
r: RoadID,
l: (Pt2D, Pt2D),
) {
let new_len = geometry::euclid_dist(l);
fn update_shortest(m: &mut HashMap<RoadID, (Line, si::Meter<f64>)>, r: RoadID, l: Line) {
let new_len = l.length();
match m.entry(r) {
Entry::Occupied(mut o) => {
if new_len < o.get().2 {
o.insert((l.0, l.1, new_len));
if new_len < o.get().1 {
o.insert((l, new_len));
}
}
Entry::Vacant(v) => {
v.insert((l.0, l.1, new_len));
v.insert((l, new_len));
}
}
}
@ -34,21 +29,21 @@ pub(crate) fn trim_lines(roads: &mut Vec<Road>, i: &Intersection) {
for outgoing in &i.outgoing_roads {
let l1 = roads[incoming.0].last_line();
let l2 = roads[outgoing.0].first_line();
if let Some(hit) = geometry::line_segment_intersection(l1, l2) {
update_shortest(&mut shortest_last_line, *incoming, (l1.0, hit));
update_shortest(&mut shortest_first_line, *outgoing, (hit, l2.1));
if let Some(hit) = l1.intersection(&l2) {
update_shortest(&mut shortest_last_line, *incoming, Line(l1.pt1(), hit));
update_shortest(&mut shortest_first_line, *outgoing, Line(hit, l2.pt2()));
}
}
}
// Apply the updates
for (id, triple) in &shortest_first_line {
roads[id.0].lane_center_pts[0] = triple.0;
roads[id.0].lane_center_pts[1] = triple.1;
for (id, pair) in &shortest_first_line {
roads[id.0].lane_center_pts[0] = pair.0.pt1();
roads[id.0].lane_center_pts[1] = pair.0.pt2();
}
for (id, triple) in &shortest_last_line {
for (id, pair) in &shortest_last_line {
let len = roads[id.0].lane_center_pts.len();
roads[id.0].lane_center_pts[len - 2] = triple.0;
roads[id.0].lane_center_pts[len - 1] = triple.1;
roads[id.0].lane_center_pts[len - 2] = pair.0.pt1();
roads[id.0].lane_center_pts[len - 1] = pair.0.pt2();
}
}

4
map_model/src/make/turns.rs
View File

@ -1,3 +1,4 @@
use Line;
use Map;
use intersection::Intersection;
use road::{LaneType, RoadID};
@ -43,8 +44,7 @@ pub(crate) fn make_turns(i: &Intersection, m: &Map, turn_id_start: usize) -> Vec
parent: i.id,
src: *src,
dst: *dst,
src_pt: src_r.last_pt(),
dst_pt: dst_r.first_pt(),
line: Line(src_r.last_pt(), dst_r.first_pt()),
});
}
}

66
map_model/src/polyline.rs
View File

@ -1,3 +1,4 @@
use Line;
use Pt2D;
use graphics::math::Vec2d;
use std::f64;
@ -49,8 +50,8 @@ pub fn polygons_for_polyline(center_pts: &Vec<Pt2D>, width: f64) -> Vec<Vec<Vec2
pub fn shift_polyline(width: f64, pts: &Vec<Pt2D>) -> Vec<Pt2D> {
assert!(pts.len() >= 2);
if pts.len() == 2 {
let (pt1_shift, pt2_shift) = shift_line(width, pts[0], pts[1]);
return vec![pt1_shift, pt2_shift];
let l = Line(pts[0], pts[1]).shift(width);
return vec![l.pt1(), l.pt2()];
}
let mut result: Vec<Pt2D> = Vec::new();
@ -62,19 +63,18 @@ pub fn shift_polyline(width: f64, pts: &Vec<Pt2D>) -> Vec<Pt2D> {
loop {
let pt3_raw = pts[pt3_idx];
let (pt1_shift, pt2_shift_1st) = shift_line(width, pt1_raw, pt2_raw);
let (pt2_shift_2nd, pt3_shift) = shift_line(width, pt2_raw, pt3_raw);
let l1 = Line(pt1_raw, pt2_raw).shift(width);
let l2 = Line(pt2_raw, pt3_raw).shift(width);
// When the lines are perfectly parallel, it means pt2_shift_1st == pt2_shift_2nd and the
// original geometry is redundant.
let pt2_shift = line_intersection((pt1_shift, pt2_shift_1st), (pt2_shift_2nd, pt3_shift))
.unwrap_or(pt2_shift_1st);
let pt2_shift = line_intersection(&l1, &l2).unwrap_or(l1.pt2());
if pt3_idx == 2 {
result.push(pt1_shift);
result.push(l1.pt1());
}
result.push(pt2_shift);
if pt3_idx == pts.len() - 1 {
result.push(pt3_shift);
result.push(l2.pt2());
break;
}
@ -87,16 +87,15 @@ pub fn shift_polyline(width: f64, pts: &Vec<Pt2D>) -> Vec<Pt2D> {
// Check that the angles roughly match up between the original and shifted line
for (orig_pair, shifted_pair) in pts.windows(2).zip(result.windows(2)) {
let orig_angle = orig_pair[0].angle_to(orig_pair[1]).normalized_radians();
let orig_angle = orig_pair[0].angle_to(orig_pair[1]).normalized_degrees();
let shifted_angle = shifted_pair[0]
.angle_to(shifted_pair[1])
.normalized_radians();
.normalized_degrees();
let delta = (shifted_angle - orig_angle).abs();
if delta > 0.00001 {
println!(
"Points changed angles from {} to {}",
orig_angle.to_degrees(),
shifted_angle.to_degrees()
orig_angle, shifted_angle
);
}
}
@ -104,26 +103,18 @@ pub fn shift_polyline(width: f64, pts: &Vec<Pt2D>) -> Vec<Pt2D> {
result
}
pub fn shift_line(width: f64, pt1: Pt2D, pt2: Pt2D) -> (Pt2D, Pt2D) {
let angle = pt1.angle_to(pt2).rotate_degs(90.0);
(
pt1.project_away(width, angle),
pt2.project_away(width, angle),
)
}
// NOT segment. Fails for parallel lines.
// https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection#Given_two_points_on_each_line
pub(crate) fn line_intersection(l1: (Pt2D, Pt2D), l2: (Pt2D, Pt2D)) -> Option<Pt2D> {
let x1 = l1.0.x();
let y1 = l1.0.y();
let x2 = l1.1.x();
let y2 = l1.1.y();
pub(crate) fn line_intersection(l1: &Line, l2: &Line) -> Option<Pt2D> {
let x1 = l1.pt1().x();
let y1 = l1.pt1().y();
let x2 = l1.pt2().x();
let y2 = l1.pt2().y();
let x3 = l2.0.x();
let y3 = l2.0.y();
let x4 = l2.1.x();
let y4 = l2.1.y();
let x3 = l2.pt1().x();
let y3 = l2.pt1().y();
let x4 = l2.pt2().x();
let y4 = l2.pt2().y();
let numer_x = (x1 * y2 - y1 * x2) * (x3 - x4) - (x1 - x2) * (x3 * y4 - y3 * x4);
let numer_y = (x1 * y2 - y1 * x2) * (y3 - y4) - (y1 - y2) * (x3 * y4 - y3 * x4);
@ -147,14 +138,14 @@ fn shift_polyline_equivalence() {
let pt5 = Pt2D::new(rand::random::<f64>() * scale, rand::random::<f64>() * scale);
let width = 50.0;
let (pt1_s, _) = shift_line(width, pt1, pt2);
let pt1_s = Line(pt1, pt2).shift(width).pt1();
let pt2_s =
line_intersection(shift_line(width, pt1, pt2), shift_line(width, pt2, pt3)).unwrap();
line_intersection(&Line(pt1, pt2).shift(width), &Line(pt2, pt3).shift(width)).unwrap();
let pt3_s =
line_intersection(shift_line(width, pt2, pt3), shift_line(width, pt3, pt4)).unwrap();
line_intersection(&Line(pt2, pt3).shift(width), &Line(pt3, pt4).shift(width)).unwrap();
let pt4_s =
line_intersection(shift_line(width, pt3, pt4), shift_line(width, pt4, pt5)).unwrap();
let (_, pt5_s) = shift_line(width, pt4, pt5);
line_intersection(&Line(pt3, pt4).shift(width), &Line(pt4, pt5).shift(width)).unwrap();
let pt5_s = Line(pt4, pt5).shift(width).pt2();
assert_eq!(
shift_polyline(width, &vec![pt1, pt2, pt3, pt4, pt5]),
@ -171,7 +162,10 @@ fn shift_short_polyline_equivalence() {
let pt2 = Pt2D::new(rand::random::<f64>() * scale, rand::random::<f64>() * scale);
let width = 50.0;
let (pt1_s, pt2_s) = shift_line(width, pt1, pt2);
let l = Line(pt1, pt2).shift(width);
assert_eq!(shift_polyline(width, &vec![pt1, pt2]), vec![pt1_s, pt2_s]);
assert_eq!(
shift_polyline(width, &vec![pt1, pt2]),
vec![l.pt1(), l.pt2()]
);
}

18
map_model/src/road.rs
View File

@ -2,10 +2,10 @@
use Angle;
use IntersectionID;
use Line;
use Pt2D;
use dimensioned::si;
use geometry;
use graphics::math::Vec2d;
use std::collections::HashMap;
use std::f64;
use std::fmt;
@ -64,17 +64,17 @@ impl PartialEq for Road {
}
impl Road {
pub fn first_pt(&self) -> Vec2d {
self.lane_center_pts[0].to_vec()
pub fn first_pt(&self) -> Pt2D {
self.lane_center_pts[0]
}
pub fn last_pt(&self) -> Vec2d {
self.lane_center_pts.last().unwrap().to_vec()
pub fn last_pt(&self) -> Pt2D {
*self.lane_center_pts.last().unwrap()
}
pub fn first_line(&self) -> (Pt2D, Pt2D) {
(self.lane_center_pts[0], self.lane_center_pts[1])
pub fn first_line(&self) -> Line {
Line(self.lane_center_pts[0], self.lane_center_pts[1])
}
pub fn last_line(&self) -> (Pt2D, Pt2D) {
(
pub fn last_line(&self) -> Line {
Line(
self.lane_center_pts[self.lane_center_pts.len() - 2],
self.lane_center_pts[self.lane_center_pts.len() - 1],
)

29
map_model/src/turn.rs
View File

@ -2,11 +2,10 @@
use Angle;
use IntersectionID;
use Line;
use Pt2D;
use RoadID;
use dimensioned::si;
use geometry;
use graphics::math::Vec2d;
use std::f64;
use vecmath;
@ -22,8 +21,7 @@ pub struct Turn {
pub dst: RoadID,
/// GeomTurn stuff
pub(crate) src_pt: Vec2d,
pub dst_pt: Vec2d,
pub line: Line,
}
impl PartialEq for Turn {
@ -34,36 +32,29 @@ impl PartialEq for Turn {
impl Turn {
pub fn conflicts_with(&self, other: &Turn) -> bool {
if self.src_pt == other.src_pt {
if self.line.pt1() == other.line.pt1() {
return false;
}
if self.dst_pt == other.dst_pt {
if self.line.pt2() == other.line.pt2() {
return true;
}
geometry::line_segments_intersect(
(&self.src_pt, &self.dst_pt),
(&other.src_pt, &other.dst_pt),
)
self.line.intersects(&other.line)
}
// TODO share impl with GeomRoad
pub fn dist_along(&self, dist_along: si::Meter<f64>) -> (Pt2D, Angle) {
let src = Pt2D::new(self.src_pt[0], self.src_pt[1]);
let dst = Pt2D::new(self.dst_pt[0], self.dst_pt[1]);
let vec = geometry::safe_dist_along_line((&src, &dst), dist_along);
(Pt2D::new(vec[0], vec[1]), src.angle_to(dst))
(self.line.dist_along(dist_along), self.line.angle())
}
pub fn length(&self) -> si::Meter<f64> {
let src = Pt2D::new(self.src_pt[0], self.src_pt[1]);
let dst = Pt2D::new(self.dst_pt[0], self.dst_pt[1]);
geometry::euclid_dist((src, dst))
self.line.length()
}
// TODO rethink this one
pub fn slope(&self) -> [f64; 2] {
vecmath::vec2_normalized([
self.dst_pt[0] - self.src_pt[0],
self.dst_pt[1] - self.src_pt[1],
self.line.pt2().x() - self.line.pt1().x(),
self.line.pt2().y() - self.line.pt1().y(),
])
}
}