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)
Exemplo n.º 35
0
    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))
Exemplo n.º 49
0
"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))