331 lines
12 KiB
Python
331 lines
12 KiB
Python
import sumolib
|
|
from math import dist, ceil, sqrt, log, cos, sin, atan2
|
|
from random import randint
|
|
from PySide6.QtGui import QPainter
|
|
from PySide6.QtCore import QPointF, Signal, QObject
|
|
|
|
class updateSignals(QObject):
|
|
updateDisp = Signal(tuple)
|
|
addGraphPt = Signal(tuple)
|
|
|
|
class Car():
|
|
def getShape(self, edgeInd):
|
|
startEdge = self.route[edgeInd]
|
|
|
|
laneId = 0
|
|
lane = startEdge.getLane(laneId)
|
|
vmax = lane.getSpeed()
|
|
laneShape = lane.getShape()
|
|
return (laneShape, vmax, laneId)
|
|
|
|
def initPath(self):
|
|
newLane = self.getShape(self.index)
|
|
|
|
self.laneShape = newLane[0]
|
|
self.vmax = newLane[1]
|
|
self.laneId = newLane[2]
|
|
|
|
if self.infoWidg is None:
|
|
return
|
|
self.signals.updateDisp.emit(("Index",f"{self.index}/{len(self.route)}"))
|
|
self.signals.updateDisp.emit(("Edge",self.route[self.index]))
|
|
|
|
#print(f"{self.id} : {startEdge.getID()} -> {nextEdge.getID()} via {laneId}")
|
|
|
|
def __init__(self,carID,route,startTime,parentMap,parentController,infoWidg):
|
|
self.id=carID
|
|
self.map=parentMap
|
|
self.controller=parentController
|
|
self.infoWidg=infoWidg
|
|
self.index=0
|
|
self.laneInd=0
|
|
self.route=[]
|
|
self.laneShape=None
|
|
self.laneId=0
|
|
self.leader=None
|
|
self.startTime=float(startTime)
|
|
|
|
self.pos=[0,0]
|
|
self.dir=0
|
|
self.v=0
|
|
self.a=10
|
|
self.b=20
|
|
|
|
self.vmax=0
|
|
|
|
self.gamma = 5
|
|
self.delta = 0
|
|
self.T = 1.3
|
|
self.size = 3
|
|
|
|
self.vroom = 0
|
|
|
|
self.rawRoute = route
|
|
|
|
if infoWidg is None:
|
|
return
|
|
self.signals=updateSignals()
|
|
self.signals.updateDisp.connect(self.infoWidg.setVal)
|
|
self.signals.addGraphPt.connect(self.infoWidg.addSpeedPoint)
|
|
|
|
self.signals.updateDisp.emit(("Position",self.pos))
|
|
self.signals.updateDisp.emit(("Vitesse",self.v))
|
|
self.signals.updateDisp.emit(("Index",f"{self.index}/{len(route)}"))
|
|
|
|
def prepareRoute(self):
|
|
route = list(map(self.map.getEdge,self.rawRoute))
|
|
for r,rn in zip(route,route[1:]):
|
|
self.route.append(r)
|
|
conn=r.getConnections(rn)
|
|
if(len(conn)==0):
|
|
continue
|
|
edge=self.map.getLane(conn[0].getViaLaneID()).getEdge()
|
|
self.route.append(edge)
|
|
|
|
secEdge=edge.getConnections(rn)[0].getViaLaneID() # Parfois je sais pas pourquoi il coupe les edges internes, mais il marque quand même la connection, ducoup pour contourner
|
|
while secEdge!="":
|
|
edge=self.map.getLane(secEdge).getEdge()
|
|
self.route.append(edge)
|
|
secEdge=edge.getConnections(rn)[0].getViaLaneID()
|
|
|
|
self.initPath()
|
|
|
|
self.pos=list(self.laneShape[0])
|
|
|
|
def getLeader(self, maxDist):
|
|
shapeInd = self.laneInd
|
|
edgeInd = self.index
|
|
prevInd = edgeInd
|
|
laneShape = self.laneShape
|
|
laneId = self.laneId
|
|
l = 0
|
|
carsHere = self.controller.getCarsOnLane(self.route[edgeInd].getID(), laneId)
|
|
carsHere = list(filter(lambda c: c.id != self.id, carsHere))
|
|
while(l<maxDist):
|
|
endPos = laneShape[shapeInd+1]
|
|
|
|
carsReallyHere = filter(lambda c: c.laneInd == shapeInd, carsHere)
|
|
|
|
if l == 0:
|
|
carsReallyHere = filter(lambda c: dist(c.pos,endPos) < dist(self.pos,endPos),carsReallyHere)
|
|
closest = None
|
|
|
|
carsReallyHere = list(carsReallyHere)
|
|
|
|
try:
|
|
closest = min(carsReallyHere, key=lambda c: dist(c.pos,laneShape[shapeInd]))
|
|
except ValueError as e:
|
|
if l == 0:
|
|
l+=dist(self.pos,endPos)
|
|
else:
|
|
l+=dist(laneShape[shapeInd],endPos)
|
|
shapeInd+=1
|
|
if(shapeInd>=len(laneShape)-1):
|
|
shapeInd = 0
|
|
edgeInd+=1
|
|
carComing = self.getLeaderAtIntersection(prevInd,edgeInd)
|
|
if(carComing is not None):
|
|
self.leaderDist = l + 0.3*carComing[0]
|
|
return carComing[1]
|
|
if(not self.route[edgeInd].isSpecial()):
|
|
prevInd = edgeInd
|
|
carsHere = self.controller.getCarsOnLane(self.route[edgeInd].getID(), laneId)
|
|
carsHere = list(filter(lambda c: c.id != self.id, carsHere)) # me demande pas pourquoi mais si on le convertit pas en liste ici le filter original est modifié
|
|
if(edgeInd>=len(self.route)-1):
|
|
return
|
|
newLane = self.getShape(edgeInd)
|
|
laneShape = newLane[0]
|
|
laneId = newLane[2]
|
|
else:
|
|
if l == 0:
|
|
l+=dist(self.pos, closest.pos)
|
|
else:
|
|
l+=dist(laneShape[shapeInd], closest.pos)
|
|
if l <= maxDist:
|
|
self.leaderDist = l
|
|
return closest
|
|
else:
|
|
return
|
|
return
|
|
|
|
def getCurrentEdge(self):
|
|
return self.route[self.index]
|
|
|
|
def getCarDist(self, car, edge, laneInd, startFromEnd = True):
|
|
lanes = edge.getLane(laneInd).getShape().copy()
|
|
if startFromEnd:
|
|
lanes.reverse()
|
|
cDist = 0
|
|
for i,l in enumerate(lanes[:-1]):
|
|
if car.laneInd != (i if not startFromEnd else len(lanes)-i-2):
|
|
cDist += dist(l, lanes[i+1])
|
|
else:
|
|
cDist += dist(l, car.pos)
|
|
return cDist
|
|
return cDist
|
|
|
|
def getLeaderAtIntersection(self, prevInd, edgeInd):
|
|
while(self.route[edgeInd].isSpecial()):
|
|
edgeInd = edgeInd + 1
|
|
if edgeInd >= len(self.route):
|
|
return None
|
|
while self.route[prevInd].isSpecial():
|
|
prevInd -= 1
|
|
if prevInd < 0:
|
|
return None
|
|
inter = self.route[edgeInd-1].getFromNode()
|
|
connection = self.route[prevInd].getConnections(self.route[edgeInd])[0]
|
|
linkInd = inter.getLinkIndex(connection)
|
|
if(linkInd == -1): # Ca devrait pas arriver, mais de toute évidence ça arrive
|
|
return;
|
|
resp = inter._prohibits[linkInd] # Si je me souvient bien les variables précédées d'un _ doivent pas être touchées?
|
|
connRaw = inter.getConnections()
|
|
conn = [0] * len(resp)
|
|
for c in connRaw:
|
|
ind = inter.getLinkIndex(c)
|
|
if(ind == -1):
|
|
continue
|
|
conn[ind] = c
|
|
cars = []
|
|
for i,f in enumerate(reversed(resp)):
|
|
if(f == '0'):
|
|
continue
|
|
edge = conn[i].getFrom()
|
|
laneInd = conn[i].getFromLane().getIndex()
|
|
intLane = self.map.getLane(conn[i].getViaLaneID())
|
|
intLaneLgt = intLane.getLength()
|
|
carsInEdge = list(self.controller.getCarsOnLane(edge.getID(), laneInd)) # doit y avoir moyen de le faire en gardant les filters, flemme
|
|
if len(carsInEdge) != 0:
|
|
carsInEdge = zip([self.getCarDist(c, edge, laneInd)+intLaneLgt for c in carsInEdge], carsInEdge)
|
|
closest = min(carsInEdge, key=lambda c: c[0])
|
|
cars.append(closest)
|
|
intEdge = intLane.getEdge()
|
|
carsInEdge = list(self.controller.getCarsOnLane(intEdge.getID(), intLane.getIndex()))
|
|
if len(carsInEdge) != 0:
|
|
carsInEdge = zip([self.getCarDist(c, intEdge, intLane.getIndex()) for c in carsInEdge], carsInEdge)
|
|
closest = min(carsInEdge, key=lambda c: c[0])
|
|
cars.append(closest)
|
|
|
|
if(len(cars) == 0):
|
|
return None
|
|
cDist,closest = min(cars, key=lambda c: c[0])
|
|
return (cDist,closest)
|
|
|
|
def draw(self,painter):
|
|
pt = QPointF(*self.pos)
|
|
painter.drawEllipse(pt,self.size,self.size)
|
|
painter.drawLine(self.pos[0], self.pos[1], self.pos[0]+5*cos(self.dir), self.pos[1]+5*sin(self.dir))
|
|
|
|
if self.vroom != 0:
|
|
painter.save()
|
|
d=(60-self.vroom)*0.2
|
|
painter.translate(pt + QPointF(0,d))
|
|
painter.scale(1,-1)
|
|
font = painter.font();
|
|
font.setPixelSize(ceil(self.vroom*0.2));
|
|
painter.setFont(font)
|
|
painter.drawText(QPointF(0,0),"vroom")
|
|
self.vroom -= 1
|
|
painter.restore()
|
|
#painter.drawEllipse(pt,100,100)
|
|
|
|
def conduite(self,vmax,leader,dt):
|
|
if self.id == "f_00" and self.controller.t%10>5:
|
|
|
|
self.v = 0
|
|
return
|
|
|
|
if(leader is None):
|
|
self.v = self.vmax
|
|
self.updateGraph(self.v, vmax, 0)
|
|
return
|
|
vleader=50#self.v
|
|
bleader=self.b
|
|
else:
|
|
vleader=leader.v # vitesse de la voiture leader
|
|
bleader=leader.b
|
|
|
|
vbar=(self.v+vleader)/2
|
|
bbar=(bleader+self.b)/2
|
|
|
|
|
|
|
|
|
|
# S = vleader * 3.6 * 0.6
|
|
Si=self.leaderDist-vleader*self.T
|
|
#S=vf**2 / self.b + vleader**2 / bleader + self.gamma * vf + self.delta
|
|
T=self.T
|
|
vsec=vleader+(Si-vmax*T)/(vbar/bbar+T)
|
|
vd=min(self.v+self.a*dt,vmax,vsec)
|
|
#vf=min(va,vb2)
|
|
|
|
#va=self.v+2.5*self.a*self.T*(1-(self.v/vd))*sqrt(0.025+(self.v/vd))
|
|
#vb2=self.b*self.T+sqrt((self.b)**2*(self.T)**2-self.b*(2*(S-Si)-self.v*self.T-((vleader)**2/bbar)))
|
|
|
|
|
|
|
|
self.v=max(0,vd)
|
|
|
|
self.updateGraph(self.v, vmax, vsec)
|
|
|
|
def updateGraph(self, v, vmax, vsec):
|
|
if self.infoWidg is None:
|
|
return
|
|
self.signals.addGraphPt.emit((2,self.controller.t,self.v))
|
|
self.signals.addGraphPt.emit((0,self.controller.t,vmax))
|
|
self.signals.addGraphPt.emit((1,self.controller.t,vsec))
|
|
|
|
|
|
|
|
def update(self,dt):
|
|
if self.controller.t < self.startTime:
|
|
return
|
|
|
|
self.leader=self.getLeader(100)
|
|
|
|
self.conduite(self.vmax,self.leader,dt)
|
|
|
|
lgt=self.v*dt
|
|
|
|
self.dir = atan2(self.laneShape[self.laneInd+1][1]-self.pos[1],self.laneShape[self.laneInd+1][0]-self.pos[0])
|
|
|
|
while(lgt>0):
|
|
endPos=self.laneShape[self.laneInd+1]
|
|
l=dist(self.pos,endPos)
|
|
if lgt>=l:
|
|
lgt-=l
|
|
pos=list(self.laneShape[-1])
|
|
self.laneInd+=1
|
|
if(self.laneInd>=len(self.laneShape)-1):
|
|
self.laneInd=0
|
|
self.index+=1
|
|
if(self.index>=len(self.route)-1):
|
|
self.index=0
|
|
self.controller.destroyCar(self)
|
|
|
|
self.initPath()
|
|
self.pos=list(self.laneShape[self.laneInd])
|
|
continue
|
|
adv=lgt/l
|
|
self.pos[0]+=(endPos[0]-self.pos[0])*adv
|
|
self.pos[1]+=(endPos[1]-self.pos[1])*adv
|
|
lgt=0
|
|
|
|
if self.controller.vroomEnable and randint(0,100) == 0:
|
|
self.vroom = 60
|
|
|
|
if self.infoWidg is None:
|
|
return
|
|
self.signals.updateDisp.emit(("Position", self.pos))
|
|
self.signals.updateDisp.emit(("Vitesse", self.v))
|
|
self.signals.updateDisp.emit(("Leader", self.leader if self.leader is None else f"{self.leader.id} @ {self.leaderDist:.2f}m"))
|
|
|
|
def __copy__(self):
|
|
copy = Car(self.id, self.rawRoute, self.startTime, self.map, self.controller, self.infoWidg)
|
|
copy.route = self.route
|
|
copy.pos = self.pos.copy()
|
|
copy.laneShape = self.laneShape
|
|
copy.laneId = self.laneId
|
|
copy.vmax = self.vmax
|
|
return copy
|