bug fixed with near 0 sine

AugmentedDesignLab · Aug 3, 2021 · f7d5dc9 · f7d5dc9
1 parent eeb975c
commit f7d5dc9
Show file tree

Hide file tree

Showing 3 changed files with 25 additions and 25 deletions.
diff --git a/log.txt b/log.txt
@@ -216,9 +216,6 @@ RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmar
 RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmark length + space = 0 - ignoring
 RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmark length + space = 0 - ignoring
 RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmark length + space = 0 - ignoring
-RoadManager.cpp / 5584 / roadmanager::Position::SetRoadMarkPos(): Truncate road mark s pos (270.01) to road length (270.00)
-RoadManager.cpp / 5584 / roadmanager::Position::SetRoadMarkPos(): Truncate road mark s pos (270.01) to road length (270.00)
-RoadManager.cpp / 5584 / roadmanager::Position::SetRoadMarkPos(): Truncate road mark s pos (270.01) to road length (270.00)
 RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmark length + space = 0 - ignoring
 RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmark length + space = 0 - ignoring
 RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmark length + space = 0 - ignoring
@@ -236,13 +233,8 @@ RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmar
 RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmark length + space = 0 - ignoring
 RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmark length + space = 0 - ignoring
 RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmark length + space = 0 - ignoring
-RoadManager.cpp / 5584 / roadmanager::Position::SetRoadMarkPos(): Truncate road mark s pos (192.01) to road length (192.01)
-RoadManager.cpp / 5584 / roadmanager::Position::SetRoadMarkPos(): Truncate road mark s pos (192.01) to road length (192.01)
-RoadManager.cpp / 5584 / roadmanager::Position::SetRoadMarkPos(): Truncate road mark s pos (192.01) to road length (192.01)
 RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmark length + space = 0 - ignoring
 RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmark length + space = 0 - ignoring
-RoadManager.cpp / 5584 / roadmanager::Position::SetRoadMarkPos(): Truncate road mark s pos (192.01) to road length (192.01)
-RoadManager.cpp / 5584 / roadmanager::Position::SetRoadMarkPos(): Truncate road mark s pos (192.01) to road length (192.01)
 RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmark length + space = 0 - ignoring
 RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmark length + space = 0 - ignoring
 RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmark length + space = 0 - ignoring
@@ -315,10 +307,7 @@ RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmar
 RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmark length + space = 0 - ignoring
 RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmark length + space = 0 - ignoring
 RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmark length + space = 0 - ignoring
-RoadManager.cpp / 5584 / roadmanager::Position::SetRoadMarkPos(): Truncate road mark s pos (82.01) to road length (82.01)
-RoadManager.cpp / 5584 / roadmanager::Position::SetRoadMarkPos(): Truncate road mark s pos (82.01) to road length (82.01)
 RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmark length + space = 0 - ignoring
-RoadManager.cpp / 5584 / roadmanager::Position::SetRoadMarkPos(): Truncate road mark s pos (82.01) to road length (82.01)
 RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmark length + space = 0 - ignoring
 RoadManager.cpp / 4288 / roadmanager::OpenDrive::SetRoadOSI(): Generating OSI lane boundaries
 RoadManager.cpp / 4290 / roadmanager::OpenDrive::SetRoadOSI(): OSI road features done
diff --git a/roadgen/controlLine/ControlPointIntersectionAdapter.py b/roadgen/controlLine/ControlPointIntersectionAdapter.py
@@ -25,6 +25,8 @@ def createIntersection(id, builder, point: ControlPoint, firstIncidentId,
         laneWidth = 3
         roadDefs = []
 
+        # point.printAdjacentPointsCW()
+
         nIncidentPoints = len(point.adjacentPointsCWOrder)
         # if nIncidentPoints > 3:
         #     print(f"4 arms")
@@ -189,9 +191,12 @@ def getMinDistance(point: ControlPoint, adjPoint, laneConfigurations = None):
         prevPoint = adjPoints[prevIndex]
         nextPoint = adjPoints[nextIndex]
 
-        print(f"adjPoint", adjPoint)
-        print(f"prevPoint", prevPoint)
-        print(f"nextPoint", nextPoint)
+        # print(f"adjPoint", adjPoint)
+        # print(f"prevPoint", prevPoint)
+        # print(f"nextPoint", nextPoint)
+        # print(f"adjHeading", headings[curIndex])
+        # print(f"prevHeading", headings[prevIndex])
+        # print(f"nextHeading", headings[nextIndex])
 
         n_left =  1
         n_right = 1
@@ -200,45 +205,51 @@ def getMinDistance(point: ControlPoint, adjPoint, laneConfigurations = None):
             (n_left, n_right) = laneConfigurations[point][adjPoint]
 
         # min distance based on the difference in headings
-        diffWithPrev = abs(headings[curIndex] - headings[prevIndex])
+        diffWithPrev = abs(headings[curIndex] - headings[prevIndex]) % (np.pi * 2)
+
+        if diffWithPrev > np.pi:
+            diffWithPrev = np.pi * 2 - diffWithPrev
+
         n_left_prev =  1
         n_right_prev = 1
 
         if laneConfigurations is not None:
             (n_left_prev, n_right_prev) = laneConfigurations[point][prevPoint]
 
-        totalLaneWidth = (n_left + n_right_prev) * laneWidth
+        totalLaneWidth = (n_left + n_right_prev + 1) * laneWidth
 
         if diffWithPrev <= np.pi / 2: # less than 90:
             minWithPrev = abs(totalLaneWidth / math.sin(diffWithPrev))
         else:
             minWithPrev = totalLaneWidth
 
-        print(f"minWithPrev", minWithPrev)
-        print(f"totalLaneWidth", totalLaneWidth)
-        print(f"diffWithPrev", diffWithPrev)
+        # print(f"minWithPrev", minWithPrev)
+        # print(f"totalLaneWidth", totalLaneWidth)
+        # print(f"diffWithPrev", diffWithPrev)
 
 
         if minDistance < minWithPrev:
             minDistance = minWithPrev
 
-        diffWithNext = abs(headings[curIndex] - headings[nextIndex])
+        diffWithNext = abs(headings[curIndex] - headings[nextIndex]) % (np.pi * 2)
+        if diffWithNext > np.pi:
+            diffWithNext = np.pi * 2 - diffWithNext
         n_left_next =  1
         n_right_next = 1
 
         if laneConfigurations is not None:
             (n_left_next, n_right_next) = laneConfigurations[point][nextPoint]
 
-        totalLaneWidth = (n_right + n_left_next) * laneWidth
+        totalLaneWidth = (n_right + n_left_next + 1) * laneWidth
 
         if diffWithNext <= np.pi / 2: # less than 90:
             minWithNext = abs(totalLaneWidth / math.sin(diffWithNext))
         else:
             minWithNext = totalLaneWidth
 
-        print(f"minWithNext", minWithNext)
-        print(f"totalLaneWidth", totalLaneWidth)
-        print(f"diffWithNext", diffWithNext)
+        # print(f"minWithNext", minWithNext)
+        # print(f"totalLaneWidth", totalLaneWidth)
+        # print(f"diffWithNext", diffWithNext)
 
         if minDistance < minWithNext:
             minDistance = minWithNext

diff --git a/roadgen/definitions/DirectionQuadrant.py b/roadgen/definitions/DirectionQuadrant.py
@@ -1,4 +1,4 @@
-from z3 import IntVal
+# from z3 import IntVal
 
 from enum import Enum, auto