def preGenerateTrajectories(ballSpeed, expInfo, allBallsList, rectangles, repulsionStrength, edgerepulsionStrength, centerAttraction): """ Precompute the trajectories of points """ trajClock = core.Clock() trajClock.reset() nFrames = int(expInfo['Duration'] / (1.0 / 60.0)) fullFrame = [None] * nFrames # preallocate space for the list from common import Geometry from numpy import array for i in range(0, nFrames): fullFrame[i] = dict() for ballListID, ballList in allBallsList.iteritems(): boundingRectangle = rectangles[ballListID] for ball1 in ballList: force = array([0, 0]) for ball2 in ballList: if (ball1 == ball2): continue # balls collide delta = ((ball1.pos() - ball2.pos())) dist2 = ((delta**2).sum()) force = force + repulsionStrength * \ (ball1.radius * ball2.radius) * delta / (dist2 * dist2) # Repulsion to borders force[0] = force[0] + edgerepulsionStrength / \ ((boundingRectangle[0] - ball1.pos()[0]) ** 2) force[0] = force[0] - edgerepulsionStrength / \ ((boundingRectangle[2] - ball1.pos()[0]) ** 2) force[1] = force[1] + edgerepulsionStrength / \ ((boundingRectangle[1] - ball1.pos()[1]) ** 2) force[1] = force[1] - edgerepulsionStrength / \ ((boundingRectangle[3] - ball1.pos()[1]) ** 2) # Add a little attraction to the center of boundingRectangle dispFromCenter = Geometry.rectangleCenter(boundingRectangle) force = force - (ball1.pos() - dispFromCenter) ball1.direction = ball1.direction + centerAttraction * force # Renormalize direction with correct speed ball1.direction = Geometry.normalized( ball1.direction) * ballSpeed # Move the ball in the computed direction and finally draw it ball1.move(ball1.direction) # if (i==1): #print displacement for debug purpose # print np.np.sqrt(np.dot(ball1.direction,ball1.direction))*60 fullFrame[i][ball1] = ball1.pos() return fullFrame
def point_on_road(tracktime, tracklist, pathlinks, linklist, t_file): points = dict() for time in tracktime: dlist = list() dvlist = list() TrackP = G.Point() TrackP.x = tracklist[time].long TrackP.y = tracklist[time].lat for link in pathlinks: linkp = linklist[link] distvertlink = distp2link(TrackP, linkp) if distvertlink[0] > 0: dlist.append(distvertlink[0]) dvlist.append(distvertlink) if len(dlist) == 1: points[time] = dvlist[0] elif len(dlist) > 1: dmin = min(dlist) for dv in dvlist: if dv[0] == dmin: points[time] = dv break p_file = open(pwd + '/point/' + t_file, 'w') map(int, points.keys()) points.keys().sort() for key in points: p_file.write(repr(points[key])[1:-1] + '\n') p_file.close()
def createBalls(win, nBalls, radius, boundRect, speed): """ Create the balls to be put in the area """ halfX = (boundRect[0] + boundRect[2]) / 2.0 halfY = (boundRect[1] + boundRect[3]) / 2.0 # The four rectangles that build the main rectangle rectanglesList = [] rectanglesList.append([boundRect[0], boundRect[1], halfX, halfY]) rectanglesList.append([halfX, boundRect[1], boundRect[2], halfY]) rectanglesList.append([boundRect[0], halfY, halfX, boundRect[3]]) rectanglesList.append([halfX, halfY, boundRect[2], boundRect[3]]) # Generate the balls in the balls = [] from random import shuffle from numpy.random import uniform from common import Geometry for i in range(0, nBalls): startPos = np.array( [uniform( rectanglesList[ i % 4][0] + radius, rectanglesList[i % 4][2] - radius), uniform(rectanglesList[i % 4][1] + radius, rectanglesList[i % 4][3] - radius)]) direction = Geometry.normalized( np.array([uniform(-1, 1), uniform(-1, 1)])) ball = Ball(win, position=startPos, direction=direction, speed=speed, radius=radius, color='Black') balls.append(ball) shuffle(balls) return balls, rectanglesList
def distp2link(P,link): vert=vertp2l(P,link) nvert=len(vert) if nvert==0: d=-1 return (d,-1,-1,link.linkid) elif nvert==1: d=G.geodist(P,vert[0]) return (d,vert[0].x,vert[0].y,link.linkid) else: dn=[] vn=[] for ver in vert: dn.append(G.geodist(P,ver)) vn.append((ver.x,ver.y)) d=min(dn) for idn in range(len(dn)): if dn[idn]==d: return (d,vn[idn][0],vn[idn][1],link.linkid)
def vertp2l(P, link): vert = list() P1 = G.Point() P2 = G.Point() if link.internumber > 0: for i in range(link.internumber-1): P1.x = link.interlist[i][0] P1.y = link.interlist[i][1] P2.x = link.interlist[i+1][0] P2.y = link.interlist[i+1][1] if P1.y==P2.y: intersection=G.Point() intersection.x=P.x intersection.y=P1.y if G.checkpointonline(intersection,P1,P2)==1: vert.append(intersection) elif P1.x==P2.x: intersection=G.Point() intersection.x=P1.x intersection.y=P.y if G.checkpointonline(intersection,P1,P2)==1: vert.append(intersection) elif P1.x!=P2.x and P1.y!=P2.y: intersection=G.Point() k=(P2.y-P1.y)/(P2.x-P1.x) intersection.x=(k**2*P1.x+k*(P.y-P1.y)+P.x)/(k**2+1.0) intersection.y=k*(intersection.x-P1.x)+P1.y if G.checkpointonline(intersection,P1,P2)==1: vert.append(intersection) elif link.internumber==0: intersection=Point() intersection.x=nodelist[link.node1].long intersection.y=nodelist[link.node1].lat vert.append(intersection) return vert
def vertp2l(P, link): vert = list() P1 = G.Point() P2 = G.Point() if link.internumber > 0: for i in range(link.internumber - 1): P1.x = link.interlist[i][0] P1.y = link.interlist[i][1] P2.x = link.interlist[i + 1][0] P2.y = link.interlist[i + 1][1] if P1.y == P2.y: intersection = G.Point() intersection.x = P.x intersection.y = P1.y if G.checkpointonline(intersection, P1, P2) == 1: vert.append(intersection) elif P1.x == P2.x: intersection = G.Point() intersection.x = P1.x intersection.y = P.y if G.checkpointonline(intersection, P1, P2) == 1: vert.append(intersection) elif P1.x != P2.x and P1.y != P2.y: intersection = G.Point() k = (P2.y - P1.y) / (P2.x - P1.x) intersection.x = (k**2 * P1.x + k * (P.y - P1.y) + P.x) / (k**2 + 1.0) intersection.y = k * (intersection.x - P1.x) + P1.y if G.checkpointonline(intersection, P1, P2) == 1: vert.append(intersection) elif link.internumber == 0: intersection = Point() intersection.x = nodelist[link.node1].long intersection.y = nodelist[link.node1].lat vert.append(intersection) return vert
def point_on_road(tracktime, tracklist, pathlinks, linklist, t_file): points = dict() for time in tracktime: dlist = list() dvlist = list() TrackP = G.Point() TrackP.x=tracklist[time].long TrackP.y=tracklist[time].lat for link in pathlinks: linkp=linklist[link] distvertlink = distp2link(TrackP,linkp) if distvertlink[0] > 0: dlist.append(distvertlink[0]) dvlist.append(distvertlink) if len(dlist) == 1: points[time] = dvlist[0] elif len(dlist) > 1: dmin = min(dlist) for dv in dvlist: if dv[0] == dmin: points[time] = dv break p_file = open(pwd+'/point/'+t_file, 'w') tmp = points.keys() tmp.sort() for key in tmp: p_file.write(repr(points[key])[1:-1]+', '+key+'\n') p_file.close() if points: origin = open('OD/Origin.txt', 'a') origin.write(repr(points[tmp[0]])[1:-1]+', '+tmp[0]+'\n') origin.close() destination = open('OD/Destination.txt', 'a') destination.write(repr(points[tmp[-1]])[1:-1]+', '+tmp[-1]+'\n') destination.close()
def CollectGridLinks(type, linklist, linkID): if type == 1: gridnumlong = GRID_LONG_NUM_A gridnumlat = GRID_LAT_NUM_A elif type == 2: gridnumlong = GRID_LONG_NUM_B gridnumlat = GRID_LAT_NUM_B grid = [[[] for i in range(3*gridnumlat)] for j in range(3*gridnumlong)] (ori_longi, ori_lat) = (121.31, 31.08) for ilink in linkID: if linklist[ilink].internumber>0: v = linklist[ilink].interlist counti = 0 for n in range(linklist[ilink].internumber): (i, j) = GetGridIndex(type, v[n][0], v[n][1]) if i>=3*gridnumlong or i<0 or j>=3*gridnumlat or j<0: break counti += 1 if counti==linklist[ilink].internumber: for n in range(1, counti): (ip, jp) = GetGridIndex(type, v[n-1][0], v[n-1][1]) if ilink not in grid[ip][jp]: grid[i][j].append(ilink) (i, j) = GetGridIndex(type, v[n][0], v[n][1]) m1=abs(i-ip) m2=abs(j-jp) i_x=min(i,ip) j_y=min(j,jp) if m1==0 and m2==0: break if m1==0 and m2>1: for mj in range(m2): if not ilink in grid[i][j_y+mj]: grid[i][j_y+mj].append(ilink) if m2==0 and m1>1: for mi in range(m1): if not ilink in grid[i_x+mi][j]: grid[i_x+mi][j].append(ilink) if m1>0 and m2>0: A=G.Point() B=G.Point() P1=G.Point() P2=G.Point() P3=G.Point() P4=G.Point() (A.x,A.y)= GetPointGridxy(type,v[n-1][0],v[n-1][1]) (B.x,B.y)= GetPointGridxy(type,v[n][0],v[n][1]) for r1 in range(m1+1): for r2 in range(m2+1): grid[i_x+r1][j_y+r2] P1.x=i_x+r1 P1.y=j_y+r2 P2.x=i_x+r1+1 P2.y=j_y+r2 P3.x=i_x+r1+1 P3.y=j_y+r2+1 P4.x=i_x+r1 P4.y=j_y+r2+1 if G.checkintersect(A,B,P1,P2,P3,P4)==1: if not ilink in grid[i_x+r1][j_y+r2]: grid[i_x+r1][j_y+r2].append(ilink) ip=i jp=j else: meshnode=linklist[ilink].node1 x=nodelist[meshnode].long y=nodelist[meshnode].lat (i, j)=GetGridIndex(type,x,y) if i>=5*gridnumlong or i<0 or j>=5*gridnumlat or j<0: continue if not ilink in grid[i][j]: grid[i][j].append(ilink) return grid
def RevisePathEndpoints(tracklist, tracktime, linklist, GLinkNode, s_links, e_links, pathnodes): strackp=G.Point() strackp.x=tracklist[tracktime[0]].long strackp.y=tracklist[tracktime[0]].lat svert=vertp2l(strackp,linklist[GLinkNode[pathnodes[0]][pathnodes[1]]]) #求垂足 if not svert: #如果垂足为空,则修正起点;否则不修正 sconnectlinks=[] #收集起点候选link集中与当前路径起点相连通的link sconnectnodes={} #保存起点候选link集中与当前路径起点相连通的link的起点 for s_link in s_links: sdrmlink=linklist[s_link] snod1=sdrmlink.node1 snod2=sdrmlink.node2 if sdrmlink.regulation==2: if snod2==pathnodes[0]: sconnectlinks.append(s_link) sconnectnodes[s_link]= snod1 continue elif sdrmlink.regulation==3: if snod1==pathnodes[0]: sconnectlinks.append(s_link) sconnectnodes[s_link]= snod2 continue elif sdrmlink.regulation==1: if snod2==pathnodes[0]: sconnectlinks.append(s_link) sconnectnodes[s_link]= snod1 continue elif snod1==pathnodes[0]: sconnectlinks.append(s_link) sconnectnodes[s_link]= snod2 continue #求sconnectlinks中与起点距离最短的link,并把相应的node添加到mmpathnodes当中 if sconnectlinks: sdistp2l=100 #匹配路段必须在40m以内,此处初始值设只要大于40即可 saddlink=sconnectlinks[0] # for sconnectlink in sconnectlinks: dtemp=distp2link(strackp,linklist[sconnectlink]) if dtemp[0]!=-1 and dtemp[0]<sdistp2l: sdistp2l=dtemp[0] saddlink=sconnectlink if sdistp2l<=40: #如果距离起点最近路段在40以内,则把此路段添加到最短路径中 pathnodes.insert(0,sconnectnodes[saddlink]) #修正止点,修正止点和修正起点的思路一样,程序略微不同 #先排除不需要修正的情况 etrackp=G.Point() etrackp.x=tracklist[tracktime[-1]].long etrackp.y=tracklist[tracktime[-1]].lat evert=vertp2l(etrackp,linklist[GLinkNode[pathnodes[-2]][pathnodes[-1]]]) #求垂足 if not evert: #如果垂足为空,则修正起点;否则不修正 econnectlinks=[] #收集止点候选link集中与当前路径终点相连通的link econnectnodes={} #保存止点候选link集中与当前路径终点相连通的link的终点 for e_link in e_links: edrmlink=linklist[e_link] enod1=edrmlink.node1 enod2=edrmlink.node2 if edrmlink.regulation==2: if enod1==pathnodes[-1]: econnectlinks.append(e_link) econnectnodes[e_link]= enod2 continue elif edrmlink.regulation==3: if enod2==pathnodes[-1]: econnectlinks.append(e_link) econnectnodes[e_link]= enod1 continue elif edrmlink.regulation==1: if enod2==pathnodes[-1]: econnectlinks.append(e_link) econnectnodes[e_link]= enod1 continue elif enod1==pathnodes[-1]: econnectlinks.append(e_link) econnectnodes[e_link]= enod2 continue #求econnectlinks中与止点距离最短的link,并把相应的node添加到pathnodes当中 if econnectlinks: edistp2l=100 #匹配路段必须在40m以内,此处初始值设只要大于40即可 eaddlink=econnectlinks[0] # for econnectlink in econnectlinks: dtemp=distp2link(etrackp,linklist[econnectlink]) if dtemp[0]!=-1 and dtemp[0]<edistp2l: edistp2l=dtemp[0] eaddlink=econnectlink if edistp2l<=40: #如果距离起点最近路段在40以内,则把此路段添加到最短路径中 pathnodes.append(econnectnodes[eaddlink]) return pathnodes