# HG changeset patch
# User Nicolas Saunier <nicolas.saunier@polymtl.ca>
# Date 1632765933 14400
# Node ID b55adb13f262c9b840d9bda6506ac02c3b725c52
# Parent  9f7a4a026dabbf252e8ca8c3a3b72b4138f9128d
added functions on line crossing orientation and important reorganization and cleaning of related code

diff -r 9f7a4a026dab -r b55adb13f262 trafficintelligence/moving.py
--- 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'
diff -r 9f7a4a026dab -r b55adb13f262 trafficintelligence/tests/moving.txt
--- 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)])