public bool TryAddLine(Line line, float distanceThreshold)
{
foreach (var l in lines)
{
if (LineUtils.LineLineDistance(l, line) < distanceThreshold && LineUtils.LineLineAngle(l, line) < Settings.lineAngleThreshold)
{
lines.Add(line);
return(true);
}
}
return(false);
}
public static void Execute(Transform meshParent, LineDetectionSettings settings)
{
var roadRenderers = GetRoadRenderers();
var correctionNeeded = settings.lineSource == LineDetectionSettings.LineSource.CorrectedHdMap || settings.generateLineSensorData;
var corrector = correctionNeeded ? new MapLineAlignmentCorrector(settings) : null;
try
{
for (var i = 0; i < roadRenderers.Count; ++i)
{
var road = roadRenderers[i];
var progress = (float)i / roadRenderers.Count;
EditorUtility.DisplayProgressBar("Detecting lines", $"Processing road {i + 1} of {roadRenderers.Count}", progress);
var mf = road.GetComponent <MeshFilter>();
var tex = road.sharedMaterial.GetTexture(AlbedoTexProp);
var imagePath = Path.Combine(Application.dataPath, "..", AssetDatabase.GetAssetPath(tex));
var uvToWorldScale = LineUtils.EstimateUvToWorldScale(mf, tex);
var lines = DetectLines(imagePath, uvToWorldScale);
var approximatedLines = ProcessUvLines(lines, settings);
var worldSpaceSegments = UvToWorldMapper.CalculateWorldSpaceNew(approximatedLines, mf, uvToWorldScale);
ProcessWorldLines(worldSpaceSegments, settings, out var worldSpaceSnappedSegments);
if (correctionNeeded)
{
corrector.AddSegments(worldSpaceSegments);
}
if (settings.lineSource == LineDetectionSettings.LineSource.IntensityMap)
{
BuildWorldObject(worldSpaceSegments, road.transform, meshParent);
}
}
if (correctionNeeded)
{
var debugCorrectedSegments = corrector.Process();
var lineDataOverride = meshParent.gameObject.AddComponent <LaneLineOverride>();
lineDataOverride.SetData(debugCorrectedSegments);
}
}
finally
{
EditorUtility.ClearProgressBar();
}
}
public bool TryMergeIgnoreAngle(ApproximatedLine approxLine)
{
foreach (var line in lines)
{
foreach (var sLine in approxLine.lines)
{
if (LineUtils.LineLineDistance(line, sLine) < Settings.lineDistanceThreshold)
{
lines.AddRange(approxLine.lines);
return(true);
}
}
}
return(false);
}
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);
}
private static TriangleSearchResult TryGetNextTriangle(Vector2 uvStart, Vector2 uvDir, int triangleStart, int[] tris, Vector2[] uv, List <int> triangleEndMatches, List <Vector2> uvEndMatches, int pointCount, out Vector2 intersection, out int nextTriangleStart)
{
var inTriangleMatch = -1;
for (var i = 0; i < triangleEndMatches.Count; ++i)
{
if (triangleStart == triangleEndMatches[i])
{
inTriangleMatch = i;
break;
}
}
if (inTriangleMatch != -1 && (triangleEndMatches[inTriangleMatch] == triangleStart || TryMatchInTriangleEnd(uvStart, uvDir, uvEndMatches[inTriangleMatch], pointCount)))
{
intersection = uvEndMatches[inTriangleMatch];
nextTriangleStart = triangleStart;
return(TriangleSearchResult.WithinTriangle);
}
nextTriangleStart = -1;
intersection = uvStart;
var uv1 = uv[tris[triangleStart]];
var uv2 = uv[tris[triangleStart + 1]];
var uv3 = uv[tris[triangleStart + 2]];
var uvEnd = uvStart + uvDir * 50f;
uvStart = uvStart - uvDir * 50f;
LineUtils.LineLineIntersection(uv1, uv2, uvStart, uvEnd, out _, out var ints12, out var pos12, out _, out _);
LineUtils.LineLineIntersection(uv2, uv3, uvStart, uvEnd, out _, out var ints23, out var pos23, out _, out _);
LineUtils.LineLineIntersection(uv3, uv1, uvStart, uvEnd, out _, out var ints31, out var pos31, out _, out _);
var indexA = -1;
var indexB = -1;
var bestSqrMag = -1f;
if (ints12 && Vector3.SqrMagnitude(pos12 - uvStart) > bestSqrMag)
{
indexA = tris[triangleStart];
indexB = tris[triangleStart + 1];
bestSqrMag = Vector3.SqrMagnitude(pos12 - uvStart);
intersection = pos12;
}
if (ints23 && Vector3.SqrMagnitude(pos23 - uvStart) > bestSqrMag)
{
indexA = tris[triangleStart + 1];
indexB = tris[triangleStart + 2];
bestSqrMag = Vector3.SqrMagnitude(pos23 - uvStart);
intersection = pos23;
}
if (ints31 && Vector3.SqrMagnitude(pos31 - uvStart) > bestSqrMag)
{
indexA = tris[triangleStart + 2];
indexB = tris[triangleStart];
intersection = pos31;
}
bool TriangleMatches(int a1, int a2, int t1, int t2, int t3)
{
var a1Match = a1 == t1 || a1 == t2 || a1 == t3;
var a2Match = a2 == t1 || a2 == t2 || a2 == t3;
return(a1Match && a2Match);
}
for (var i = 0; i < tris.Length; i += 3)
{
if (i == triangleStart)
{
continue;
}
var t1 = tris[i];
var t2 = tris[i + 1];
var t3 = tris[i + 2];
if (TriangleMatches(indexA, indexB, t1, t2, t3))
{
nextTriangleStart = i;
break;
}
}
return(nextTriangleStart == -1 ? TriangleSearchResult.EdgeTermination : TriangleSearchResult.Continues);
}
private static List <SegmentedLine3D> GetWorldSpaceLines(Line line, MeshFilter meshFilter, float uvToWorldSpace)
{
var mesh = meshFilter.sharedMesh;
var tris = mesh.triangles;
var uv = mesh.uv;
var uvStart = new Vector2(line.Start.x / uvToWorldSpace, line.Start.y / uvToWorldSpace);
var uvEnd = new Vector2(line.End.x / uvToWorldSpace, line.End.y / uvToWorldSpace);
var uvDir = (uvEnd - uvStart).normalized;
var result = new List <SegmentedLine3D>();
var startUvMatches = new List <Vector2>();
var endUvMatches = new List <Vector2>();
var startTriangleMatches = new List <int>();
var endTriangleMatches = new List <int>();
for (var i = 0; i < tris.Length; i += 3)
{
var uv1 = uv[tris[i]];
var uv2 = uv[tris[i + 1]];
var uv3 = uv[tris[i + 2]];
var uvMin = Vector2.Min(Vector2.Min(uv1, uv2), uv3);
var uvMax = Vector2.Max(Vector2.Max(uv1, uv2), uv3);
for (var u = Mathf.Floor(uvMin.x); u < Mathf.Ceil(uvMax.x); ++u)
{
for (var v = Mathf.Floor(uvMin.y); v < Mathf.Ceil(uvMax.y); ++v)
{
var luvStart = new Vector2(u + uvStart.x, v + uvStart.y);
var luvEnd = new Vector2(u + uvEnd.x, v + uvEnd.y);
var bStart = GetBarycentric(uv1, uv2, uv3, luvStart);
var bEnd = GetBarycentric(uv1, uv2, uv3, luvEnd);
if (IsInTriangle(bStart))
{
var pos = bStart.x * uv1 + bStart.y * uv2 + bStart.z * uv3;
startUvMatches.Add(pos);
startTriangleMatches.Add(i);
}
if (IsInTriangle(bEnd))
{
var pos = bEnd.x * uv1 + bEnd.y * uv2 + bEnd.z * uv3;
endUvMatches.Add(pos);
endTriangleMatches.Add(i);
}
}
}
}
if (startUvMatches.Count > 0 || endUvMatches.Count > 0)
{
for (var i = 0; i < startUvMatches.Count; ++i)
{
var segment = Traverse(startUvMatches[i], uvDir, startTriangleMatches[i], endTriangleMatches, endUvMatches, meshFilter);
result.Add(segment);
}
for (var i = 0; i < endUvMatches.Count; ++i)
{
var segment = Traverse(endUvMatches[i], -uvDir, endTriangleMatches[i], startTriangleMatches, startUvMatches, meshFilter);
segment.Invert();
var duplicate = false;
foreach (var resSegment in result)
{
if (LineUtils.IsSegmentDuplicate(segment, resSegment, MergeThreshold))
{
duplicate = true;
break;
}
}
if (duplicate)
{
continue;
}
result.Add(segment);
}
}
// TODO: Sample mid-points in case only mid-section is visible - below is incomplete
/*
* else
* {
* // If no ends found - check midpoints
* Debug.LogError("No points on mesh");
* for (var j = 0; j < 8; ++j)
* {
* var uvMid = Vector3.Lerp(uvStart, uvEnd, (float) j / 8);
*
* for (var i = 0; i < tris.Length; i += 3)
* {
* var uv1 = uv[tris[i]];
* var uv2 = uv[tris[i + 1]];
* var uv3 = uv[tris[i + 2]];
*
* var pos1 = trans.TransformPoint(verts[tris[i]]);
* var pos2 = trans.TransformPoint(verts[tris[i + 1]]);
* var pos3 = trans.TransformPoint(verts[tris[i + 2]]);
*
* var bMid = GetBarycentric(uv1, uv2, uv3, uvStart);
*
* if (IsInTriangle(bMid))
* {
* var pos = bMid.x * pos1 + bMid.y * pos2 + bMid.z * pos3;
* line.TryAddMid(pos);
* }
* }
*
* if (line.midMatches.Count > 0)
* break;
* }
* }
*/
return(result);
}
public void Recalculate()
{
BestFitLine = LineUtils.GenerateLinearBestFit(lines, out var maxDist, out var avgDist);
WorstFit = maxDist;
AverageFit = avgDist;
}
private static List <ApproximatedLine> ProcessUvLines(List <Line> lines, LineDetectionSettings settings)
{
var approximatedLines = new List <ApproximatedLine>();
foreach (var line in lines)
{
var assigned = false;
foreach (var approxLine in approximatedLines)
{
if (approxLine.TryAddLine(line))
{
assigned = true;
break;
}
}
if (!assigned)
{
approximatedLines.Add(new ApproximatedLine(line, settings));
}
}
var changed = true;
var safety = 0;
while (changed)
{
if (safety++ > 20000)
{
throw new Exception("Broken infinite loop");
}
changed = false;
for (var i = 0; i < approximatedLines.Count - 1; ++i)
{
for (var j = i + 1; j < approximatedLines.Count; ++j)
{
if (approximatedLines[i].TryMerge(approximatedLines[j]))
{
approximatedLines.Remove(approximatedLines[j]);
changed = true;
break;
}
}
if (changed)
{
break;
}
}
}
for (var i = 0; i < approximatedLines.Count - 1; ++i)
{
for (var j = i + 1; j < approximatedLines.Count; ++j)
{
if (approximatedLines[j].lines.Count > 1)
{
continue;
}
if (approximatedLines[i].TryMergeIgnoreAngle(approximatedLines[j]))
{
approximatedLines.Remove(approximatedLines[j]);
}
}
}
for (var i = 0; i < approximatedLines.Count; ++i)
{
approximatedLines[i].Recalculate();
if (approximatedLines[i].BestFitLine.Length > settings.maxLineSegmentLength)
{
LineUtils.SplitByLength(approximatedLines[i], out var a, out var b);
approximatedLines.RemoveAt(i--);
approximatedLines.Add(a);
approximatedLines.Add(b);
}
}
for (var i = 0; i < approximatedLines.Count; ++i)
{
var line = approximatedLines[i];
if (!line.IsValid)
{
LineUtils.RemovePastThresholdLines(line);
}
if (!line.IsValid)
{
approximatedLines.RemoveAt(i--);
}
}
return(approximatedLines);
}
