Mercurial > hg > nsaunier > traffic-intelligence

comparison python/moving.py @ 571:a9c1d61a89b4

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
corrected bug for segment intersection
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Thu, 07 Aug 2014 00:05:14 -0400
parents 5adaab9ad160
children cae4e5f3fe9f
comparison
equal deleted inserted replaced
570:5adaab9ad160 571:a9c1d61a89b4
526 def similar(f1, f2, maxDistance2, maxDeltavelocity2): 526 def similar(f1, f2, maxDistance2, maxDeltavelocity2):
527 return (f1.position-f2.position).norm2Squared()<maxDistance2 and (f1.velocity-f2.velocity).norm2Squared()<maxDeltavelocity2 527 return (f1.position-f2.position).norm2Squared()<maxDistance2 and (f1.velocity-f2.velocity).norm2Squared()<maxDeltavelocity2
528 528
529 def intersection(p1, p2, p3, p4): 529 def intersection(p1, p2, p3, p4):
530 ''' Intersection point (x,y) of lines formed by the vectors p1-p2 and p3-p4 530 ''' Intersection point (x,y) of lines formed by the vectors p1-p2 and p3-p4
531 http://paulbourke.net/geometry/lineline2d/ 531 http://paulbourke.net/geometry/pointlineplane/'''
532
533 If these lines are parralel, there will be a division by zero error.
534 '''
535 dp12 = p2-p1 532 dp12 = p2-p1
536 det = ((p4.y-p3.y)*(p2.x-p1.x)-(p4.x-p3.x)*(p2.y-p1.y)) 533 dp34 = p4-p3
534 #det = (p4.y-p3.y)*(p2.x-p1.x)-(p4.x-p3.x)*(p2.y-p1.y)
535 det = dp34.y*dp12.x-dp34.x*dp12.y
537 if det == 0: 536 if det == 0:
538 return None 537 return None
539 else: 538 else:
540 ua = ((p4.x-p3.x)*(p1.y-p3.y)-(p4.y-p3.y)*(p1.x-p3.x))/det 539 ua = (dp34.x*(p1.y-p3.y)-dp34.y*(p1.x-p3.x))/det
541 dp12.multiply(ua) 540 return p1+dp12.multiply(ua)
542 #x = p1.x + ua*(p2.x - p1.x)
543 #y = p1.y + ua*(p2.y - p1.y)
544 return p1+dp12
545 541
546 # def intersection(p1, p2, dp1, dp2): 542 # def intersection(p1, p2, dp1, dp2):
547 # '''Returns the intersection point between the two lines 543 # '''Returns the intersection point between the two lines
548 # defined by the respective vectors (dp) and origin points (p)''' 544 # defined by the respective vectors (dp) and origin points (p)'''
549 # from numpy import matrix 545 # from numpy import matrix
563 '''Returns the intersecting point of the segments [p1, p2] and [p3, p4], None otherwise''' 559 '''Returns the intersecting point of the segments [p1, p2] and [p3, p4], None otherwise'''
564 560
565 if (Interval.intersection(Interval(p1.x,p2.x,True), Interval(p3.x,p4.x,True)).empty()) or (Interval.intersection(Interval(p1.y,p2.y,True), Interval(p3.y,p4.y,True)).empty()): 561 if (Interval.intersection(Interval(p1.x,p2.x,True), Interval(p3.x,p4.x,True)).empty()) or (Interval.intersection(Interval(p1.y,p2.y,True), Interval(p3.y,p4.y,True)).empty()):
566 return None 562 return None
567 else: 563 else:
568 #dp1 = p2-p1 564 inter = intersection(p1, p2, p3, p4)
569 #dp3 = p4-p3
570 inter = intersection(p1, p2, p3, p4)#(p1, p3, dp1, dp3)
571 if (inter != None 565 if (inter != None
572 and utils.inBetween(p1.x, p2.x, inter.x) 566 and utils.inBetween(p1.x, p2.x, inter.x)
573 and utils.inBetween(p3.x, p4.x, inter.x) 567 and utils.inBetween(p3.x, p4.x, inter.x)
574 and utils.inBetween(p1.y, p2.y, inter.y) 568 and utils.inBetween(p1.y, p2.y, inter.y)
575 and utils.inBetween(p3.y, p4.y, inter.y)): 569 and utils.inBetween(p3.y, p4.y, inter.y)):
576 return inter 570 return inter
577 else: 571 else:
578 return None 572 return None
579
580 # TODO: implement a better algorithm for intersections of sets of segments http://en.wikipedia.org/wiki/Line_segment_intersection
581 573
582 class Trajectory(object): 574 class Trajectory(object):
583 '''Class for trajectories: temporal sequence of positions 575 '''Class for trajectories: temporal sequence of positions
584 576
585 The class is iterable''' 577 The class is iterable'''