from math import hypot, acos, cos, sin, pi, atan2, sqrt
from collections import defaultdict
import heapq as hq
def is_between_angles(x, a, b):
a %= 2 * pi
b %= 2 * pi
x %= 2 * pi
delta = (b - a) % (2 * pi)
if delta <= pi:
return (x - a) % (2 * pi) <= delta
else:
return (a - x) % (2 * pi) <= (2 * pi - delta)
def angle(center, pt):
return atan2(pt.y - center.y, pt.x - center.x)
class Point:
def __init__(self, x, y):
self.x = x
self.y = y
def __str__(self):
return f"({self.x}, {self.y})"
def __add__(self, other):
return Point(self.x + other.x, self.y + other.y)
def __sub__(self, other):
return Point(self.x - other.x, self.y - other.y)
def __neg__(self):
return Point(-self.x, - self.y)
def __rmul__(self, k):
return Point(k * self.x, k * self.y)
def __eq__(self, other):
return self.x == other.x and self.y == other.y
class Segment:
def __init__(self, p1, p2):
self.p1 = p1
self.p2 = p2
def length(self):
return hypot(self.p1.x - self.p2.x, self.p1.y - self.p2.y)
def __rep__(self):
return f"Segment {self.p1} --- {self.p2}"
def intersect(self, circle):
u = ((self.p2.x - self.p1.x) * (circle.center.x - self.p1.x) +
(self.p2.y - self.p1.y) * (circle.center.y - self.p1.y)) / self.length()**2
pt = self.p1 + max(0, min(u, 1)) * (self.p2 - self.p1)
seg = Segment(circle.center, pt)
return seg.length() < circle.radius and abs(seg.length() - circle.radius) > 1e-6
class Circle:
def __init__(self, center, radius):
self.center = center
self.radius = radius
def __repr__(self):
return f"Circle of center {self.center} and radius {self.radius}"
def tangents_to_point(self, p):
c, r = self.center, self.radius
d = Segment(c, p).length()
if r > d:
return (False, [])
t = acos(r / d)
q = c + r / d * (p - c)
p1 = Point(c.x + cos(t) * (q.x - c.x) - sin(t) * (q.y - c.y),
c.y + sin(t) * (q.x - c.x) + cos(t) * (q.y - c.y))
p2 = Point(c.x + cos(t) * (q.x - c.x) + sin(t) * (q.y - c.y),
c.y - sin(t) * (q.x - c.x) + cos(t) * (q.y - c.y))
return (True, [p1, p2])
def internal_bitangents(self, other):
r1, r2 = self.radius, other.radius
x1, y1 = self.center.x, self.center.y
x2, y2 = other.center.x, other.center.y
dis = hypot(x2 - x1, y2 - y1)
if r1 + r2 >= dis:
return (False, [])
t = acos((r1 + r2) / dis)
p = self.center + r1 / dis * (other.center - self.center)
p1 = Point(x1 + cos(t) * (p.x - x1) - sin(t) * (p.y - y1),
y1 + sin(t) * (p.x - x1) + cos(t) * (p.y - y1))
p2 = Point(x1 + cos(t) * (p.x - x1) + sin(t) * (p.y - y1),
y1 - sin(t) * (p.x - x1) + cos(t) * (p.y - y1))
#########
q = other.center + r2 / dis * (self.center - other.center)
q1 = Point(x2 + cos(t) * (q.x - x2) - sin(t) * (q.y - y2),
y2 + sin(t) * (q.x - x2) + cos(t) * (q.y - y2))
q2 = Point(x2 + cos(t) * (q.x - x2) + sin(t) * (q.y - y2),
y2 - sin(t) * (q.x - x2) + cos(t) * (q.y - y2))
return (True, [[p1, p2], [q1, q2]])
def external_bitangents(self, other):
r1, r2 = self.radius, other.radius
x1, y1 = self.center.x, self.center.y
x2, y2 = other.center.x, other.center.y
dis = hypot(x2 - x1, y2 - y1)
if abs(r1 - r2) > dis:
reutrn (False, [])
t = acos(abs(r1 - r2) / dis)
p = self.center + r1 / dis * (other.center - self.center)
p1 = Point(x1 + cos(t) * (p.x - x1) - sin(t) * (p.y - y1),
y1 + sin(t) * (p.x - x1) + cos(t) * (p.y - y1))
p2 = Point(x1 + cos(t) * (p.x - x1) + sin(t) * (p.y - y1),
y1 - sin(t) * (p.x - x1) + cos(t) * (p.y - y1))
#########
q = self.center + (dis + r2) / dis * (other.center - self.center)
q1 = Point(x2 + cos(t) * (q.x - x2) - sin(t) * (q.y - y2),
y2 + sin(t) * (q.x - x2) + cos(t) * (q.y - y2))
q2 = Point(x2 + cos(t) * (q.x - x2) + sin(t) * (q.y - y2),
y2 - sin(t) * (q.x - x2) + cos(t) * (q.y - y2))
return (True, [[p1, p2], [q1, q2]])
def intersect(self, other):
assert type(other) == Circle, "Element is not a circle"
ca, cb = self.center, other.center
ra, rb = self.radius, other.radius
d = Segment(ca, cb).length()
if ra + rb < d or abs(ra - rb) > d:
return (False, [])
a = (ra**2 + d**2 - rb**2) / (2 * d)
if abs(a - ra) <= 1e-6:
c = ca + a / d * (cb - ca)
return (False, [c])
d = ca + ra / d * (cb - ca)
t = acos(a / ra)
p = Point(ca.x + cos(t) * (d.x - ca.x) - sin(t) * (d.y - ca.y),
ca.y + sin(t) * (d.x - ca.x) + cos(t) * (d.y - ca.y))
q = Point(ca.x + cos(t) * (d.x - ca.x) + sin(t) * (d.y - ca.y),
ca.y - sin(t) * (d.x - ca.x) + cos(t) * (d.y - ca.y))
return (True, [p, q])
class PathFinder:
def __init__(self, pa, pb, circles):
self.start = pa
self.end = pb
self.circles = circles
self.elements = [[self.start, None], [self.end, None]]
self.points = defaultdict(list)
self.adj = defaultdict(set)
for i, c in enumerate(circles):
for p in (pa, pb):
b, ps = c.tangents_to_point(p)
if b:
self.elements.append([ps[0], i])
self.elements.append([ps[1], i])
self.points[i].extend(ps)
n = len(circles)
for i in range(n):
for j in range(i + 1, n):
b, ps = circles[i].internal_bitangents(circles[j])
c, qs = circles[i].external_bitangents(circles[j])
if b:
self.elements.extend([[p, i] for p in ps[0]])
self.elements.extend([[p, j] for p in ps[1]])
self.points[i].extend(ps[0])
self.points[j].extend(ps[1])
if c:
self.elements.extend([[q, i] for q in qs[0]])
self.elements.extend([[q, j] for q in qs[1]])
self.points[i].extend(qs[0])
self.points[j].extend(qs[1])
N = len(self.elements)
# for i in range(N):
# p, _ = self.elements[i]
# print(f"A_{{{i}}} = Point({{{p.x}, {p.y}}})")
for i in range(N):
pi, fi = self.elements[i]
for j in range(i + 1, N):
pj, fj = self.elements[j]
if fi is not None and fj is not None and fi == fj:
ci = self.circles[fi]
if self.arc_is_good(pi, pj, fi):
l = PathFinder.arc_distance(pi, pj, ci)
self.adj[i].add((j, l, "arc"))
self.adj[j].add((i, l, "arc"))
else:
s = Segment(pi, pj)
# if i == 20:
# print("j: ", j, self.elements[j][0], [(k, s.intersect(c)) for k, c in enumerate(self.circles)])
if all(not s.intersect(c) for k, c in enumerate(self.circles)):
d = s.length()
self.adj[i].add((j, d, "segment"))
self.adj[j].add((i, d, "segment"))
# for k in [26]:
# print("k:", k, self.elements[k][0], self.elements[k][1])
# for idx, distance, _type in self.adj[k]:
# print(idx, distance, _type)
# print('#' * 20)
def __repr__(self):
sep = '\n' + '#' * 20 + '\n'
arr = []
for i, (p, t) in enumerate(self.elements):
arr.append(f"Point{i}:{p} of type {t}")
n = f"Number of elements : {len(self.elements)}"
# m = '\n'.join("{1} ({2}): {3}".format(key, self.elements[key], value) for key, value in self.adj)
return f"Tangent Visibility Graph\n{n}\n{sep.join(arr)}\nAdjacency matrix :\n"
def arc_is_good(self, p, q, circle_idx):
circle = self.circles[circle_idx]
for k, c in enumerate(self.circles):
if k == circle_idx:
continue
b, pts = circle.intersect(c)
if not b:
continue
center = circle.center
t1 = angle(center, p)
t2 = angle(center, q)
if any(is_between_angles(angle(center, p), t1, t2) for p in pts):
return False
return True
@staticmethod
def arc_distance(p, q, c):
center, radius = c.center, c.radius
try:
u = ((p.x - center.x) * (q.x - center.x) + (p.y - center.y) * (q.y - center.y)) / radius**2
u = max(-1, min(u, 1))
t = acos(u)
return min(radius * t, radius * (2 * pi - t))
except:
print(p, q)
print(((p.x - center.x) * (q.x - center.x) + (p.y - center.y) * (q.y - center.y)))
print(radius)
@staticmethod
def heuristic(p, q):
return hypot(p.x - q.x, p.y - q.y)
def a_star_search(self):
N = len(self.elements)
G = {i: float('inf') for i in range(N)} # Actual movement cost to each position from the start position
F = {i: float('inf') for i in range(N)} # Estimated movement cost of start to end going via this position
start, end = 0, 1
# Initialize starting values
G[start] = 0
p_start, _ = self.elements[start]
p_end, _ = self.elements[end]
d_init = PathFinder.heuristic(p_start, p_end)
F[start] = d_init
open_vertices = [(d_init, start)]
seen = set([start])
hq.heapify(open_vertices)
came_from = {}
while len(open_vertices) > 0:
lowest, current = hq.heappop(open_vertices)
# Check if we have reached the goal
if current == end:
# Retrace our route backward
# print("path:", came_from)
path = [current]
while current in came_from:
current = came_from[current]
path.append(current)
path.reverse()
return path, F[end]
#Update scores for vertices near the current position
for neighbor, dis, _type in self.adj[current]:
p_end, _ = self.elements[end]
tentative_gscore = G[current] + dis
if tentative_gscore < G[neighbor]:
came_from[neighbor] = current
G[neighbor] = tentative_gscore
p_neighbor, _ = self.elements[neighbor]
F[neighbor] = tentative_gscore + PathFinder.heuristic(p_neighbor, p_end)
if not neighbor in seen:
seen.add(neighbor)
hq.heappush(open_vertices, (F[neighbor], neighbor))
raise RuntimeError("A* failed to find a solution")
pa, pb = Point(-3.5,0.1), Point(3.5,0.0)
r = 2.01
circles = [Circle(Point(0,0), 1), Circle(Point(r,0), 1), Circle(Point(r*0.5, r*sqrt(3)/2), 1), Circle(Point(-r*0.5, r*sqrt(3)/2), 1),
Circle(Point(-r, 0), 1), Circle(Point(r*0.5, -r*sqrt(3)/2), 1), Circle(Point(-r*0.5, -r*sqrt(3)/2), 1)]
pf = PathFinder(pa, pb, circles)
r = pf.a_star_search()
print("r :", r)
To embed this program on your website, copy the following code and paste it into your website's HTML: