Mercurial Hosting > traffic-intelligence
comparison trafficintelligence/moving.py @ 1099:4ab5c63c13a3
first working version of newell model
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Wed, 20 Feb 2019 00:08:20 -0500 |
parents | 469e36eea158 |
children | 1e833fd8490d |
comparison
equal
deleted
inserted
replaced
1098:469e36eea158 | 1099:4ab5c63c13a3 |
---|---|
1260 def updateCurvilinearPositions(self, method, instant, timeStep, _nextAlignmentIdx = None, maxSpeed = None, acceleration = None): | 1260 def updateCurvilinearPositions(self, method, instant, timeStep, _nextAlignmentIdx = None, maxSpeed = None, acceleration = None): |
1261 '''Update curvilinear position of user at new instant''' | 1261 '''Update curvilinear position of user at new instant''' |
1262 | 1262 |
1263 if method == 'newell': | 1263 if method == 'newell': |
1264 if self.curvilinearPositions is None: # vehicle without positions, all vehicles should have leader (?) | 1264 if self.curvilinearPositions is None: # vehicle without positions, all vehicles should have leader (?) |
1265 if self.leader is None: | 1265 if self.instantAtX0 is None: |
1266 s = (timeStep*instant-self.initialHeadway)*self.desiredSpeed | 1266 if self.leader is None: |
1267 self.timeInterval = TimeInterval(instant, instant) | 1267 s = (timeStep*instant-self.initialHeadway)*self.desiredSpeed |
1268 self.curvilinearPositions = CurvilinearTrajectory([s], [0.], [self.initialAlignmentIdx]) | 1268 self.timeInterval = TimeInterval(instant, instant) |
1269 self.curvilinearVelocities = CurvilinearTrajectory() | 1269 self.curvilinearPositions = CurvilinearTrajectory([s], [0.], [self.initialAlignmentIdx]) |
1270 self.instantAtX0 = self.initialHeadway | 1270 self.curvilinearVelocities = CurvilinearTrajectory() |
1271 elif self.leader.curvilinearPositions is not None and self.leader.curvilinearPositions.getSCoordAt(-1) > self.d and len(self.leader.curvilinearPositions) >=2: | 1271 self.instantAtX0 = self.initialHeadway# to avoid next test |
1272 firstInstantAfterD = self.leader.getLastInstant() | 1272 elif self.leader.curvilinearPositions is not None and self.leader.curvilinearPositions.getSCoordAt(-1) > self.d and len(self.leader.curvilinearPositions) >=2: |
1273 while self.leader.existsAtInstant(firstInstantAfterD) and self.leader.getCurvilinearPositionAtInstant(firstInstantAfterD-1)[0] > self.d:# find first instant after d | 1273 firstInstantAfterD = self.leader.getLastInstant() |
1274 firstInstantAfterD -= 1 # if not recorded position before self.d, we extrapolate linearly from first known position | 1274 while self.leader.existsAtInstant(firstInstantAfterD) and self.leader.getCurvilinearPositionAtInstant(firstInstantAfterD-1)[0] > self.d:# find first instant after d |
1275 leaderSpeed = self.leader.getCurvilinearVelocityAtInstant(firstInstantAfterD)[0] | 1275 firstInstantAfterD -= 1 # if not recorded position before self.d, we extrapolate linearly from first known position |
1276 self.instantAtX0 = self.tau + instant*timeStep - (self.leader.getCurvilinearPositionAtInstant(firstInstantAfterD)[0]-self.d)/leaderSpeed # second part is the time at which leader is at self.d | 1276 leaderSpeed = self.leader.getCurvilinearVelocityAtInstant(firstInstantAfterD-1)[0] |
1277 if self.instantAtX0 < self.initialHeadway: #obj appears at instant initialHeadway at x=0 with desiredSpeed | 1277 self.instantAtX0 = self.tau + instant*timeStep - (self.leader.getCurvilinearPositionAtInstant(firstInstantAfterD)[0]-self.d)/leaderSpeed # second part is the time at which leader is at self.d |
1278 self.instantAtX0 = self.initialHeadway | 1278 if self.instantAtX0 < self.initialHeadway: #obj appears at instant initialHeadway at x=0 with desiredSpeed |
1279 if self.instantAtX0 is not None and instant >= self.instantAtX0: | 1279 self.instantAtX0 = self.initialHeadway |
1280 elif instant*timeStep >= self.instantAtX0: | |
1280 firstInstant = int(ceil(self.instantAtX0/timeStep)) | 1281 firstInstant = int(ceil(self.instantAtX0/timeStep)) |
1281 self.timeInterval = TimeInterval(firstInstant, firstInstant) | 1282 self.timeInterval = TimeInterval(firstInstant, firstInstant) |
1282 freeFlowCoord = (firstInstant*timeStep - self.instantAtX0)*self.desiredSpeed | 1283 freeFlowCoord = (firstInstant*timeStep - self.instantAtX0)*self.desiredSpeed |
1283 # constrainedCoord at firstInstant = xn-1(t = firstInstant*timeStep-self.tau)-self.d | 1284 # constrainedCoord at firstInstant = xn-1(t = firstInstant*timeStep-self.tau)-self.d |
1284 t = firstInstant*timeStep-self.tau | 1285 t = firstInstant*timeStep-self.tau |
1285 i = int(floor(t/timeStep)) | 1286 i = int(floor(t/timeStep)) |
1286 leaderSpeed = self.leader.getCurvilinearVelocityAtInstant(i)[0] | 1287 leaderSpeed = self.leader.getCurvilinearVelocityAtInstant(i)[0] |
1287 constrainedCoord = self.leader.getCurvilinearPositionAtInstant(i)[0]+leaderSpeed*(t-i*timeStep)-self.d | 1288 constrainedCoord = self.leader.getCurvilinearPositionAtInstant(i)[0]+leaderSpeed*(t-i*timeStep)-self.d |
1288 self.curvilinearPositions = CurvilinearTrajectory([min(freeFlowCoord, constrainedCoord)], [0.], [self.initialAlignmentIdx]) | 1289 self.curvilinearPositions = CurvilinearTrajectory([min(freeFlowCoord, constrainedCoord)], [0.], [self.initialAlignmentIdx]) |
1289 self.curvilinearVelocities = CurvilinearTrajectory() | 1290 self.curvilinearVelocities = CurvilinearTrajectory() |
1290 print(firstInstant+1, instant+1) | |
1291 for i in range(firstInstant+1, instant+1): | 1291 for i in range(firstInstant+1, instant+1): |
1292 s1 = self.curvilinearPositions.getSCoordAt(-1) | 1292 s1 = self.curvilinearPositions.getSCoordAt(-1) |
1293 freeFlowCoord = s1 + timeStep*self.desiredSpeed | 1293 freeFlowCoord = s1 + timeStep*self.desiredSpeed |
1294 constrainedCoord = self.leader.interpolateCurvilinearPositions(i-self.tau/timeStep)[0]-self.d | 1294 constrainedCoord = self.leader.interpolateCurvilinearPositions(i-self.tau/timeStep)[0]-self.d |
1295 s2 = min(freeFlowCoord, constrainedCoord) | 1295 s2 = min(freeFlowCoord, constrainedCoord) |