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 __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_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 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_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_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_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.is_equal(translated)) self.assertTrue(translated.is_equal(tan))
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_polycircle_intersection(self): "test intersection between polygon and circle" dummy = dummy_polygon() circ = Circle(Vec2D(4, 4)) tan1 = Tangent(Vec2D(4, 5), Circle(Vec2D(4, 4)), -1, Vec2D(0, 5), Circle(Vec2D(0, 4)), 1) tan2 = Tangent(Vec2D(5, 4), Circle(Vec2D(4, 4)), 1, Vec2D(5, 0), Circle(Vec2D(4, 0)), -1) tans = dummy.tangent_circle(circ) self.assertTrue(tan1 in tans and tan2 in tans)
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_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 dummy_polygon(): "return a dummy triangular polygon" circ1 = Circle() circ2 = Circle(Vec2D(4, 0)) circ3 = Circle(Vec2D(0, 4)) 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, -1), circ1, -1, Vec2D(4, -1), circ2, 1) side2 = Tangent(side2_pos1, circ2, -1, side2_pos2, circ3, 1) side3 = Tangent(Vec2D(-1, 4), circ3, -1, Vec2D(-1, 0), circ1, 1) return Polygon([circ1, circ2, circ3], [side1, side2, side3])
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 main(): "draw loop" start = Vec2D(0.1, 0.1) end = Vec2D(2.1, 1.1) direction = Vec2D(0, 0.1) obs1 = Circle(Vec2D(1.5, 0.75), 0.2) #obs2 = Circle(Vec2D(0.9, 0.6), 0.2) corner1 = Circle(Vec2D(), 0.1) corner2 = Circle(Vec2D(0.5, 0), 0.1) side1 = Tangent(Vec2D(0, -0.1), corner1, -1, Vec2D(0.5, -0.1), corner2, 1) side2 = Tangent(Vec2D(0, 0.1), corner1, 1, Vec2D(0.5, 0.1), corner2, -1) poly1 = Polygon([corner1, corner2], [side1, side2]) + Vec2D(0.15, 0.3) circs = [obs1] polys = [poly1] paused = False while True: for event in pygame.event.get(): if event.type == pygame.QUIT: pygame.quit() sys.exit() if event.type == pygame.KEYDOWN: if event.key == pygame.K_p: paused = not paused if not paused: SCREEN.fill(BLACK) #SCRREN.blit(BACKGROUND, (0, 0)) path = get_path(SETTINGS, polys, circs, start, direction, 0, end) draw_pos(start) draw_pos(end) draw_env(SETTINGS, circs, polys) draw_path(path) pygame.display.update()
def test_translate_general(self): "test translation by non-zero vector" circ1 = Circle() circ2 = Circle(Vec2D(4, 0)) translation = Vec2D(1, 0) original = Tangent(Vec2D(0, 1), circ1, 1, Vec2D(4, 1), circ2, -1) translated = original + translation v_circ1 = Circle(Vec2D(1, 0)) v_circ2 = Circle(Vec2D(5, 0)) verifier = Tangent(Vec2D(1, 1), v_circ1, 1, Vec2D(5, 1), v_circ2, -1) self.assertTrue(verifier == translated) self.assertTrue(translated == verifier)
def test_translate_general(self): "test translation by non-zero vector" circ1 = Circle() circ2 = Circle(Vec2D(4, 0)) translation = Vec2D(1, 0) original = Tangent(Vec2D(0, 1), circ1, 1, Vec2D(4, 1), circ2, -1) translated = original + translation v_circ1 = Circle(Vec2D(1, 0)) v_circ2 = Circle(Vec2D(5, 0)) verifier = Tangent(Vec2D(1, 1), v_circ1, 1, Vec2D(5, 1), v_circ2, -1) self.assertTrue(verifier.is_equal(translated)) self.assertTrue(translated.is_equal(verifier))
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_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_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_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_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 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_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_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_constructor(self): "test constructor" start_pos = Vec2D(0, 1) start_circ = Circle() start_orient = 1 end_pos = Vec2D(2, 1) end_circ = Circle(Vec2D(2, 0)) end_orient = -1 tan = Tangent(start_pos, start_circ, start_orient, end_pos, end_circ, end_orient) self.assertTrue(tan.start_pos == start_pos) self.assertTrue(tan.start_pos == start_pos) self.assertTrue(tan.start_circle == start_circ) self.assertTrue(tan.end_orient == end_orient) self.assertTrue(tan.end_circle == end_circ) self.assertTrue(tan.end_orient == end_orient)
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_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))
"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))