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 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
