from typing import TypeVar,List,Sequence; from math import pi,sqrt,atan2; from enum import Enum; #------------------------------------------------------------------------------# Real=TypeVar('Real',int,float); #------------------------------------------------------------------------------# twicePi=2.0*pi; halfPi=0.5*pi; r2d=180.0/pi; d2r=pi/180.0; #------------------------------------------------------------------------------# # The constants have to be adapted to the problem maxLength=10000.0; # area extension epsilon=0.005 deltaAngles=epsilon/maxLength; # radian deltaCoordinates=epsilon; def areCoordinatesEqual(c1,c2:Real)->bool: return abs(c2-c1)bool: return abs(l2-l1)bool: return abs(a2-a1)None: self.X=x; self.Y=y; def toClone(self)->'Vectors': return Vectors(self.X,self.Y); def __str__(self)->str: return str(self.X)+', '+str(self.Y); def __eq__(self,vc:'Vectors')->bool: """ Test for equality og two vectors """ return (areCoordinatesEqual(self.X,vc.X) and areCoordinatesEqual(self.Y,vc.Y)); def lengthOf(self)->Real: """ Returns the length of the vector """ return sqrt(self.X*self.X+self.Y*self.Y); def toUnitVector(self)->'Vectors': """ Returns a unit vector in the direction of self """ lg=sqrt(self.X*self.X+self.Y*self.Y); return Vectors(self.X/lg,self.Y/lg); # end toUnitVector def __add__(self,vc:'Vectors')->'Vectors': """ Addition of two vectors """ return Vectors(self.X+vc.X,self.Y+vc.Y); def __sub__(self,vc:'Vectors')->'Vectors': """ Subtraction of two vectors """ return Vectors(self.X-vc.X,self.Y-vc.Y); def __mul__(self,sc:Real)->'Vectors': """ Multiplication with scalar to vector """ return Vectors(self.X*sc,self.Y*sc); def smul(self,sc:Real)->None: """ Multiplication with scalar to self """ self.X*=sc; self.Y*=sc; def __rmul__(self,sc:Real)->'Vectors': """ Right multiplication with scalar to vector """ return Vectors(self.X*sc,self.Y*sc); def __truediv__(self,sc:Real)->'Vectors': """ Division with scalar to vector """ return Vectors(self.X/sc,self.Y/sc); def sdiv(self,sc:Real)->None: """ Division with scalar to self """ self.X/=sc; self.Y/=sc; def dotOp(self,vc:'Vectors')->Real: """ Dot product of two vectors """ return self.X*vc.X+self.Y*vc.Y; def crossOp(self,vc:'Vectors')->Real: """ Cross product of two vectors """ return self.X*vc.Y-self.Y*vc.X; def dirAngleOf(self)->Real: """ Direction angle of the vector relative to the x axis 0<=dirAngleOf<2*Pi """ angle=atan2(self.Y,self.X); if angle<0.0: return angle+twicePi; else: return angle; # end directionAngle def sameDirectionOf(self,withV:'Vectors')->bool: """ Test for equality of the directions of two vectors """ angleDiff=abs(withV.dirAngleOf()-self.dirAngleOf()); if angleDiffbool: return abs(self.dotOp(vc))None: self.X=x; self.Y=y; def toClone(self)->'Points': return Points(self.X,self.Y); def updateOf(self,withP:'Points')->None: self.X=withP.X; self.Y=withP.Y; # end updateOf def __str__(self)->str: return str(self.X)+', '+str(self.Y); def __eq__(self,pt:'Points')->bool: """ Test for equality of two points """ return (areCoordinatesEqual(self.X,pt.X) and areCoordinatesEqual(self.Y,pt.Y)); def __add__(self,vc:Vectors)->'Points': """ Addition: Point with vector to point """ return Points(self.X+vc.X,self.Y+vc.Y); def __sub__(self,pt:'Points')->'Vectors': """ Subtraction: Point with point to vector """ return Vectors(self.X-pt.X,self.Y-pt.Y); def distanceOf(self,toPoint:'Points')->Real: """ Distance between two points """ return Vectors.lengthOf(toPoint-self); def areaOf(self,p1:'Points',p2:'Points')->Real: """ Area of the triangle with three points """ return 0.5*Vectors.crossOp(p1-self,p2-self); #end Points #------------------------------------------------------------------------------# class Lines(object): def __init__(self,a:Real,b:Real,c:Real)->None: """ Implicite line equation is A*x+B*y=C with sqrt(A*A+B*B)=1 """ d=sqrt(a**2+b**2); self.A=a/d; self.B=b/d; self.C=c/d; if areCoordinatesEqual(self.A,0.0): xlp=0.0; ylp=self.C/self.B; xdir=1.0; ydir=0.0; elif areCoordinatesEqual(self.B,0.0): xlp=self.C/self.A; ylp=0.0; xdir=0.0; ydir=1.0; else: xlp=0.0; ylp=self.C/self.B; xdir= self.B; ydir= -self.A; # end if self.LP=Points(xlp,ylp); self.DV=Vectors(xdir,ydir); # Unit vector # end __init__ def toClone(self)->'Lines': return Lines(self.A,self.B,self.C); def __str__(self)->str: return ('ABC: '+str(self.A)+' '+str(self.B)+' '+str(self.C)+ '\n\tLP: '+str(self.LP)+' | DV: '+str(self.DV)); def __eq__(self,ln:'Lines')->bool: # Equality of two lines return (self.LP==ln.LP) and ((self.DV==ln.DV) or (self.DV==(-1.0*ln.DV))); @staticmethod def toLine(p1:Points,p2:Points)->'Lines': """ A line through two points :raises InvalidDataError if p1==p2 """ if p1==p2: raise InvalidDataError("toLine with identical points") a=p1.Y-p2.Y; b=p2.X-p1.X; c=a*p1.X + b*p1.Y; return Lines(a,b,c); # end toLine def distanceOf(self,pt:Points)->Real: """ Signed distance of a point to the line """ return self.A*pt.X + self.B*pt.Y - self.C; def nearestPointOf(self,fromP:Points)->Points: """ Nearest point on the line from a point """ lP=self.LP; dV=self.DV; t=Vectors.dotOp(dV,fromP-lP); lP=lP+t*dV; return lP; # end nearestPointOf def isPointInside(self,pt:Points)->bool: """ Is a point on the line? The mathematical equation for a point pt on line is pt.X*self.A+pt.Y*self.B-self.C=0 """ np=self.nearestPointOf(pt); return pt==np; # end isPointInside def areOnSameSide(self,pt1:'Points',pt2:Points)->bool: """ Are both points on the same side relative to the line? :Pre self.isPointInside(pt1)==False :Pre self.isPointInside(pt2)==False """ ds1=self.distanceOf(pt1); ds2=self.distanceOf(pt2); return (ds1>0 and ds2>0) or ((ds1<0 and ds2<0)) # end areOnSameSide def areParallel(self,withLine:'Lines')->bool: """ Test whether two lines are parallel Note: Equal lines are considered to be NOT parallel """ if (self.LP==withLine.LP): return False; d1=self.DV; d2=withLine.DV; return (d1==d2) or (d1==(-1)*d2); # end areParallel def intersectionOf(self,withLine:'Lines')->Points: """ Returns the point of intersection of two lines :raises InfinityPoints if self==withLine :raises NoPoint if self.areParallel(withLine)==true """ L1=self; L2=withLine; if L1==L2: raise InfinityPoints; if L1.areParallel(L2): raise NoPoint; lP1=L1.LP.toClone(); ### ??? lP2=L2.LP; dV1=L1.DV; dV2Ortho=Vectors(-L2.DV.Y,L2.DV.X); q=Vectors.dotOp(dV1,dV2Ortho); ts=Vectors.dotOp(dV2Ortho,lP2-lP1); ts/=q; return lP1+ts*dV1; # end intersectionOf def getNormalVector(self)->Vectors: """ Returns a unit vector perpendicular to the line """ return Vectors(self.A,self.B); # end getNormalVector def getParallelLine(self,withDistance:Real)->'Lines': """ Returns the line parallel to self with withDistance """ return Lines(self.A,self.B,self.C+withDistance); # end getParallelLine # end Lines #------------------------------------------------------------------------------# class Rays(object): def __init__(self,startPoint:Points, direction:Vectors)->None: self.StartPoint=startPoint; self.Direction=direction.toUnitVector(); # end __init__ def toClone(self)->'Rays': return Rays(self.StartPoint,self.Direction); def __str__(self)->str: return 'SP: '+str(self.StartPoint)+' | DV: '+str(self.Direction); def __eq__(self,withRay:'Rays')->bool: # Equality of two rays return ((self.StartPoint==withRay.StartPoint) and self.Direction==withRay.Direction); @staticmethod def toRay(pt1:Points,pt2:Points)->'Rays': """ Returns a ray from point pt1 through point pt2 """ return Rays(pt1,pt2-pt1); def toLine(self)->Lines: """ To extend a ray to a line """ return (Lines.toLine(self.StartPoint, self.StartPoint+self.Direction)); def nearestPointOf(self,fromP:Points)->Points: """ Nearest point on the ray from a point """ lP=self.StartPoint.toClone(); dV=self.Direction; t=Vectors.dotOp(dV,fromP-lP); if t>=0.0: lP=lP+t*dV; return lP; # end nearestPointOf def isPointInside(self,pt:Points)->bool: """ Is a point inside the ray (Inside means different from the start point) """ np=self.nearestPointOf(pt); if np==self.StartPoint: return False; else: return np==pt; # end isPointInside def intersectionOf(self,withRay:'Rays')->Points: """ Returns the point of intersection of two rays :raises NoPoint :raises InfinityPoints """ ry1=self; ry2=withRay; ln1=ry1.toLine(); ln2=ry2.toLine(); if Lines.areParallel(ln1,ln2): raise NoPoint; if ln1==ln2: if (ry1.isPointInside(ry2.StartPoint) or ry2.isPointInside(ry1.StartPoint)): raise InfinityPoints; elif ((ry1.StartPoint==ry2.StartPoint) and (ry1.Direction==ry2.Direction)): raise InfinityPoints; else: raise NoPoint; # end if ip=Lines.intersectionOf(ln1,ln2); if (ry1.isPointInside(ip) and ry2.isPointInside(ip)): return ip; else: raise NoPoint; # end if # end intersectionOf # end Rays #------------------------------------------------------------------------------# class Segments(object): def __init__(self,p1:Points,p2:Points)->None: """ The oriented line segment from point p1 to point p2 """ self.P1=p1; self.P2=p2; def toClone(self)->'Segments': return Segments(self.P1,self.P2); def update(self,withSegment:'Segments')->None: self.P1=withSegment.P1; self.P2=withSegment.P2; # end update def __eq__(self,sgm:'Segments')->bool: """ Test for oriented equality of two segments """ return (self.P1==sgm.P1) and (self.P2==sgm.P2); def __str__(self)->str: return str(self.P1)+' | '+str(self.P2); def toLine(self)->Lines: """ Returns the extension of the segment to a line """ return Lines.toLine(self.P1,self.P2); def isPointInside(self,pt:Points)->bool: """ Is a point inside the segment? """ ps1=self.P1; ps2=self.P2; rs1=Rays.toRay(ps1,ps2); rs2=Rays.toRay(ps2,ps1); return rs1.isPointInside(pt) and rs2.isPointInside(pt); # end isPointInside def distanceOf(self,pt:Points)->bool: """ Returns the signed distance of a point from the segment :Pre pt is not a point of the segment """ From=pt; To=self; ln=To.toLine(); dOrtho=Vectors(ln.A,ln.B); if (Vectors.dotOp(From-To.P2,To.P2-To.P1)>0.0): sgn=1 if (Vectors.dotOp(dOrtho,From-To.P2)>=0) else -1; return sgn*From.distanceOf(To.P2); elif (Vectors.dotOp(From-To.P1,To.P1-To.P2)>0.0): sgn=1 if (Vectors.dotOp(dOrtho,From-To.P1)>=0) else -1; return sgn*From.distanceOf(To.P1); else: return ln.distanceOf(From); # end if # end distanceOf def intersectionOf_withRay(self,withRay:Rays)->Points: """ Returns the point of intersection of the segment with the ray :raises NoPoint :raises InfinityPoints """ ps1=self.P1; ps2=self.P2; rs1=Rays.toRay(ps1,ps2); rs2=Rays.toRay(ps2,ps1); intersectionKind1=IntersectionKind.onePoint; intersectionKind2=IntersectionKind.onePoint; try: ip1=withRay.intersectionOf(rs1); if (ip1==ps1) or (ip1==ps2): raise NoPoint; except NoPoint: intersectionKind1=IntersectionKind.noPoint; except InfinityPoints: intersectionKind1=IntersectionKind.infinityPoints; # end try try: ip2=withRay.intersectionOf(rs2); if (ip2==ps1) or (ip2==ps2): raise NoPoint; except NoPoint: intersectionKind2=IntersectionKind.noPoint; except InfinityPoints: intersectionKind1=IntersectionKind.infinityPoints; # end try if ((intersectionKind1==IntersectionKind.onePoint) and (intersectionKind2==IntersectionKind.onePoint)): assert(ip1==ip2); # TBD return ip1; elif ((intersectionKind1==IntersectionKind.noPoint) and (intersectionKind2==IntersectionKind.noPoint)): raise NoPoint; elif ((intersectionKind1==IntersectionKind.noPoint) and (intersectionKind2==IntersectionKind.onePoint)): raise NoPoint; elif ((intersectionKind2==IntersectionKind.noPoint) and (intersectionKind1==IntersectionKind.onePoint)): raise NoPoint; else: raise InfinityPoints; # TBD # end if # end intersectionOf_withRay def intersectionOf(self,withSegment:'Segments')->Points: """ Returns the point of intersection of two segments :raises NoPoint :raises InfinityPoints """ ps1=self.P1; ps2=self.P2; rs1=Rays.toRay(ps1,ps2); rs2=Rays.toRay(ps2,ps1); try: ip1=withSegment.intersectionOf_withRay(rs1); ip2=withSegment.intersectionOf_withRay(rs2); except InfinityPoints: if (withSegment.isPointInside(ps1) or withSegment.isPointInside(ps2)): raise InfinityPoints; else: raise NoPoint; # end if except NoPoint: raise NoPoint; else: assert ip1==ip2; return ip1; # end try # end intersectionOf def bisectorOf(self)->Lines: """ Returns the bisector to self, i.e. the line that is perpendicular to self and goes through its middle """ ln=self.toLine(); xMid=(self.P1.X+self.P2.X)/2.0; yMid=(self.P1.Y+self.P2.Y)/2.0; return Lines(-ln.B, ln.A, -ln.B*xMid + ln.A*yMid); # end bisectorOf @staticmethod def areNotIntersecting(segments:Sequence['Segments'])->None: """ The segments in the list must not intersect :raises InvalidDataError if two segments are intersecting """ for sgo in segments: for sgi in segments: if not sgo==sgi: try: sgo.intersectionOf(sgi); except NoPoint: pass; # Requirement except InfinityPoints: raise InvalidDataError('Overlaping segments'); else: raise InvalidDataError('Overlaping segments'); # end try # end if # end for # end for # end areNotIntersecting # end Segments #------------------------------------------------------------------------------# class Sectors(object): def __init__(self,startPoint:Points, direction1:Vectors, direction2:Vectors)->None: self.StartPoint=startPoint; self.Direction1=direction1.toUnitVector(); self.Direction2=direction2.toUnitVector(); # end __init__ def toClone(self)->'Sectors': return Sectors(self.StartPoint,self.Direction1,self.Direction2); def __str__(self)->str: return ('SP: '+str(self.StartPoint)+ ' | DV1: '+str(self.Direction1)+ ' | DV2: '+str(self.Direction2)); def __eq__(self,withSector:'Sectors')->bool: return ((self.StartPoint==withSector.StartPoint) and ((self.Direction1==withSector.Direction1) and (self.Direction2==withSector.Direction2)) or ((self.Direction1==withSector.Direction2) and (self.Direction2==withSector.Direction1))); @staticmethod def toSector(pt0:Points,pt1:Points,pt2:Points)->'Sectors': return Sectors(pt0,pt1-pt0,pt2-pt0); def openingAngleOf(self)->Real: """ Gibt den nicht-orientierten Öffnungswinkel zurück """ return abs(self.Direction2.dirAngleOf()-self.Direction1.dirAngleOf()); # end Sectors #------------------------------------------------------------------------------# class SectorWithSegments(Sectors): def __init__(self,startPoint:Points, direction1:Vectors, direction2:Vectors)->None: self.Segments=[]; Sectors.__init__(self,startPoint,direction1,direction2); # end __init__ def __str__(self)->str: output=''; output+=Sectors.__str__(self); output+=' || '; for sgm in self.Segments: output+=str(sgm)+' | '; return(output); # end __str__ def add(self,segment:Segments)->bool: """ Tries to add the segment to self """ p0=self.StartPoint; p1=segment.P1; p2=segment.P2; sec=Sectors.toSector(p0,p1,p2); if self==sec: self.Segments.append(segment); return True; else: return False; # end if # end add @staticmethod def addToList(secwsgm:Sequence['SectorWithSegments'], sgm:Segments)->None: """ Adds the segment to one of the sectors of the list :raises InvalidDataError if there is no sector suitable for the segment """ added=False; for sec in secwsgm: added=False; if sec.add(sgm): added=True; break; # end for if not added: raise InvalidDataError('addToList'); # end addToList def getMinimalDistanceSegment(self)->Segments: """ Returns the segment that is nearest to the StartPoint of the sector """ segments=self.Segments; toPoint=self.StartPoint; minSegment=segments[0]; minlg1=Vectors.lengthOf(minSegment.P1-toPoint); minlg2=Vectors.lengthOf(minSegment.P2-toPoint); for sgm in segments: lg1=Vectors.lengthOf(sgm.P1-toPoint); lg2=Vectors.lengthOf(sgm.P2-toPoint); if ((lg1None: self.P0=p0; self.P1=p1; self.P2=p2; def __str__(self)->str: return str(self.P0)+' | '+str(self.P1)+' | '+str(self.P2); def areaOf(self)->Real: return Points.areaOf(self.P0,self.P1,self.P2); def isPointInside(self,pt:Points)->bool: areaT=Points.areaOf(self.P0,self.P1,self.P2); # Barycentric coordinates with (alpha+beta+gamma)=1 alpha=Points.areaOf(pt,self.P1,self.P2)/areaT; beta =Points.areaOf(pt,self.P2,self.P0)/areaT; gamma=Points.areaOf(pt,self.P0,self.P1)/areaT; return (0.0None: self.NbPoints=len(points); self.Points=[]; self.Points+=points; # end __init__ def toClone(self)->'Polygons': return Polygons(self.Points); def __str__(self)->str: output=''; for pt in self.Points: output+=str(pt)+' | '; return(output); # end __str__ def distanceOf(self,pt:Points)->Real: dist=-1; for Ix in range(self.NbPoints-1): p1=self.Points[Ix]; p2=self.Points[Ix+1]; sgm=Segments(p1,p2); dist=min(maxLength,sgm.distanceOf(pt)); # end for p1=self.Points[1]; p2=self.Points[self.NbPoints-1]; sgm=Segments(p1,p2); dist=min(dist,sgm.distanceOf(pt)); return dist; # end distanceOf def areaOf(self)->Real: """ Returns the signed area of the polygon """ p0=self.Points[0]; area=0.0; for Ix in range(1,self.NbPoints-1): p1=self.Points[Ix]; p2=self.Points[Ix+1]; area+=Vectors.crossOp(p1-p0,p2-p0); # end for return 0.5*area; # end areaOf def isPointOnside (self,pt0:Points)->bool: """ Is a point onside the polygon? Onside means: The point is inside or on the edges of the polygon This algorithm is adapted from 'The GNAT Components Collection', www.adacore.com """ Ie=self.NbPoints-1; onInside= False; deltaY=-1.0; points=self.Points; for Ix in range(Ie): deltaY=pt0.Y-points[Ix].Y; if (((0.0<=deltaY and pt0.YNone: self.C=point; self.R=radius; # end def def toClone(self)->'Circles': return Circles(self.C,self.R); def __str__(self)->str: return str(self.C)+' | '+str(self.R); @staticmethod def toCircle(pt1:Points,pt2:Points,pt3:Points)->'Circles': """ Creates the circle through 3 points :raises NoCircle if points are on a line """ sgm1=Segments(pt1,pt2); sgm2=Segments(pt2,pt3); bis1=sgm1.bisectorOf(); bis2=sgm2.bisectorOf(); try: center=Lines.intersectionOf(bis1,bis2); except (NoPoint,InfinityPoints): raise Circles.NoCircle; # end try radius=pt1.distanceOf(center); return Circles(center,radius); # end toCircle def areaOf(self)->Real: return pi*self.R*self.R; def distanceOf(self,pt:Points)->Real: """ Returns the signed distance of a point to the circle """ d=Points.distanceOf(self.C,pt); return d-self.R; # end distanceOf def isPointInside(self,pt:Points)->bool: dist=self.distanceOf(pt); return (dist<0) and not areLengthsEqual(dist,0); # end isPointInside # end Circles #------------------------------------------------------------------------------# class PolygonTrains(object): # Polygonzug def __init__(self,points:Sequence[Points])->None: self.Points=[]; self.Points+=points; # end __init__ def getClone(self)->'PolygonTrains': return PolygonTrains(self.Points); def __str__(self)->str: output=''; for pt in self.Points: output+=str(pt)+' | '; return(output); # end __str__ def getSegments(self)->List[Segments]: """ Returns the segments of the polygon train """ la=self.Points; fIx=0; lIx=len(la)-1; lf=la[fIx:lIx]; ll=la[fIx+1:lIx+1]; return [Segments(*tp) for tp in list(zip(lf,ll))]; # end getSegments # end PolygonTrains #------------------------------------------------------------------------------# class ConesOfRays(object): # Strahlenbueschel def __init__(self,startPoint:Points)->None: self.StartPoint=startPoint; self.Rays=[]; # end __init__ def __str__(self)->str: output=''; for ray in self.Rays: angle=ray.Direction.dirAngleOf()*r2d; output+='\n'+str(angle)+' - '+str(ray)+' | '; # end for return(output); # end __str__ def add(self,ray:Rays)->None: """ Add the ray to self :raises InvalidDataError if start points of rays are different """ if not self.StartPoint==ray.StartPoint: raise InvalidDataError('different start points'); for elem in self.Rays: if elem==ray: return None; # No dublicates self.Rays.insert(0,ray); # end add @staticmethod def createFromPoints(startPoint:Points, points:Sequence[Points])->'ConesOfRays': """ Creates a cone of rays using a start point for the rays and some other points """ cone=ConesOfRays(startPoint); for pt in points: dirc=(pt-startPoint).toUnitVector(); cone.add(Rays(startPoint,dirc)); # end for return cone; # end createFromPoints # ConesOfRays #------------------------------------------------------------------------------# def divideIntersectedSegments(self,sgms:List[Segments])->None: """ Schneidet ein Strahl ein Segment im Innern, so wird dieses Segment in zwei Einzelsegmente zerlegt """ for ray in self.Rays: for sgm in sgms[:]: try: ip=sgm.intersectionOf_withRay(ray); except NoPoint: pass; except InfinityPoints: sgms.remove(sgm); # Segment is part of ray; no extension, no visibility else: sgm1=Segments(sgm.P1,ip); sgms.append(sgm1); sgm2=Segments(sgm.P2,ip); sgms.append(sgm2); sgms.remove(sgm); # end try # end for # end for # divideIntersectedSegments # end ConesOfRays #------------------------------------------------------------------------------# #------------------------------------------------------------------------------# if __name__ == '__main__': print('\n# Beginning main ...'); print('\n# Finished main.\n'); # end if main #------------------------------------------------------------------------------# #------------------------------------------------------------------------------#