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