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))