Mercurial Hosting > traffic-intelligence
comparison trafficintelligence/moving.py @ 1094:c96388c696ac
adding functions for simulation, with contribution from student Lionel Nebot-Janvier, lionel.nebot-janvier@polymtl.ca
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 |