Skip to content

Commit f7d5dc9

Browse files
adhocmasteradhocmaster
committed
bug fixed with near 0 sine
1 parent eeb975c commit f7d5dc9

File tree

3 files changed

+25
-25
lines changed

3 files changed

+25
-25
lines changed

‎log.txt‎

Lines changed: 0 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -216,9 +216,6 @@ RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmar
216216
RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmark length + space = 0 - ignoring
217217
RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmark length + space = 0 - ignoring
218218
RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmark length + space = 0 - ignoring
219-
RoadManager.cpp / 5584 / roadmanager::Position::SetRoadMarkPos(): Truncate road mark s pos (270.01) to road length (270.00)
220-
RoadManager.cpp / 5584 / roadmanager::Position::SetRoadMarkPos(): Truncate road mark s pos (270.01) to road length (270.00)
221-
RoadManager.cpp / 5584 / roadmanager::Position::SetRoadMarkPos(): Truncate road mark s pos (270.01) to road length (270.00)
222219
RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmark length + space = 0 - ignoring
223220
RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmark length + space = 0 - ignoring
224221
RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmark length + space = 0 - ignoring
@@ -236,13 +233,8 @@ RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmar
236233
RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmark length + space = 0 - ignoring
237234
RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmark length + space = 0 - ignoring
238235
RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmark length + space = 0 - ignoring
239-
RoadManager.cpp / 5584 / roadmanager::Position::SetRoadMarkPos(): Truncate road mark s pos (192.01) to road length (192.01)
240-
RoadManager.cpp / 5584 / roadmanager::Position::SetRoadMarkPos(): Truncate road mark s pos (192.01) to road length (192.01)
241-
RoadManager.cpp / 5584 / roadmanager::Position::SetRoadMarkPos(): Truncate road mark s pos (192.01) to road length (192.01)
242236
RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmark length + space = 0 - ignoring
243237
RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmark length + space = 0 - ignoring
244-
RoadManager.cpp / 5584 / roadmanager::Position::SetRoadMarkPos(): Truncate road mark s pos (192.01) to road length (192.01)
245-
RoadManager.cpp / 5584 / roadmanager::Position::SetRoadMarkPos(): Truncate road mark s pos (192.01) to road length (192.01)
246238
RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmark length + space = 0 - ignoring
247239
RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmark length + space = 0 - ignoring
248240
RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmark length + space = 0 - ignoring
@@ -315,10 +307,7 @@ RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmar
315307
RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmark length + space = 0 - ignoring
316308
RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmark length + space = 0 - ignoring
317309
RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmark length + space = 0 - ignoring
318-
RoadManager.cpp / 5584 / roadmanager::Position::SetRoadMarkPos(): Truncate road mark s pos (82.01) to road length (82.01)
319-
RoadManager.cpp / 5584 / roadmanager::Position::SetRoadMarkPos(): Truncate road mark s pos (82.01) to road length (82.01)
320310
RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmark length + space = 0 - ignoring
321-
RoadManager.cpp / 5584 / roadmanager::Position::SetRoadMarkPos(): Truncate road mark s pos (82.01) to road length (82.01)
322311
RoadManager.cpp / 4151 / roadmanager::OpenDrive::SetRoadMarkOSIPoints(): Roadmark length + space = 0 - ignoring
323312
RoadManager.cpp / 4288 / roadmanager::OpenDrive::SetRoadOSI(): Generating OSI lane boundaries
324313
RoadManager.cpp / 4290 / roadmanager::OpenDrive::SetRoadOSI(): OSI road features done

‎roadgen/controlLine/ControlPointIntersectionAdapter.py‎

