def collide(self, other_ball):
# Present and future positions of both balls
p1 = (self.coord[0], self.coord[2])
q1 = (self.coord[0] + COF*self.vel[0], self.coord[2] + COF*self.vel[2])
p2 = (other_ball.coord[0], other_ball.coord[2])
q2 = (other_ball.coord[0] + COF*other_ball.vel[0], other_ball.coord[2] + COF*other_ball.vel[2])
# Both balls are moving
if self.isMoving() and other_ball.isMoving():
segment1 = LineString([p1,q1])
segment2 = LineString([p2,q2])
return segment1.distance(segment2) <= self.radius+other_ball.radius
# Only this ball is moving
elif self.isMoving():
segment1 = LineString([p1,q1])
return segment1.distance(Point(p2)) <= self.radius+other_ball.radius
#Only the other ball is moving
elif other_ball.isMoving():
segment2 = LineString([p2,q2])
return segment2.distance(Point(p1)) <= self.radius+other_ball.radius
return False
def shortest_distance(line1, line2):
line_string_1 = LineString(line1)
line_string_2 = LineString(line2)
return np.amin([line_string_2.distance(Point(p)) for p in line1] +
[line_string_1.distance(Point(p)) for p in line2])
def calculate_distance_prerelease(self, point):
# same situation here, peep the comment on draw_wall
if len(self.points) == 1:
x = self.points[0][0]
y = self.points[0][1]
wall_point = Point(x, y)
input_point = Point(point[0], point[1])
return wall_point.distance(input_point)
elif len(self.points) == 2:
wall_line = LineString(self.points)
input_point = Point(point[0], point[1])
return wall_line.distance(input_point)
else:
x = point[0]
y = point[1]
min_distance = float('inf')
input_point = Point(point[0], point[1])
for point1 in self.points:
for point2 in self.points:
if point1 is point2:
continue
wall_line = LineString([point1, point2])
distance = wall_line.distance(input_point)
if distance < min_distance:
min_distance = distance
return min_distance
def simplify_linestring(init_ls: LineString,
admiss_region: Polygon) -> LineString:
'''
Finds the simplest linestring maintaining the endpoints
of the initial linestring, but only within the
admissable region
'''
EPS = 1e-6
ambient_space = admiss_region.envelope.buffer(0.00001)
invalid_region = ambient_space.difference(admiss_region.buffer(EPS))
init_pts = pd.Series([Point(c) for c in init_ls.coords])
if init_ls.intersects(invalid_region):
print("Invalid region should not intersect orig line")
return init_ls
cur_pt_idxs = [0, len(init_pts) - 1]
cur_linestring = LineString(list(init_pts[cur_pt_idxs]))
while cur_linestring.intersects(invalid_region):
# Get the pt that is the farthest from the cur_linestring
distances = [cur_linestring.distance(p) for p in init_pts]
farthest_idx = np.argmax(distances)
# Add it to our pt idx list and our linestring
# NOTE: we maintain the idx list so the linestring
# is in the right order
cur_pt_idxs.append(farthest_idx)
cur_pt_idxs.sort()
cur_linestring = LineString(list(init_pts[cur_pt_idxs]))
return cur_linestring
def set_edge_width(
self,
other_geoms: List[Polygon],
simplify: bool = True,
) -> None:
"""
For each Edge, sets a 'width' attribute, which is the minimum
distance from that Edge to a set of Polygons, provided in
other_geoms. Functionally, this captures the possible width of
the road segment built along the edge.
Args:
other_geoms: Edge width is calculated relative these geometries
simplify: if True, the width is set to be the min over continous
paths. So given subgraph w/ Nodes A-B-C-D, the width
is the min(AB,BC,CD). Wrt road segments, this is more
reflective of real life where road width is constant
between intersections
Returns:
Modifies graph in-place, no return value
"""
for e in self.es:
e_line = LineString(self.edge_to_coords(e))
distances = [e_line.distance(g) for g in other_geoms]
e['width'] = min(distances)
if simplify:
self._simplify_edge_width()
def checkObstaclev2(node, lineList, edgeDistance):
"""
Checks if the nodes path collides with obstacle. \n
Also checks how close to the obstacle the line \n
will go. If to close, it will return false as if \n
there is a collision.
:param node: Node to check collision for
:param lineList: List of lines for obstacles
:param edgeDistance: Distance to keep from the obstacles
:return: True if no collision, false if collision
"""
dx_list = [x for x in node.path_x]
dy_list = [y for y in node.path_y]
node_line = LineString([(x, y) for (x, y) in zip(dx_list, dy_list)])
for data in lineList:
x1 = data[0][0]
y1 = data[0][1]
x2 = data[0][2]
y2 = data[0][3]
obst = LineString([(x1, y1), (x2, y2)])
if obst.intersects(node_line):
return False
if obst.distance(node_line) < edgeDistance:
return False
return True
def _sanitize_edges(self, edges):
"""
Checks if there are points that lay on the edges.
"""
new_edges = set()
for edge in edges:
found_point_between = False
segment = LineString([(edge[0].x, edge[0].y),
(edge[1].x, edge[1].y)])
for point in self.points:
if point is not edge[0] and point is not edge[1]:
if segment.distance(ShapelyPoint(point.x, point.y)) < 1e-8:
if (edge[0], point) not in self.edges and (
point, edge[0]) not in self.edges:
new_edges.add((edge[0], point))
if (edge[1], point) not in self.edges and (
point, edge[1]) not in self.edges:
new_edges.add((point, edge[1]))
found_point_between = True
break
if not found_point_between:
new_edges.add(edge)
if set(edges) == new_edges:
return list(edges)
else:
return self._sanitize_edges(list(new_edges))
def process_path_obstacle(self, fpath):
if self.path_obstacle_processed:
return
path_x, path_y = fpath.get_xy()
self.obstacle_lat_dist = {}
path = []
self.mobileye.process_obstacles()
for i in range(len(path_x)):
path.append((path_x[i], path_y[i]))
line = LineString(path)
for obs_id, obstacle in self.mobileye.obstacles.items():
point = Point(obstacle.x, obstacle.y)
dist = line.distance(point)
if dist < self.LAT_DIST + obstacle.width + self.left_edge_to_center:
proj_len = line.project(point)
if proj_len == 0 or proj_len >= line.length:
continue
p1 = line.interpolate(proj_len)
if (proj_len + 1) > line.length:
p2 = line.interpolate(line.length)
else:
p2 = line.interpolate(proj_len + 1)
d = (point.x - p1.x) * (p2.y - p1.y) - (point.y -
p1.y) * (p2.x - p1.x)
if d > 0:
dist *= -1
self.obstacle_lat_dist[obstacle.obstacle_id] = dist
self.path_obstacle_processed = True
def getPathFromPrm(tspTour, pathDict, milestones, polygonList, minDist):
'''
'''
multiPolygonList = list()
for val in polygonList:
currPolygon = Polygon(val)
multiPolygonList.append(currPolygon)
obstacles = MultiPolygon(multiPolygonList)
overallPath = list()
for i in range(len(tspTour) - 1):
directEdge = LineString(
[milestones[tspTour[i]], milestones[tspTour[i + 1]]])
distance = directEdge.distance(obstacles)
if distance > minDist:
overallPath.append(milestones[tspTour[i]])
overallPath.append(milestones[tspTour[i + 1]])
else:
currEdge = (tspTour[i], tspTour[i + 1])
if currEdge[0] < currEdge[1]:
currPath = pathDict[currEdge].copy()
else:
currPath = pathDict[(currEdge[1], currEdge[0])].copy()
currPath.reverse()
overallPath += currPath[:-1]
overallPath.append(milestones[tspTour[-1]])
return overallPath
def point_projects_to_edges(self, point, distance_tolerance=0.01):
"""
Project a point to graph edges considering specific distance tolerance.
Note the tolerance is measured by great circle distance to point per se.
:param point: a shapely Point instance or (lon, lat) tuple
:param distance_tolerance: tolerance of distance in km
:return: a list of projected edges, reversely sorted by offsets.
"""
point_buffer = distance_to_buffer(distance_tolerance)
p_buf = Point(point).buffer(point_buffer)
projected_edges = []
projected_segments = []
major = self.major_component()
for i in range(0, len(major)):
line_index = major[i]
line = self.geoms[line_index]
if line.intersects(p_buf):
cuts = self.line_cuts(line_index)
if cuts is None:
continue
for j in range(1, len(cuts)):
sinx = cuts[j - 1]
einx = cuts[j]
segment = line.coords[sinx:einx + 1]
ls = LineString(segment)
if ls.intersects(p_buf):
edge = self.edge_key(segment[0], segment[-1])
offset = ls.distance(Point(point)) # no buffer
projected_edges.append((edge, offset))
projected_segments.append(segment)
result = sorted(projected_edges, key=lambda x: x[1], reverse=True)
edges = list(set([i[0] for i in result]))
return edges, projected_segments
def check_drag_type(self, off_x, off_y, hl_size, px, py):
if not self.visible:
return -1
offset_points = (self.points + np.array([[off_x, off_y]])).tolist()
half = hl_size / 2
# against any highlighted point
for idx, (x, y) in enumerate(offset_points):
if (x - half <= px <= x + half) and (y - half <= py <= y + half):
# the index of the point being dragged + 1
return 1 + idx
# against a side of the polygon ...
point = Point(px, py)
for idx in range(len(offset_points)):
p1 = offset_points[idx]
p2 = offset_points[(idx + 1) % len(offset_points)]
side = LineString([p1, p2])
if side.distance(point) < half:
return 1 + idx + len(offset_points)
# intersection
offset_polygon = Polygon(offset_points)
if offset_polygon.contains(point):
return 0
# reaches here if it doesn't intersect the polygon (or its points) at all.
return -1
def _generalize_traj(self, traj, tolerance):
prev_pt = None
pts = []
keep_rows = []
i = 0
for index, row in traj.df.iterrows():
current_pt = row[traj.get_geom_column_name()]
if prev_pt is None:
prev_pt = current_pt
keep_rows.append(i)
continue
line = LineString([prev_pt, current_pt])
for pt in pts:
if line.distance(pt) > tolerance:
prev_pt = current_pt
pts = []
keep_rows.append(i)
continue
pts.append(current_pt)
i += 1
keep_rows.append(i)
new_df = traj.df.iloc[keep_rows]
new_traj = Trajectory(new_df, traj.id)
return new_traj
def check_drag_type(self, off_x, off_y, hl_size, px, py):
if not self.visible:
return -1
offset_points = (self.points + np.array([[off_x, off_y]])).tolist()
half = hl_size / 2
# against any highlighted point
for idx, (x, y) in enumerate(offset_points):
if (x - half <= px <= x + half) and (y - half <= py <= y + half):
# the index of the point being dragged + 1
return 1 + idx
# against a side of the polygon ...
point = Point(px, py)
for idx in range(len(offset_points) - 1):
p1 = offset_points[idx]
p2 = offset_points[idx + 1]
side = LineString([p1, p2])
if side.distance(point) < half:
return 1 + idx + len(offset_points)
# there is no intersection dragging for polyline ...
# so, at the moment there is no way to define drag type 0 (move everything)
# so, it reaches here if it doesn't intersect the polyline points or edges at all.
return -1
def get_segment(self, utm_x, utm_y):
if self.routing_str is None:
return None
point = Point(utm_x, utm_y)
routing = LineString(self.routing_points)
if routing.distance(point) > 10:
return []
if routing.length < 10:
return []
vehicle_distance = routing.project(point)
points = []
total_length = routing.length
for i in range(self.SMOOTH_BACKWARD_DIST):
backward_dist = vehicle_distance - self.SMOOTH_BACKWARD_DIST + i
if backward_dist < 0:
continue
p = routing.interpolate(backward_dist)
points.append(p.coords[0])
for i in range(self.SMOOTH_FORWARD_DIST):
forward_dist = vehicle_distance + i
if forward_dist >= total_length:
break
p = routing.interpolate(forward_dist)
points.append(p.coords[0])
return points
def get_frenet_from_cartesian(
linestring: LineString,
cartesian_point: Point,
cartesian_heading: float = None,
resolution: float = 0.01,
) -> Tuple[float, float, float, Point]:
"""
Converts a point (x,y,heading) in the cartesian frame to it's frenet frame representation.
:param linestring:
A shapely 'LineString' (Polyline) that represents the centre line of the lane being used as
the reference for the frenet frame.
:param cartesian_point:
A shapely 'Point' of the point in cartesian frame that has to be represented in the frenet frame.
:param cartesian_heading:
The heading associated with the cartesian point, in degrees.
:param resolution:
The resolution used to calculate the heading direction of local sections of the lane LineString.
:returns:
Tuple of (s, d, frenet_heading, projection of cartesian_point on linestring)
"""
# Get s and d. d is not associated with a direction here.
s = linestring.project(cartesian_point)
d = linestring.distance(cartesian_point)
# Find closest point (to the input point) on the lane LineString.
closest_point_on_linestring = linestring.interpolate(s)
# Find heading direction of the lane LineString by using a small section of the LineString,
# around closest_point_on_linestring computed above.
local_section_of_spline_start = linestring.interpolate(
max(0, s - resolution))
local_section_of_spline_end = linestring.interpolate(
min(linestring.length, s + resolution))
local_section_heading_in_cartesian_coordinates = np.degrees(
np.arctan2(
local_section_of_spline_end.y - local_section_of_spline_start.y,
local_section_of_spline_end.x - local_section_of_spline_start.x,
))
# Find heading in frenet frame.
heading_relative_to_local_section_heading = (
cartesian_heading - local_section_heading_in_cartesian_coordinates)
# Assign a direction (+ or -) to the distance d.
heading_of_line_joining_input_points_and_its_closest_point_on_linestring = (
np.degrees(
np.arctan2(
cartesian_point.y - closest_point_on_linestring.y,
cartesian_point.x - closest_point_on_linestring.x,
)))
relative_heading = (
heading_of_line_joining_input_points_and_its_closest_point_on_linestring
- local_section_heading_in_cartesian_coordinates)
if relative_heading < 0 or relative_heading > 180:
d = -1 * d
return s, d, heading_relative_to_local_section_heading, closest_point_on_linestring
def distancePolygon2ExternalPointAndCenter(points, point):
if np.all(points[0] == points[-1]):
size = len(points) - 1
else:
size = len(points)
polyCenter = getPolygonCenter(points)
#arrow = (point-polyCenter)*10000
lineCenterPoint = np.array([polyCenter, point], dtype=np.float64)
#lineCenterPoint = np.array([polyCenter, polyCenter+arrow],dtype=np.float64)
minDistance = None
for i in range(size):
if i == len(points) - 1:
j = 0
else:
j = i + 1
line = [points[i], points[j]]
intersection = LineString(lineCenterPoint).intersection(
LineString(line))
dis = intersection.distance(Point(point))
if dis > 0:
intersectionPt = np.array(
[intersection.xy[0][0], intersection.xy[1][0]], dtype=int)
minDistance = dis
centerDistance = norm(intersectionPt - polyCenter)
break
"""
This means the point is inside the shape, not outside of it.
"""
if minDistance is None:
return None, None
return minDistance, centerDistance
def read_sensor_cache_of(self, vehicle: Vehicle, _: Scenario) -> Optional[Tuple[str, float]]:
from shapely.geometry import Point, LineString
from numpy import rad2deg, arctan2, array
if vehicle.state:
x, y, _ = vehicle.state["pos"]
car_pos = Point(x, y)
x_dir, y_dir, _ = vehicle.state["dir"]
car_angle = rad2deg(arctan2(y_dir, x_dir))
min_dist = None
angle_diff = None
road_id = None
for cur_road_id, cur_road in self.road_lines.items():
cur_coord = cur_road.coords[0]
for i in range(1, len(cur_road.coords)):
next_coord = cur_road.coords[i]
cur_line = LineString([cur_coord, next_coord])
cur_dist = cur_line.distance(car_pos)
if not min_dist or cur_dist < min_dist:
min_dist = cur_dist
diff = array(next_coord) - array(cur_coord)
cur_angle = rad2deg(arctan2(diff[1], diff[0]))
angle_diff = car_angle - cur_angle
road_id = cur_road_id
cur_coord = next_coord
return road_id, angle_diff
else:
return None
def make_lines_new(points: List[Tuple[float, float]],
max_point_distance: float = 3,
min_points_per_segment: int = 7,
hough_lines_threshold: int = 5) -> List[Line]:
lines = []
new_lines_added = True
while new_lines_added and points:
new_lines_added = False
x, y = [i[0] for i in points], [i[1] for i in points]
max_x, max_y = max(x), max(y)
img = np.zeros((max_y + 1, max_x + 1), dtype=np.uint8)
for x, y in points:
img[y, x] = 1
line, angle = get_main_orientation(img,
angle_in_degrees=True,
max_threshold=hough_lines_threshold)
if line:
ls = LineString(line.coords)
first_line = []
for p in points:
if Point(p).distance(ls) <= max_point_distance:
first_line.append(p)
while len(first_line) >= min_points_per_segment:
segment = [first_line.pop()]
neighbours = nearest_neighbours_recursive(
segment[0], first_line)
segment.extend(neighbours)
if len(segment) >= min_points_per_segment:
new_lines_added = True
for p in segment:
points.remove(p)
segment = sorted(segment)
t_1 = ls.interpolate(ls.project(Point(segment[0])))
t_2 = ls.interpolate(ls.project(Point(segment[-1])))
line = Line(nr=1, p1=(t_1.x, t_1.y), p2=(t_2.x, t_2.y))
lines.append(line)
# assert not len(first_line)
if lines:
lines_reordered = [lines.pop()]
lines_reordered[0].set_nr(0)
while lines:
current_line = LineString(lines_reordered[-1].coords)
nearest_line = None
min_dist = None
for l in lines:
dist = current_line.distance(LineString(l.coords))
if min_dist is None or dist < min_dist:
min_dist = dist
nearest_line = l
lines.remove(nearest_line)
nearest_line.set_nr(len(lines_reordered))
lines_reordered.append(nearest_line)
lines = lines_reordered
return lines
def remove_collinear_points(polygon, threshold=1.0):
"""
Removes points in a polygon that lie on the straight line created by its
adjacent points.
aThreshold defaults to 1, less than 1 will be more precise (remove less points) and greater
than 1 will be less precise (remove more points)
Args:
polygon (shapely.geometry.Polygon): the shape to be simplified
threshold (float): Accuracy threshold. Default: 1
Returns:
The simplified polygon
"""
if threshold < 0.0:
print("Threshold must be greater than 0")
return None
points = list(polygon.exterior.coords)
# Threshold of how far a point can be from a linestring in order to define it as collinear
threshold = threshold * 10**(-6)
index = 0
while index != len(points) - 1:
linestring = LineString([points[index - 1], points[index + 1]])
if 0 < linestring.distance(Point(points[index])) < threshold:
del points[index]
else:
index += 1
return Polygon(points)
def split_line_at_MultiPoint(line_geometry, intersection):
"""
The function checks whether the coordinates of Point(s) in a Point Collections coordinate are part of the sequence of coordinates of a LineString.
When this has been ascerted or fixed, the LineString line_geometry is split at each of the intersecting points in the collection.
The input intersection, must be an actual intersection.
Parameters
----------
line_geometry: LineString
the LineString which has to be split
intersection: MultiPoint
the intersecting points
Returns
-------
MultiLineString
"""
for point in intersection:
new_line_coords = list(line_geometry.coords)
for n, v in enumerate(new_line_coords):
if n == 0:
continue
line = LineString([Point(new_line_coords[n-1]), Point(v)])
if ((point.intersects(line)) | (line.distance(point) < 1e-8)):
new_line_coords.insert(n, point.coords[0])
break
line_geometry = LineString([coor for coor in new_line_coords])
lines = split(line_geometry, intersection)
return lines
def process_path_obstacle(self, path_x, path_y):
self.obstacle_lat_dist = {}
path = []
self.mobileye.process_obstacles()
for i in range(len(path_x)):
path.append((path_x[i], path_y[i]))
line = LineString(path)
for obstacle in self.mobileye.obstacles:
point = Point(obstacle.x, obstacle.y)
dist = line.distance(point)
if dist < self.MASTER_WIDTH / 2.0 + self.LAT_DIST:
proj_len = line.project(point)
if proj_len == 0 or proj_len >= line.length:
continue
p1 = line.interpolate(proj_len)
if (proj_len + 1) > line.length:
p2 = line.interpolate(line.length)
else:
p2 = line.interpolate(proj_len + 1)
d = (point.x - p1.x) * (p2.y - p1.y) - (point.y -
p1.y) * (p2.x - p1.x)
if d < 0:
dist *= -1
self.obstacle_lat_dist[obstacle.obstacle_id] = dist
def identify_first_point_in_polygon(points, ddd=0.5):
"""
Identify the cycle beginning for the last point
:param points: array of objects of PointX
:param ddd: distance to the fist point ddd>0.
:return -1 if do not close, or i if close in point i.
"""
if len(points) < 3:
return -1
# last point
lp = Point(points[-1])
first_point = len(points) - 2
# Distance between last point and a line segment
distance_point_line = 2 * ddd
# Distance is not less than ddd or less than 3 points.
while not (distance_point_line < ddd and polyline_length(points[first_point:]) > 2):
# next segment
first_point -= 1
if first_point == -1:
break
pt1 = points[first_point + 1]
pt2 = points[first_point]
# Distance to the first point to the line segment
line_segment = LineString([pt1, pt2])
distance_point_line = line_segment.distance(lp)
return first_point
def generalize_douglas_peucker(self, tolerance):
"""Return new generalized Trajectory using Douglas-Peucker Algorithm."""
prev_pt = None
pts = []
keep_rows = []
i = 0
for index, row in self.df.iterrows():
current_pt = row.geometry
if prev_pt is None:
prev_pt = current_pt
keep_rows.append(i)
continue
line = LineString([prev_pt, current_pt])
for pt in pts:
if line.distance(pt) > tolerance:
prev_pt = current_pt
pts = []
keep_rows.append(i)
continue
pts.append(current_pt)
i += 1
keep_rows.append(i)
new_df = self.df.iloc[keep_rows]
new_traj = Trajectory(self.id, new_df)
return new_traj
def douglas_peucker(self, tolerance):
prev_pt = None
pts = []
keep_rows = []
i = 0
for index, row in self.df.iterrows():
current_pt = row.geometry
if prev_pt is None:
prev_pt = current_pt
keep_rows.append(i)
continue
line = LineString([prev_pt, current_pt])
for pt in pts:
if line.distance(pt) > tolerance:
prev_pt = current_pt
pts = []
keep_rows.append(i)
continue
pts.append(current_pt)
i += 1
keep_rows.append(i)
new_df = self.df.iloc[keep_rows]
new_traj = Trajectory(self.id, new_df)
new_traj.get_length() # to recompute prev_pt and dist_to_prev
return new_traj
def process_path_obstacle(self, fpath):
if self.path_obstacle_processed:
return
path_x, path_y = fpath.get_xy()
self.obstacle_lat_dist = {}
path = []
self.mobileye.process_obstacles()
for i in range(len(path_x)):
path.append((path_x[i], path_y[i]))
line = LineString(path)
for obs_id, obstacle in self.mobileye.obstacles.items():
point = Point(obstacle.x, obstacle.y)
dist = line.distance(point)
if dist < self.LAT_DIST + obstacle.width + self.left_edge_to_center:
proj_len = line.project(point)
if proj_len == 0 or proj_len >= line.length:
continue
p1 = line.interpolate(proj_len)
if (proj_len + 1) > line.length:
p2 = line.interpolate(line.length)
else:
p2 = line.interpolate(proj_len + 1)
d = (point.x - p1.x) * (p2.y - p1.y) - (point.y - p1.y) * (
p2.x - p1.x)
if d > 0:
dist *= -1
self.obstacle_lat_dist[obstacle.obstacle_id] = dist
self.path_obstacle_processed = True
def getExactMask(self, path, budget=float('inf')):
budgeted_path, budgeted_path_len = applyBudget(path, budget)
mask = np.zeros(self.info_field.shape)
if isinstance(budgeted_path, np.ndarray):
#shapely_path = [Location(xlon=pt[0], ylat=pt[1]).shapelyPoint() for pt in budgeted_path]
shapely_path = [Point((pt[0], pt[1])) for pt in budgeted_path]
# else:
# shapely_path = [pt.shapelyPoint() for pt in budgeted_path]
ls = LineString(shapely_path)
distances = np.array([ls.distance(pt) for pt in self.pts])
distances = distances.reshape(self.info_field.shape)
if self.method == 'binary':
mask = distances <= self.radius
elif self.method == 'linear':
mask = (self.radius -
np.minimum(distances, self.radius)) / self.radius
elif self.method == 'squared':
mask = ((self.radius - np.minimum(distances, self.radius)) /
self.radius)**2
return mask
def add_point_to_linestring(linestring, closest_point=0):
lx, ly = linestring.xy
lines_list = []
distance = 100000
for i in range(len(lx)-1):
simple_line = LineString([Point(lx[i], ly[i]), Point(lx[i+1], ly[i+1])])
lines_list.append(simple_line)
if simple_line.distance(closest_point) < distance:
distance = simple_line.distance(closest_point)
print(distance)
closest_line_index = i
extend_line = lines_list.pop(closest_line_index)
xe, ye = extend_line.xy
lines_list.insert(closest_line_index, LineString([Point(xe[0], ye[0]),
closest_point,
Point(xe[1], ye[1])]))
return linemerge(lines_list)
class Route:
def __init__(self, goals, width):
self.goals = goals
self.linestring = LineString(goals)
self.width = width
def distance(self, p):
return self.linestring.distance(Point(*p))
def check_straight_line_collision(self, p0, p1, with_dist=False):
"""Check if a path from p0 to p1 has collision, also return shortest distance."""
path_line = LineString([p0, p1])
is_collision = path_line.intersects(self.multi_line)
if not with_dist:
return is_collision
else:
dist = path_line.distance(self.multi_line)
return is_collision, dist
def curve_dist_aux(curve_points, points, n_points):
curve = spline(curve_points, n_points)
curve = np.stack(curve, axis=1)
curve = LineString(curve)
distance = 0
for point in points:
distance += curve.distance(Point(point))
distance /= len(points)
return distance
def line_intersection(line1, line2):
ls1 = LineString(line1)
ls2 = LineString(line2)
if ls1.distance(ls2) < 0.1:
return True
else:
return False
def _generalize_traj(self, traj, tolerance,
columnNamesToDistributeValues=None):
prev_pt = None
pts = []
keep_rows = []
i = 0
trajCopy = deepcopy(traj)
for index, row in trajCopy.df.iterrows():
current_pt = row.geometry
# Handle first row and skip the loop
if prev_pt is None:
prev_pt = current_pt
keep_rows.append(i)
continue
line = LineString([prev_pt, current_pt])
for pt in pts:
if line.distance(pt) > tolerance:
prev_pt = current_pt
pts = []
keep_rows.append(i)
continue
pts.append(current_pt)
i += 1
# Keep the last row
keep_rows.append(i)
if (columnNamesToDistributeValues):
# Distribute the selected values of dropped rows to the
# neighboring rows
for i, rowIndex in enumerate(keep_rows):
if (i < len(keep_rows) - 1 and keep_rows[i+1] - rowIndex > 1):
nextRowIndex = keep_rows[i + 1]
discardedRows = trajCopy.df.iloc[rowIndex +
1: nextRowIndex]
discardedRowsSelectedColumns = \
discardedRows[columnNamesToDistributeValues]
discardedRowsSelectedColumnsSum = \
discardedRowsSelectedColumns.sum()
aboveRow = trajCopy.df.iloc[rowIndex]
belowRow = trajCopy.df.iloc[nextRowIndex]
aboveRow[columnNamesToDistributeValues] = \
aboveRow[columnNamesToDistributeValues] + (
discardedRowsSelectedColumnsSum/2)
belowRow[columnNamesToDistributeValues] = \
belowRow[columnNamesToDistributeValues] + (
discardedRowsSelectedColumnsSum/2)
trajCopy.df.iloc[rowIndex] = aboveRow
trajCopy.df.iloc[nextRowIndex] = belowRow
new_df = trajCopy.df.iloc[keep_rows]
removedRowsCount = len(traj.df.index) - len(keep_rows)
new_columns = {'Generalized': True, 'Generalization.Method': 'Douglas-Peucker',
'Generalization.RemovedRowsCount': removedRowsCount}
new_df = new_df.assign(**new_columns)
new_traj = Trajectory(new_df, trajCopy.id)
return new_traj
def compute_excess_area_for_circle_cover(cover, possible_centers,
protected_region):
"""
compute the excess area for circle cover.
Parameters:
cover : A set of circles representing the circle cover of the region.
interference_contour : The interference contour.
possible_centers : The land loctions where the sensors may be placed.
"""
# Line string representing the interference contour. Add first and last point
# of possible_centers to this. This is the interference contour that is on the sea.
# we add the two points to complete the polygon
interference_contour_linestring = protected_region.extererior.coords
# Line string representing the possible sensor locations
possible_centers_linestring = LineString(possible_centers)
cover_union = cover[0].get_geometry()
for i in range(1, len(cover)):
cover_union = cover_union.union(cover[i].get_geometry())
# take the union of
union = cover_union.union(bounding_polygon)
minx, miny, maxx, maxy = union.bounds
# Generate a point set and classify.
ndivs = antennacover.NDIVISIONS
deltax, deltay = float(maxx - minx) / ndivs, float(maxy - miny) / ndivs
area_per_grid_point = deltax * deltay
excess_sea_coverage_count = 0
excess_land_coverage_count = 0
for i in range(0, ndivs):
for j in range(0, ndivs):
p = Point(minx + i * deltax, miny + j * deltay)
if cover_union.contains(p) and not bounding_polygon.contains(p):
# The point is inside the boundary of the cover but NOT in the bounding polygon
# that needs to be covered.
# The point p is now either on sea or land. We need to classify it.
d1 = interference_contour_linestring.distance(p)
d2 = possible_centers_linestring.distance(p)
if d1 < d2:
excess_sea_coverage_count = excess_sea_coverage_count + 1
else:
excess_land_coverage_count = excess_land_coverage_count + 1
ExcessArea = namedtuple(
"ExcessArea",
"excess_sea_coverage excess_land_coverage outage_area protected_region"
)
outage = bounding_polygon.difference(union).area
protected_region = bounding_polygon.area
return ExcessArea(round2(excess_sea_coverage_count * area_per_grid_point),
round2(excess_land_coverage_count * area_per_grid_point),
outage, protected_region)
def find_corners(self, bounding_polygon, tol=1.0e-08, print_flag=False):
"""Find the corners of a layer cut by a bounding polygon.
Saves: self.corners
"""
list_of_corners = []
bounding_line = LineString(bounding_polygon.exterior)
for coord in self.polygon.exterior.coords[:-1]:
pt = Point(coord[0],coord[1])
# determine if this point is on the bounding_polygon
if bounding_line.distance(pt) < tol:
list_of_corners.append(pt)
if print_flag:
print pt
self.corners = list_of_corners
def closest_side_alignment_norm(sample0,sample1):
polygons=mp.map_layoutsamples_to_geometricobjects([sample0,sample1],shape_name="shape")
min_dist=float("inf")
min_l0=None
min_l1=None
for p00,p01 in ut.pairwise(polygons[0].exterior.coords):
for p10,p11 in ut.pairwise(polygons[1].exterior.coords):
l0=LineString([p00,p01])
l1=LineString([p10,p11])
dist=l1.distance(l0)
if dist<min_dist:
min_dist=dist
min_l0=l0.coords
min_l1=l1.coords
#the constraint of alignement between 2 closest sides of 2 polygons as defined in a paper
#the difference is defined over 90 degrees
angle0=_angle(min_l0)
angle1=_angle(min_l1)
return (1+np.cos(4*(angle0-angle1)))/2
def _sanitize_edges(self, edges):
"""
Checks if there are points that lay on the edges.
"""
new_edges = set()
for edge in edges:
found_point_between = False
segment = LineString([(edge[0].x, edge[0].y), (edge[1].x, edge[1].y)])
for point in self.points:
if point is not edge[0] and point is not edge[1]:
if segment.distance(ShapelyPoint(point.x, point.y)) < 1e-8:
if (edge[0], point) not in self.edges and (point, edge[0]) not in self.edges:
new_edges.add((edge[0], point))
if (edge[1], point) not in self.edges and (point, edge[1]) not in self.edges:
new_edges.add((point, edge[1]))
found_point_between = True
break
if not found_point_between:
new_edges.add(edge)
if set(edges) == new_edges:
return list(edges)
else:
return self._sanitize_edges(list(new_edges))
def err(p,coor,pts,pt):
line=LineString([coor[0]+p[0]*(coor[1]-coor[0]),pt])
distance=[line.distance(pt) for pt in pts]
return distance
def track_to_segment_projction(track,
road_segments,
angle_threshold = np.pi/4.0):
""" Project tracks to road segments
Args:
- track: a GPS track
- road_segments: a list of RoadSegment
- dist_threshold: in meters
Return:
- related_segments: an index list
"""
road_segment_linestrings = []
for r_seg in road_segments:
r_start = r_seg.center - r_seg.half_length*r_seg.direction
r_end = r_seg.center + r_seg.half_length*r_seg.direction
r_linestring = LineString([r_start, r_end])
road_segment_linestrings.append(r_linestring)
related_segments = []
line = LineString([(pt[0], pt[1]) for pt in track.utm])
simplified_line = line.simplify(10.0)
coords = simplified_line.coords
for pt_idx in range(1, len(coords)):
track_segment_dir = np.array(coords[pt_idx]) - np.array(coords[pt_idx-1])
track_segment_dir_norm = np.linalg.norm(track_segment_dir)
if track_segment_dir_norm < 1.0:
continue
track_segment_dir /= np.linalg.norm(track_segment_dir)
track_segment = LineString([coords[pt_idx-1], coords[pt_idx]])
for r_seg_idx in np.arange(len(road_segments)):
r_segment = road_segments[r_seg_idx]
if np.dot(track_segment_dir, r_segment.direction) >= np.cos(angle_threshold):
dist = track_segment.distance(road_segment_linestrings[r_seg_idx])
if dist <= r_segment.half_width:
related_segments.append(r_seg_idx)
nearby_segments = list(set(related_segments))
if len(nearby_segments) <= 1:
return nearby_segments
# Select through nearby segments to get the cover of the trajctory
m = len(track.utm)
n = len(nearby_segments)
A = np.zeros((m,n))
for pt_idx in np.arange(len(track.utm)):
point = Point(track.utm[pt_idx][0], track.utm[pt_idx][1])
for nb_seg_idx in np.arange(len(nearby_segments)):
seg_idx = nearby_segments[nb_seg_idx]
if point.distance(road_segment_linestrings[seg_idx]) <= road_segments[seg_idx].half_width:
A[pt_idx, nb_seg_idx] = 1.0
alpha = 0.5
x = Variable(n)
delta = Variable(m)
objective = Minimize(sum(alpha*x)+sum(delta))
constraints = [0 <= x,
x <= 1,
0 <= delta,
delta <= 1,
A*x >= 1 - delta]
prob = Problem(objective, constraints)
prob.solve()
selected_segments = []
order = []
for i in np.arange(len(x.value)):
if x.value[i] > 0.5:
selected_segments.append(nearby_segments[i])
order_of_this_segment = np.average(np.where(A[:, i])[0])
order.append(order_of_this_segment)
order = np.array(order)
sorted_idxs = np.argsort(order)
sorted_segments = []
for idx in sorted_idxs:
sorted_segments.append(selected_segments[idx])
return sorted_segments
def merge_parallel_street_segments(self, parallel_pairs):
"""
Note: Maybe I don't even have to merge any path (which breaks the original street network data structure.
Instead, I can mark ways that have parallel neighbors not make sidewalks on both sides...
:param parallel_pairs: pairs of street_ids.
Todo: This method needs to be optimized using some spatial data structure (e.g., r*-tree) and other metadata..
# Expand streets into rectangles, then find intersections between them.
# http://gis.stackexchange.com/questions/90055/how-to-find-if-two-polygons-intersect-in-python
"""
# Merge parallel pairs
for pair in parallel_pairs:
streets_to_remove = []
street_pair = (self.ways.get(pair[0]), self.ways.get(pair[1]))
# First find parts of the street pairs that you want to merge (you don't want to merge entire streets
# because, for example, one could be much longer than the other and it doesn't make sense to merge
subset_nids, street1_segment, street2_segment = self.segment_parallel_streets((street_pair[0], street_pair[1]))
if not subset_nids:
continue
# Get two parallel segments and the distance between them
try:
street1_node = self.nodes.get(street1_segment[1][0])
street2_node = self.nodes.get(street2_segment[1][0])
except IndexError:
log.debug("Warning! Segment to merge was empty for one or both streets, so skipping this merge...")
continue
street1_end_node = self.nodes.get(street1_segment[1][-1])
street2_end_node = self.nodes.get(street2_segment[1][-1])
LS_street1 = LineString((street1_node.location(), street1_end_node.location()))
LS_street2 = LineString((street2_node.location(), street2_end_node.location()))
distance = LS_street1.distance(LS_street2) / 2
# Merge streets
node_to = {}
new_street_nids = []
street1_idx = 0
street2_idx = 0
street1_nid = street1_segment[1][0]
street2_nid = street2_segment[1][0]
for nid in subset_nids:
try:
if nid == street1_nid:
street1_idx += 1
street1_nid = street1_segment[1][street1_idx]
node = self.nodes.get(nid)
opposite_node_1 = self.nodes.get(street2_nid)
opposite_node_2_nid = street2_segment[1][street2_idx + 1]
opposite_node_2 = self.nodes.get(opposite_node_2_nid)
else:
street2_idx += 1
street2_nid = self.ways.get(pair[1]).nids[street2_idx]
node = self.nodes.get(nid)
opposite_node_1 = self.nodes.get(street1_nid)
opposite_node_2_nid = street1_segment[1][street1_idx + 1]
opposite_node_2 = self.nodes.get(opposite_node_2_nid)
v = opposite_node_1.vector_to(opposite_node_2, normalize=True)
v2 = opposite_node_1.vector_to(node, normalize=True)
if np.cross(v, v2) > 0:
normal = np.array([v[1], v[0]])
else:
normal = np.array([- v[1], v[0]])
new_position = node.location() + normal * distance
new_node = Node(None, new_position[0], new_position[1])
self.add_node(new_node)
new_street_nids.append(new_node.id)
except IndexError:
# Take care of the last node.
# Use the previous perpendicular vector but reverse the direction
node = self.nodes.get(nid)
new_position = node.location() - normal * distance
new_node = Node(None, new_position[0], new_position[1])
self.add_node(new_node)
new_street_nids.append(new_node.id)
log.debug(pair)
node_to[subset_nids[0]] = new_street_nids[0]
node_to[subset_nids[-1]] = new_street_nids[-1]
merged_street = Street(None, new_street_nids)
merged_street.distance_to_sidewalk *= 2
streets_to_remove.append(street_pair[0].id)
streets_to_remove.append(street_pair[1].id)
# Create streets from the unmerged nodes.
# Todo: I think this part of the code can be prettier
if street1_segment[0] or street2_segment[0]:
if street1_segment[0] and street2_segment[0]:
if street1_segment[0][0] == street2_segment[0][0]:
# The two segments street1 and street2 share a common node. Just connect one of them to the
# new merged street.
if subset_nids[0] in street1_segment[1]:
street1_segment[0][-1] = node_to[street1_segment[0][-1]]
s = Street(None, street1_segment[0])
self.add_way(s)
else:
street2_segment[0][-1] = node_to[street2_segment[0][-1]]
s = Street(None, street2_segment[0])
self.add_way(s)
else:
# Both street1_segment and street2_segment exist, but they do not share a common node
street1_segment[0][-1] = node_to[street1_segment[0][-1]]
s = Street(None, street1_segment[0])
self.add_way(s)
street2_segment[0][-1] = node_to[street2_segment[0][-1]]
s = Street(None, street2_segment[0])
self.add_way(s)
elif street1_segment[0]:
# Only street1_segment exists
street1_segment[0][-1] = node_to[street1_segment[0][-1]]
s = Street(None, street1_segment[0])
self.add_way(s)
else:
# Only street2_segment exists
street2_segment[0][-1] = node_to[street2_segment[0][-1]]
s = Street(None, street2_segment[0])
self.add_way(s)
if street1_segment[2] or street2_segment[2]:
if street1_segment[2] and street2_segment[2]:
if street1_segment[2][-1] == street2_segment[2][-1]:
# The two segments street1 and street2 share a common node. Just connect one of them to the
# new merged street.
if subset_nids[-1] in street1_segment[1]:
street1_segment[2][0] = node_to[subset_nids[-1]]
s = Street(None, street1_segment[2])
self.add_way(s)
else:
street2_segment[2][0] = node_to[subset_nids[-1]]
s = Street(None, street2_segment[2])
self.add_way(s)
else:
# Both street1_segment and street2_segment exist, but they do not share a common node
street1_segment[2][0] = node_to[subset_nids[-1]]
s = Street(None, street1_segment[2])
self.add_way(s)
street2_segment[2][0] = node_to[subset_nids[-1]]
s = Street(None, street2_segment[2])
self.add_way(s)
elif street1_segment[2]:
# Only street1_segment exists
street1_segment[2][0] = node_to[subset_nids[-1]]
s = Street(None, street1_segment[2])
self.add_way(s)
else:
# Only street2_segment exists
street2_segment[2][0] = node_to[subset_nids[-1]]
s = Street(None, street2_segment[2])
self.add_way(s)
self.add_way(merged_street)
self.simplify(merged_street.id, 0.1)
for street_id in set(streets_to_remove):
for nid in self.ways.get(street_id).nids:
node = self.nodes.get(nid)
for parent_id in node.way_ids:
if not parent_id in streets_to_remove:
# FIXME
parent = self.ways.get(parent_id)
dist = dist2 = 100
final_node = final_node2 = None
for merged_nid in merged_street.nids:
merged_node = self.nodes.get(merged_nid)
if dist > np.linalg.norm(merged_node.vector()-node.vector()):
final_node2 = final_node
final_node = merged_node
dist2 = dist
dist = np.linalg.norm(merged_node.vector()-node.vector())
#final_node.append_way(self.ways.get(parent_id))
#pos = merged_street.nids.index(final_node.id)
#merged_street.nids.insert(pos, node.id)
node2 = parent.nids[parent.nids.index(nid) + 1]
x, y = node.vector()
x2, y2 = final_node.vector()
slopea = (node.vector()[1] - node.vector()[0])/(node2.vector()[1] - node2.vector()[0])
slopeb = (final_node.vector()[1] - final_node.vector()[0])/(final_node2.vector()[1] - final_node2.vector()[0])
# fx = (-ya + yb + sxa - sbxb) / (s - sb)
fx = (-y + y2 + (slope * x) - (slope2 * x2)) / (slope - slope2)
fy = slope * (fx - x) + y
n = Node(None, fx, fy)
self.add_node(n)
merged_street.add_node(n)
break
self.remove_way(street_id)
#print self.export()
return
#Create a map.
map = folium.Map()
#Draw the main path.
map.line(reducedList, line_color = 'Red', line_weight = 2)
#Draw the buffers.
map.line(bufferLeftPoints, line_color = 'Blue', line_weight = 2)
map.line(bufferRightPoints, line_color = 'Blue', line_weight = 2)
#Draw markers randomly and create Points (Shapely) to be able to use
#the distance() function. Counts the number of markers in between
#the buffers.
markerCount = 0;
for i in range(len(reducedList)):
yValue = random.uniform(reducedList[i][1] - OFFSET, reducedList[i][1] + OFFSET)
map.polygon_marker(location=[reducedList[i][0], yValue], radius = 2,
line_color = 'Black', fill_opacity = 1)
point = Point(reducedList[i][0], yValue)
if (line.distance(point) < 0.125):
markerCount += 1
#Sums up the number of panels seen.
print 'Approximately %d panels seen' % markerCount
#Creates map.
map.create_map(path='map.html')
class RoutingProvider:
def __init__(self):
self.routing_str = None
self.routing_points = []
self.routing = None
self.routing_lock = threading.Lock()
self.SMOOTH_FORWARD_DIST = 150
self.SMOOTH_BACKWARD_DIST = 150
self.human = False
def update_navigation(self, navigation_info_pb):
self.routing_str = navigation_info_pb
self.human = True
routing_points = []
for navi_path in navigation_info_pb.navigation_path:
for path_point in navi_path.path.path_point:
routing_points.append([path_point.x, path_point.y])
self.routing_lock.acquire()
self.routing_points = routing_points
self.routing_lock.release()
self.routing = LineString(self.routing_points)
def update(self, routing_str):
self.routing_str = routing_str
routing_json = json.loads(routing_str.data)
routing_points = []
self.human = False
for step in routing_json:
if step.get('human'):
self.human = True
points = step['polyline']['points']
for point in points:
routing_points.append(point)
self.routing_lock.acquire()
self.routing_points = routing_points
self.routing_lock.release()
self.routing = LineString(self.routing_points)
def get_segment(self, utm_x, utm_y):
if self.routing_str is None:
return None
point = Point(utm_x, utm_y)
if self.routing.distance(point) > 10:
return []
if self.routing.length < 10:
return []
vehicle_distance = self.routing.project(point)
points = []
total_length = self.routing.length
for i in range(self.SMOOTH_BACKWARD_DIST):
backward_dist = vehicle_distance - self.SMOOTH_BACKWARD_DIST + i
if backward_dist < 0:
continue
p = self.routing.interpolate(backward_dist)
points.append(p.coords[0])
for i in range(self.SMOOTH_FORWARD_DIST):
forward_dist = vehicle_distance + i
if forward_dist >= total_length:
break
p = self.routing.interpolate(forward_dist)
points.append(p.coords[0])
return points
def get_local_segment(self, utm_x, utm_y, heading):
points = self.get_segment(utm_x, utm_y)
if points is None or len(points) < 30:
return [], []
points_x = []
points_y = []
for point in points:
points_x.append(point[0])
points_y.append(point[1])
path_x = [x - utm_x for x in points_x]
path_y = [y - utm_y for y in points_y]
npath_x = []
npath_y = []
for i in range(len(path_x)):
x = path_x[i]
y = path_y[i]
newx = x * math.cos(-heading) - y * math.sin(-heading)
newy = y * math.cos(-heading) + x * math.sin(-heading)
npath_x.append(newx)
npath_y.append(newy)
return npath_x, npath_y
def to_monotonic_segment(self, seg_x, seg_y):
left_cut_idx = 0
right_cut_idx = len(seg_x)
for i in range(len(seg_x) - 1):
if seg_x[i + 1] < seg_x[i]:
if seg_x[i] >= 0:
right_cut_idx = i + 1
break
else:
left_cut_idx = i + 1
mono_seg_x = seg_x[left_cut_idx:right_cut_idx]
mono_seg_y = seg_y[left_cut_idx:right_cut_idx]
return mono_seg_x, mono_seg_y
def get_local_ref(self, local_seg_x, local_seg_y):
ref_x = []
ref_y = []
points = []
for i in range(len(local_seg_x)):
x = local_seg_x[i]
y = local_seg_y[i]
points.append((x, y))
line = LineString(points)
dist = line.project(Point((0, 0)))
for i in range(int(line.length - dist) + 1):
p = line.interpolate(i + dist)
ref_x.append(p.x)
ref_y.append(p.y)
return ref_x, ref_y
def get_local_segment_spline(self, utm_x, utm_y, heading):
local_seg_x, local_seg_y = self.get_local_segment(
utm_x, utm_y, heading)
if len(local_seg_x) <= 10:
return [], []
if self.human:
return self.get_local_ref(local_seg_x, local_seg_y)
mono_seg_x, mono_seg_y = self.to_monotonic_segment(
local_seg_x, local_seg_y)
if len(mono_seg_x) <= 10:
return [], []
k = 3
n = len(mono_seg_x)
std = 0.5
sp = optimized_spline(mono_seg_x, mono_seg_y, k, s=n * std)
X = np.linspace(0, int(local_seg_x[-1]), int(local_seg_x[-1]))
return X, sp(X)
def get_local_segment_spline_debug(self, utm_x, utm_y, heading, k=3,
std=0.5):
local_seg_x, local_seg_y = self.get_local_segment(
utm_x, utm_y, heading)
mono_seg_x, mono_seg_y = self.to_monotonic_segment(
local_seg_x, local_seg_y)
if len(mono_seg_x) <= 10:
return [], []
n = len(mono_seg_x)
sp = optimized_spline(mono_seg_x, mono_seg_y, k, s=n * std)
X = np.linspace(int(mono_seg_x[0]), int(mono_seg_x[-1]),
int(mono_seg_x[-1]))
return X, sp(X)
def get_local_path(self, adv, path_range):
utm_x = adv.x
utm_y = adv.y
heading = adv.heading
local_seg_x, local_seg_y = self.get_local_segment(
utm_x, utm_y, heading)
if len(local_seg_x) <= 10:
return LocalPath([])
if self.human:
x, y = self.get_local_ref(local_seg_x, local_seg_y)
points = []
for i in range(path_range):
if i < len(x):
points.append([x[i], y[i]])
return LocalPath(points)
mono_seg_x, mono_seg_y = self.to_monotonic_segment(
local_seg_x, local_seg_y)
if len(mono_seg_x) <= 10:
return LocalPath([])
k = 3
n = len(mono_seg_x)
std = 0.5
sp = optimized_spline(mono_seg_x, mono_seg_y, k, s=n * std)
X = np.linspace(0, int(local_seg_x[-1]), int(local_seg_x[-1]))
y = sp(X)
points = []
for i in range(path_range):
if i < len(X):
points.append([X[i], y[i]])
return LocalPath(points)
class ADPolyline(object):
def __init__(self, shapely_geo=None, vertices=None, use_arcpy=False, use_shapely=False):
"""
Can be initiated with either a shapely Linestring or a list of ADPoints (vertices)
:param shapely_geo: Linestring object
:param vertices: list of ADPoint objects
:param use_arcpy: not implemented yet
:param use_shapely: default
:return: None
"""
if vertices is not None and shapely_geo is None:
# vertices supplied, create geo
# assume vertices are all ADPoint
self.vertices = vertices
self.__geo_from_vertices(vertices)
elif vertices is None and shapely_geo is not None:
# Extract vertices from shapely geo
self.vertices = []
self.shapely_geo = shapely_geo
# Make coordinates python friendly
coord_list = list(shapely_geo.coords)
if len(coord_list[0]) == 2:
# 2d linestring
for x, y in coord_list:
vertex = ADPoint(x, y)
self.vertices.append(vertex)
elif len(coord_list[0]) == 3:
# 3d linestring - ignore z value
for x, y, _ in coord_list:
vertex = ADPoint(x, y)
self.vertices.append(vertex)
self.first_point = self.vertices[0]
self.last_point = self.vertices[-1]
else:
# got nothing, bail
raise
# Only allowed to use arcpy or shapely
if use_arcpy and use_arcpy:
raise
if use_arcpy:
raise NotImplementedError
# self.geometry = arcpy.yadayada
# self.point_at_distance() = arcpy.yada
if use_shapely:
pass
self.length = self.shapely_geo.length
def __geo_from_vertices(self, vertices):
temp_vertices = []
for vertex in vertices:
temp_vertex = (vertex.X, vertex.Y)
temp_vertices.append(temp_vertex)
self.shapely_geo = LineString(temp_vertices)
self.first_point = self.vertices[0]
self.last_point = self.vertices[-1]
def __str__(self):
s = ''
for vertex in self.vertices:
s += str(vertex) + ', '
return s[:-2]
def crosses(self, line):
return self.shapely_geo.crosses(line.shapely_geo)
def is_same_as(self, polyline):
if not isinstance(polyline, ADPolyline):
raise # something
if DEBUG_same_as:
print 'comparing 2 polylines'
for vertex1, vertex2 in zip(self.vertices, polyline.vertices):
if not vertex1.is_same_as(vertex2):
return False
return True
def intersection(self, polyline):
"""
Intersects self with polyline. Returns either ADPoint, list of ADPoints, or returns None
:param polyline: ADPolyline
:return: ADPoint, list of ADPoints or None
"""
new_geo = self.shapely_geo.intersection(polyline.shapely_geo)
if type(new_geo) is Point:
return ADPoint(shapely_geo=new_geo)
elif type(new_geo) is MultiPoint:
shapely_points = list(new_geo)
ad_points = []
for point in shapely_points:
ad_points.append(ADPoint(shapely_geo=point))
return ad_points
else:
return None
def mid_point(self):
"""
Returns midpoint of self
:return: ADPoint
"""
return self.interpolate(0.5, normalized=True)
def nearest_intersection(self, line, test_point):
"""
Intersects self.shapely_geo with line and returns the intersection nearest to test_point
If test_point is the location of an intersection it will be returned
:param line: ADPolyline
:param point: ADPoint
:return: ADPoint
"""
intersects = self.intersection(line)
# Check for multipoint return or no intersection
if type(intersects) is ADPoint:
# single intersection
return intersects
elif type(intersects) is None:
raise UnknownIntersection('BFE doesn\'t intersect with contour')
elif type(intersects) is not list:
raise UnknownIntersection('Unknown return type from intersection: '+str(type(intersects)))
# Assume intersects is a list of ADPoints
# Sort by distance along self, relative to test_point
test_dist = self.project(test_point)
for point in intersects:
point.station = abs(self.project(point)-test_dist)
intersects.sort(key=lambda x: x.station)
# Return closest point
return intersects[0]
def num_intersects(self, line):
"""
Intersects self with line. returns the number of point intersections
:param line: ADPolyline
:return: int
"""
intersect = self.intersection(line)
if intersect is None:
return 0
elif type(intersect) is ADPoint:
return 1
elif type(intersect) is list:
return len(intersect)
else:
# Returned polyline or something weird. Maybe raise an exception here?
return 0
def point_at_distance(self, distance, normalize=False):
new_pt = self.shapely_geo.interpolate(distance, normalized=normalize)
return ADPoint(shapely_geo=new_pt)
def distance_to(self, gis_thing):
#print type(gis_thing)
return self.shapely_geo.distance(gis_thing.shapely_geo)
def interpolate(self, distance, normalized=False):
""" Returns ADPoint at distance along polyline """
geo = self.shapely_geo.interpolate(distance, normalized)
return ADPoint(shapely_geo=geo)
def project(self, gis_thing):
"""Returns the distance along this geometric object to a point nearest the other object."""
return self.shapely_geo.project(gis_thing.shapely_geo)
def plot(self, *args, **kwargs):
pyplot.plot(self.shapely_geo.xy[0], self.shapely_geo.xy[1], *args, **kwargs)
def label(self, text='insert text here', reverse=False, *args, **kwargs):
if reverse:
X = self.last_point.X
Y = self.last_point.Y
else:
X = self.first_point.X
Y = self.first_point.Y
pyplot.annotate(str(text), xy=(X, Y), *args, **kwargs)
def clip(self, point1, point2):
"""
Returns ADPolyline of the current polyline clipped between point1 and point2
Searches for loop (closed) contours and returns the shortest portion of the contour
between point1 and point2
:param point1: ADPoint
:param point2: ADPoint
:return: ADPolyline
"""
# Don't screw up the real vertex order
vertices = copy.copy(self.vertices)
if DEBUG_contour_loop:
print '..before sort start/end vertices', vertices[0], vertices[-1]
# Check for loop contour
if vertices[-1].is_same_as(vertices[0]):
loop_flag = True
else:
loop_flag = False
# tag existing vertices
for vertex in vertices:
vertex.flag = False
# tag new vertices
point1.flag = True
point2.flag = True
# calculate distances for contour vertices the fast way
last_point = vertices[0]
last_point.station = 0
station = 0
for vertex in vertices[1:]:
station += vertex.distance(last_point)
vertex.station = station
last_point = vertex
# calculate distances for new vertices the slow way
point1.station = self.project(point1)
point2.station = self.project(point2)
vertices += [point1, point2]
# sort the vertices
vertices.sort(key=lambda x: x.station)
if DEBUG_contour_loop:
print '..after sort start/end vertices', vertices[0], vertices[-1]
# extract middle points, keep beginning and end of line for loop calcs
start_vertices = []
new_vertices = []
end_vertices = []
state = 'start'
for vertex in vertices:
if state == 'start' and vertex.flag is False:
# not one of our points
start_vertices.append(vertex)
elif state == 'start' and vertex.flag is True:
state = 'in'
new_vertices.append(vertex)
elif state == 'in' and vertex.flag is False:
new_vertices.append(vertex)
elif state == 'in' and vertex.flag is True:
# last vertex in middle
state = 'end'
new_vertices.append(vertex)
elif state == 'end':
end_vertices.append(vertex)
# outside portion of line, for loop contour tests
outside_vertices = end_vertices + start_vertices[1:]
if DEBUG_contour_loop:
print '..len vertices = ', len(vertices)
print '..len new_vertices = ', len(new_vertices)
print '..line outside_vertices = ', len(outside_vertices)
print '..start/end vertices', start_vertices[0], end_vertices[-1]
inside_line = ADPolyline(vertices=new_vertices)
# Check for loop contour
if loop_flag:
outside_line = ADPolyline(vertices=outside_vertices)
# loop contour, see if outside is shorter
if DEBUG_contour_loop:
print '..loop contour'
print '..outside line lenght = ', outside_line.length
print '..inside line length = ', inside_line.length
if outside_line.length < inside_line.length:
return outside_line
if DEBUG_contour_loop:
print '..returning inside line'
return inside_line
def flip(self):
self.vertices = self.vertices[::-1]
self.__geo_from_vertices(self.vertices)
def line_dist(boundary1, boundary2):
line1 = LineString([boundary1[0], boundary1[1]])
line2 = LineString([boundary2[0], boundary2[1]])
l_dist = line1.distance(line2)
return l_dist
def pdfer(data, page_size=PAGE_SIZES['letter'], output='pdf'):
shape_overlays = data.get('shape_overlays')
point_overlays = data.get('point_overlays')
grid = {'zoom': data.get('zoom')}
center_lon, center_lat = data['center']
center_tile_x, center_tile_y = tileXY(float(center_lat),
float(center_lon),
int(data['zoom']))
dim_across, dim_up = data['dimensions']
if dim_across > dim_up:
page_height, page_width, tiles_up, tiles_across = page_size
else:
page_width, page_height, tiles_across, tiles_up = page_size
min_tile_x = center_tile_x - int((tiles_across / 2))
min_tile_y = center_tile_y - int((tiles_up / 2))
max_tile_x = min_tile_x + tiles_across
max_tile_y = min_tile_y + tiles_up
# Get base layer tiles
base_pattern = 'http://d.tile.stamen.com/toner/{z}/{x}/{y}.png'
if data.get('base_tiles'):
base_pattern = data['base_tiles']
base_links = generateLinks(base_pattern,
grid['zoom'],
min_tile_x,
min_tile_y,
max_tile_x,
max_tile_y)
base_names = dl_write_all(base_links, 'base')
# Get overlay tiles
overlay_pattern = None
if data.get('overlay_tiles'):
overlay_pattern = data['overlay_tiles']
overlay_links = generateLinks(overlay_pattern,
grid['zoom'],
min_tile_x,
min_tile_y,
max_tile_x,
max_tile_y)
overlay_names = dl_write_all(overlay_links, 'overlay')
now = datetime.now()
date_string = datetime.strftime(now, '%Y-%m-%d_%H-%M-%S')
outp_name = os.path.join('/tmp', '{0}.png'.format(date_string))
base_image_names = ['-'.join(l.split('/')[-3:]) for l in base_names]
base_image_names = sorted([i.split('-')[-3:] for i in base_image_names], key=itemgetter(1))
for parts in base_image_names:
z,x,y = parts
y = y.rstrip('.png').rstrip('.jpg')
z = z.rsplit('_', 1)[1]
key = '-'.join([z,x,y])
grid[key] = {'bbox': tileEdges(float(x),float(y),int(z))}
keys = sorted(grid.keys())
mercator = GlobalMercator()
bb_poly = None
bmin_rx = None
bmin_ry = None
if shape_overlays or point_overlays:
polys = []
for k,v in grid.items():
try:
one,two,three,four = grid[k]['bbox']
polys.append(box(two, one, four, three))
except TypeError:
pass
mpoly = MultiPolygon(polys)
bb_poly = box(*mpoly.bounds)
min_key = keys[0]
max_key = keys[-2]
bminx, bminy = grid[min_key]['bbox'][0], grid[min_key]['bbox'][1]
bmaxx, bmaxy = grid[max_key]['bbox'][2], grid[max_key]['bbox'][3]
bmin_mx, bmin_my = mercator.LatLonToMeters(bminx, bminy)
bmax_mx, bmax_my = mercator.LatLonToMeters(bmaxx, bmaxy)
bmin_px, bmin_py = mercator.MetersToPixels(bmin_mx,bmin_my,float(grid['zoom']))
bmax_px, bmax_py = mercator.MetersToPixels(bmax_mx,bmax_my,float(grid['zoom']))
bmin_rx, bmin_ry = mercator.PixelsToRaster(bmin_px,bmin_py,int(grid['zoom']))
if shape_overlays:
all_polys = []
for shape_overlay in shape_overlays:
if shape_overlay:
shape_overlay = json.loads(shape_overlay)
if shape_overlay.get('geometry'):
shape_overlay = shape_overlay['geometry']
coords = shape_overlay['coordinates'][0]
all_polys.append(Polygon(coords))
mpoly = MultiPolygon(all_polys)
one, two, three, four, five = list(box(*mpoly.bounds).exterior.coords)
left, right = LineString([one, two]), LineString([three, four])
top, bottom = LineString([two, three]), LineString([four, five])
left_to_right = left.distance(right)
top_to_bottom = top.distance(bottom)
if left_to_right > top_to_bottom:
page_height, page_width, _, _ = page_size
else:
page_width, page_height, _, _ = page_size
center_lon, center_lat = list(mpoly.centroid.coords)[0]
if point_overlays:
all_points = []
for point_overlay in point_overlays:
point_overlay = json.loads(point_overlay)
for p in point_overlay['points']:
if p[0] and p[1]:
all_points.append(p)
mpoint = MultiPoint(all_points)
center_lon, center_lat = list(mpoint.centroid.coords)[0]
one, two, three, four, five = list(box(*mpoint.bounds).exterior.coords)
left, right = LineString([one, two]), LineString([three, four])
top, bottom = LineString([two, three]), LineString([four, five])
left_to_right = left.distance(right)
top_to_bottom = top.distance(bottom)
if left_to_right > top_to_bottom:
page_height, page_width, _, _ = page_size
else:
page_width, page_height, _, _ = page_size
center_lon, center_lat = list(mpoint.centroid.coords)[0]
print(center_lon, center_lat)
arrays = []
for k,g in groupby(base_image_names, key=itemgetter(1)):
images = list(g)
fnames = ['/tmp/%s' % ('-'.join(f)) for f in images]
array = []
for img in fnames:
i = cv2.imread(img, -1)
if isinstance(i, type(None)):
i = np.zeros((256,256,4), np.uint8)
elif i.shape[2] != 4:
i = cv2.cvtColor(cv2.imread(img), cv2.COLOR_BGR2BGRA)
array.append(i)
arrays.append(np.vstack(array))
outp = np.hstack(arrays)
cv2.imwrite(outp_name, outp)
if overlay_pattern:
overlay_outp_name = os.path.join('/tmp', 'overlay_{0}.png'.format(date_string))
overlay_image_names = ['-'.join(l.split('/')[-3:]) for l in overlay_names]
overlay_image_names = sorted([i.split('-')[-3:] for i in overlay_image_names], key=itemgetter(1))
arrays = []
for k,g in groupby(overlay_image_names, key=itemgetter(1)):
images = list(g)
fnames = ['/tmp/%s' % ('-'.join(f)) for f in images]
array = []
for img in fnames:
i = cv2.imread(img, -1)
if isinstance(i, type(None)):
i = np.zeros((256,256,4), np.uint8)
elif i.shape[2] != 4:
i = cv2.cvtColor(cv2.imread(img), cv2.COLOR_BGR2BGRA)
array.append(i)
arrays.append(np.vstack(array))
nuked = [os.remove(f) for f in fnames]
outp = np.hstack(arrays)
cv2.imwrite(overlay_outp_name, outp)
base = cv2.imread(outp_name, -1)
overlay = cv2.imread(overlay_outp_name, -1)
overlay_g = cv2.cvtColor(overlay, cv2.COLOR_BGR2GRAY)
ret, mask = cv2.threshold(overlay_g, 10, 255, cv2.THRESH_BINARY)
inverted = cv2.bitwise_not(mask)
overlay = cv2.bitwise_not(overlay, overlay, mask=inverted)
base_alpha = 0.55
overlay_alpha = 1
for channel in range(3):
x,y,d = overlay.shape
base[:,:,channel] = (base[:,:,channel] * base_alpha + \
overlay[:,:,channel] * overlay_alpha * \
(1 - base_alpha)) / \
(base_alpha + overlay_alpha * (1 - base_alpha))
cv2.imwrite(outp_name, base)
###########################################################################
# Code below here is for drawing vector layers within the PDF #
# Leaving it in just because it was a pain to come up with the first time #
###########################################################################
if shape_overlays or point_overlays:
im = cairo.ImageSurface.create_from_png(outp_name)
ctx = cairo.Context(im)
if shape_overlays:
for shape_overlay in shape_overlays:
if shape_overlay:
shape_overlay = json.loads(shape_overlay)
if shape_overlay.get('geometry'):
shape_overlay = shape_overlay['geometry']
color = hex_to_rgb('#f06eaa')
coords = shape_overlay['coordinates'][0]
x, y = get_pixel_coords(coords[0], grid['zoom'], bmin_rx, bmin_ry)
ctx.move_to(x,y)
ctx.set_line_width(4.0)
red, green, blue = [float(c) for c in color]
ctx.set_source_rgba(red/255, green/255, blue/255, 0.3)
for p in coords[1:]:
x, y = get_pixel_coords(p, grid['zoom'], bmin_rx, bmin_ry)
ctx.line_to(x,y)
ctx.close_path()
ctx.fill()
ctx.set_source_rgba(red/255, green/255, blue/255, 0.5)
for p in coords[1:]:
x, y = get_pixel_coords(p, grid['zoom'], bmin_rx, bmin_ry)
ctx.line_to(x,y)
ctx.close_path()
ctx.stroke()
ctx.set_line_width(2.0)
if point_overlays:
for point_overlay in point_overlays:
point_overlay = json.loads(point_overlay)
color = hex_to_rgb(point_overlay['color'])
for p in point_overlay['points']:
if p[0] and p[1]:
pt = Point((float(p[0]), float(p[1])))
if bb_poly.contains(pt):
nx, ny = get_pixel_coords(p, grid['zoom'], bmin_rx, bmin_ry)
red, green, blue = [float(c) for c in color]
ctx.set_source_rgba(red/255, green/255, blue/255, 0.6)
ctx.arc(nx, ny, 5.0, 0, 50) # args: center-x, center-y, radius, ?, ?
ctx.fill()
ctx.arc(nx, ny, 5.0, 0, 50)
ctx.stroke()
im.write_to_png(outp_name)
scale = 1
# Crop image from center
center_point_x, center_point_y = latlon2xy(float(center_lat),
float(center_lon),
float(data['zoom']))
offset_x = (center_point_x - float(center_tile_x)) + 50
offset_y = (center_point_y - float(center_tile_y)) - 50
outp_image = cv2.imread(outp_name, -1)
pixels_up, pixels_across, channels = outp_image.shape
center_x, center_y = (pixels_across / 2) + offset_x, (pixels_up / 2) + offset_y
start_y, end_y = center_y - (page_height / 2), center_y + (page_height / 2)
start_x, end_x = center_x - (page_width / 2), center_x + (page_width / 2)
cv2.imwrite(outp_name, outp_image[math.floor(start_y):math.ceil(end_y), math.floor(start_x):math.ceil(end_x)])
if output == 'pdf':
outp_file_name = outp_name.rstrip('.png') + '.pdf'
pdf = cairo.PDFSurface(outp_file_name, page_width, page_height)
ctx = cairo.Context(pdf)
image = cairo.ImageSurface.create_from_png(outp_name)
ctx.set_source_surface(image)
ctx.paint()
pdf.finish()
elif output == 'jpeg':
outp_file_name = outp_name.rstrip('.png') + '.jpg'
jpeg = cv2.cvtColor(cv2.imread(outp_name, -1), cv2.COLOR_RGBA2RGB)
cv2.imwrite(outp_file_name, jpeg)
return outp_file_name
