Mercurial Hosting > traffic-intelligence
comparison trafficintelligence/moving.py @ 1170:b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Mon, 27 Sep 2021 14:05:33 -0400 |
parents | b219d5a1bb55 |
children | aa88acf06876 |
comparison
equal
deleted
inserted
replaced
1169:9f7a4a026dab | 1170:b55adb13f262 |
---|---|
266 | 266 |
267 def plot(self, options = 'o', **kwargs): | 267 def plot(self, options = 'o', **kwargs): |
268 plot([self.x], [self.y], options, **kwargs) | 268 plot([self.x], [self.y], options, **kwargs) |
269 | 269 |
270 @staticmethod | 270 @staticmethod |
271 def plotSegment(p1, p2, options = 'o', **kwargs): | 271 def plotSegment(p1, p2, options = 'o', withOrigin = True, **kwargs): |
272 plot([p1.x, p2.x], [p1.y, p2.y], options, **kwargs) | 272 plot([p1.x, p2.x], [p1.y, p2.y], options, **kwargs) |
273 p1.plot('or', **kwargs) | |
273 | 274 |
274 def angle(self): | 275 def angle(self): |
275 return atan2(self.y, self.x) | 276 return atan2(self.y, self.x) |
276 | 277 |
277 def norm2Squared(self): | 278 def norm2Squared(self): |
403 @staticmethod | 404 @staticmethod |
404 def boundingRectangle(points, v): | 405 def boundingRectangle(points, v): |
405 '''Returns the bounding rectangle of the points, aligned on the vector v | 406 '''Returns the bounding rectangle of the points, aligned on the vector v |
406 A list of points is returned: front left, front right, rear right, rear left''' | 407 A list of points is returned: front left, front right, rear right, rear left''' |
407 e1 = v.normalize() | 408 e1 = v.normalize() |
408 e2 = e1.orthogonal() | 409 e2 = e1.orthogonal(False) |
409 xCoords = [] | 410 xCoords = [] |
410 yCoords = [] | 411 yCoords = [] |
411 for p in points: | 412 for p in points: |
412 xCoords.append(Point.dot(e1, p)) | 413 xCoords.append(Point.dot(e1, p)) |
413 yCoords.append(Point.dot(e2, p)) | 414 yCoords.append(Point.dot(e2, p)) |
414 xmin = min(xCoords) | 415 xmin = min(xCoords) |
415 xmax = max(xCoords) | 416 xmax = max(xCoords) |
416 ymin = min(yCoords) | 417 ymin = min(yCoords) |
417 ymax = max(yCoords) | 418 ymax = max(yCoords) |
418 frontLeft = Point(xmax, ymin) | 419 frontLeft = Point(xmax, ymax) |
419 frontRight = Point(xmax, ymax) | 420 frontRight = Point(xmax, ymin) |
420 rearLeft = Point(xmin, ymin) | 421 rearLeft = Point(xmin, ymax) |
421 rearRight = Point(xmin, ymax) | 422 rearRight = Point(xmin, ymin) |
422 return [Point(Point.dot(e1, p), Point.dot(e2, p)) for p in [frontLeft, frontRight, rearRight, rearLeft]] | 423 originalE1 = Point(Point.dot(e1, Point(1,0)),Point.dot(e2, Point(1,0))) |
424 originalE2 = Point(Point.dot(e1, Point(0,1)),Point.dot(e2, Point(0,1))) | |
425 return [Point(Point.dot(originalE1, p), Point.dot(originalE2, p)) for p in [frontLeft, frontRight, rearRight, rearLeft]] | |
423 | 426 |
424 if shapelyAvailable: | 427 if shapelyAvailable: |
425 def pointsInPolygon(points, polygon): | 428 def pointsInPolygon(points, polygon): |
426 '''Optimized tests of a series of points within (Shapely) polygon (not prepared)''' | 429 '''Optimized tests of a series of points within (Shapely) polygon (not prepared)''' |
427 if type(polygon) == PreparedGeometry: | 430 if type(polygon) == PreparedGeometry: |
603 @staticmethod | 606 @staticmethod |
604 def similar(f1, f2, maxDistance2, maxDeltavelocity2): | 607 def similar(f1, f2, maxDistance2, maxDeltavelocity2): |
605 return (f1.position-f2.position).norm2Squared()<maxDistance2 and (f1.velocity-f2.velocity).norm2Squared()<maxDeltavelocity2 | 608 return (f1.position-f2.position).norm2Squared()<maxDistance2 and (f1.velocity-f2.velocity).norm2Squared()<maxDeltavelocity2 |
606 | 609 |
607 def intersection(p1, p2, p3, p4): | 610 def intersection(p1, p2, p3, p4): |
608 ''' Intersection point (x,y) of lines formed by the vectors p1-p2 and p3-p4 | 611 ''' Intersection point (x,y) of the segments [p1, p2] and [p3, p4] |
609 http://paulbourke.net/geometry/pointlineplane/''' | 612 Returns the intersection point and the ratio of its position along [p1, p2] from p1 |
613 | |
614 Based on http://paulbourke.net/geometry/pointlineplane/''' | |
610 dp12 = p2-p1 | 615 dp12 = p2-p1 |
611 dp34 = p4-p3 | 616 dp34 = p4-p3 |
612 #det = (p4.y-p3.y)*(p2.x-p1.x)-(p4.x-p3.x)*(p2.y-p1.y) | 617 #det = (p4.y-p3.y)*(p2.x-p1.x)-(p4.x-p3.x)*(p2.y-p1.y) |
613 det = float(dp34.y*dp12.x-dp34.x*dp12.y) | 618 det = float(dp34.y*dp12.x-dp34.x*dp12.y) |
614 if det == 0.: | 619 if det == 0.: |
615 return None | 620 return None, None |
616 else: | 621 else: |
617 ua = (dp34.x*(p1.y-p3.y)-dp34.y*(p1.x-p3.x))/det | 622 ua = (dp34.x*(p1.y-p3.y)-dp34.y*(p1.x-p3.x))/det |
618 return p1+dp12.__mul__(ua) | 623 return p1+dp12.__mul__(ua), ua |
619 | 624 |
620 # def intersection(p1, p2, dp1, dp2): | 625 # def intersection(p1, p2, dp1, dp2): |
621 # '''Returns the intersection point between the two lines | 626 # '''Returns the intersection point between the two lines |
622 # defined by the respective vectors (dp) and origin points (p)''' | 627 # defined by the respective vectors (dp) and origin points (p)''' |
623 # from numpy import matrix | 628 # from numpy import matrix |
632 # else: | 637 # else: |
633 # intersection = linalg.solve(A,B) | 638 # intersection = linalg.solve(A,B) |
634 # return Point(intersection[0,0], intersection[1,0]) | 639 # return Point(intersection[0,0], intersection[1,0]) |
635 | 640 |
636 def segmentIntersection(p1, p2, p3, p4): | 641 def segmentIntersection(p1, p2, p3, p4): |
637 '''Returns the intersecting point of the segments [p1, p2] and [p3, p4], None otherwise''' | 642 '''Returns the intersecting point (and ratio along [p1, p2]) of the segments [p1, p2] and [p3, p4], None otherwise''' |
638 | 643 |
639 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()): | 644 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()): |
640 return None | 645 return None, None |
641 else: | 646 else: |
642 inter = intersection(p1, p2, p3, p4) | 647 inter, ratio = intersection(p1, p2, p3, p4) |
643 if (inter is not None | 648 if (inter is not None |
644 and utils.inBetween(p1.x, p2.x, inter.x) | 649 and utils.inBetween(p1.x, p2.x, inter.x) |
645 and utils.inBetween(p3.x, p4.x, inter.x) | 650 and utils.inBetween(p3.x, p4.x, inter.x) |
646 and utils.inBetween(p1.y, p2.y, inter.y) | 651 and utils.inBetween(p1.y, p2.y, inter.y) |
647 and utils.inBetween(p3.y, p4.y, inter.y)): | 652 and utils.inBetween(p3.y, p4.y, inter.y)): |
648 return inter | 653 return inter, ratio |
649 else: | 654 else: |
650 return None | 655 return None, None |
651 | 656 |
652 def segmentLineIntersection(p1, p2, p3, p4): | 657 def segmentLineIntersection(p1, p2, p3, p4): |
653 '''Indicates if the line going through p1 and p2 intersects inside p3, p4''' | 658 '''Indicates if the line going through p1 and p2 intersects inside p3, p4''' |
654 inter = intersection(p1, p2, p3, p4) | 659 inter, ratio = intersection(p1, p2, p3, p4) |
655 if inter is not None and utils.inBetween(p3.x, p4.x, inter.x) and utils.inBetween(p3.y, p4.y, inter.y): | 660 if inter is not None and utils.inBetween(p3.x, p4.x, inter.x) and utils.inBetween(p3.y, p4.y, inter.y): |
656 return inter | 661 return inter, ratio |
657 else: | 662 else: |
658 return None | 663 return None, None |
659 | 664 |
665 def segmentOrientationCrossing(p1, p2, p3, p4): | |
666 '''Returns the direction of the crossing, assuming there is a crossing: True means right (p3) to left (p4) (along the orthogonal vector to [p1, p2] (positive trigonometric orientation), False the other way''' | |
667 dp12 = p2-p1 | |
668 ortho = dp12.orthogonal(False) | |
669 return Point.dot(ortho, p4-p3) > 0 | |
660 | 670 |
661 class Trajectory(object): | 671 class Trajectory(object): |
662 '''Class for trajectories: temporal sequence of positions | 672 '''Class for trajectories: temporal sequence of positions |
663 | 673 |
664 The class is iterable''' | 674 The class is iterable''' |
931 if straightDistance > 0: | 941 if straightDistance > 0: |
932 return self.getCumulativeDistance(self.length()-1)/float(straightDistance) | 942 return self.getCumulativeDistance(self.length()-1)/float(straightDistance) |
933 else: | 943 else: |
934 return None | 944 return None |
935 | 945 |
936 def getIntersections(self, p1, p2): | 946 def getIntersections(self, p1, p2, computeOrientations = False): |
937 '''Returns a list of the indices at which the trajectory | 947 '''Returns a list of the indices at which the trajectory |
938 intersects with the segment of extremities p1 and p2 | 948 intersects with the segment of extremities p1 and p2 |
939 Returns an empty list if there is no crossing''' | 949 Returns an empty list if there is no crossing''' |
940 indices = [] | 950 indices = [] |
941 intersections = [] | 951 intersections = [] |
952 rightToLeftOrientations = [] | |
942 | 953 |
943 for i in range(self.length()-1): | 954 for i in range(self.length()-1): |
944 q1=self.__getitem__(i) | 955 q1=self.__getitem__(i) |
945 q2=self.__getitem__(i+1) | 956 q2=self.__getitem__(i+1) |
946 p = segmentIntersection(q1, q2, p1, p2) | 957 p, ratio = segmentIntersection(p1, p2, q1, q2) |
947 if p is not None: | 958 if p is not None: |
948 if q1.x != q2.x: | 959 # if q1.x != q2.x: |
949 ratio = (p.x-q1.x)/(q2.x-q1.x) | 960 # ratio = (p.x-q1.x)/(q2.x-q1.x) |
950 elif q1.y != q2.y: | 961 # elif q1.y != q2.y: |
951 ratio = (p.y-q1.y)/(q2.y-q1.y) | 962 # ratio = (p.y-q1.y)/(q2.y-q1.y) |
952 else: | 963 # else: |
953 ratio = 0 | 964 # ratio = 0 |
954 indices.append(i+ratio) | 965 indices.append(i+ratio) |
955 intersections.append(p) | 966 intersections.append(p) |
956 return indices, intersections | 967 if computeOrientations: |
968 rightToLeftOrientations.append(segmentOrientationCrossing(p1, p2, q1, q2)) | |
969 return indices, intersections, rightToLeftOrientations | |
957 | 970 |
958 def getLineIntersections(self, p1, p2): | 971 def getLineIntersections(self, p1, p2): |
959 '''Returns a list of the indices at which the trajectory | 972 '''Returns a list of the indices at which the trajectory |
960 intersects with the line going through p1 and p2 | 973 intersects with the line going through p1 and p2 |
961 Returns an empty list if there is no crossing''' | 974 Returns an empty list if there is no crossing''' |
963 intersections = [] | 976 intersections = [] |
964 | 977 |
965 for i in range(self.length()-1): | 978 for i in range(self.length()-1): |
966 q1=self.__getitem__(i) | 979 q1=self.__getitem__(i) |
967 q2=self.__getitem__(i+1) | 980 q2=self.__getitem__(i+1) |
968 p = segmentLineIntersection(p1, p2, q1, q2) | 981 p, ratio = segmentLineIntersection(q1, q2, p1, p2) |
969 if p is not None: | 982 if p is not None: |
970 if q1.x != q2.x: | 983 # if q1.x != q2.x: |
971 ratio = (p.x-q1.x)/(q2.x-q1.x) | 984 # ratio = (p.x-q1.x)/(q2.x-q1.x) |
972 elif q1.y != q2.y: | 985 # elif q1.y != q2.y: |
973 ratio = (p.y-q1.y)/(q2.y-q1.y) | 986 # ratio = (p.y-q1.y)/(q2.y-q1.y) |
974 else: | 987 # else: |
975 ratio = 0 | 988 # ratio = 0 |
976 indices.append(i+ratio) | 989 indices.append(i+ratio) |
977 intersections.append(p) | 990 intersections.append(p) |
978 return indices, intersections | 991 return indices, intersections |
979 | 992 |
980 def subTrajectoryInInterval(self, inter): | 993 def subTrajectoryInInterval(self, inter): |
1677 | 1690 |
1678 def setRoutes(self, startRouteID, endRouteID): | 1691 def setRoutes(self, startRouteID, endRouteID): |
1679 self.startRouteID = startRouteID | 1692 self.startRouteID = startRouteID |
1680 self.endRouteID = endRouteID | 1693 self.endRouteID = endRouteID |
1681 | 1694 |
1682 def getInstantsCrossingLane(self, p1, p2): | 1695 def getInstantsCrossingLine(self, p1, p2, computeOrientations = False): |
1683 '''Returns the instant(s) | 1696 '''Returns the instant(s) |
1684 at which the object passes from one side of the segment to the other | 1697 at which the object passes from one side of the segment to the other |
1685 empty list if there is no crossing''' | 1698 empty list if there is no crossing''' |
1686 indices, intersections = self.positions.getIntersections(p1, p2) | 1699 indices, intersections, rightToLeftOrientations = self.positions.getIntersections(p1, p2, computeOrientations) |
1687 return [t+self.getFirstInstant() for t in indices] | 1700 return [t+self.getFirstInstant() for t in indices], intersections, rightToLeftOrientations |
1688 | 1701 |
1689 def computeTrajectorySimilarities(self, prototypes, lcss): | 1702 def computeTrajectorySimilarities(self, prototypes, lcss): |
1690 'Computes the similarities to the prototypes using the LCSS' | 1703 'Computes the similarities to the prototypes using the LCSS' |
1691 if not hasattr(self, 'prototypeSimilarities'): | 1704 if not hasattr(self, 'prototypeSimilarities'): |
1692 self.prototypeSimilarities = [] | 1705 self.prototypeSimilarities = [] |