Mercurial Hosting > traffic-intelligence
changeset 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 | 9f7a4a026dab |
children | afdbfba94fbc |
files | trafficintelligence/moving.py trafficintelligence/tests/moving.txt |
diffstat | 2 files changed, 83 insertions(+), 40 deletions(-) [+] |
line wrap: on
line diff
--- a/trafficintelligence/moving.py Mon Jul 19 11:25:16 2021 -0400 +++ b/trafficintelligence/moving.py Mon Sep 27 14:05:33 2021 -0400 @@ -268,8 +268,9 @@ plot([self.x], [self.y], options, **kwargs) @staticmethod - def plotSegment(p1, p2, options = 'o', **kwargs): + def plotSegment(p1, p2, options = 'o', withOrigin = True, **kwargs): plot([p1.x, p2.x], [p1.y, p2.y], options, **kwargs) + p1.plot('or', **kwargs) def angle(self): return atan2(self.y, self.x) @@ -405,7 +406,7 @@ '''Returns the bounding rectangle of the points, aligned on the vector v A list of points is returned: front left, front right, rear right, rear left''' e1 = v.normalize() - e2 = e1.orthogonal() + e2 = e1.orthogonal(False) xCoords = [] yCoords = [] for p in points: @@ -415,11 +416,13 @@ xmax = max(xCoords) ymin = min(yCoords) ymax = max(yCoords) - frontLeft = Point(xmax, ymin) - frontRight = Point(xmax, ymax) - rearLeft = Point(xmin, ymin) - rearRight = Point(xmin, ymax) - return [Point(Point.dot(e1, p), Point.dot(e2, p)) for p in [frontLeft, frontRight, rearRight, rearLeft]] + frontLeft = Point(xmax, ymax) + frontRight = Point(xmax, ymin) + rearLeft = Point(xmin, ymax) + rearRight = Point(xmin, ymin) + originalE1 = Point(Point.dot(e1, Point(1,0)),Point.dot(e2, Point(1,0))) + originalE2 = Point(Point.dot(e1, Point(0,1)),Point.dot(e2, Point(0,1))) + return [Point(Point.dot(originalE1, p), Point.dot(originalE2, p)) for p in [frontLeft, frontRight, rearRight, rearLeft]] if shapelyAvailable: def pointsInPolygon(points, polygon): @@ -605,17 +608,19 @@ return (f1.position-f2.position).norm2Squared()<maxDistance2 and (f1.velocity-f2.velocity).norm2Squared()<maxDeltavelocity2 def intersection(p1, p2, p3, p4): - ''' Intersection point (x,y) of lines formed by the vectors p1-p2 and p3-p4 - http://paulbourke.net/geometry/pointlineplane/''' + ''' Intersection point (x,y) of the segments [p1, p2] and [p3, p4] + Returns the intersection point and the ratio of its position along [p1, p2] from p1 + + Based on http://paulbourke.net/geometry/pointlineplane/''' dp12 = p2-p1 dp34 = p4-p3 #det = (p4.y-p3.y)*(p2.x-p1.x)-(p4.x-p3.x)*(p2.y-p1.y) det = float(dp34.y*dp12.x-dp34.x*dp12.y) if det == 0.: - return None + return None, None else: ua = (dp34.x*(p1.y-p3.y)-dp34.y*(p1.x-p3.x))/det - return p1+dp12.__mul__(ua) + return p1+dp12.__mul__(ua), ua # def intersection(p1, p2, dp1, dp2): # '''Returns the intersection point between the two lines @@ -634,29 +639,34 @@ # return Point(intersection[0,0], intersection[1,0]) def segmentIntersection(p1, p2, p3, p4): - '''Returns the intersecting point of the segments [p1, p2] and [p3, p4], None otherwise''' + '''Returns the intersecting point (and ratio along [p1, p2]) of the segments [p1, p2] and [p3, p4], None otherwise''' 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()): - return None + return None, None else: - inter = intersection(p1, p2, p3, p4) + inter, ratio = intersection(p1, p2, p3, p4) if (inter is not None and utils.inBetween(p1.x, p2.x, inter.x) and utils.inBetween(p3.x, p4.x, inter.x) and utils.inBetween(p1.y, p2.y, inter.y) and utils.inBetween(p3.y, p4.y, inter.y)): - return inter + return inter, ratio else: - return None + return None, None def segmentLineIntersection(p1, p2, p3, p4): '''Indicates if the line going through p1 and p2 intersects inside p3, p4''' - inter = intersection(p1, p2, p3, p4) + inter, ratio = intersection(p1, p2, p3, p4) if inter is not None and utils.inBetween(p3.x, p4.x, inter.x) and utils.inBetween(p3.y, p4.y, inter.y): - return inter + return inter, ratio else: - return None + return None, None +def segmentOrientationCrossing(p1, p2, p3, p4): + '''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''' + dp12 = p2-p1 + ortho = dp12.orthogonal(False) + return Point.dot(ortho, p4-p3) > 0 class Trajectory(object): '''Class for trajectories: temporal sequence of positions @@ -933,27 +943,30 @@ else: return None - def getIntersections(self, p1, p2): + def getIntersections(self, p1, p2, computeOrientations = False): '''Returns a list of the indices at which the trajectory intersects with the segment of extremities p1 and p2 Returns an empty list if there is no crossing''' indices = [] intersections = [] + rightToLeftOrientations = [] for i in range(self.length()-1): q1=self.__getitem__(i) q2=self.__getitem__(i+1) - p = segmentIntersection(q1, q2, p1, p2) + p, ratio = segmentIntersection(p1, p2, q1, q2) if p is not None: - if q1.x != q2.x: - ratio = (p.x-q1.x)/(q2.x-q1.x) - elif q1.y != q2.y: - ratio = (p.y-q1.y)/(q2.y-q1.y) - else: - ratio = 0 +# if q1.x != q2.x: +# ratio = (p.x-q1.x)/(q2.x-q1.x) +# elif q1.y != q2.y: +# ratio = (p.y-q1.y)/(q2.y-q1.y) +# else: +# ratio = 0 indices.append(i+ratio) intersections.append(p) - return indices, intersections + if computeOrientations: + rightToLeftOrientations.append(segmentOrientationCrossing(p1, p2, q1, q2)) + return indices, intersections, rightToLeftOrientations def getLineIntersections(self, p1, p2): '''Returns a list of the indices at which the trajectory @@ -965,14 +978,14 @@ for i in range(self.length()-1): q1=self.__getitem__(i) q2=self.__getitem__(i+1) - p = segmentLineIntersection(p1, p2, q1, q2) + p, ratio = segmentLineIntersection(q1, q2, p1, p2) if p is not None: - if q1.x != q2.x: - ratio = (p.x-q1.x)/(q2.x-q1.x) - elif q1.y != q2.y: - ratio = (p.y-q1.y)/(q2.y-q1.y) - else: - ratio = 0 + # if q1.x != q2.x: + # ratio = (p.x-q1.x)/(q2.x-q1.x) + # elif q1.y != q2.y: + # ratio = (p.y-q1.y)/(q2.y-q1.y) + # else: + # ratio = 0 indices.append(i+ratio) intersections.append(p) return indices, intersections @@ -1679,12 +1692,12 @@ self.startRouteID = startRouteID self.endRouteID = endRouteID - def getInstantsCrossingLane(self, p1, p2): + def getInstantsCrossingLine(self, p1, p2, computeOrientations = False): '''Returns the instant(s) at which the object passes from one side of the segment to the other empty list if there is no crossing''' - indices, intersections = self.positions.getIntersections(p1, p2) - return [t+self.getFirstInstant() for t in indices] + indices, intersections, rightToLeftOrientations = self.positions.getIntersections(p1, p2, computeOrientations) + return [t+self.getFirstInstant() for t in indices], intersections, rightToLeftOrientations def computeTrajectorySimilarities(self, prototypes, lcss): 'Computes the similarities to the prototypes using the LCSS'
--- a/trafficintelligence/tests/moving.txt Mon Jul 19 11:25:16 2021 -0400 +++ b/trafficintelligence/tests/moving.txt Mon Sep 27 14:05:33 2021 -0400 @@ -90,12 +90,42 @@ ((1.0...,1.0...), (10.0...,10.0...)) >>> segmentIntersection(Point(0,0), Point(0,1), Point(1,1), Point(2,3)) +(None, None) >>> segmentIntersection(Point(0,1), Point(0,3), Point(1,0), Point(3,1)) +(None, None) >>> segmentIntersection(Point(0.,0.), Point(2.,2.), Point(0.,2.), Point(2.,0.)) -(1.000000,1.000000) +((1.000000,1.000000), 0.5) >>> segmentIntersection(Point(0,0), Point(4,4), Point(0,4), Point(4,0)) -(2.000000,2.000000) +((2.000000,2.000000), 0.5) +>>> segmentIntersection(Point(0,0), Point(0,3), Point(1,1), Point(-1,1)) # doctest:+ELLIPSIS +((0.000000,1.000000), 0.333...) >>> segmentIntersection(Point(0,1), Point(1,2), Point(2,0), Point(3,2)) +(None, None) + +>>> segmentOrientationCrossing(Point(0.,0.), Point(2.,2.), Point(0.,2.), Point(2.,0.)) +False +>>> segmentOrientationCrossing(Point(0.,0.), Point(2.,2.), Point(2.,0.), Point(0.,2.)) +True +>>> segmentOrientationCrossing(Point(0,0), Point(0,3), Point(1,1), Point(-1,1)) +True + +>>> o1 = MovingObject.generate(1, Point(1.,0.), Point(1.,0.), TimeInterval(0,10)) +>>> instants, intersections, rightToLeftOrientations = o1.getInstantsCrossingLine(Point(0.,3.5), Point(2.,3.5)) +>>> rightToLeftOrientations == [] +True +>>> len(instants) +0 +>>> o1 = MovingObject.generate(1, Point(0.,1.), Point(1.,0.), TimeInterval(0,10)) +>>> instants, intersections, rightToLeftOrientations = o1.getInstantsCrossingLine(Point(3.5,0.), Point(3.5, 2.), False) +>>> rightToLeftOrientations == [] +True +>>> instants[0] +3.5 +>>> instants, intersections, rightToLeftOrientations = o1.getInstantsCrossingLine(Point(3.5,0.), Point(3.5, 2.), True) +>>> len(rightToLeftOrientations) +1 +>>> rightToLeftOrientations[0] +False >>> t1 = Trajectory.fromPointList([(92.2, 102.9), (56.7, 69.6)]) >>> t2 = Trajectory.fromPointList([(92.2, 102.9), (56.7, 69.6)])