def test_tangent_vector(self):
"vector tangent to unitcircle"
circ = Circle()
pos = Vec2D(1, 0)
self.assertTrue(circ.tangent_vector(pos, 1) == Vec2D(0, 1))
def test_overlapping_intersection(self):
"test that overlapping counts as intersection"
tan1 = Tangent(Vec2D(-1, 0), None, None, Vec2D(1, 0), None, None)
tan2 = Tangent(Vec2D(-2, 0), None, None, Vec2D(0, 0), None, None)
self.assertTrue(tan1.intersect(tan2))
self.assertTrue(tan2.intersect(tan1))
def test_sort_points_singleelement(self):
"test sort_points_on_circle one element list"
sorted_list = pp.sort_points_on_circle([Vec2D(1, 0)], Circle())
self.assertTrue(len(sorted_list) == 1)
self.assertTrue(sorted_list[0] == Vec2D(1, 0))
def test_general_intersection(self):
"test non intersecting segments"
tan1 = Tangent(Vec2D(1, 0), None, None, Vec2D(2, 0), None, None)
tan2 = Tangent(Vec2D(0, -2), None, None, Vec2D(0, -1), None, None)
self.assertFalse(tan1.intersect(tan2))
self.assertFalse(tan2.intersect(tan1))
def test_parallel_intersection(self):
"test that parallel segments don't intersect"
tan1 = Tangent(Vec2D(1, 0), None, None, Vec2D(2, 0), None, None)
tan2 = Tangent(Vec2D(1, 1), None, None, Vec2D(2, 1), None, None)
self.assertFalse(tan1.intersect(tan2))
self.assertFalse(tan2.intersect(tan1))
def test_tangents_containing_circle(self):
"test circle tangents when one circle contains another"
circle1 = Circle(Vec2D(), 10)
circle2 = Circle(Vec2D(), 5)
self.assertTrue(len(circle1.tangent_circle(circle2)) == 0)
self.assertTrue(len(circle2.tangent_circle(circle1)) == 0)
def test_radial_acc(self):
"test radial_acc method"
circle = Circle()
seg = CircleSegment(Vec2D(1, 0), Vec2D(0, 1), circle, 1)
self.assertTrue(seg.radial_acc(Vec2D(1, 0), 1) == Vec2D(-1, 0))
def test_simple_intersection(self):
"test simple 1 point intersection"
tan1 = Tangent(Vec2D(-1, 0), None, None, Vec2D(1, 0), None, None)
tan2 = Tangent(Vec2D(0, -1), None, None, Vec2D(0, 1), None, None)
self.assertTrue(tan1.intersect(tan2))
self.assertTrue(tan2.intersect(tan1))
def test_contains_tangent(self):
"test start and end inside circle"
circle = Circle()
tangent = Tangent(Vec2D(0, -0.5), None, None, Vec2D(0, 0.5), None,
None)
self.assertFalse(circle.intersects_tangent(tangent))
def test_can_transform_speed(self):
traj = [(Vec2D(), Vec2D(0., 1.), Vec2D(), 0.3)]
t = convert_from_molly(traj, start_time=10., sampling_time=0.1)
t = t.points[0]
self.assertAlmostEqual(t.speed, 1., 5)
self.assertAlmostEqual(t.theta, pi / 2, 3)
def test_can_transform_acceleration(self):
# Circular trajectory, a = v ** 2 / r, v = 2, r = 1
# -> a = 4 m / s^2, omega = 2 rad / s
traj = [(Vec2D(), Vec2D(2., 0.), Vec2D(0., 4), 0.3)]
t = convert_from_molly(traj, start_time=10., sampling_time=0.1)
t = t.points[0]
self.assertAlmostEqual(t.omega, 2., 3)
def test_orientangle_simple(self):
vec1 = Vec2D(1, 0)
vec2 = Vec2D(math.sqrt(2.0) / 2, math.sqrt(2.0) / 2)
self.assertAlmostEqual(vec1.oriented_angle(vec2), 0.25 * math.pi)
self.assertAlmostEqual(vec2.oriented_angle(vec1), 1.75 * math.pi)
def test_colinear_nonoverlapping(self):
"test that colinear nonoverlapping doesn't count as intersection"
tan1 = Tangent(Vec2D(1, 0), None, None, Vec2D(2, 0), None, None)
tan2 = Tangent(Vec2D(-2, 0), None, None, Vec2D(0, 0), None, None)
self.assertFalse(tan1.intersect(tan2))
self.assertFalse(tan2.intersect(tan1))
def __init__(self,
max_acc=1.6,
max_v=0.6,
time_resolution=0.1,
static_poly_obs=[STAIRS],
static_circ_obs=[],
obs_min_r=0.1,
playground_dim=(3.0, 2.0)):
self.max_acc = max_acc
self.max_v = max_v
self.obs_min_r = obs_min_r
(self.width, self.height) = playground_dim
self.static_poly_obs = static_poly_obs
self.static_circ_obs = static_circ_obs
self.time_resolution = time_resolution
corner1 = Vec2D(0, 0)
corner2 = Vec2D(self.width, 0)
corner3 = Vec2D(self.width, self.height)
corner4 = Vec2D(0, self.height)
border1 = Tangent(corner1, None, 0, corner2, None, 0)
border2 = Tangent(corner2, None, 0, corner3, None, 0)
border3 = Tangent(corner3, None, 0, corner4, None, 0)
border4 = Tangent(corner4, None, 0, corner1, None, 0)
self.bounds = [border1, border2, border3, border4]
def test_orientation_colinear(self):
"test that orientation returns (almost) zero for colinear points"
vec1 = Vec2D(0, 0)
vec2 = Vec2D(1, 0)
vec3 = Vec2D(2, 0)
self.assertAlmostEqual(0, Vec2D.orientation(vec1, vec2, vec3))
def test_self_tangent(self):
"test if tangent is part of circle"
circle1 = Circle()
circle2 = Circle(Vec2D(0, 2), 1)
tan = Tangent(Vec2D(0, 1), circle1, None, Vec2D(0, 2), circle2, None)
self.assertFalse(circle1.intersects_tangent(tan))
self.assertFalse(circle2.intersects_tangent(tan))
def test_next_pos(self):
"test next_pos method"
circle = Circle()
seg = CircleSegment(Vec2D(1, 0), Vec2D(0, 1), circle, 1)
self.assertTrue(seg.next_pos(Vec2D(1, 0), pi * 0.5) == Vec2D(0, 1))
def tangent_vector(self, point, orientation):
"tangent vector to self at point with orientation"
vec = (point - self.pos).normalized()
if orientation < 0:
return Vec2D(vec.pos_y, -vec.pos_x)
else:
return Vec2D(-vec.pos_y, vec.pos_x)
def test_negative_orientation(self):
"test that orientation returns postive value for positively oriented \
points"
vec1 = Vec2D(0, 0)
vec2 = Vec2D(0, 1)
vec3 = Vec2D(1, 0)
self.assertTrue(Vec2D.orientation(vec1, vec2, vec3) < 0)
def test_approximate_equality(self):
"vectors within Vec2D.EPSILON distance of each other should count as \
equal"
vec = Vec2D(1, 1)
approx = vec + Vec2D(1, 0) * (0.9 * Vec2D.EPSILON)
self.assertTrue(vec == approx)
self.assertFalse(vec != approx)
def test_neighs_circle_one_element(self):
"single point has no neighbours"
points = [Vec2D(1, 0)]
circle = Circle()
(pos, neg) = pp.neighbours_on_circle(points, circle, Vec2D(1, 0))
self.assertTrue(pos is None)
self.assertTrue(neg is None)
def test_rotate_around_point(self):
"rotate around some point"
vec = Vec2D()
center = Vec2D(-1, 0)
rot = vec.rotate(math.pi / 4, center)
self.assertAlmostEqual(1.0 / math.sqrt(2) - 1, rot.pos_x)
self.assertAlmostEqual(1.0 / math.sqrt(2), rot.pos_y)
def test_can_transform_position_timestamp(self):
traj = [(Vec2D(1., 2.), Vec2D(), Vec2D(), 0.3)]
traj = convert_from_molly(traj, start_time=10., sampling_time=0.1)
self.assertEqual(traj.start_time, 10.)
self.assertEqual(traj.sampling_time, .1)
t = traj.points[0]
self.assertEqual(t.x, 1.)
self.assertEqual(t.y, 2.)
def test_equality(self):
"test equality (and implicitely non-equality) of cirlces"
circle1 = Circle(Vec2D(1, 0), 1.0)
circle2 = Circle(Vec2D(1, 0.9 * Vec2D.EPSILON),
1.0 + 0.9 * Vec2D.EPSILON)
self.assertTrue(circle1 == circle2)
self.assertTrue(circle2 == circle1)
# equality is symmetric
self.assertFalse(circle1 != circle2)
self.assertFalse(circle2 != circle1)
def test_length(self):
"test length method"
circle = Circle()
inv_rt2 = sqrt(2.0) / 2.0
seg1 = CircleSegment(Vec2D(1, 0), Vec2D(inv_rt2, inv_rt2), circle, 1)
seg2 = CircleSegment(Vec2D(1, 0), Vec2D(inv_rt2, inv_rt2), circle, -1)
self.assertAlmostEqual(seg1.length(), pi * 0.25, delta=Vec2D.EPSILON)
self.assertAlmostEqual(seg2.length(), pi * 1.75, delta=Vec2D.EPSILON)
def test_translate_zero(self):
"test that tangent equals itself after translation of (0, 0)"
circ1 = Circle()
circ2 = Circle(Vec2D(4, 0))
tan = Tangent(Vec2D(0, 1), circ1, 1, Vec2D(4, 1), circ2, -1)
translated = tan + Vec2D()
self.assertTrue(tan == translated)
self.assertTrue(translated == tan)
def test_start_circles(self):
"test generation of starting circles"
pos = Vec2D()
direction = Vec2D(0, 1)
pos_circ = Circle(Vec2D(-1, 0), 1)
neg_circ = Circle(Vec2D(1, 0), 1)
(test_pos, test_neg) = pp.get_start_circles(pos, direction, 1)
self.assertTrue(test_pos == pos_circ)
self.assertTrue(test_neg == neg_circ)
def test_cross_product(self):
"test the cross product of 2 vectors"
posx1 = 1.0
posy1 = 2.0
posx2 = 3.0
posy2 = 4.0
vec1 = Vec2D(posx1, posy1)
vec2 = Vec2D(posx2, posy2)
expected = posx1 * posy2 - posy1 * posx2
self.assertAlmostEqual(expected, vec1.cross(vec2))
self.assertAlmostEqual(vec1.cross(vec2), -vec2.cross(vec1))
def _circle_tangents(self, other, invert_second_radius):
"compute tangents as explained in \
https://en.wikipedia.org/wiki/Tangent_lines_to_circles#Analytic_geometry"
dx = other.pos.pos_x - self.pos.pos_x
dy = other.pos.pos_y - self.pos.pos_y
if not invert_second_radius:
dr = other.radius - self.radius
else:
dr = - other.radius - self.radius
d = sqrt(dx*dx + dy*dy)
x = dx/d
y = dy/d
r = dr/d
a1 = r*x - y*sqrt(1 - r*r)
b1 = r*y + x*sqrt(1 - r*r)
c1 = self.radius - (a1*self.pos.pos_x + b1*self.pos.pos_y)
a2 = r*x + y*sqrt(1 - r*r)
b2 = r*y - x*sqrt(1 - r*r)
c2 = self.radius - (a2*self.pos.pos_x + b2*self.pos.pos_y)
x11 = (b1*(b1*self.pos.pos_x - a1*self.pos.pos_y) - a1*c1)
y11 = (a1*(-b1*self.pos.pos_x + a1*self.pos.pos_y) - b1*c1)
x12 = (b1*(b1*other.pos.pos_x - a1*other.pos.pos_y) - a1*c1)
y12 = (a1*(-b1*other.pos.pos_x + a1*other.pos.pos_y) - b1*c1)
x21 = (b2*(b2*self.pos.pos_x - a2*self.pos.pos_y) - a2*c2)
y21 = (a2*(-b2*self.pos.pos_x + a2*self.pos.pos_y) - b2*c2)
x22 = (b2*(b2*other.pos.pos_x - a2*other.pos.pos_y) - a2*c2)
y22 = (a2*(-b2*other.pos.pos_x + a2*other.pos.pos_y) - b2*c2)
start1 = Vec2D(x11, y11)
end1 = Vec2D(x12, y12)
orient11 = Vec2D.orientation(end1, start1, self.pos)
orient12 = Vec2D.orientation(start1, end1, other.pos)
start2 = Vec2D(x21, y21)
end2 = Vec2D(x22, y22)
orient21 = Vec2D.orientation(end2, start2, self.pos)
orient22 = Vec2D.orientation(start2, end2, other.pos)
tan1 = Tangent(start1, self, orient11, end1, other, orient12)
tan2 = Tangent(start2, self, orient21, end2, other, orient22)
return [tan1, tan2]
def test_neighs_circle_general(self):
"point has two distinct neighbours"
point1 = Vec2D(1, 0)
point2 = point1.rotate(pi / 2)
point3 = point1.rotate(pi)
point4 = point1.rotate(-pi / 2)
unsorted = [point2, point4, point3, point1]
(pos, neg) = pp.neighbours_on_circle(unsorted, Circle(), Vec2D(1, 0))
self.assertTrue(pos == point2)
self.assertTrue(neg == point4)
def test_all_tangents_simple1(self):
"test that all_tangents returns all tangents of two circles \
when only those 2 circles are present"
circ1 = Circle(Vec2D(-2, 0), 1)
circ2 = Circle(Vec2D(2, 0), 1)
tangents = circ1.tangent_circle(circ2)
to_test = pp.all_tangents([circ1, circ2], [])
verificator = all(test in tangents for test in to_test)
self.assertTrue(verificator)
def test_negative_orientation(self):
"test that orientation returns postive value for positively oriented \
points"
vec1 = Vec2D(0, 0)
vec2 = Vec2D(0, 1)
vec3 = Vec2D(1, 0)
self.assertTrue(Vec2D.orientation(vec1, vec2, vec3) < 0)
def test_orientation_colinear(self):
"test that orientation returns (almost) zero for colinear points"
vec1 = Vec2D(0, 0)
vec2 = Vec2D(1, 0)
vec3 = Vec2D(2, 0)
self.assertAlmostEqual(0, Vec2D.orientation(vec1, vec2, vec3))
def sort_points_on_circle(points, circle):
"sort points by their angle on the circle"
if not points:
return []
reference = points[0]
rest = points[1: len(points)]
sorted_points = sorted(rest,
key=lambda p:
Vec2D.orientation(reference, circle.pos, p))
return [reference] + sorted_points
