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_point_containment(self):
"test point inside circle"
point = Vec2D()
circle = Circle()
self.assertTrue(circle.contains_point(point))
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_point_on_border(self):
"test point exactly on circle border"
point = Vec2D(1, 0)
circle = Circle()
self.assertTrue(circle.contains_point(point))
def test_point_not_inside(self):
"test point not inside circle"
point = Vec2D(2, 0)
circle = Circle()
self.assertFalse(circle.contains_point(point))
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_circle_circle_intersection(self):
"test intersection of intersecting circles"
circ1 = Circle()
circ2 = Circle(Vec2D(0.5, 0))
self.assertTrue(circ1.intersects(circ2))
self.assertTrue(circ2.intersects(circ1))
def test_not_intersecting_circles(self):
"test intersection of not intersecting circles"
circ1 = Circle()
circ2 = Circle(Vec2D(10, 0))
self.assertFalse(circ1.intersects(circ2))
self.assertFalse(circ2.intersects(circ1))
def test_intersection_equal_circles(self):
"test intersection of equal circles"
circ1 = Circle()
circ2 = Circle()
self.assertTrue(circ1.intersects(circ2))
self.assertTrue(circ2.intersects(circ1))
def test_touching_intersection(self):
"test intersection of tangent circles"
circ1 = Circle()
circ2 = Circle(Vec2D(1, 0))
self.assertTrue(circ1.intersects(circ2))
self.assertTrue(circ2.intersects(circ1))
def test_containing_circles(self):
"test circle containment"
circ1 = Circle()
circ2 = Circle(Vec2D(), 0.5)
self.assertTrue(circ1.contains_circle(circ2))
self.assertFalse(circ2.contains_circle(circ1))
def test_containment_equal_circles(self):
"if two circles are equal one contains the other and vice versa"
circ1 = Circle()
circ2 = Circle()
self.assertTrue(circ1.contains_circle(circ2))
self.assertTrue(circ2.contains_circle(circ1))
def test_contains_impl_intersects(self):
"containment implies intersection"
circ1 = Circle()
circ2 = Circle(Vec2D(), 0.5)
self.assertTrue(circ1.contains_circle(circ2))
self.assertTrue(circ1.intersects(circ2))
self.assertTrue(circ2.intersects(circ1))
def test_all_tangents_simple2(self):
"test that all_tangents returns all tangents of a circle \
and a polygon and additionally all sides of the polygon"
circ = Circle(Vec2D(4, 4), 1)
poly = dummy_polygon()
correct_tans = circ.tangent_polygon(poly) + poly.sides
to_test = pp.all_tangents([circ], [poly])
verificator = all(test in correct_tans for test in to_test)
self.assertTrue(verificator)
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_tangents_intersec_circles(self):
"test circle tangents of intersecting circles"
circle1 = Circle(Vec2D(), 2)
circle2 = Circle(Vec2D(3, 0), 2)
tans = circle1.tangent_circle(circle2)
self.assertTrue(len(tans) == 2)
tan1 = Tangent(Vec2D(0, 2), circle1, 1, Vec2D(3, 2), circle2, -1)
tan2 = Tangent(Vec2D(0, -2), circle1, -1, Vec2D(3, -2), circle2, 1)
verify = all(tan == tan1 or tan == tan2 for tan in tans)
self.assertTrue(verify)
def test_quadratic_two_solutions(self):
"test result of quadratic equation with two solutions"
results = Circle.solve_quadratic(1, -3, 2)
self.assertTrue(len(results) == 2)
verify_results = all(res == 1 or res == 2 for res in results)
self.assertTrue(verify_results)
def test_contains_impl_intersects(self):
"containment implies intersection"
circ1 = Circle()
circ2 = Circle(Vec2D(), 0.5)
self.assertTrue(circ1.contains_circle(circ2))
self.assertTrue(circ1.intersects(circ2))
self.assertTrue(circ2.intersects(circ1))
def translated_dummy(vec):
"return dummy triangular polygon manually translated by vec"
delta_x = vec.pos_x
delta_y = vec.pos_y
circ1 = Circle(Vec2D(delta_x, delta_y))
circ2 = Circle(Vec2D(4 + delta_x, 0 + delta_y))
circ3 = Circle(Vec2D(0 + delta_x, 4 + delta_y))
side2_helper = Vec2D(sqrt(2.0) / 2.0, sqrt(2.0) / 2.0)
side2_pos1 = circ2.pos + side2_helper
side2_pos2 = circ3.pos + side2_helper
side1 = Tangent(Vec2D(0 + delta_x, -1 + delta_y), circ1, -1,
Vec2D(4 + delta_x, -1 + delta_y), circ2, 1)
side2 = Tangent(side2_pos1, circ2, -1, side2_pos2, circ3, 1)
side3 = Tangent(Vec2D(-1 + delta_x, 4 + delta_y), circ3, -1,
Vec2D(-1 + delta_x, 0 + delta_y), circ1, 1)
return Polygon([circ1, circ2, circ3], [side1, side2, side3])
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_neighs_circle_two_elements(self):
"point has only one neighbour"
point1 = Vec2D(1, 0)
point2 = point1.rotate(pi / 2)
points = [point1, point2]
(pos, neg) = pp.neighbours_on_circle(points, Circle(), point1)
self.assertTrue(pos == point2)
self.assertTrue(neg == point2)
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_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_touching_intersection(self):
"test intersection of tangent circles"
circ1 = Circle()
circ2 = Circle(Vec2D(1, 0))
self.assertTrue(circ1.intersects(circ2))
self.assertTrue(circ2.intersects(circ1))
def test_intersection_equal_circles(self):
"test intersection of equal circles"
circ1 = Circle()
circ2 = Circle()
self.assertTrue(circ1.intersects(circ2))
self.assertTrue(circ2.intersects(circ1))
def test_containing_circles(self):
"test circle containment"
circ1 = Circle()
circ2 = Circle(Vec2D(), 0.5)
self.assertTrue(circ1.contains_circle(circ2))
self.assertFalse(circ2.contains_circle(circ1))
def test_not_intersecting_circles(self):
"test intersection of not intersecting circles"
circ1 = Circle()
circ2 = Circle(Vec2D(10, 0))
self.assertFalse(circ1.intersects(circ2))
self.assertFalse(circ2.intersects(circ1))
def test_circle_circle_intersection(self):
"test intersection of intersecting circles"
circ1 = Circle()
circ2 = Circle(Vec2D(0.5, 0))
self.assertTrue(circ1.intersects(circ2))
self.assertTrue(circ2.intersects(circ1))
def test_containment_equal_circles(self):
"if two circles are equal one contains the other and vice versa"
circ1 = Circle()
circ2 = Circle()
self.assertTrue(circ1.contains_circle(circ2))
self.assertTrue(circ2.contains_circle(circ1))
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_circle_tangents_full(self):
"test tangents between two disjoint circles"
circ1 = Circle()
circ2 = Circle(Vec2D(4, 0))
midpoint = Vec2D(2, 0)
hypothenuse = (midpoint - circ1.pos).length()
opp_side = circ1.radius
tan_len = sqrt(hypothenuse * hypothenuse - opp_side * opp_side)
angle = asin(opp_side / hypothenuse)
start1 = (circ1.pos - midpoint).rotate(angle).normalized() * tan_len
start1 = start1 + midpoint
end1 = (circ2.pos - midpoint).rotate(angle).normalized() * tan_len
end1 = end1 + midpoint
start2 = (circ1.pos - midpoint).rotate(-angle).normalized() * tan_len
start2 = start2 + midpoint
end2 = (circ2.pos - midpoint).rotate(-angle).normalized() * tan_len
end2 = end2 + midpoint
r_tans = []
r_tans.append(Tangent(start1, circ1, -1, end1, circ2, -1))
r_tans.append(Tangent(start2, circ1, 1, end2, circ2, 1))
r_tans.append(Tangent(Vec2D(0, 1), circ1, 1, Vec2D(4, 1), circ2, -1))
r_tans.append(Tangent(Vec2D(0, -1), circ1, -1, Vec2D(4, -1), circ2, 1))
tans = circ1.tangent_circle(circ2)
self.assertTrue(len(tans) == 4)
valid = all(t in r_tans for t in tans)
self.assertTrue(valid)
def test_circle_tangents_full(self):
"test tangents between two disjoint circles"
circ1 = Circle()
circ2 = Circle(Vec2D(4, 0))
midpoint = Vec2D(2, 0)
hypothenuse = (midpoint - circ1.pos).length()
opp_side = circ1.radius
tan_len = sqrt(hypothenuse * hypothenuse - opp_side * opp_side)
angle = asin(opp_side / hypothenuse)
start1 = (circ1.pos - midpoint).rotate(angle).normalized() * tan_len
start1 = start1 + midpoint
end1 = (circ2.pos - midpoint).rotate(angle).normalized() * tan_len
end1 = end1 + midpoint
start2 = (circ1.pos - midpoint).rotate(-angle).normalized() * tan_len
start2 = start2 + midpoint
end2 = (circ2.pos - midpoint).rotate(-angle).normalized() * tan_len
end2 = end2 + midpoint
r_tans = []
r_tans.append(Tangent(start1, circ1, -1, end1, circ2, -1))
r_tans.append(Tangent(start2, circ1, 1, end2, circ2, 1))
r_tans.append(Tangent(Vec2D(0, 1), circ1, 1, Vec2D(4, 1), circ2, -1))
r_tans.append(Tangent(Vec2D(0, -1), circ1, -1, Vec2D(4, -1), circ2, 1))
tans = circ1.tangent_circle(circ2)
self.assertTrue(len(tans) == 4)
valid = all(t in r_tans for t in tans)
self.assertTrue(valid)
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_sort_points_general(self):
"general test on unit circle"
point1 = Vec2D(1, 0)
point2 = point1.rotate(pi / 2)
point3 = point1.rotate(pi)
point4 = point1.rotate(-pi / 2)
unsorted = [point2, point4, point3, point1]
to_test = pp.sort_points_on_circle(unsorted, Circle())
self.assertTrue(len(to_test) == 4)
self.assertTrue(to_test[0] == point2)
self.assertTrue(to_test[1] == point3)
self.assertTrue(to_test[2] == point4)
self.assertTrue(to_test[3] == point1)
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.is_equal(circle2))
self.assertTrue(circle2.is_equal(circle1))
# equality is symmetric
self.assertFalse(not circle1.is_equal(circle2))
self.assertFalse(not circle2.is_equal(circle1))
def get_path(settings, polygons, circles, start_pos, start_heading, start_v,
target_pos):
"returns list of points from_pos to end_pos"
start_circles = get_start_circles(start_pos, start_heading,
settings.obs_min_r)
end_circle = [Circle(target_pos, 0)]
all_circs = settings.static_circ_obs + circles + start_circles + end_circle
all_polys = settings.static_poly_obs + polygons
all_tans = all_tangents(all_circs, all_polys)
#remove tangents between the two start circles
start_circle_tans = start_circles[0].tangent_circle(start_circles[1])
all_tans = [tan for tan in all_tans if not tan in start_circle_tans]
# remove tangents leading out of bounds
all_tans = [
tan for tan in all_tans if tangent_inside_bounds(settings, tan)
]
graph = build_graph(all_tans, start_pos, start_circles, settings)
segments = a_star(graph, target_pos)
if segments:
traj = discretize_trajectory(segments, start_v, 0.0, settings)
else:
traj = ramp_down(start_pos, start_heading, start_v, settings)
return traj
def test_sort_points_empty(self):
"test sort_points_on_circle on empty list"
self.assertTrue(not pp.sort_points_on_circle([], Circle()))
def test_nonintersecting_tangent(self):
"test intersection with nonintersecting tangent"
circle = Circle()
tan = Tangent(Vec2D(10, 10), None, None, Vec2D(11, 11), None, None)
self.assertFalse(circle.intersects_tangent(tan))
def test_polygon_from_circles_bad2(self):
Polygon([Circle()])
def test_quadratic_single_solution(self):
"test result of quadratic equation with unique solution"
result = Circle.solve_quadratic(1, 2, 1)
self.assertTrue(len(result) == 1)
self.assertTrue(result[0] == -1)
def test_quadratic_no_solutions(self):
"test result of unsatisfiable quadratic equation"
result = Circle.solve_quadratic(1, 0, 1)
self.assertTrue(len(result) == 0)
def test_quadratic_single_solution(self):
"test result of quadratic equation with unique solution"
result = Circle.solve_quadratic(1, 2, 1)
self.assertTrue(len(result) == 1)
self.assertTrue(result[0] == -1)
def test_touching_tangent(self):
"test start and end inside circle"
circle = Circle()
tangent = Tangent(Vec2D(0, 1), None, None, Vec2D(0, 2), None, None)
self.assertTrue(circle.intersects_tangent(tangent))
def test_nonintersecting_tangent(self):
"test intersection with nonintersecting tangent"
circle = Circle()
tan = Tangent(Vec2D(10, 10), None, None, Vec2D(11, 11), None, None)
self.assertFalse(circle.intersects_tangent(tan))
def test_intersecting_tangent(self):
"test single intersection point"
circle = Circle()
tangent = Tangent(Vec2D(), None, None, Vec2D(0, 2), None, None)
self.assertTrue(circle.intersects_tangent(tangent))
def test_touching_tangent(self):
"test start and end inside circle"
circle = Circle()
tangent = Tangent(Vec2D(0, 1), None, None, Vec2D(0, 2), None, None)
self.assertTrue(circle.intersects_tangent(tangent))
def test_fullintersection_tangent(self):
"test 2 intersection points"
circle = Circle()
tangent = Tangent(Vec2D(0, -2), None, None, Vec2D(0, 2), None, None)
self.assertTrue(circle.intersects_tangent(tangent))
def test_intersecting_tangent(self):
"test single intersection point"
circle = Circle()
tangent = Tangent(Vec2D(), None, None, Vec2D(0, 2), None, None)
self.assertTrue(circle.intersects_tangent(tangent))
"general settings class for molly"
from molly.Circle import Circle
from molly.Vec2D import Vec2D
from molly.Tangent import Tangent
from molly.Polygon import Polygon
CIRCLE1 = Circle(Vec2D(0, 0), 0.2)
CIRCLE2 = Circle(Vec2D(1.066, 0), 0.2)
CIRCLE3 = Circle(Vec2D(1.066, 0.6), 0.2)
CIRCLE4 = Circle(Vec2D(0, 0.6), 0.2)
CORNERS = [CIRCLE1, CIRCLE2, CIRCLE3, CIRCLE4]
SIDE1 = Tangent(Vec2D(0, -0.2), CIRCLE1, -1, Vec2D(1.066, -0.2), CIRCLE2, 1)
SIDE2 = Tangent(Vec2D(1.266, 0), CIRCLE2, -1, Vec2D(1.266, 0.6), CIRCLE3, 1)
SIDE3 = Tangent(Vec2D(1.066, 0.8), CIRCLE3, -1, Vec2D(0, 0.8), CIRCLE4, 1)
SIDE4 = Tangent(Vec2D(-0.2, 0.6), CIRCLE4, -1, Vec2D(-0.2, 0), CIRCLE1, 1)
SIDES = [SIDE1, SIDE2, SIDE3, SIDE4]
STAIRS = Polygon(CORNERS, SIDES)
STAIRS = STAIRS + Vec2D(1.5 - 1.066 / 2, 0)
class Settings(object):
"settings class memorizing settings for molly"
def __init__(self,
max_acc=1.6,
def test_fullintersection_tangent(self):
"test 2 intersection points"
circle = Circle()
tangent = Tangent(Vec2D(0, -2), None, None, Vec2D(0, 2), None, None)
self.assertTrue(circle.intersects_tangent(tangent))
def test_quadratic_no_solutions(self):
"test result of unsatisfiable quadratic equation"
result = Circle.solve_quadratic(1, 0, 1)
self.assertTrue(len(result) == 0)
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))