def from_points(cls, points):
"""Create a line segment from one or more collinear points. The first
point is assumed to be the anchor. The order of the remaining points
is unimportant, however they must all be collinear. The furthest
point from the anchor determines the line segment's vector.
:param points: Iterable of at least 2 distinct points.
"""
points = iter(points)
try:
start = end = planar.Vec2(*points.next())
except StopIteration:
raise ValueError("Expected iterable of 1 or more points")
furthest = 0.0
pt_vectors = []
for p in points:
p = planar.Vec2(*p)
dist = (p - start).length2
if dist > furthest:
furthest = dist
end = p
pt_vectors.append(p)
segment = _LinearGeometry.__new__(cls)
if end != start:
segment.vector = end - start
else:
# degenerate case
segment.direction = (1, 0)
segment.length = 0.0
segment._anchor = start
for p in pt_vectors:
if not segment.contains_point(p):
raise ValueError("All points provided must be collinear")
return segment
def from_shapes(cls, shapes):
"""Creating a bounding box that completely encloses all of the
shapes provided.
"""
shapes = iter(shapes)
try:
shape = next(shapes)
except StopIteration:
raise ValueError(
("BoundingBox.from_shapes(): requires at least one shape"))
min_x, min_y = shape.bounding_box.min_point
max_x, max_y = shape.bounding_box.max_point
for shape in shapes:
x, y = shape.bounding_box.min_point
if x < min_x:
min_x = x
if y < min_y:
min_y = y
x, y = shape.bounding_box.max_point
if x > max_x:
max_x = x
if y > max_y:
max_y = y
box = object.__new__(cls)
box._min = planar.Vec2(min_x, min_y)
box._max = planar.Vec2(max_x, max_y)
return box
def _pt_tangents(self, point):
"""Return the pair of tangent points for the given exterior point.
This general algorithm works for all polygons in O(n) time.
"""
px, py = point
left_tan = right_tan = self[0]
verts = iter(self)
v0_x, v0_y = self[-2]
v1_x, v1_y = self[-1]
prev_turn = (v1_x - v0_x) * (py - v0_y) - (px - v0_x) * (v1_y - v0_y)
v0_x = v1_x
v0_y = v1_y
for v1_x, v1_y in self:
next_turn = (v1_x - v0_x) * (py - v0_y) - (px - v0_x) * (v1_y -
v0_y)
if prev_turn <= 0.0 and next_turn > 0.0:
if ((v0_x - px) * (right_tan.y - py) - (right_tan.x - px) *
(v0_y - py) >= 0.0):
right_tan = planar.Vec2(v0_x, v0_y)
elif prev_turn > 0.0 and next_turn <= 0.0:
if ((v0_x - px) * (left_tan.y - py) - (left_tan.x - px) *
(v0_y - py) <= 0.0):
left_tan = planar.Vec2(v0_x, v0_y)
v0_x = v1_x
v0_y = v1_y
prev_turn = next_turn
return left_tan, right_tan
def test_set_epsilon():
import planar
old_e = planar.EPSILON
assert not planar.Vec2(0, 0).almost_equals((0.01, 0))
try:
planar.set_epsilon(0.02)
assert_equal(planar.EPSILON, 0.02)
assert_equal(planar.EPSILON2, 0.0004)
assert planar.Vec2(0, 0).almost_equals((0.01, 0))
finally:
planar.set_epsilon(old_e)
assert_equal(planar.EPSILON, old_e)
assert_equal(planar.EPSILON2, old_e**2)
assert not planar.Vec2(0, 0).almost_equals((0.01, 0))
def __mul__(self, other):
"""Apply the transform using matrix multiplication, creating a
resulting object of the same type. A transform may be applied to
another transform, a vector, vector array, or shape.
:param other: The object to transform.
:type other: Affine, :class:`~planar.Vec2`,
:class:`~planar.Vec2Array`, :class:`~planar.Shape`
:rtype: Same as ``other``
"""
sa, sb, sc, sd, se, sf, _, _, _ = self
if isinstance(other, Affine):
oa, ob, oc, od, oe, of, _, _, _ = other
return tuple.__new__(
Affine,
(sa * oa + sb * od, sa * ob + sb * oe, sa * oc + sb * of + sc,
sd * oa + se * od, sd * ob + se * oe, sd * oc + se * of + sf,
0.0, 0.0, 1.0))
elif hasattr(other, 'from_points'):
# Point/vector array
Point = planar.Point
return other.from_points(
Point(px * sa + py * sd + sc, px * sb + py * se + sf)
for px, py in other)
else:
try:
vx, vy = other
except Exception:
return NotImplemented
return planar.Vec2(vx * sa + vy * sd + sc, vx * sb + vy * se + sf)
def centroid(self):
"""The geometric center point of the polygon. This point only exists
for simple polygons. For non-simple polygons it is ``None``. Note
in concave polygons, this point may lie outside of the polygon itself.
If the centroid is unknown, it is calculated from the vertices and
cached. If the polygon is known to be simple, this takes O(n) time. If
not, then the simple polygon check is also performed, which has an
expected complexity of O(n log n).
"""
if self._centroid is _unknown:
if self.is_simple:
# Compute the centroid using by summing the centroids
# of triangles made from each edge with vertex[0] weighted
# (positively or negatively) by each triangle's area
a = self[0]
b = self[1]
total_area = 0.0
centroid = planar.Vec2(0, 0)
for i in range(2, len(self)):
c = self[i]
area = ((b[0] - a[0]) * (c[1] - a[1]) - (c[0] - a[0]) *
(b[1] - a[1]))
centroid += (a + b + c) * area
total_area += area
b = c
self._centroid = centroid / (3.0 * total_area)
else:
self._centroid = None
return self._centroid
def point_right(self, point):
"""Return True if the specified point is in the space
to the right of, but not behind the ray.
"""
to_point = planar.Vec2(*point) - self._anchor
return (self._direction.dot(to_point) > -planar.EPSILON
and self._normal.dot(to_point) >= planar.EPSILON)
def regular(cls, vertex_count, radius, center=(0, 0), angle=0):
"""Create a regular polygon with the specified number of vertices
radius distance from the center point. Regular polygons are
always convex.
:param vertex_count: The number of vertices in the polygon.
Must be >= 3.
:type vertex_count: int
:param radius: distance from vertices to center point.
:type radius: float
:param center: The center point of the polygon. If omitted,
the polygon will be centered on the origin.
:type center: Vec2
:param angle: The starting angle for the vertices, in degrees.
:type angle: float
"""
cx, cy = center
angle_step = 360.0 / vertex_count
verts = []
for i in range(vertex_count):
x, y = cos_sin_deg(angle)
verts.append((x * radius + cx, y * radius + cy))
angle += angle_step
poly = cls(verts, is_convex=True)
poly._centroid = planar.Vec2(*center)
poly._max_r = radius
poly._max_r2 = radius * radius
poly._min_r = min_r = ((poly[0] + poly[1]) * 0.5 - center).length
poly._min_r2 = min_r * min_r
poly._dupe_verts = False
return poly
def point_right(self, point):
"""Return True if the specified point is in the space
to the right of, but not behind the line segment.
"""
to_point = planar.Vec2(*point) - self._anchor
along = self._direction.dot(to_point)
return (self.length + planar.EPSILON > along > -planar.EPSILON
and self._normal.dot(to_point) >= planar.EPSILON)
def _init_min_max(self, points):
points = iter(points)
try:
min_x, min_y = max_x, max_y = next(points)
except StopIteration:
raise ValueError("BoundingBox() requires at least one point")
for x, y in points:
if x < min_x:
min_x = x * 1.0
elif x > max_x:
max_x = x * 1.0
if y < min_y:
min_y = y * 1.0
elif y > max_y:
max_y = y * 1.0
self._min = planar.Vec2(min_x, min_y)
self._max = planar.Vec2(max_x, max_y)
def vector(self, value):
vector = planar.Vec2(*value)
length = vector.length
if length:
self.direction = vector
else:
self.direction = (1, 0)
self.length = vector.length
def point_behind(self, point):
"""Return True if the specified point is behind the anchor point with
respect to the direction of the ray. In other words, the angle
between the ray direction and the vector pointing from the ray's
anchor to the given point is greater than 90 degrees.
"""
to_point = planar.Vec2(*point) - self._anchor
return self.direction.dot(to_point) <= -planar.EPSILON
def distance_to(self, point):
"""Return the distance between the given point and the ray."""
to_point = planar.Vec2(*point) - self._anchor
if self.direction.dot(to_point) >= 0.0:
# Point "beside" ray
return abs(to_point.dot(self._normal))
else:
# Point "behind" ray
return to_point.length
def reflect(self, point):
"""Reflect a point across the line.
:param point: The point to reflect.
:type point: Vec2
"""
point = planar.Vec2(*point)
offset_distance = point.dot(self._normal) - self.offset
return point - 2.0 * self._normal * offset_distance
def from_points(cls, points):
"""Create a line from two or more collinear points. The direction of
the line is derived from the first two distinct points, the order of
the remaining points is unimportant.
:param points: Iterable of at least 2 distinct points.
"""
points = iter(points)
try:
start = end = planar.Vec2(*points.next())
while end == start:
end = planar.Vec2(*points.next())
except StopIteration:
raise ValueError("Expected iterable of 2 or more distinct points")
line = _LinearGeometry.__new__(cls)
line.direction = end - start
line.offset = start.dot(line.normal)
for p in points:
if not line.contains_point(p):
raise ValueError("All points provided must be collinear")
return line
def distance_to(self, point):
"""Return the signed distance from the line to the specified point.
The sign indicates which half-plane contains the point. If the
distance is negative, the point is in the "left" half plane with
respect to the line, if it is positive, the point is in the "right"
half plane.
:param point: The point to measure the distance to.
:type point: Vec2
"""
point = planar.Vec2(*point)
return point.dot(self._normal) - self.offset
def wallsPrediction(data):
speed_vector = planar.Vec2(estimation.ball_x_pred - data.ball_x,
estimation.ball_y_pred - data.ball_y)
speed_vector = planar.Vec2.normalized(speed_vector)
ball_position = planar.Vec2(data.ball_x, data.ball_y)
if (speed_vector.x != 0) and (speed_vector.y != 0):
t_x_pos = (0.65 - ball_position.x) / speed_vector.x
t_y_pos = (0.75 - ball_position.y) / speed_vector.y
t_x_neg = (-0.65 - ball_position.x) / speed_vector.x
t_y_neg = (-0.75 - ball_position.y) / speed_vector.y
time_list = [(t_x_pos), (t_y_pos), (t_x_neg), (t_y_neg)]
time = min([i for i in time_list if i > 0])
walls_y = speed_vector.y * time + ball_position.y
walls_x = speed_vector.x * time + ball_position.x
elif speed_vector.x != 0:
t_x_pos = (0.65 - ball_position.x) / speed_vector.x
t_x_neg = (-0.65 - ball_position.x) / speed_vector.x
time_list = [(t_x_pos), (t_x_neg)]
time = min([i for i in time_list if i > 0])
walls_y = ball_position.y
walls_x = ball_position.x + speed_vector.y * time
elif speed_vector.y != 0:
t_y_pos = (0.75 - ball_position.x) / speed_vector.x
t_y_neg = (-0.75 - ball_position.x) / speed_vector.x
time_list = [(t_y_pos), (t_y_neg)]
time = min([i for i in time_list if i > 0])
walls_x = ball_position.x
walls_y = ball_position.y + speed_vector.y * time
else:
walls_x = 0
walls_y = 0
return walls_x, walls_y
def from_points(cls, points):
"""Create a ray from two or more collinear points. The direction of
the ray is derived from the first two distinct points, with the first
point assumed to be the anchor. The order of the remaining points is
unimportant, however they must all be on the ray.
:param points: Iterable of at least 2 distinct points.
"""
points = iter(points)
try:
start = end = planar.Vec2(*points.next())
while end == start:
end = planar.Vec2(*points.next())
except StopIteration:
raise ValueError("Expected iterable of 2 or more distinct points")
ray = _LinearGeometry.__new__(cls)
ray.direction = end - start
ray.anchor = start
for p in points:
if not ray.contains_point(p):
raise ValueError("All points provided must be collinear")
return ray
def distance_to(self, point):
"""Return the distance between the given point and the line segment."""
point = planar.Vec2(*point)
to_point = point - self._anchor
along = self.direction.dot(to_point)
if along < 0.0:
# Point "behind"
return to_point.length
if along > self.length:
# Point "ahead"
return (point - self.end).length
else:
# Point "beside"
return abs(to_point.dot(self._normal))
def star(cls, peak_count, radius1, radius2, center=(0, 0), angle=0):
"""Create a radial pointed star polygon with the specified number
of peaks.
:param peak_count: The number of peaks. The resulting polygon will
have twice this number of vertices. Must be >= 2.
:type peak_count: int
:param radius1: The peak or valley vertex radius. A vertex
is aligned on ``angle`` with this radius.
:type radius1: float
:param radius2: The alternating vertex radius.
:type radius2: float
:param center: The center point of the polygon. If omitted,
the polygon will be centered on the origin.
:type center: Vec2
:param angle: The starting angle for the vertices, in degrees.
:type angle: float
"""
if peak_count < 2:
raise ValueError("star polygon must have a minimum of 2 peaks")
cx, cy = center
angle_step = 180.0 / peak_count
verts = []
for i in range(peak_count):
x, y = cos_sin_deg(angle)
verts.append((x * radius1 + cx, y * radius1 + cy))
angle += angle_step
x, y = cos_sin_deg(angle)
verts.append((x * radius2 + cx, y * radius2 + cy))
angle += angle_step
is_simple = (radius1 > 0.0) == (radius2 > 0.0)
poly = cls(verts,
is_convex=(radius1 == radius2),
is_simple=is_simple or None)
if is_simple:
poly._centroid = planar.Vec2(*center)
poly._max_r = max_r = max(abs(radius1), abs(radius2))
poly._max_r2 = max_r * max_r
if (radius1 >= 0.0) == (radius2 >= 0.0):
if not poly.is_convex:
poly._min_r = min_r = min(abs(radius1), abs(radius2))
poly._min_r2 = min_r * min_r
else:
poly._min_r = min_r = ((poly[0] + poly[1]) * 0.5 -
center).length
poly._min_r2 = min_r * min_r
if radius1 > 0.0 and radius2 > 0.0:
poly._dupe_verts = False
return poly
def project(self, point):
"""Compute the projection of a point onto the ray. This
is the closest point on the ray to the specified point.
:param point: The point to project.
:type point: Vec2
"""
to_point = planar.Vec2(*point) - self._anchor
parallel = self.direction.project(to_point)
if parallel.dot(self.direction) > -planar.EPSILON:
# Point "beside" ray
return parallel + self._anchor
else:
# Point "behind" ray
return self._anchor
def get_laser_pts(self, scan):
pts = list()
for i, r in enumerate(scan.ranges):
if r < scan.range_max and r > scan.range_min:
angle = scan.angle_min + i * scan.angle_increment
x = r * math.cos(angle)
y = r * math.sin(angle)
pt = self.make_point_stamped(x, y, scan.header.frame_id)
try:
pt_tf = self.tf_buffer.transform(pt, self.map_frame)
except tf2_ros.LookupException:
rospy.logerr("TF tree is not ready yet")
return None
cur_pt = planar.Vec2(pt_tf.point.x, pt_tf.point.y)
pts.append(cur_pt)
return pts
def inflate(self, amount):
"""Return a new box resized from this one. The new
box has its size changed by the specified amount,
but remains centered on the same point.
:param amount: The quantity to add to the width and
height of the box. A scalar value changes
both the width and height equally. A vector
will change the width and height independently.
Negative values reduce the size accordingly.
:type amount: float or :class:`~planar.Vec2`
"""
try:
dx, dy = amount
except (TypeError, ValueError):
dx = dy = amount * 1.0
dv = planar.Vec2(dx, dy) / 2.0
return self.from_points((self._min - dv, self._max + dv))
def project(self, point):
"""Compute the projection of a point onto the line segment. This
is the closest point on the segment to the specified point.
:param point: The point to project.
:type point: Vec2
"""
to_point = planar.Vec2(*point) - self._anchor
parallel = self.direction.project(to_point)
along = parallel.dot(self.direction)
if along <= -planar.EPSILON:
# Point "behind"
return self._anchor
elif along >= self.length + planar.EPSILON:
# Point "ahead"
return self.end
else:
# Point "beside"
return parallel + self._anchor
def ellipse_1(mean, covar, confidence):
'''Return an ellipse of the given confidence for the given Gaussian.'''
# Draw a circle of radius 1 around the origin.
cir = My_Polygon.regular(POLYGON_EDGES, radius=1)
# Compute ellipse parameters.
(eivals, eivecs) = np.linalg.eigh(covar)
crit = chisq_crit2(confidence)
scale0 = math.sqrt(crit * eivals[0])
scale1 = math.sqrt(crit * eivals[1])
angle = planar.Vec2(eivecs[0][0], eivecs[0][1]).angle
# Transform the circle into an ellipse. NOTE: Originally, I set this up to
# build a full transformation matrix and then multiply the polygon by that
# composite matrix. However, the order of operations was really wierd --
# e.g., t * s * r works, even though the transformation order is s, r, t. I
# think this is due to some mismatch between the associativity of the
# multiplication operator and calls to the __mult__() special method.
# Anyway, this way is a lot easier to follow.
return (planar.Affine.translation(mean) *
(planar.Affine.rotation(angle) * (planar.Affine.scale(
(scale0, scale1)) * cir))).as_geos
def load_graph(nyc_dir, fn_edge_times, hour):
# print "Making it a graph..."
sts = np.loadtxt(nyc_dir + "points.csv", delimiter=",")
edges = np.loadtxt(nyc_dir + "edges.csv", delimiter=",")
times = np.loadtxt(nyc_dir + fn_edge_times, delimiter=",")
G = nx.DiGraph()
for i in xrange(sts.shape[0]):
G.add_node(i, lat=sts[i][1], lon=sts[i][2])
for i in xrange(edges.shape[0]):
t = times[i][hour + 1]
G.add_edge(edges[i][1] - 1, edges[i][2] - 1, weight=t)
poly = planar.Polygon.from_points(common.nyc_poly)
for i in G.nodes():
data = G.node[i]
contains = poly.contains_point(planar.Vec2(data["lon"], data["lat"]))
if not contains:
G.remove_node(i)
# print "Finding the largest connected component subgraph"
G_final = max(nx.strongly_connected_component_subgraphs(G), key=len)
return G_final, np.fliplr(sts[:, 1:])
def simplify_by_degree(G, max_distance):
G_simple = G
poly = planar.Polygon.from_points(common.nyc_poly)
for n in G_simple.nodes():
vec = planar.Vec2(G.node[n]["data"].lon, G.node[n]["data"].lat)
if not poly.contains_point(vec):
G_simple.remove_node(n)
for n in G_simple.nodes():
dead_node = G_simple.out_degree(n) == 0 or G_simple.in_degree(n) == 0
if G_simple.in_degree(n) == 1 and G_simple.out_degree(n) == 1:
weight_in_n = G[G.predecessors(n)[0]][n]['weight']
weight_out_n = G[n][G.successors(n)[0]]['weight']
weight_new = weight_in_n + weight_out_n
G_simple.add_edge(G.predecessors(n)[0],
G.successors(n)[0],
weight=weight_new)
G_simple.remove_node(n)
elif G_simple.has_node(n) and dead_node:
G_simple.remove_node(n)
G_final = max(nx.strongly_connected_component_subgraphs(G_simple), key=len)
return G_final
def test_center(self):
import planar
box = self.BoundingBox([(-3, -1), (1, 4)])
assert isinstance(box.center, planar.Vec2)
assert_equal(box.center, planar.Vec2(-1, 1.5))
assert_equal(self.BoundingBox([(8, 12)]).center, planar.Vec2(8, 12))
def column_vectors(self):
"""The values of the transform as three 2D column vectors"""
a, b, c, d, e, f, _, _, _ = self
return planar.Vec2(a, d), planar.Vec2(b, e), planar.Vec2(c, f)
def _ahull_partition_points(hull, points, p0, p1):
"""Partition the points 'above' p0->p1 to compute the sub-hull"""
# Find point furthest from line p0->p1 as partition point
furthest = -1.0
p0_x, p0_y = p0
pline_dx = p1[0] - p0[0]
pline_dy = p1[1] - p0[1]
for p in points:
dist = pline_dx * (p[1] - p0_y) - (p[0] - p0_x) * pline_dy
if dist > furthest:
furthest = dist
partition_point = p
partition_point = planar.Vec2(*partition_point)
# Compute the triangle partition_point->p0->p1
# in barycentric coordinates
# All points inside this triangle are not in the hull
# divide the remaining points into left and right sets
left_points = []
right_points = []
add_left = left_points.append
add_right = right_points.append
v0 = p0 - partition_point
v1 = p1 - partition_point
dot00 = v0.length2
dot01 = v0.dot(v1)
dot11 = v1.length2
denom = (dot00 * dot11 - dot01 * dot01)
# If denom is zero, the triangle has no area and
# all points lie on the partition line
# and thus can be culled
if denom:
inv_denom = 1.0 / denom
for p in points:
v2 = p - partition_point
dot02 = v0.dot(v2)
dot12 = v1.dot(v2)
u = (dot11 * dot02 - dot01 * dot12) * inv_denom
v = (dot00 * dot12 - dot01 * dot02) * inv_denom
# Since the partition point is the furthest from p0->p1
# u and v cannot both be negative
# Note the partition point is discarded here
if v < 0.0:
add_left(p)
elif u < 0.0:
add_right(p)
left_count = len(left_points)
right_count = len(right_points)
# Heuristic to determine if we should continue to partition
# recursively, or complete the sub-hull via a sorted scan.
# The more points culled by this partition, the greater
# the chance we will partition further. If paritioning
# culled few points, it is likely that a sorted scan
# will be the more efficient algorithm. Note the scaling
# factor here is not particularly sensitive.
max_partition = (len(points) - left_count - right_count) * 4
if left_count <= 1:
# Trivial partition
hull.append(p0)
hull.extend(left_points)
elif left_count <= max_partition:
_ahull_partition_points(hull, left_points, p0, partition_point)
else:
_ahull_sort_points(hull, left_points, p0, partition_point)
if right_count <= 1:
# Trivial partition
hull.append(partition_point)
hull.extend(right_points)
elif right_count <= max_partition:
_ahull_partition_points(hull, right_points, partition_point, p1)
else:
_ahull_sort_points(hull, right_points, partition_point, p1)
