Mercurial > hg > nsaunier > traffic-intelligence

comparison trafficintelligence/moving.py @ 1094:c96388c696ac

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
adding functions for simulation, with contribution from student Lionel Nebot-Janvier, [email protected]
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Mon, 04 Feb 2019 10:08:23 -0500
parents 8734742c08c0
children 9a32d63bae3f
comparison
equal deleted inserted replaced
1093:05ccd8ef150c 1094:c96388c696ac
387 else: # ttc1 < 0 and ttc2 < 0: 387 else: # ttc1 < 0 and ttc2 < 0:
388 return None 388 return None
389 else: 389 else:
390 return None 390 return None
391 391
392 @staticmethod 392 @staticmethod
393 def midPoint(p1, p2): 393 def midPoint(p1, p2):
394 'Returns the middle of the segment [p1, p2]' 394 'Returns the middle of the segment [p1, p2]'
395 return Point(0.5*p1.x+0.5*p2.x, 0.5*p1.y+0.5*p2.y) 395 return Point(0.5*p1.x+0.5*p2.x, 0.5*p1.y+0.5*p2.y)
396 396
397 @staticmethod 397 @staticmethod
419 for i in range(len(alignment)-1): 419 for i in range(len(alignment)-1):
420 p2 = alignment[i+1] 420 p2 = alignment[i+1]
421 if(round(p1.x, 10) == round(p2.x, 10)): 421 if(round(p1.x, 10) == round(p2.x, 10)):
422 p2.x += 0.0000000001 422 p2.x += 0.0000000001
423 if(round(p1.y, 10) == round(p2.y, 10)): 423 if(round(p1.y, 10) == round(p2.y, 10)):
424 p2.y += 0.0000000001 424 p2.y += 0.0000000001
425 p1 = p2 425 p1 = p2
426 426
427 def ppldb2p(qx,qy, p0x,p0y, p1x,p1y): 427 def ppldb2p(qx,qy, p0x,p0y, p1x,p1y):
428 ''' Point-projection (Q) on line defined by 2 points (P0,P1). 428 ''' Point-projection (Q) on line defined by 2 points (P0,P1).
429 http://cs.nyu.edu/~yap/classes/visual/03s/hw/h2/math.pdf 429 http://cs.nyu.edu/~yap/classes/visual/03s/hw/h2/math.pdf
430 ''' 430 '''
431 if(p0x == p1x and p0y == p1y): 431 if(p0x == p1x and p0y == p1y):
432 return None 432 return None
433 try: 433 try:
434 #Approximate slope singularity by giving some slope roundoff; account for roundoff error 434 #Approximate slope singularity by giving some slope roundoff; account for roundoff error
435 # if(round(p0x, 10) == round(p1x, 10)): 435 # if(round(p0x, 10) == round(p1x, 10)):
436 # p1x += 0.0000000001 436 # p1x += 0.0000000001
437 # if(round(p0y, 10) == round(p1y, 10)): 437 # if(round(p0y, 10) == round(p1y, 10)):
438 # p1y += 0.0000000001 438 # p1y += 0.0000000001
439 #make the calculation 439 #make the calculation
440 Y = (-(qx)*(p0y-p1y)-(qy*(p0y-p1y)**2)/(p0x-p1x)+p0x**2*(p0y-p1y)/(p0x-p1x)-p0x*p1x*(p0y-p1y)/(p0x-p1x)-p0y*(p0x-p1x))/(p1x-p0x-(p0y-p1y)**2/(p0x-p1x)) 440 Y = (-(qx)*(p0y-p1y)-(qy*(p0y-p1y)**2)/(p0x-p1x)+p0x**2*(p0y-p1y)/(p0x-p1x)-p0x*p1x*(p0y-p1y)/(p0x-p1x)-p0y*(p0x-p1x))/(p1x-p0x-(p0y-p1y)**2/(p0x-p1x))
441 X = (-Y*(p1y-p0y)+qx*(p1x-p0x)+qy*(p1y-p0y))/(p1x-p0x) 441 X = (-Y*(p1y-p0y)+qx*(p1x-p0x)+qy*(p1y-p0y))/(p1x-p0x)
442 except ZeroDivisionError: 442 except ZeroDivisionError:
443 print('Error: Division by zero in ppldb2p. Please report this error with the full traceback:') 443 print('Error: Division by zero in ppldb2p. Please report this error with the full traceback:')
444 print('qx={0}, qy={1}, p0x={2}, p0y={3}, p1x={4}, p1y={5}...'.format(qx, qy, p0x, p0y, p1x, p1y)) 444 print('qx={0}, qy={1}, p0x={2}, p0y={3}, p1x={4}, p1y={5}...'.format(qx, qy, p0x, p0y, p1x, p1y))
445 import pdb; pdb.set_trace() 445 import pdb; pdb.set_trace()
446 return Point(X,Y) 446 return Point(X,Y)
447 447
448 def getSYfromXY(p, alignments, goodEnoughAlignmentDistance = 0.5): 448 def getSYfromXY(p, alignments, goodEnoughAlignmentDistance = 0.5):
449 ''' Snap a point p to its nearest subsegment of it's nearest alignment (from the list alignments). 449 ''' Snap a point p to its nearest subsegment of it's nearest alignment (from the list alignments).
450 A alignment is a list of points (class Point), most likely a trajectory. 450 A alignment is a list of points (class Point), most likely a trajectory.
451 451
452 Output: 452 Output:
453 ======= 453 =======
454 [alignment index, 454 [alignment index,
455 subsegment leading point index, 455 subsegment leading point index,
456 snapped point, 456 snapped point,
457 subsegment distance, 457 subsegment distance,
458 alignment distance, 458 alignment distance,
459 orthogonal point offset] 459 orthogonal point offset]
460 460
461 or None 461 or None
462 ''' 462 '''
1047 S.append(S[-1]+v) 1047 S.append(S[-1]+v)
1048 Y = [y]*nPoints 1048 Y = [y]*nPoints
1049 lanes = [lane]*nPoints 1049 lanes = [lane]*nPoints
1050 return CurvilinearTrajectory(S, Y, lanes) 1050 return CurvilinearTrajectory(S, Y, lanes)
1051 1051
1052 def append(self,other):
1053 '''adds positions of other to the curvilinear trajectory (in-place modification)'''
1054 for p in other:
1055 self.addPosition(p)
1056
1052 @staticmethod 1057 @staticmethod
1053 def fromTrajectoryProjection(t, alignments, halfWidth = 3): 1058 def fromTrajectoryProjection(t, alignments, halfWidth = 3):
1054 ''' Add, for every object position, the class 'moving.CurvilinearTrajectory()' 1059 ''' Add, for every object position, the class 'moving.CurvilinearTrajectory()'
1055 (curvilinearPositions instance) which holds information about the 1060 (curvilinearPositions instance) which holds information about the
1056 curvilinear coordinates using alignment metadata. 1061 curvilinear coordinates using alignment metadata.
1233 return MovingObject(num = num, timeInterval = timeInterval, positions = positions, velocities = Trajectory([[v.x]*nPoints, [v.y]*nPoints])) 1238 return MovingObject(num = num, timeInterval = timeInterval, positions = positions, velocities = Trajectory([[v.x]*nPoints, [v.y]*nPoints]))
1234 1239
1235 def updatePositions(self): 1240 def updatePositions(self):
1236 inter, self.positions, self.velocities = MovingObject.aggregateTrajectories(self.features, self.getTimeInterval()) 1241 inter, self.positions, self.velocities = MovingObject.aggregateTrajectories(self.features, self.getTimeInterval())
1237 1242
1243 def updateCurvilinearPositions(self, method, changeOfAlignment, nextAlignment_idx, timeStep = None, time = None,
1244 leaderVehicleCurvilinearPositionAtPrecedentTime = None, previousAlignmentId = None,
1245 maxSpeed = None, acceleration = None, seed = None, delta = None):
1246 import math
1247
1248 if method == 'newell':
1249 self.curvilinearPositions.addPositionSYL(leaderVehicleCurvilinearPositionAtPrecedentTime - self.dn, 0, nextAlignment_idx)
1250 if changeOfAlignment:
1251 self.velocities.addPositionSYL((self.curvilinearPositions[-1][0]-self.curvilinearPositions[-2][0])/timeStep,
1252 (self.curvilinearPositions[-1][1]-self.curvilinearPositions[-1][1])/timeStep,
1253 (previousAlignmentId, nextAlignment_idx))
1254 else:
1255 self.velocities.addPositionSYL((self.curvilinearPositions[-1][0]-self.curvilinearPositions[-2][0])/timeStep,
1256 (self.curvilinearPositions[-1][1]-self.curvilinearPositions[-1][1])/timeStep,
1257 None)
1258 elif method == 'constantSpeed':
1259 random.seed(seed)
1260 self.curvilinearPositions.addPositionSYL(self.curvilinearPositions[time-1][0] + self.desiredSpeed * timeStep,
1261 0,
1262 nextAlignment_idx)
1238 @staticmethod 1263 @staticmethod
1239 def concatenate(obj1, obj2, num = None, newFeatureNum = None, computePositions = False): 1264 def concatenate(obj1, obj2, num = None, newFeatureNum = None, computePositions = False):
1240 '''Concatenates two objects, whether overlapping temporally or not 1265 '''Concatenates two objects, whether overlapping temporally or not
1241 1266
1242 Positions will be recomputed only if computePositions is True 1267 Positions will be recomputed only if computePositions is True