def getAngleFromLongestEdge(areaPoly, origin=None): """ finds the angle of the longest edge in relation to the x-axis. if there is a draw, the edge closest to the origin wins. This function is only usable for konvex polygons, but works for concave as well. the square distance is used since it's faster to compute """ if not origin: origin=(0,0) last=areaPoly[-1] longest=None dmax=0 for node in areaPoly: d2=fun.getDistanceSq(node, last) if d2==dmax: #look for distance to origin dtmp=min([fun.getDistanceSq(node, origin), fun.getDistanceSq(last, origin)]) dtmp2=min([fun.getDistanceSq(node, longest[0]), fun.getDistanceSq(last, longest[1])]) if dtmp<dtmp2: longest = (node,last) dmax=d2 elif d2>dmax: longest = (node,last) dmax=d2 last=node return fun.angleToXAxis(longest)
def getAngleFromLongestEdge(self, areaPoly=None, origin=None): """ finds the angle of the longest edge in relation to the x-axis. if there is a draw, the edge closest to the origin wins. This function is only usable for konvex polygons, but works for concave as well. the square distance is used since it's faster to compute the shift thing only works if the origin is in the "lower left" corner. """ if not origin: origin=self.origin if not areaPoly: areaPoly=self.areaPoly last=areaPoly[-1] longest=None dmax=0 for node in areaPoly: d2=fun.getDistanceSq(node, last) if d2==dmax: #look for distance to origin dtmp=min([fun.getDistanceSq(node, origin), fun.getDistanceSq(last, origin)]) dtmp2=min([fun.getDistanceSq(node, longest[0]), fun.getDistanceSq(last, longest[1])]) if dtmp<dtmp2: longest = (node,last) dmax=d2 elif d2>dmax: longest = (node,last) dmax=d2 last=node #now, calculate the distance to this line. #we need to make a line, not a ray, in order to get the extension as well. infRay=np.array(longest) infRay=infRay+1e5*(infRay-infRay[1])+1e5*(infRay-infRay[0]) #infinite extension of longest pTmp, t=col.closestLinePoint(origin, infRay, True) assert t!=1 and t!=0 d=fun.getDistance(pTmp,origin) assert d>=0 #now, we know that d+shift=n*L+C..where n is an integer n=round((d-self.C)/float(self.L)) shift=self.L*n+self.C-d assert abs(shift)<=self.L angle=fun.angleToXAxis(longest) assert angle>=0 if 0<angle<=pi/2: #shift x negative shift=(-shift,0) elif pi/2<angle<=pi: #shift y negative shift=(0,-shift) elif pi<angle<=3*pi/2.0: #shift x positive shift=(shift,0) else: shift=(0, -shift) return angle, shift
def closestPolygonPoint(pos, nodes): """ algorithm: go through all the edges, calculate closest old note: closest point in polygon not defined if point is INSIDE polygon. .. I think this is incorrect by inspecting the code. should work inside the polygon as well. BUT I have not tested this.. Skip assertions since I will use it for pos inside polygon. """ d=1e10 closest=None index=0 for node in nodes: last=nodes[int(index-1)] edge=np.array((last, node)) P=closestLinePoint(pos, edge) dist=fun.getDistanceSq(pos, P) if dist<d: closest=P d=dist index+=1 assert d<1e10 return [closest[0], closest[1]] #not the numpy array, jsut a list
def getNextTree(self, road=None):
"""returns the tree that is closest to the machine, i.e lowest y-value in road cart.coord.sys.
also does analysis if a tree has the desired proportions to be chopped. """
if not road:
if not self.road: raise Exception('getNextTree can only be called when a road is assigned')
road=self.road
if len(road.trees)==0: return False
closest=100000
t=None
for tree in road.trees:
d2=getDistanceSq(self.m.pos, tree.pos)
if not tree.harvested and d2<closest:
closest=d2
t=tree
if not t: print road.pos, "did not find any trees"
return t
def closestPolygonPoint(pos, nodes):
"""
algorithm: go through all the edges, calculate closest point.
"""
#if pointInPolygon(pos,nodes): raise Exception('closest point in polygon not defined if point is INSIDE polygon.') #removed for speedup
d=10000
closest=None
index=0
for node in nodes:
last=nodes[int(index-1)]
edge=np.array((last, node))
P=closestLinePoint(pos, edge)
dist=fun.getDistanceSq(pos, P)
if dist<d:
closest=P
d=dist
index+=1
return [closest[0], closest[1]] #not the numpy array, jsut a list
