77 lines
2.3 KiB
Python
77 lines
2.3 KiB
Python
import sumolib
|
|
import pygame as pg
|
|
import math
|
|
|
|
class Car:
|
|
def initPath(self):
|
|
startEdge=self.map.getEdge(self.route[self.index])
|
|
nextEdge=self.map.getEdge(self.route[self.index+1])
|
|
|
|
inverted=not (startEdge.getToNode()==nextEdge.getFromNode() or startEdge.getToNode()==nextEdge.getToNode())
|
|
|
|
laneId=0 if not inverted else 1
|
|
self.laneShape=startEdge.getLane(laneId).getShape()
|
|
if(inverted):
|
|
self.laneShape.reverse()
|
|
|
|
print(f"{self.id} : {startEdge.getID()} -> {nextEdge.getID()} via {laneId}")
|
|
|
|
def __init__(self,carID,route,parentMap,surface):
|
|
self.id=carID
|
|
self.route=route
|
|
self.map=parentMap
|
|
self.index=0
|
|
self.laneInd=0
|
|
|
|
startEdge=self.map.getEdge(route[0])
|
|
nextEdge=self.map.getEdge(route[1])
|
|
|
|
self.initPath()
|
|
|
|
self.pos=list(self.laneShape[0])
|
|
self.v=0
|
|
self.a=0
|
|
self.b=20
|
|
|
|
self.surf=surface
|
|
|
|
def draw(self,col):
|
|
pg.draw.circle(self.surf,col,self.map.convertPos(self.pos),5)
|
|
|
|
def conduite(self):
|
|
vmax=10 # vitesse max sur la route actuelle
|
|
vleader=vmax # vitesse de la voiture leader
|
|
bleader=15
|
|
vbar=(self.v+vleader)/2
|
|
bbar=(bleader+self.b)/2 # decelleration de la vleader et de self
|
|
S=5
|
|
T=0.3
|
|
vsec=vleader+(S-vmax*T)/(vbar/bbar+T)
|
|
vd=min(vmax,vsec)
|
|
self.v=max(0,vd)
|
|
|
|
def update(self,dt):
|
|
self.conduite()
|
|
lgt=self.v*dt
|
|
|
|
while(lgt>0):
|
|
endPos=self.laneShape[self.laneInd+1]
|
|
l=math.dist(self.pos,self.laneShape[self.laneInd+1])
|
|
if lgt>=l:
|
|
lgt-=l
|
|
pos=list(self.laneShape[-1])
|
|
self.laneInd+=1
|
|
if(self.laneInd>=len(self.laneShape)-2):
|
|
self.laneInd=0
|
|
self.index+=1
|
|
if(self.index>=len(self.route)-2):
|
|
self.index=0
|
|
|
|
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
|