def getRoll(R,p1,p2,points=20,style='naive'): """ Provided choice of style is 'naive' or 'weighted', this method outputs the roll along a road specified by the start point p1 and end point p2. """ if not R: raise Exception('getRoll need the graph R to get interpolation of the terrain data.') alpha=atan2((p2[0]-p1[0]),(p2[1]-p1[1])) length=fun.getDistance(p1,p2) piInv=1/pi roll=[] p11=fun.getCartesian([-R.roadWidth*0.5,0], direction=pi*0.5-alpha, origin=p1,fromLocalCart=True) p12=fun.getCartesian([R.roadWidth*0.5,0], direction=pi*0.5-alpha, origin=p1,fromLocalCart=True) p21=fun.getCartesian([-R.roadWidth*0.5,0], direction=pi*0.5-alpha, origin=p2,fromLocalCart=True) p22=fun.getCartesian([R.roadWidth*0.5,0], direction=pi*0.5-alpha, origin=p2,fromLocalCart=True) x1=np.linspace(p11[0], p21[0], points) x2=np.linspace(p12[0], p22[0], points) y1=np.linspace(p11[1], p21[1], points) y2=np.linspace(p12[1], p22[1], points) z1=R.interpol.ev(x1,y1) z2=R.interpol.ev(x2,y2) if style=='naive':#This is the most naive method, called naiveroll in intro_interpol.py for ent in range(len(z1)): roll.append(180*piInv*atan2((z2[ent]-z1[ent]),R.roadWidth)) elif style=='weighted':#This method was originally called GISroll or GIScopy-method in intro_interpol.py for ent in range(len(z1)): if ent==0: roll.append(180*piInv*atan2(((2*z2[ent]+z2[ent+1])-(2*z1[ent]+z1[ent+1])),6*R.roadWidth*0.5)) elif ent==len(z1)-1: roll.append(180*piInv*atan2(((z2[ent-1]+2*z2[ent])-(z1[ent-1]+2*z1[ent])),6*R.roadWidth*0.5)) break else: roll.append(180*piInv*atan2(((z2[ent-1]+2*z2[ent]+z2[ent+1])-(z1[ent-1]+2*z1[ent]+z1[ent+1])),8*R.roadWidth*0.5)) else: raise Exception('getRoll need a correct style to be supplied at call') return roll
def getBPos(self, direction=None): """ Updates the position of the bundler each time the machine moves. This method is called from the setpos in thMachine """ if direction==None: direction=pi/2. return getCartesian([self.s['dropPosJ'],pi/2.],origin=self.m.pos,direction=direction)
def dumpBundle(self, direction=None): """ Releases the bundle at the current position. (And dumps the bundle in terrain) """ if direction is None: direction=pi/2 #here the nodes of the bundle are set when the bundle is put in the terrain cB=self.currentBundle cB.pos=np.array(self.pos)+np.array([-2.5,0])# c1=getCartesian([-cB.diameter/2,cB.length/2], origin=cB.pos, direction=direction, fromLocalCart=True) c2=getCartesian([-cB.diameter/2, -cB.length/2], origin=cB.pos, direction=direction, fromLocalCart=True) c3=getCartesian([cB.diameter/2, -cB.length/2], origin=cB.pos, direction=direction, fromLocalCart=True) c4=getCartesian([cB.diameter/2,cB.length/2], origin=cB.pos, direction=direction, fromLocalCart=True) cB.nodes=[c1,c2,c3,c4] self.m.G.terrain.piles.append(cB)#adds the pile to the list of piles in terrain self.m.G.terrain.addObstacle(cB) print '*SAVED the bundle with',len(self.m.G.terrain.piles[-1].trees),'trees at pos:',cB.pos
def plot_coverage(G=None, ax=None, color='#666677'): if not G or not ax: raise Exception() points=15 angles=np.linspace(0, np.pi, points) #for half circle for e in G.edges(data=False): """p1=np.array(e[0]) p2=np.array(e[1]) d=p1-p2 circles=int(np.sqrt(np.dot(d,d))/6.)#one every 6m for i in range(circles): p=p2+d*(i+1)/(circles+1) c=Circle(p, radius=G.C, alpha=470, color='#666677') ax.add_patch(c)""" x=e[0][0], e[1][0] y=e[0][1], e[1][1] try: w=G.C*2 except: w=24 nodes=[] dir=fun.angleToXAxis(e)-np.pi/2. l=fun.getDistance(e[0], e[1]) nodes.append(fun.getCartesian([w/2.,0], origin=e[0], direction=dir, fromLocalCart=True)) nodes.append(fun.getCartesian([w/2.,l], origin=e[0], direction=dir, fromLocalCart=True)) #now, fix end half circle. for ang in angles: loc=fun.getCartesian([w/2., ang]) #get local cartesian from cyl. p=fun.getCartesian(loc, origin=e[1], direction=dir, fromLocalCart=True) nodes.append(p) nodes.append(fun.getCartesian([-w/2.,l], origin=e[0], direction=dir, fromLocalCart=True)) nodes.append(fun.getCartesian([-w/2.,0], origin=e[0], direction=dir, fromLocalCart=True)) #fix the other half circle for ang in angles: loc=fun.getCartesian([w/2., ang]) #get local cartesian from cyl. p=fun.getCartesian(loc, origin=e[0], direction=dir-np.pi, fromLocalCart=True) nodes.append(p) pol = Polygon(nodes, alpha=150, color=color, edgecolor='none') ax.add_patch(pol) return ax
def plot_coverage(G=None, ax=None, color='#666677'): """ plots coverage from the road. Shows overlaps and spots missed. """ if not G or not ax: raise Exception('need ax and G to plot coverage') points=15 angles=np.linspace(0, np.pi, points) #for half circle for e in G.edges(data=False): p1=np.array(e[0]) p2=np.array(e[1]) d=p1-p2 x=e[0][0], e[1][0] y=e[0][1], e[1][1] w=G.C*2 nodes=[] r,dir=fun.getCylindrical(e[1], origin=e[0]) l=fun.getDistance(e[0], e[1]) nodes.append(fun.getCartesian([w/2.,0], origin=e[0], direction=dir, fromLocalCart=True)) nodes.append(fun.getCartesian([w/2.,l], origin=e[0], direction=dir, fromLocalCart=True)) #now, fix end half circle. for ang in angles: loc=fun.getCartesian([w/2., ang]) #get local cartesian from cyl. p=fun.getCartesian(loc, origin=e[1], direction=dir, fromLocalCart=True) nodes.append(p) nodes.append(fun.getCartesian([-w/2.,l], origin=e[0], direction=dir, fromLocalCart=True)) nodes.append(fun.getCartesian([-w/2.,0], origin=e[0], direction=dir, fromLocalCart=True)) #fix the other half circle for ang in angles: loc=fun.getCartesian([w/2., ang]) #get local cartesian from cyl. p=fun.getCartesian(loc, origin=e[0], direction=dir-np.pi, fromLocalCart=True) nodes.append(p) pol = Polygon(nodes, alpha=150, color=color, edgecolor='none') ax.add_patch(pol) return ax