AnasSarkiz/muse-voice-remote

scripts/precise_path.py

"""Visibility-simplified route through a constrained free-space triangulation."""
import heapq,math
from shapely import constrained_delaunay_triangles
from shapely.geometry import Point,LineString

def visible_path(region,start,end):
 if region.covers(LineString([start,end])):return [start,end]
 triangles=list(constrained_delaunay_triangles(region).geoms)
 starts=[i for i,t in enumerate(triangles) if t.covers(Point(start))]
 goals={i for i,t in enumerate(triangles) if t.covers(Point(end))}
 if not starts or not goals:raise RuntimeError('Triangulation does not contain fanout endpoints')
 centres=[(t.centroid.x,t.centroid.y) for t in triangles];edges={};neighbors=[[] for _ in triangles]
 for i,t in enumerate(triangles):
  vertices=list(t.exterior.coords)[:-1]
  for a,b in zip(vertices,vertices[1:]+vertices[:1]):
   key=tuple(sorted((a,b)))
   if key in edges:
    j=edges[key];mid=((a[0]+b[0])/2,(a[1]+b[1])/2);length=math.dist(centres[i],mid)+math.dist(mid,centres[j]);neighbors[i].append((j,mid,length));neighbors[j].append((i,mid,length))
   else:edges[key]=i
 cost={i:math.dist(start,centres[i]) for i in starts};queue=[(cost[i],i) for i in starts];heapq.heapify(queue);previous={};target=None
 while queue:
  score,i=heapq.heappop(queue)
  if score>cost[i]:continue
  if i in goals:target=i;break
  for j,mid,length in neighbors[i]:
   candidate=score+length
   if candidate<cost.get(j,math.inf):cost[j]=candidate;previous[j]=(i,mid);heapq.heappush(queue,(candidate,j))
 if target is None:raise RuntimeError('No constrained fanout path')
 chain=[target]
 while chain[-1] in previous:chain.append(previous[chain[-1]][0])
 chain.reverse();points=[start,centres[chain[0]]]
 for i,j in zip(chain,chain[1:]):points.extend([previous[j][1],centres[j]])
 points.append(end);simple=[points[0]];position=0
 while position<len(points)-1:
  candidate=len(points)-1
  while candidate>position+1 and not region.covers(LineString([points[position],points[candidate]])):candidate-=1
  simple.append(points[candidate]);position=candidate
 return simple