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