diff --git a/test/util/test_geometry.py b/test/util/test_geometry.py index 28fa1af..6697003 100644 --- a/test/util/test_geometry.py +++ b/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__': diff --git a/tracklib/util/geometry.py b/tracklib/util/geometry.py index a6e0808..e1d59ec 100644 --- a/tracklib/util/geometry.py +++ b/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 # --------------------------------------------------------------------------