private static void FindClosestEnds(SegmentedLine3D segA, SegmentedLine3D segB, out bool useStartA, out bool useStartB)
{
var min = (segA.Start - segB.Start).sqrMagnitude;
useStartA = true;
useStartB = true;
var sqrMag = (segA.Start - segB.End).sqrMagnitude;
if (sqrMag < min)
{
min = sqrMag;
useStartA = true;
useStartB = false;
}
sqrMag = (segA.End - segB.Start).sqrMagnitude;
if (sqrMag < min)
{
min = sqrMag;
useStartA = false;
useStartB = true;
}
sqrMag = (segA.End - segB.End).sqrMagnitude;
if (sqrMag < min)
{
useStartA = false;
useStartB = false;
}
}
public static bool IsSegmentDuplicate(SegmentedLine3D a, SegmentedLine3D b, float threshold)
{
if (Vector3.Distance(a.Start, b.Start) < threshold && Vector3.Distance(a.End, b.End) < threshold)
{
return(true);
}
if (Vector3.Distance(a.Start, b.End) < threshold && Vector3.Distance(a.End, b.Start) < threshold)
{
return(true);
}
return(false);
}
public Dictionary <MapTrafficLane, LaneLineOverrideData> Process()
{
var mapManagerData = new MapManagerData();
var lanes = mapManagerData.GetTrafficLanes();
var lineDict = new Dictionary <MapLine, SegmentedLine3D>();
var laneDict = new Dictionary <MapTrafficLane, LaneLineSegments>();
foreach (var lane in lanes)
{
var laneData = new LaneLineSegments();
if (lineDict.ContainsKey(lane.rightLineBoundry))
{
laneData.rightLine = lineDict[lane.rightLineBoundry];
}
else
{
var segment = LaneLineToSegmentedLine(lane.rightLineBoundry);
laneData.rightLine = segment;
lineDict[lane.rightLineBoundry] = segment;
}
if (lineDict.ContainsKey(lane.leftLineBoundry))
{
laneData.leftLine = lineDict[lane.leftLineBoundry];
}
else
{
var segment = LaneLineToSegmentedLine(lane.leftLineBoundry);
laneData.leftLine = segment;
lineDict[lane.leftLineBoundry] = segment;
}
laneDict.Add(lane, laneData);
}
connectedSegments = ConnectSegments(segments, settings);
const float correctionThreshold = 1f;
SegmentedLine3D prevMatch = null;
var index = 0;
try
{
foreach (var mapLine in lineDict.Values)
{
var progress = (float)index++ / lineDict.Count;
EditorUtility.DisplayProgressBar("Correcting HD map lines", $"Processing lane {index} of {lineDict.Count}", progress);
for (var i = 0; i < mapLine.lines.Count; ++i)
{
Vector3 start, end;
if (i == 0)
{
var startVec = mapLine.lines[i].Vector;
var startnVec = new Vector2(-startVec.z, startVec.x).normalized *correctionThreshold;
start = FindBestSegmentMatch(mapLine.lines[i].Start, startnVec, prevMatch, out prevMatch);
}
else
{
start = mapLine.lines[i - 1].End;
}
var vec = mapLine.lines[i].Vector;
if (i != mapLine.lines.Count - 1)
{
vec += mapLine.lines[i + 1].Vector;
}
var nVec = new Vector2(-vec.z, vec.x).normalized *correctionThreshold;
end = FindBestSegmentMatch(mapLine.lines[i].End, nVec, prevMatch, out prevMatch);
var width = mapLine.lines[i].width;
var color = mapLine.lines[i].color;
mapLine.lines[i] = new Line3D(start, end, width)
{
color = color
};
}
}
}
finally
{
EditorUtility.ClearProgressBar();
}
var result = new Dictionary <MapTrafficLane, LaneLineOverrideData>();
foreach (var kvp in laneDict)
{
var entry = new LaneLineOverrideData();
var rightPoints = new List <Vector3>();
rightPoints.Add(kvp.Value.rightLine.lines[0].Start);
for (var i = 0; i < kvp.Value.rightLine.lines.Count; ++i)
{
rightPoints.Add(kvp.Value.rightLine.lines[i].End);
}
entry.rightLineWorldPositions = rightPoints;
var leftPoints = new List <Vector3>();
leftPoints.Add(kvp.Value.leftLine.lines[0].Start);
for (var i = 0; i < kvp.Value.leftLine.lines.Count; ++i)
{
leftPoints.Add(kvp.Value.leftLine.lines[i].End);
}
entry.leftLineWorldPositions = leftPoints;
result.Add(kvp.Key, entry);
}
return(result);
}
private Vector3 FindBestSegmentMatch(Vector3 position, Vector2 nVector, SegmentedLine3D previousMatchedSegment, out SegmentedLine3D matchedSegment)
{
const float angleThreshold = 30f;
var pos2D = new Vector2(position.x, position.z);
var nVec0 = pos2D - nVector;
var nVec1 = pos2D + nVector;
var nVec = nVec1 - nVec0;
var bestSqrMag = float.MaxValue;
var bestPos = Vector3.zero;
var matchFound = false;
SegmentedLine3D currentMatchedSegment = null;
void CheckSegment(SegmentedLine3D segment)
{
// foreach (var line in segment.lines)
for (var i = 0; i < segment.lines.Count; ++i)
{
var lnStart = segment.lines[i].Start;
var lnEnd = segment.lines[i].End;
if (i == 0)
{
lnStart -= segment.lines[i].Vector.normalized;
}
if (i == segment.lines.Count - 1)
{
lnEnd += segment.lines[i].Vector.normalized;
}
var lnStart2D = new Vector2(lnStart.x, lnStart.z);
var lnEnd2D = new Vector2(lnEnd.x, lnEnd.z);
if (Angle(lnEnd2D - lnStart2D, nVec) < 90 - angleThreshold)
{
continue;
}
LineUtils.LineLineIntersection(nVec0, nVec1, lnStart2D, lnEnd2D, out var _, out var segmentsIntersect, out var intersection);
if (!segmentsIntersect)
{
continue;
}
var sqrMag = (intersection - pos2D).sqrMagnitude;
if (sqrMag < bestSqrMag)
{
matchFound = true;
bestSqrMag = sqrMag;
currentMatchedSegment = segment;
bestPos = Vector3.Lerp(lnStart, lnEnd, (intersection.x - lnStart.x) / (lnEnd.x - lnStart.x));
}
}
}
if (previousMatchedSegment != null)
{
CheckSegment(previousMatchedSegment);
}
if (!matchFound)
{
foreach (var segment in connectedSegments)
{
CheckSegment(segment);
}
}
matchedSegment = currentMatchedSegment;
return(!matchFound ? position : bestPos);
}