Lines changed: 24 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -25,6 +25,8 @@ def createIntersection(id, builder, point: ControlPoint, firstIncidentId,
2525
laneWidth = 3
2626
roadDefs = []
2727

28+
# point.printAdjacentPointsCW()
29+
2830
nIncidentPoints = len(point.adjacentPointsCWOrder)
2931
# if nIncidentPoints > 3:
3032
# print(f"4 arms")
@@ -189,9 +191,12 @@ def getMinDistance(point: ControlPoint, adjPoint, laneConfigurations = None):
189191
prevPoint = adjPoints[prevIndex]
190192
nextPoint = adjPoints[nextIndex]
191193

192-
print(f"adjPoint", adjPoint)
193-
print(f"prevPoint", prevPoint)
194-
print(f"nextPoint", nextPoint)
194+
# print(f"adjPoint", adjPoint)
195+
# print(f"prevPoint", prevPoint)
196+
# print(f"nextPoint", nextPoint)
197+
# print(f"adjHeading", headings[curIndex])
198+
# print(f"prevHeading", headings[prevIndex])
199+
# print(f"nextHeading", headings[nextIndex])
195200

196201
n_left = 1
197202
n_right = 1
@@ -200,45 +205,51 @@ def getMinDistance(point: ControlPoint, adjPoint, laneConfigurations = None):
200205
(n_left, n_right) = laneConfigurations[point][adjPoint]
201206

202207
# min distance based on the difference in headings
203-
diffWithPrev = abs(headings[curIndex] - headings[prevIndex])
208+
diffWithPrev = abs(headings[curIndex] - headings[prevIndex]) % (np.pi * 2)
209+
210+
if diffWithPrev > np.pi:
211+
diffWithPrev = np.pi * 2 - diffWithPrev
212+
204213
n_left_prev = 1
205214
n_right_prev = 1
206215

207216
if laneConfigurations is not None:
208217
(n_left_prev, n_right_prev) = laneConfigurations[point][prevPoint]
209218

210-
totalLaneWidth = (n_left + n_right_prev) * laneWidth
219+
totalLaneWidth = (n_left + n_right_prev + 1) * laneWidth
211220

212221
if diffWithPrev <= np.pi / 2: # less than 90:
213222
minWithPrev = abs(totalLaneWidth / math.sin(diffWithPrev))
214223
else:
215224
minWithPrev = totalLaneWidth
216225

217-
print(f"minWithPrev", minWithPrev)
218-
print(f"totalLaneWidth", totalLaneWidth)
219-
print(f"diffWithPrev", diffWithPrev)
226+
# print(f"minWithPrev", minWithPrev)
227+
# print(f"totalLaneWidth", totalLaneWidth)
228+
# print(f"diffWithPrev", diffWithPrev)
220229

221230

222231
if minDistance < minWithPrev:
223232
minDistance = minWithPrev
224233

225-
diffWithNext = abs(headings[curIndex] - headings[nextIndex])
234+
diffWithNext = abs(headings[curIndex] - headings[nextIndex]) % (np.pi * 2)
235+
if diffWithNext > np.pi:
236+
diffWithNext = np.pi * 2 - diffWithNext
226237
n_left_next = 1
227238
n_right_next = 1
228239

229240
if laneConfigurations is not None:
230241
(n_left_next, n_right_next) = laneConfigurations[point][nextPoint]
231242

232-
totalLaneWidth = (n_right + n_left_next) * laneWidth
243+
totalLaneWidth = (n_right + n_left_next + 1) * laneWidth
233244

234245
if diffWithNext <= np.pi / 2: # less than 90:
235246
minWithNext = abs(totalLaneWidth / math.sin(diffWithNext))
236247
else:
237248
minWithNext = totalLaneWidth
238249

239-
print(f"minWithNext", minWithNext)
240-
print(f"totalLaneWidth", totalLaneWidth)
241-
print(f"diffWithNext", diffWithNext)
250+
# print(f"minWithNext", minWithNext)
251+
# print(f"totalLaneWidth", totalLaneWidth)
252+
# print(f"diffWithNext", diffWithNext)
242253

243254
if minDistance < minWithNext:
244255
minDistance = minWithNext

‎roadgen/definitions/DirectionQuadrant.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
from z3 import IntVal
1+
# from z3 import IntVal
22

33
from enum import Enum, auto
44

0 commit comments

Comments
 (0)