def __init__(self, pos=None, nodes=None, G=None, direction=None, endPoints=None, width=4, radius=None): if not nodes and not endPoints: raise Exception('too little information about road. Endpoints or nodes is needed') self.G=G self.direction=direction #if none, fixnodes will take care of it if not nodes: #construct from endpoints and width nodes=self._fixNodes(endPoints, width) elif not endPoints: #construct from nodes if len(nodes)<3: raise Exception('Road instance must have more than two edges.') #if not direction: raise Exception('if only nodes are given for road, direction is needed as well.') self.nodes=nodes self.endPoints=endPoints self.color='g' self.isSpherical=False if not pos: pos= self.getPos() #pretty time consuming... self.pos=pos if radius: self.radius=radius else: self.radius=-1 for n in self.nodes: d=fn.getDistance(n,self.pos) if d>self.radius: self.radius=d if endPoints: self.length=fn.getDistance(self.endPoints[0], self.endPoints[1]) self.harvestedTrees=[]
def correct_outlier(data, col1, col2, i1, max_tolerated_movement): """ Recursive function to adjust distance between i1 and i1+1 so that it is not marked as outliers. Recursively removes outliers created this way I am pretty sure this is a more special case of correct_wrongly_interpolated_outliers. But refactoring is not the job now """ n_rows, n_cols = data.shape assert i1 + 1 < n_rows replace_point_by_middle(data, col1, i1, i1 + 1, i1 + 2) replace_point_by_middle(data, col2, i1, i1 + 1, i1 + 2) dis_1 = functions.getDistance(data[i1, col1], data[i1, col2], data[i1 + 1, col1], data[i1 + 1, col2]) if i1 + 2 < n_rows: dis_2 = functions.getDistance(data[i1 + 1, col1], data[i1 + 1, col2], data[i1 + 2, col1], data[i1 + 2, col2]) if abs(dis_1) > max_tolerated_movement: if i1 == 0: # Edge case when you are at beginning, replace with value above data[i1, col1] = data[i1 + 1, col1] data[i1, col2] = data[i1 + 1, col2] else: # Recursively adjust values correct_outlier(data, col1, col2, i1 - 1, max_tolerated_movement) if i1 + 2 < n_rows and abs(dis_2) > max_tolerated_movement: if i1 + 3 == n_rows: # Edge case when you are at the start, replace with value below data[i1 + 2, col1] = data[i1 + 1, col1] data[i1 + 2, col2] = data[i1 + 1, col2] else: # Recursively adjust values correct_outlier(data, col1, col2, i1 + 1, max_tolerated_movement)
def UpdateVertexTheta(self,s, sPrime, queue, goal): val=self.graph.getVertex(sPrime) parentS=int(self.parentTheta[s]) parentSVal=self.graph.getVertex(parentS) #print "UpdateVertex: " + str(s) + " " + str(sPrime) + " " + str(self.parentTheta[s]) if( self.lineOfSight( self.parentTheta[s], sPrime,self.obstacles) ==True): # Path 2 if((self.pathCostT[parentS]+functions.getDistance(parentSVal[0], parentSVal[1], val[0], val[1])) < self.pathCostT[sPrime]): self.pathCostT[sPrime] = self.pathCostT[parentS]+functions.getDistance(parentSVal[0], parentSVal[1], val[0], val[1]) self.parentTheta[sPrime] = self.parentTheta[s] #print str(self.parentTheta[sPrime]) + " " + str(sPrime) # insert into open list checkVertex=self.graph.getVertex(sPrime) if(checkVertex[3]==1): cpyQueue=queue queue=functions.removeFromHeap(cpyQueue, sPrime) distance=functions.getDistanceToGoal(val[0],val[1], goal) queue.push([self.pathCostT[sPrime]+distance,self.pathCostT[sPrime], sPrime, True]) #mark it, and give it f, g, h values VertexInfo=self.graph.getVertex(sPrime) VertexInfo[3]=1 VertexInfo[7]=self.pathCostT[sPrime] VertexInfo[8]=distance VertexInfo[9]=self.pathCostT[sPrime]+distance self.graph.setVertex(sPrime, VertexInfo) else: # Path 1 sVal=self.graph.getVertex(s) if( ( self.pathCostT[s]+functions.getDistance(sVal[0], sVal[1], val[0], val[1])) < self.pathCostT[sPrime] ): self.pathCostT[sPrime] = self.pathCostT[s]+functions.getDistance(sVal[0], sVal[1], val[0], val[1]) self.parentTheta[sPrime] =s #print str(self.parentTheta[sPrime]) + " " + str(sPrime) # insert into open list checkVertex=self.graph.getVertex(sPrime) if(checkVertex[3]==1): cpyQueue=queue queue=functions.removeFromHeap(cpyQueue, sPrime) distance=functions.getDistanceToGoal(val[0],val[1], goal) queue.push([self.pathCostT[sPrime]+distance,self.pathCostT[sPrime], sPrime, True]) #mark vertex, and assign f,g, h VertexInfo=self.graph.getVertex(sPrime) VertexInfo[3]=1 VertexInfo[7]=self.pathCostT[sPrime] #g VertexInfo[8]=distance #h VertexInfo[9]=self.pathCostT[sPrime]+distance self.graph.setVertex(sPrime, VertexInfo) return queue
def generateTrees(self,dens, dbh_mean, dbh_std): """ Generates trees. Can only handle square areas right now. This is only done once, so code is not optimized. """ self.dbh_mean=dbh_mean self.dens=dens self.dbh_std=dbh_std trees=dens*self.area idealCell=[5.0,5.0] W=self.xlim[1]-self.xlim[0] L=self.ylim[1]-self.ylim[0] if self.G: self.G.xlim=self.xlim #override. The polynomial for A given to terrain is master self.G.ylim=self.ylim cellW=W/float(ceil(W/idealCell[0])) #ideal 5*5m, but adapt to the polygon cellL=L/float(ceil(L/idealCell[1])) cellA=cellW*cellL treesPerCell=int(floor(trees*cellA/self.area)) #if the whole cell is in areaPoly xAr=np.arange(self.xlim[0], stop=self.xlim[1], step=cellW) #will omitt last row in x and y, correct yAr=np.arange(self.ylim[0], stop=self.ylim[1], step=cellW) n=0 for x in xAr: for y in yAr: if not col.pointInPolygon((x,y), self.areaPoly): continue tint=[] neighGrids=self.getNeighborObst([x,y]) for i in range(treesPerCell): r=-1 while r<0: r=random.normalvariate(dbh_mean, dbh_std)*0.5 placed=False while not placed: pos=[x+random.uniform(0,cellW), y+random.uniform(0,cellL)] placed=True for neigh in tint: if getDistance(pos, neigh.pos) < r+neigh.radius: placed=False break #just for negih, while is still on if pos[0]-x<r or pos[0]-x>cellW-r or pos[1]-y<r or pos[1]-y>cellL-r: #iterate through neighboring trees. for neigh in neighGrids: if getDistance(neigh.pos, pos)<r+neigh.radius: #collide placed=False break t=Tree(pos, r, terrain=self) if random.uniform(0,1)<0.4: t.marked=True tint.append(t) n=n+1 print "trees left over during generation: ",trees-n, "this is a problem, solve it..." """thoughts around this problem. These trees needs to be distributed inside the cells,
def correct_wrongly_interpolated_outliers(data, col1, col2, out_group, max_tolerated_movement): """ Interpolates the values newly for which due to prediction mistakes data was interpolated wrongly """ n_rows, n_cols = data.shape assert n_cols > 1 assert len(out_group) > 2 i_first = out_group[0] if out_group[-1] + 1 < n_rows: # Interpolate values newly between first and last non outlier value i_last = out_group[-1] + 1 n_vals = len(out_group) fill_steps_in(data, col1, i_first, i_last, n_vals) # x fill_steps_in(data, col2, i_first, i_last, n_vals) # y else: # Edge Case, you reached the end of the file (I wonder if this code will ever be executed) print("OH MY GOD, THIS REALLY HAS BEEN EXECUTED") # Take vector from last defined value i_last = out_group[-1] fill_vector_in(data, col1, i_first, i_last) fill_vector_in(data, col2, i_first, i_last) # Check if distance for first value has gotten better, (others are checked later anyway) dis = functions.getDistance(data[i_first, col1], data[i_first, col2], data[i_first + 1, col1], data[i_first + 1, col2]) if abs(dis) > max_tolerated_movement: # Enter fixing loop correct_outlier(data, col1, col2, i_first, max_tolerated_movement)
def treeStats(self): """ calculates some statistics.. should maybe be done in terrain class instead? """ trees=float(len(self.G.terrain.trees)) self.saveGeneralStats("stand no", self.G.terrain.treeFile, "-") #see simSeries for how it works.. self.saveGeneralStats("trees", trees, "-") if trees==0: return None self.saveGeneralStats("pine percentage", 100*len([t for t in self.G.terrain.trees if t.specie=='pine'])/trees, "%") self.saveGeneralStats("spruce percentage", 100*len([t for t in self.G.terrain.trees if t.specie=='spruce'])/trees, "%") self.saveGeneralStats("leaf tree percentage", 100*len([t for t in self.G.terrain.trees if t.specie=='leaf'])/trees, "%") treeDensity=trees/self.G.terrain.area self.saveGeneralStats("tree density", treeDensity, "trees/m2") mLogs=len(self.m.mainRoadTrees) mNum=0 cLogs=len(self.m.corridorTrees) cNum=0 for r in self.m.roadList: if r.radius>2*self.m.craneMaxL/2.+2: #main road. mNum+=1 else: cNum+=1 self.cNum=cNum self.mNum=mNum self.mLogs=mLogs self.cLogs=cLogs cWeight=sum([t.logWeight for w in self.m.corridorTrees ]) mWeight=sum([t.logWeight for w in self.m.corridorTrees ]) print "number of main roads:", mNum, "number of corridors:", cNum if cNum==0: self.avTreesPerCorr=0 else: self.avTreesPerCorr=cLogs/float(cNum) self.treesPerRoad=mLogs if cLogs==0: self.avCorrTreeWeight=0 else: self.avCorrTreeWeight=cWeight/float(cNum) if mLogs==0: self.avRoadTreeWeight=0 else: self.avRoadTreeWeight=mWeight/float(mNum) #the cumulative sum #clustering... see Clark and Evans 1954 rexp=0.5*1/(sqrt(treeDensity)) #expected r robs=0 for t in self.G.terrain.trees: tmp=[] R=5 while len(tmp)==0 and R<sqrt(self.G.terrain.area): #in 99.9% of the cases, this loop only runs once. #we are not using self.G.terrain.trees for speedup reasons. tmp=self.G.terrain.GetTrees(t.pos, R) R+=2 if len(tmp)==0: self.saveGeneralStats("clustering agg. index", "no trees", "trees/m2") break shortest=1e10 for treeTmp in tmp: if treeTmp==t: continue #same tree d=fun.getDistance(t.pos, treeTmp.pos) if d<shortest: shortest=d if shortest==1e10: raise Exception('could not find shortest tree...') robs+=shortest robs/=trees print "rexp:", rexp, "robs.", robs aggregationIndex=robs/rexp self.saveGeneralStats("clustering agg. index", aggregationIndex, "-")
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 collide(o1, o2, o1pos=None, o2pos=None): """ This function is the main function, it categorizes the objects and make the required tests Some functions requre """ if not o1pos: o1pos=o1.pos if not o2pos: o2pos=o2.pos d=fun.getDistance(o1pos,o2pos) if d>=o1.radius+o2.radius: #the bounding volumes, spheres/circles in this case return False if o1.isSpherical and o2.isSpherical: return True #collides elif o1.isSpherical or o2.isSpherical: #one sphere, one polygon..? if o1.isSpherical: s=o1 spos=o1pos o=o2 opos=o2pos else: s=o2 spos=o2pos o=o1 opos=o1pos nodes=o.getNodes(opos) if pointInPolygon(s.pos, nodes): return True index=0 for node in nodes: last=nodes[int(index-1)] if last != node: edge=np.array((last, node)) if intersectRaySphere(edge, s.radius, spos): return True #one of the rays collided. index+=1 return False #all edges tested, does not collide else: #two polygons #check if o2 is inside o1 and vice versa: nodes1=o1.getNodes(o1pos) nodes2=o2.getNodes(o2pos) if pointInPolygon(nodes2[0], nodes1) or pointInPolygon(nodes1[0], nodes2): return True index1=0 for node1 in nodes1: #loop through all the edges in both polygons last1=nodes1[int(index1-1)] if last1 != node1: ray1=np.array([last1, node1]) index2=0 for node2 in nodes2: last2=nodes2[int(index2-1)] if last2 != node2: ray2=np.array([last2,node2]) if linesIntersect(ray1, ray2): return True index2+=1 index1+=1 return False #did not collide
def pickup(self, treeList=None): """ picups the trees at the current location. Adds to cargo sums of trees and handles all connected to this returns False if cargo is full. if treeList is given it consists of the trees in range at current pos """ cmnd=[] trees=[] if treeList: roadTrees=treeList #faster else: roadTrees=self.pickup_road[2]['r'].harvestedTrees for t in roadTrees: d=fun.getDistance(self.pos, t.pos) if d<self.craneMax: #reachable t.tmpDist=d trees.append(t) trees=sorted(trees, key=lambda t: -t.tmpDist) #sort, yayy tmpList=[] told=None breakOut=False while True: #until we break out try: t=trees.pop() except IndexError: #empty list breakOut=True #but not yet.. at end of list.. told=None if len(tmpList)==0 or (told and fun.getDistance(t.pos,told.pos)<0.1): #we can grip them together! tmpList.append(t) told=t continue #since told!=None this is valid #pickup single tree, or cumulative tree bunch if len(tmpList)>0: #pickup the accumulated trees for tTmp in tmpList: if self.reached_limit(tTmp): cmnd.append(False) print "cargo full!" break else: self.removeTree(tTmp) #also saves statistics. Saves memory tmpList.remove(tTmp) #local variable cmnd.append((hold, self, self.times['pickUpTrees'])) if breakOut: break tmpList=[t] #new list. told=t return cmnd
def avg_dis(data, col1, col2, i_start, i_end, n_points): """ Computes the average distance between i_start and i_end for given amount of n_points """ assert n_points > 1 return functions.getDistance(data[i_start, col1], data[i_start, col2], data[i_end, col1], data[i_end, col2]) / (n_points - 1)
def pointInCircle(p, pm, r): """ pm: middle of circle only works for 2D """ d=fun.getDistance(p,pm) if d<=r: return True else: return False
def normalizedPitchDist(R, x, y, z): """ see the documentation. Look at the two extreme values for the pitch roll not in there yet. Also does only care for the extreme values.. not really optimal """ A = 0.3 # importance of distance compared to pitch B = 1.0 - A alim = 15 * pi / 180.0 imin, zmin = min(enumerate(z), key=operator.itemgetter(1)) imax, zmax = max(enumerate(z), key=operator.itemgetter(1)) alpha = atan(zmax - zmin) / fun.getDistance((x[imin], y[imin]), (x[imax], y[imax])) if alpha > alim: return 1e15 d = fun.getDistance((x[0], y[0]), (x[-1], y[-1])) w = d * (A + B * alpha / alim) return w
def weightFunctionFirst(R, x, y, z): """ a weight function that has 3 vectors with positions as input and returns a weight that is independent of the sampling rate etc. The first one is just a simple test. """ d = fun.getDistance((x[0], y[0]), (x[-1], y[-1])) mu = np.mean(z) w = d + sum(abs(z - mu)) / len(z) return w
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 setupDefaultRoadNet(G): """setups the default net, with two loops""" G.roadNet=nx.Graph() rN=G.roadNet nodes=[(12,0), (36,0), (60,0), (84,0), (84,70), (60,70), (36,70), (12,70)] rN.add_nodes_from(nodes) edges=[((12,0),(36,0)), ((36,0),(60,0)), ((60,0),(84,0)), ((84,0),(84,70)), ((84,70),(60,70)), ((60,70),(60,0)), ((36,0),(36,70)),((36,70), (12,70)), ((12,70),(12,0)), ((60,70),(36,70))] rN.add_edges_from(edges) for e in rN.edges(data=True): e[2]['weight']=fun.getDistance(e[0], e[1]) for e in rN.edges(data=True): e[2]['r']=Road(endPoints=e[0:2])
def edgeWeightCalc(self,p1,p2): """ calculates the weight of the edge between p1 and p2. So far this is just a simple example, but it should be expanded. reason for using self.weightFunction is that several externa functions can be tested.. """ d=fun.getDistance(p1,p2) x,y,z=tt.getLineElevationCurve(self,p1,p2, points=max(5, int(d/2))) #every 2 m at least.. w=self.weightFunction(self,x,y,z) return w
def getClosestTree(self): """ returns the closest tree in road """ closest=None dist=1e10 for t in self.pickup_road[2]['r'].harvestedTrees: d=fun.getDistance(t.pos, self.pos) if d<dist: closest=t dist=d return closest
def _getWallRays(self, start_pos, look_vector): pos_x_axis = np.array([1, 0]) angle_pos_x_axis_look_vector = getAngle(look_vector, pos_x_axis) distances = [0 for i in range(0, len(self._wall))] for i in range(0, len(self._wall)): #first we create a vector that has a certain degree to our look vector angle_relative_to_look_vector = (angle_pos_x_axis_look_vector + (360 - self._wall[i])) % 360 new_ray = np.array([ math.cos(math.radians(angle_relative_to_look_vector)), math.sin(math.radians(angle_relative_to_look_vector)) ]) #and now we check where it collides with which wall and compute the distance to that wall for j in range(0, len(self._wall_lines)): intersection = get_intersect( (self._wall_lines[j][0], self._wall_lines[j][1]), (self._wall_lines[j][2], self._wall_lines[j][3]), (start_pos[0], start_pos[1]), (start_pos[0] + new_ray[0], start_pos[1] + new_ray[1])) #check if it is between the two points of the line and if it is in max_view_range if intersection[0] >= min( self._wall_lines[j][0], self._wall_lines[j][2]) and intersection[0] <= max( self._wall_lines[j][0], self._wall_lines[j][2]) and getDistance( start_pos[0], start_pos[1], intersection[0], intersection[1]) < self._max_view_range: distances[i] = ( 1 - getDistance(start_pos[0], start_pos[1], intersection[0], intersection[1]) / self._max_view_range) break return distances
def find_next_road(self): """ locates the next road segment with trees to pickup, and saves it in self.pickup_road. If no road was found, pickup_road == None The algorithm will increase in complexity, starting with a simple one. The road furthest away, with trees left to pickup, is currently returned """ best=None for road in self.roadNet.edges_iter(data=True): r=road[2]['r'] if len(r.harvestedTrees)>0: d=fun.getDistance(r.pos,self.origin) r.dist_to_origin=d if not best or d>best[2]['r'].dist_to_origin: best=road return best
def run(self): tic=time.clock() rN=self.G.roadNet for e in rN.edges_iter(data=True): r=e[2]['r'] direction=r.direction tList=self.G.terrain.GetTrees(r.pos, self.reach+r.radius, onlyNonHarvested=True) ray=np.array(r.endPoints) for t in tList: p1, tval=col.closestLinePoint(t.pos, ray, additionalInfo=True) #tval will be used later, if col.collide(t, r): self.chop(t, p1, tval, r) #could get into problems if t is on ray... hmm.. elif t.marked and fn.getDistance(p1, t.pos)<=self.reach: #marked foo thinning self.chop(t, p1, tval, r) print "harvester routine took %.4f seconds"%(time.clock()-tic)
def getnViewSingle(posFish, posOther, nnodes, nfish=3): """ Same as above but for one row """ center = np.array(posFish[2:4]) head = np.array(posFish[0:2]) vec_ch = head - center out = np.empty((nnodes * (nfish - 1) * 2)) for f in range(nfish - 1): for n in range(nnodes): ixy = [2 * nnodes * f + 2 * n, 2 * nnodes * f + 2 * n + 1] # node - center vec_cn = posOther[ixy] - center out[nnodes * 2 * f + 2 * n] = getDistance( (posOther[ixy])[0], (posOther[ixy])[1], center[0], center[1]) out[nnodes * 2 * f + 2 * n + 1] = getAngle(vec_ch, vec_cn, "radians") return out
def plotWorstRoad(R, ax1, ax2): # just for show.. worst = None inf = 1e15 for edge in R.edges(data=True): if not worst or edge[2]["weight"] > worst[2]["weight"] and edge[2]["weight"] < inf: worst = edge # plot road on road-net graph. print "worst road, weight:", worst[2]["weight"] from matplotlib.lines import Line2D l = Line2D((worst[0][0], worst[1][0]), (worst[0][1], worst[1][1]), color="r", lw=5) ax1.add_line(l) # plot elevation curve. x, y, z = R.getLineElevationCurve(worst[0], worst[1], points=max(5, int(fun.getDistance(worst[0], worst[1]) * 5))) x0, y0 = worst[0] d = np.sqrt((x - x0) ** 2 + (y - y0) ** 2) ax2.plot(d, z, lw=2)
def setPos(self,pos): """ Changes position for machine, and records monitors associated with this. Overrides superclass method """ dnorm=fun.getDistance(pos, self.pos) if self.sim.vis and dnorm >0.00001: d=np.array(np.array(pos)-np.array(self.pos)) th=acos(np.dot(d,np.array([1,0])/dnorm)) if d[1]<0: self.direction=2*pi-th else: self.direction=th if self.moveEvent: self.moveEvent.signal() #tell the world that a movement is about to occur. traveltime=self.velocities['machine']*dnorm self.pos=pos self.distTot+=dnorm self.distMoni.observe(self.distTot, self.sim.now()) return traveltime
def convLocToCart( loc, startpoints ): """ Converts locomotion np array to coordinates, assumens first fish "looks" upwards loc: 2d array, per row 3 entries per fish, [linear movement, angulare movement, turn movement]: [ [fish1_lin, fish1_ang, fish1_trn, fish2_lin, fish2_ang, fish2_trn, ...] [fish1_lin, fish1_ang, fish1_trn, fish2_lin, fish2_ang, fish2_trn, ...] ... ] startpoints: two nodes per fish exactly: [head1_x, head1_y, center1_x, center1_y, head2_x, head2_y, center2_x, center2_y,...] Output: [ [center1_x, center1_y, orientation1, ...] [center1_x, center1_y, orientation1, ...] ... ] """ row, col = loc.shape assert row > 1 assert col % 3 == 0 nfish = col // 3 assert len(startpoints) / nfish == 4 # save [center1_x, center2_y, orientation1, center2_x, ...] for every fish out = np.empty([row + 1,nfish * 3]) # 1. Distances Center - Head, out setup disCH = [] for f in range(nfish): disCH.append( getDistance( startpoints[4 * f], startpoints[4 * f + 1], startpoints[4 * f + 2], startpoints[4 * f + 3] ) ) out[0,3 * f] = startpoints[4 * f + 2] out[0,3 * f + 1] = startpoints[4 * f + 3] # Angle between Fish Orientation and the unit vector out[0,3 * f + 2] = getAngle( (1,0,), (startpoints[4 * f] - startpoints[4 * f + 2], startpoints[4 * f + 1] - startpoints[4 * f + 3],), "radians" ) for i in range(0, row): out[i + 1] = row_l2c( out[i], loc[i] ) return convPolarToCart( out, disCH )
def _getFishRays(self, fish_id, row, look_vector): return_list = [[], []] for i in range(0, len(self._fishes)): if i != fish_id: #get a vector to each other fish, from the current fish in question vector_to_fish = np.array([ self._fishes[i][row][2] - self._fishes[fish_id][row][2], self._fishes[i][row][3] - self._fishes[fish_id][row][3] ]) distance = getDistance(self._fishes[i][row][2], self._fishes[i][row][3], self._fishes[fish_id][row][2], self._fishes[fish_id][row][3]) angle = getAngle(look_vector, vector_to_fish) return_list[0].append(angle) return_list[1].append(distance) return return_list
def dumpTree(self, tree, pos, tval, road): """releases the trees at the current position. pos is in the middle of the road, but dumps at the side.""" if tree.getNodes(): raise Exception('Tree has already been choped..') dir=road.direction cart=fn.getCartesian #get the dump-pos: L=road.length l=tree.h w=self.roadWidth #so, we want it on the long edge of the road, w/2+l/2 from the extreme edge=l*0.5+w*0.5 a=edge/float(L) #the ratio. if a>0.5 we cannot place trees beautifully.. tlim=[a, 1-a] [pl, pm] =road.endPoints if a<0.5: if tval>=tlim[1]: #over the limit, tval= np.dot(n, C-A)/(np.dot(n, B-A)) linear.. #skip the side for now, just put it at a good position. p=cart([-w*0.6, -edge], direction=dir, fromLocalCart=True, origin=pm) elif tval<=tlim[0]: #over the limit on the other side of the road p=cart([-w*0.6, edge], direction=dir, fromLocalCart=True, origin=pl) else: p1=np.array(pos) tp=np.array(tree.pos) d=tp-p1 p=list(p1+0.6*w*d/fn.getDistance(p1,tp)) #pretty beautiful. This is the dump spot. 0.6 to get beside the road else: p=cart([-w*0.6, 0], direction=dir, fromLocalCart=True, origin=road.pos) tree.isSpherical=False tree.color='#5C3317' #brown, same as stumps dth=np.pi*0.05 direct=random.uniform(dir-dth, dir+dth) r=tree.radius ainv=0.5 tree.pos=p c1=cart([-r, l*ainv], origin=p, direction=direct, fromLocalCart=True) c2=cart([-r, 0], origin=p, direction=direct, fromLocalCart=True) c3=cart([r, 0], origin=p, direction=direct, fromLocalCart=True) c4=cart([r, l*ainv], origin=p, direction=direct, fromLocalCart=True) tree.nodes=[c1,c2,c3,c4] tree.radius=np.sqrt(r**2+l**2) road.harvestedTrees.append(tree)
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 chopNext(self): c=[] CC=self.chopConst#constant for felling,zero for BC head t=self.getNextTree(self.road) if not t: col=self.road.color self.road.color='r' self.road.color=col return c elif t.weight+self.treeWeight>self.maxTreeWeight or t.dbh**2+self.gripArea>self.maxGripArea: #go back and dump trees if the head cannot hold any more trees print "Goes back to DUMP trees", self.treeWeight, self.gripArea if not self.m.hasBundler: if self.road == self.m.roads['main']: time=self.setPos(self.m.getTreeDumpSpot(self.side)) else: time=self.setPos(self.road.startPoint) else: time=self.setPos(self.m.bundler.pos) self.cmnd(c, time, auto=self.automatic['moveArmIn']) c.extend(self.dumpTrees()) #dumps them down. elif not getDistance(t.pos , self.m.pos)>self.m.craneMaxL: time=self.setPos(self.harvestPos(t)) self.cmnd(c, time, auto=self.automatic['moveArmOut']) #determine choptime cross_sec_area=t.dbh**2*pi choptime=CC+cross_sec_area/self.velFell self.cmnd(c, choptime, auto=self.automatic['chop']) t.pos=[5000, 5000] #far away, for visual reasons. t.h-=0.5 #harvester is at least half a meter above ground t.harvested=True self.road.trees.remove(t) self.road.harvestTrees-=1 self.trees.append(t) self.treeWeight+=t.weight self.gripArea+=t.dbh**2 self.m.trees.append(t) self.m.treeMoni.observe(len(self.m.trees), self.sim.now()) return c
def normPitchRollDist(R, x, y, z): """ Still only uses the maximum value of the pitch and the roll. The weight of each edge need be modeled in a way thought through thoroughly. """ inf = 1e9 A = 0.3 # Pitch B = 0.3 # Roll C = 1.0 - A - B # Distance pitchlim = 18.4 # deg This limit is approximate rollim = 11.3 # deg This limit is approximate p1 = (x[0], y[0]) p2 = (x[-1], y[-1]) d = fun.getDistance(p1, p2) points = max(10, int(d)) # every meter rollist = getRoll(R, p1, p2, points=points, style="weighted") pitchlist = getSimplePitch(R, p1, p2, points=points) rollmax = max(np.abs(np.array(rollist))) # Doesnt work fix with np pitchmax = max(np.abs(np.array(pitchlist))) # Doesnt work fix with np if rollmax > rollim or pitchmax > pitchlim: return inf w = d * (A * pitchmax / pitchlim + B * rollmax / rollim + C) return w
def getSimplePitch(R,p1,p2,points=20): """ Gives the pitch along a line from point p1 to point p2. output: pitch - a list with the pitch in every point on the line """ if not R: raise Exception('getSimplePitch need the graph R in order to have the interpolation of terrain data.') pitch=[] x=np.linspace(p1[0], p2[0], points) y=np.linspace(p1[1], p2[1], points) z=R.interpol.ev(x,y) length=fun.getDistance(p1,p2) piInv=1/pi for ent in range(len(z)): if ent==0: pitch.append(180*piInv*atan2(z[ent+1]-z[ent],length/float(points))) #forward difference elif ent==len(z)-1: pitch.append(180*piInv*atan2(z[ent]-z[ent-1],length/float(points))) #backward difference break else: pitch.append(180*piInv*atan2(z[ent+1]-z[ent-1],2*length/float(points))) #central difference if len(pitch)!=len(z): raise Exception('Something wrong with the dimensions of the pitch') return pitch
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
def interpolate_outliers(data, max_tolerated_movement=20, verbose=False): """ input: values in the format of extract_coordinates() output: values in same format, without outlier values careful: this directly modifies your data do not set max_tolerated_movement less then 16 (15.06) - it will not work :) """ if verbose: print("Interpolate Outliers:" ) # Announce this function loudly and passionately n_rows, n_cols = data.shape # Get distances of all points between 2 frames dist = functions.get_distances(data) if verbose: print("Before:") print("avg:", np.mean(dist, axis=0)) print("max:", np.amax(dist, axis=0)) print("min:", np.amin(dist, axis=0)) assert n_rows >= 2 for col in range(int(n_cols / 2)): # Get outliers of specific column i_out = list(np.where(dist[:, col] > max_tolerated_movement)[0]) # column indices in 'data' col1, col2 = 2 * col, 2 * col + 1 for outlier in i_out: # recheck if outlier candidate is still valid, often when fixing an outlier you fix it for the next distance aswell curr_dis = functions.getDistance(data[outlier, col1], data[outlier, col2], data[outlier + 1, col1], data[outlier + 1, col2]) if abs(curr_dis) > max_tolerated_movement: i_curr = outlier + 1 i_start = outlier # Find next point which is no outlier while (dist[i_curr, col] > max_tolerated_movement): i_curr += 1 if i_curr >= n_rows: error_end(outlier) i_end = i_curr # Compute distances and check if we would interpolate, # would the dis be > then max_tolerated_movement # if yes, take further points until it works switch = True while (avg_dis(data, col1, col2, i_start, i_end, (i_end - i_start + 1)) > max_tolerated_movement): if switch: i_end += 1 if (i_end >= n_rows): error_end(outlier) else: i_start -= 1 if (i_start < 0): error_start( i_curr ) #i_curr because it has the latest og outlier fill_steps_in(data, col1, i_start, i_end, (i_end - i_start)) fill_steps_in(data, col2, i_start, i_end, (i_end - i_start)) # To check, we recalculate distances and look if there is any outliers still left if verbose: dist = functions.get_distances(data) print("After:") print("avg:", np.mean(dist, axis=0)) print("max:", np.amax(dist, axis=0)) print("min:", np.amin(dist, axis=0)) print("Outliers left: ", np.where(dist[:, ] > max_tolerated_movement))
'TAC': 'Y', 'TAT': 'Y', 'TAA': '*', 'TAG': '*', 'TGC': 'C', 'TGT': 'C', 'TGA': '*', 'TGG': 'W', } singleSNPcodons = {} for codon1 in codonTable: # initialize codon dictionary if codon1 not in singleSNPcodons: # add codon if not yet in dictionary singleSNPcodons[codon1] = [] for codon2 in codonTable: codon2_AA = codonTable[codon2] if f.getDistance(codon1, codon2) == 1: # if distance between codons is one SNP if codon2_AA not in singleSNPcodons[codon1]: singleSNPcodons[codon1].append( codon2_AA) # add the amino acid that is 1 SNP away def insert_codons(seq1, seq2, pad_left, pad_right, homology_length, triplets_to_insert): seqlen = len(seq1) for codon_position in range(0, len(seq1), 3): # note that seq2 is the wild type replaced_codon = seq2[codon_position:codon_position + 3] replaced_AA = f.translate(replaced_codon) left_padding_length = max([homology_length - codon_position, 0]) if left_padding_length > 0: left_padding_seq = pad_left[-left_padding_length:].lower()
def GetStumps(self,pos,R): obstList=self.getNeighborObst(pos, Lmax=R) return [s for s in obstList if isinstance(s,Stump) and getDistance(pos, s.pos)< s.radius+R ]
def GetRoots(self, pos, R): obstList=self.getNeighborObst(pos, Lmax=R) return [r for r in obstList if isinstance(r,Root) and getDistance(pos, r.pos)< r.radius+R ]
def GetTrees(self, pos, R, onlyNonHarvested=False): obstList=self.getNeighborObst(pos, Lmax=R) if onlyNonHarvested: obstList=[t for t in obstList if isinstance(t,Tree) and not t.harvested] return [t for t in obstList if isinstance(t,Tree) and getDistance(pos, t.pos)< t.radius+R]
def GetVisibleObstacles(self,pos, R): #Get obstacles in a radius R from point pos. Optimize: let the obstacles be in a grid and only search those in adjacent grids. obstList=self.getNeighborObst(pos, Lmax=R) return [o for o in obstList if o.visible and getDistance(pos, o.pos)< o.radius+R]
def dumpTrees(self, direction=None): if not self.m.hasBundler: """releases the trees at the current position. (And dumps the trees in piles)""" if direction is None: direction=self.road.direction cart=self.m.getCartesian t=self.getNextTree(self.road)# c=[] if len(self.trees)==0: return [] if not t or self.currentPile==None: if self.road==self.m.roads['main']: self.currentPile=Pile(pos=self.pos,terrain=self.m.G.terrain) else: self.currentPile=Pile(pos=self.pos,terrain=self.m.G.terrain) for index, tree in enumerate(copy.copy(self.trees)): tree.isSpherical=False tree.nodes=[[-0.1,1],[-0.1,0],[0.1,0],[0.1,-1]]#just some nodes see next line tree.pos=[5000,5000]#just moves the trees such that they are not plotted self.currentPile.trees.append(tree)#adds the tree to the current pile self.trees.remove(tree) if len(self.trees)!=0: raise Exception('dumptrees does not remove the trees..') self.treeWeight=0 self.gripArea=0 self.currentPile.craneCycles+=1#This should give cranecycles involved in this pile self.currentPile.updatePile(direction)#sets pile parameters in a nice way c.extend(self.twigCrack()) if not t or getDistance(t.pos , self.m.pos)>self.m.craneMaxL: #check if more trees in this corridor or within reach in mainroad self.m.G.terrain.piles.append(self.currentPile)#adds the pile to the list of piles in terrain print '*Saved a pile with',len(self.currentPile.trees),'trees at pos:', self.currentPile.pos self.currentPile=None self.cmnd(c, time=self.timeDropTrees, auto=self.automatic['dumpTrees']) return c else: """releases the trees at the current position. (And dumps the trees in piles)""" if direction is None: direction=self.road.direction cart=self.m.getCartesian t=self.getNextTree(self.road)# b=self.m.bundler c=[] if len(self.trees)==0: return [] if b.currentBundle==None: b.currentBundle=Bundle(pos=b.pos) i=0 for index, tree in enumerate(copy.copy(self.trees)): tree.isSpherical=False tree.nodes=[[-0.1,1],[-0.1,0],[0.1,0],[0.1,-1]] tree.pos=[5000,5000] b.currentBundle.trees.append(tree)#adds the tree to the current bundle in bundler i+=1 self.trees.remove(tree) print 'added',i,' trees to the bundler', self.sim.now() if len(self.trees)!=0: raise Exception('dumptrees does not remove the trees..') self.treeWeight=0 self.gripArea=0 b.currentBundle.craneCycles+=1 b.currentBundle.updatePile(direction)#sets pile parameters in a nice way c.extend(self.twigCrack()) #Yes this one comes after the trees have been dumped to the bundler. It's a code thing and doesn't matter self.cmnd(c, time=self.timeDropTrees, auto=self.automatic['dumpTrees']) self.cmnd(c,time=self.setPos(self.getStartPos()), auto=self.automatic['moveArmOut'])#return to the start position return c
def run(self): while True: yield waituntil, self, self.roadAssigned if self.road==self.m.roads['main']: #clear the mainroad. roadList=[self.road] sPoint=self.pos else: #road is a thinning corridor. if len(self.m.heads)==2: roadList=self.m.roads[self.m.pos[1]][self.side] if roadList[0] is not self.road: raise Exception('road system does not work as expected.%s, %s, %s'%(self.side, self.road, roadList[0])) else: a=self.m.roads[self.m.pos[1]].values() #two list with 3 roads in each roadList=a[0]+a[1] for road in roadList: self.road=road mainRoad=True print self.side,"starts harvesting", self.sim.now() if self.road != self.m.roads['main']: mainRoad=False sPoint=road.startPoint time=self.setPos(sPoint) for c in self.cmnd([], time, auto=self.automatic['moveArmOut']): yield c while True:#chopnext but with bundler is here if self.m.hasBundler: choplist=[] t=self.getNextTree(self.road) if not t: col=self.road.color self.road.color='r' self.road.color=col break elif t.weight+self.treeWeight>self.maxTreeWeight or t.dbh**2+self.gripArea>self.maxGripArea: #go back and dump trees if the head cannot hold any more trees for c in self.chopNextWithBundler1(): yield c """check if it is possible to do the dumping!""" self.waitingForBundler=1 for c in self.checkBundler(): yield c self.waitingForBundler=0 for c in self.dumpTrees(): yield c#dumps them down elif not getDistance(t.pos , self.m.pos)>self.m.craneMaxL: for c in self.chopNextWithBundler2(t,self.chopConst): yield c else: break else:#here you find the original chopnext, still valid when no bundler cmd=self.chopNext() if len(cmd)==0: break for c in cmd: yield c if not self.m.hasBundler: time=self.setPos(sPoint) # trees have been gathered. return to machine after each maxAcc if mainRoad: time+=self.setPos(self.m.getTreeDumpSpot(self.side)) else: time=self.setPos(self.m.bundler.pos)#if bundler: always leave the trees there for c in self.cmnd([], time, auto=self.automatic['moveArmIn']): yield c # print self.side, "crane is in position ready to dump the trees" """check if it is possible to do the dumping!""" self.waitingForBundler=1 for c in self.checkBundler(): yield c #checks so there are space in bundler self.waitingForBundler=0 for c in self.dumpTrees(): yield c #dumps the trees for c in self.releaseDriver(): yield c print self.side,"done at site", self.pos self.reset()
def interpolate_outliers_rec(data, max_tolerated_movement=20, verbose=False): """ input: values in the format of extract_coordinates() output: values in same format, without outlier values careful: this directly modifies your data do not set max_tolerated_movement less then 16 (15.06) - it will not work :) """ if verbose: print("Interpolate Outliers:" ) # Announce this function loudly and passionately n_rows, n_cols = data.shape # Get distances of all points between 2 frames dist = functions.get_distances(data) if verbose: print("Before:") print("avg:", np.mean(dist, axis=0)) print("max:", np.amax(dist, axis=0)) print("min:", np.amin(dist, axis=0)) assert n_rows >= 2 for col in range(int(n_cols / 2)): # Get outliers of specific column i_out = list(np.where(dist[:, col] > max_tolerated_movement)[0]) # column indices in 'data' col1, col2 = 2 * col, 2 * col + 1 for outlier in i_out: # recheck if outlier candidate is still valid, often when fixing an outlier you fix it for the next distance aswell dis = functions.getDistance(data[outlier, col1], data[outlier, col2], data[outlier + 1, col1], data[outlier + 1, col2]) if abs(dis) > max_tolerated_movement: if outlier + 1 in i_out and outlier + 2 in i_out: # This case is if the interpolation lead to outliers, since the positions outside of the nan values are too far away. # Get all frames which are behind each other+ out_group = [ outlier, outlier + 1, outlier + 2, ] outlier += 2 while outlier + 1 in i_out: out_group.append(outlier + 1) outlier += 1 correct_wrongly_interpolated_outliers( data, col1, col2, out_group, max_tolerated_movement) else: # correct outlier by replacing it with the value in the middle from points before and after it correct_outlier(data, col1, col2, outlier, max_tolerated_movement) # To check, we recalculate distances and look if there is any outliers still left if verbose: dist = functions.get_distances(data) print("After:") print("avg:", np.mean(dist, axis=0)) print("max:", np.amax(dist, axis=0)) print("min:", np.amin(dist, axis=0)) print("Outliers left: ", np.where(dist[:, ] > max_tolerated_movement))