Mercurial Hosting > traffic-intelligence
comparison python/moving.py @ 577:d0abd2ee17b9
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 |