Mercurial > hg > nsaunier > traffic-intelligence

comparison trafficintelligence/moving.py @ 1170:b55adb13f262

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
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 = []