detect if a point is on left or right side of track

test/util/test_geometry.py

@@ -248,26 +248,26 @@ def testSide(self):
         trace1.addObs(p4)
 
         s = detect_side(trace1, 18, 8)
-        self.assertEqual(s, -1)
+        self.assertEqual(s, [-1])
 
         # Gauche
         trace2 = trace1.reverse()
         s = detect_side(trace2, 18, 8)
-        self.assertEqual(s, 1)
+        self.assertEqual(s, [1])
 
         # Line
         s = detect_side(trace1, 22, 10)
-        self.assertEqual(s, 0)
+        self.assertEqual(s, [0])
 
         s = detect_side(trace1, 15, 5)
-        self.assertEqual(s, -1)
+        self.assertEqual(s, [-1])
 
         # Cas réél - Gauche
         x = 904436.02026022097561508
         y = 6435693.33668548986315727
         trace3 = TrackReader.parseWkt("LineString (904449.30000000004656613 6435691.5, 904445.59999999997671694 6435692, 904442.19999999995343387 6435692.70000000018626451, 904438.90000000002328306 6435692.40000000037252903, 904438.40000000002328306 6435692.5, 904438.40000000002328306 6435692.5, 904437.19999999995343387 6435696, 904435.59999999997671694 6435699.70000000018626451, 904434.69999999995343387 6435704, 904434 6435708.40000000037252903, 904433.5 6435712.70000000018626451, 904433.09999999997671694 6435716.79999999981373549, 904432.5 6435720.79999999981373549, 904432.09999999997671694 6435722.29999999981373549, 904432.09999999997671694 6435722.29999999981373549, 904431.30000000004656613 6435724.90000000037252903, 904428.59999999997671694 6435728.20000000018626451, 904425.40000000002328306 6435730.5, 904422.09999999997671694 6435733, 904418.80000000004656613 6435735.40000000037252903)")
         s = detect_side(trace3, x, y)
-        self.assertEqual(s, 1)
+        self.assertEqual(s, [1])
 
 
 if __name__ == '__main__':

tracklib/util/geometry.py

@@ -323,7 +323,6 @@ def proj_polyligne(Xp, Yp, x, y):
             continue
 
         dist, xp, yp = proj_segment([x1, y1, x2, y2], x, y)
-
         if dist < distmin:
             distmin = dist
             xproj = xp
@@ -340,26 +339,54 @@ def proj_polyligne(Xp, Yp, x, y):
 #   - track       :: Track
 #   - x,y         :: coordinate of the point we want to detect the side
 # --------------------------------------------------------------------------
-# Output : 0 if P is on the line
+# Output : list of sides
+#          0 if P is on the line
 #         +1 if P is on left side of the track
 #         -1 if P is on right side of the track
 # --------------------------------------------------------------------------
-def detect_side(track, x, y):
-    # First, get the coordinate of the projected point on track
-    distmin, xproj, yproj, iproj = proj_polyligne(track.getX(), track.getY(), x, y)
-    xa = track[iproj].position.getX()
-    ya = track[iproj].position.getY()
-    xb = track[iproj+1].position.getX()
-    yb = track[iproj+1].position.getY()
-
-    pdt = (xb-xa)*(y-ya) - (yb-ya)*(x-xa)
-    if pdt > 0:
-        return 1
-    elif pdt < 0:
-        return -1
-    else:
-        return 0
+def detect_side(track, x, y, seuilMemeProj=0.1):
+    SIDES = []
+
+    Xp = track.getX()
+    Yp = track.getY()
+
+    distmin = 1e400
+    INDICES = []
+    for i in range(len(Xp) - 1):
+        x1 = Xp[i]
+        y1 = Yp[i]
+        x2 = Xp[i + 1]
+        y2 = Yp[i + 1]
+
+        if (abs(x1-x2) + abs(y1-y2) < 1e-16):
+            continue
+
+        dist, xp, yp = proj_segment([x1, y1, x2, y2], x, y)
+
+        if abs(dist - distmin) < seuilMemeProj:
+            if dist < distmin:
+                distmin = dist
+            INDICES.append(i)
+        elif dist < distmin:
+            distmin = dist
+            INDICES = []
+            INDICES.append(i)
+
+    for iproj in INDICES:
+        xa = track[iproj].position.getX()
+        ya = track[iproj].position.getY()
+        xb = track[iproj+1].position.getX()
+        yb = track[iproj+1].position.getY()
+
+        pdt = (xb-xa)*(y-ya) - (yb-ya)*(x-xa)
+        if pdt > 0:
+            SIDES.append(1)
+        elif pdt < 0:
+            SIDES.append(-1)
+        else:
+            SIDES.append(0)
 
+    return SIDES
 
 
 # --------------------------------------------------------------------------