Mercurial > hg > nsaunier > traffic-intelligence

comparison python/moving.py @ 577:d0abd2ee17b9

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
changed arguments to type Point
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Thu, 28 Aug 2014 17:22:38 -0400
parents 0eff0471f9cb
children fe4e9d2b807d
comparison
equal deleted inserted replaced
576:0eff0471f9cb 577:d0abd2ee17b9
360 def ppldb2p(qx,qy, p0x,p0y, p1x,p1y): 360 def ppldb2p(qx,qy, p0x,p0y, p1x,p1y):
361 ''' Point-projection (Q) on line defined by 2 points (P0,P1). 361 ''' Point-projection (Q) on line defined by 2 points (P0,P1).
362 http://cs.nyu.edu/~yap/classes/visual/03s/hw/h2/math.pdf 362 http://cs.nyu.edu/~yap/classes/visual/03s/hw/h2/math.pdf
363 ''' 363 '''
364 if(p0x == p1x and p0y == p1y): 364 if(p0x == p1x and p0y == p1y):
365 return False,False 365 return None
366 try: 366 try:
367 #Approximate slope singularity by giving some slope roundoff; account for roundoff error 367 #Approximate slope singularity by giving some slope roundoff; account for roundoff error
368 if(round(p0x, 10) == round(p1x, 10)): 368 if(round(p0x, 10) == round(p1x, 10)):
369 p1x += 0.0000000001 369 p1x += 0.0000000001
370 if(round(p0y, 10) == round(p1y, 10)): 370 if(round(p0y, 10) == round(p1y, 10)):
374 X = (-Y*(p1y-p0y)+qx*(p1x-p0x)+qy*(p1y-p0y))/(p1x-p0x) 374 X = (-Y*(p1y-p0y)+qx*(p1x-p0x)+qy*(p1y-p0y))/(p1x-p0x)
375 except ZeroDivisionError: 375 except ZeroDivisionError:
376 print('Error: Division by zero in ppldb2p. Please report this error with the full traceback:') 376 print('Error: Division by zero in ppldb2p. Please report this error with the full traceback:')
377 print('qx={0}, qy={1}, p0x={2}, p0y={3}, p1x={4}, p1y={5}...'.format(qx, qy, p0x, p0y, p1x, p1y)) 377 print('qx={0}, qy={1}, p0x={2}, p0y={3}, p1x={4}, p1y={5}...'.format(qx, qy, p0x, p0y, p1x, p1y))
378 import pdb; pdb.set_trace() 378 import pdb; pdb.set_trace()
379 return X,Y 379 return Point(X,Y)
380 380
381 def getSYfromXY(qx, qy, splines, goodEnoughSplineDistance = 0.5): 381 def getSYfromXY(p, splines, goodEnoughSplineDistance = 0.5):
382 ''' Snap a point (coordinates qx, qy) to it's nearest subsegment of it's nearest spline (from the list splines). 382 ''' Snap a point p to it's nearest subsegment of it's nearest spline (from the list splines).
383 383
384 To force snapping to a single spline, pass the spline alone through splines (e.g. splines=[splines[splineNum]]). 384 To force snapping to a single spline, pass the spline alone through splines (e.g. splines=[splines[splineNum]]).
385 385
386 Output: 386 Output:
387 ======= 387 =======
391 snapped y coordinate, 391 snapped y coordinate,
392 subsegment distance, 392 subsegment distance,
393 spline distance, 393 spline distance,
394 orthogonal point offset] 394 orthogonal point offset]
395 ''' 395 '''
396
397 #(buckle in, it gets ugly from here on out)
398 ss_spline_d = subsec_spline_dist(splines)
399
400 minOffsetY = float('inf') 396 minOffsetY = float('inf')
401 #For each spline 397 #For each spline
402 for spline in range(len(splines)): 398 for spline in range(len(splines)):
403 #For each spline point 399 #For each spline point index
404 for spline_p in range(len(splines[spline])-1): 400 for spline_p in range(len(splines[spline])-1):
405 #Get closest point on spline 401 #Get closest point on spline
406 X,Y = ppldb2p(qx,qy,splines[spline][spline_p][0],splines[spline][spline_p][1],splines[spline][spline_p+1][0],splines[spline][spline_p+1][1]) 402 closestPoint = ppldb2p(p.x,p.y,splines[spline][spline_p][0],splines[spline][spline_p][1],splines[spline][spline_p+1][0],splines[spline][spline_p+1][1])
407 if X == False and Y == False: 403 if closestPoint == None:
408 print('Error: Spline {0}, segment {1} has identical bounds and therefore is not a vector. Projection cannot continue.'.format(spline, spline_p)) 404 print('Error: Spline {0}, segment {1} has identical bounds and therefore is not a vector. Projection cannot continue.'.format(spline, spline_p))
409 return None 405 return None
410 if utils.inBetween(splines[spline][spline_p][0], splines[spline][spline_p+1][0], X) and utils.inBetween(splines[spline][spline_p][1], splines[spline][spline_p+1][1], Y): 406 if utils.inBetween(splines[spline][spline_p][0], splines[spline][spline_p+1][0], p.x) and utils.inBetween(splines[spline][spline_p][1], splines[spline][spline_p+1][1], p.y):
411 offsetY = utils.pointDistanceL2(qx,qy,X,Y) 407 offsetY = Point.distanceNorm2(closestPoint, p)#utils.pointDistanceL2(p.x,p.y,X,Y)
412 if offsetY < minOffsetY: 408 if offsetY < minOffsetY:
413 minOffsetY = offsetY 409 minOffsetY = offsetY
414 snappedSpline = spline 410 snappedSpline = spline
415 snappedSplineLeadingPoint = spline_p 411 snappedSplineLeadingPoint = spline_p
416 snapped_x = X 412 snappedPoint = Point(closestPoint.x, closestPoint.y)
417 snapped_y = Y
418 #Jump loop if significantly close 413 #Jump loop if significantly close
419 if offsetY < goodEnoughSplineDistance: 414 if offsetY < goodEnoughSplineDistance:
420 break 415 break
421 #Get sub-segment distance 416 #Get sub-segment distance
422 if minOffsetY != float('inf'): 417 if minOffsetY != float('inf'):
423 subsegmentDistance = utils.pointDistanceL2(splines[snappedSpline][snappedSplineLeadingPoint][0],splines[snappedSpline][snappedSplineLeadingPoint][1],snapped_x,snapped_y) 418 subsegmentDistance = Point.distanceNorm2(snappedPoint, splines[snappedSpline][snappedSplineLeadingPoint])
424 #Get total segment distance 419 #subsegmentDistance = utils.pointDistanceL2(splines[snappedSpline][snappedSplineLeadingPoint][0],splines[snappedSpline][snappedSplineLeadingPoint][1],snapped_x,snapped_y)
425 splineDistanceS = ss_spline_d[snappedSpline][1][snappedSplineLeadingPoint] + subsegmentDistance 420 #Get cumulative alignment distance (total segment distance)
426 orthogonalSplineVector = Point(splines[snappedSpline][snappedSplineLeadingPoint+1][1]-splines[snappedSpline][snappedSplineLeadingPoint][1], 421 splineDistanceS = splines[snappedSpline].getCumulativeDistance(snappedSplineLeadingPoint) + subsegmentDistance
427 splines[snappedSpline][snappedSplineLeadingPoint][0]-splines[snappedSpline][snappedSplineLeadingPoint+1][0])#positive orthogonal vector of vector (x,y) is (y, -x) 422 orthogonalSplineVector = (splines[snappedSpline][snappedSplineLeadingPoint+1]-splines[snappedSpline][snappedSplineLeadingPoint]).orthogonal()
428 offsetVector = Point(qx-snapped_x, qy-snapped_y) 423 #Point(splines[snappedSpline][snappedSplineLeadingPoint+1][1]-splines[snappedSpline][snappedSplineLeadingPoint][1],
424 # splines[snappedSpline][snappedSplineLeadingPoint][0]-splines[snappedSpline][snappedSplineLeadingPoint+1][0])#positive orthogonal vector of vector (x,y) is (y, -x)
425 offsetVector = p-snappedPoint#Point(qx-snapped_x, qy-snapped_y)
429 if Point.dot(orthogonalSplineVector, offsetVector) < 0: 426 if Point.dot(orthogonalSplineVector, offsetVector) < 0:
430 minOffsetY = -minOffsetY 427 minOffsetY = -minOffsetY
431 return [snappedSpline, snappedSplineLeadingPoint, snapped_x, snapped_y, subsegmentDistance, splineDistanceS, minOffsetY] 428 return [snappedSpline, snappedSplineLeadingPoint, snappedPoint, subsegmentDistance, splineDistanceS, minOffsetY]
432 else: 429 else:
433 return None 430 return None
434 431
435 def getXYfromSY(s, y, splineNum, splines, mode = 0): 432 def getXYfromSY(s, y, splineNum, splines, mode = 0):
436 ''' Find X,Y coordinate from S,Y data. 433 ''' Find X,Y coordinate from S,Y data.
741 # sq = map(add, [x*x for x in self.positions[0]], [y*y for y in self.positions[1]]) 738 # sq = map(add, [x*x for x in self.positions[0]], [y*y for y in self.positions[1]])
742 # return sqrt(sq) 739 # return sqrt(sq)
743 from numpy import hypot 740 from numpy import hypot
744 return hypot(self.positions[0], self.positions[1]) 741 return hypot(self.positions[0], self.positions[1])
745 742
746 def cumulatedDisplacement(self): 743 # def cumulatedDisplacement(self):
747 'Returns the sum of the distances between each successive point' 744 # 'Returns the sum of the distances between each successive point'
748 displacement = 0 745 # displacement = 0
749 for i in xrange(self.length()-1): 746 # for i in xrange(self.length()-1):
750 displacement += Point.distanceNorm2(self.__getitem__(i),self.__getitem__(i+1)) 747 # displacement += Point.distanceNorm2(self.__getitem__(i),self.__getitem__(i+1))
751 return displacement 748 # return displacement
752 749
753 def computeCumulativeDistances(self): 750 def computeCumulativeDistances(self):
754 '''Computes the distance from each point to the next and the cumulative distance up to the point 751 '''Computes the distance from each point to the next and the cumulative distance up to the point
755 Can be accessed through getDistance(idx) and getCumulativeDistance(idx)''' 752 Can be accessed through getDistance(idx) and getCumulativeDistance(idx)'''
756 self.distances = [] 753 self.distances = []
757 self.cumulativeDistances = [] 754 self.cumulativeDistances = [0.]
758 p1 = self[0] 755 p1 = self[0]
759 cumulativeDistance = 0. 756 cumulativeDistance = 0.
760 for i in xrange(self.length()-1): 757 for i in xrange(self.length()-1):
761 p2 = self[i+1] 758 p2 = self[i+1]
762 self.distances.append(Point.distanceNorm2(p1,p2)) 759 self.distances.append(Point.distanceNorm2(p1,p2))
770 return self.distances[i] 767 return self.distances[i]
771 else: 768 else:
772 print('Index {} beyond trajectory length {}-1'.format(i, self.length())) 769 print('Index {} beyond trajectory length {}-1'.format(i, self.length()))
773 770
774 def getCumulativeDistance(self, i): 771 def getCumulativeDistance(self, i):
775 '''Return the cumulative distance between the beginning and point i+1''' 772 '''Return the cumulative distance between the beginning and point i'''
776 if i < self.length()-1: 773 if i < self.length():
777 return self.cumulativeDistances[i] 774 return self.cumulativeDistances[i]
778 else: 775 else:
779 print('Index {} beyond trajectory length {}-1'.format(i, self.length())) 776 print('Index {} beyond trajectory length {}'.format(i, self.length()))
780 777
781 def similarOrientation(self, refDirection, cosineThreshold, minProportion = 0.5): 778 def similarOrientation(self, refDirection, cosineThreshold, minProportion = 0.5):
782 '''Indicates whether the minProportion (<=1.) (eg half) of the trajectory elements (vectors for velocity) 779 '''Indicates whether the minProportion (<=1.) (eg half) of the trajectory elements (vectors for velocity)
783 have a cosine with refDirection is smaller than cosineThreshold''' 780 have a cosine with refDirection is smaller than cosineThreshold'''
784 count = 0 781 count = 0