private bool FindIntersects(List <ITrajectory> A, List <ITrajectory> B, bool isClockWise, int skip)
{
for (var x = isClockWise ? A.Count - 1 : 0; isClockWise?x >= 0 : x < A.Count; x += isClockWise ? -1 : 1)
{
var xPart = new StraightTrajectory(A[x]);
for (var y = isClockWise ? skip : B.Count - 1 - skip; isClockWise?y < B.Count : y >= 0; y += isClockWise ? 1 : -1)
{
var yPart = new StraightTrajectory(B[y]);
var intersect = Intersection.CalculateSingle(xPart, yPart);
if (intersect.IsIntersect)
{
if (isClockWise)
{
A[x] = xPart.Cut(0f, intersect.FirstT);
B[y] = yPart.Cut(intersect.SecondT, 1f);
B.RemoveRange(0, y);
}
else
{
A[x] = xPart.Cut(intersect.FirstT, 1f);
B[y] = yPart.Cut(0f, intersect.SecondT);
B.RemoveRange(y + 1, B.Count - (y + 1));
}
return(true);
}
}
}
return(false);
}
public void Divide(out ILineTrajectory trajectory1, out ILineTrajectory trajectory2)
{
var middle = (Trajectory.a + Trajectory.b) / 2;
trajectory1 = new StraightTrajectory(Trajectory.a, middle);
trajectory2 = new StraightTrajectory(middle, Trajectory.b);
}
protected IEnumerable <MarkupStyleDash> CalculateCroswalkDash(ILineTrajectory trajectory, float startT, float endT, Vector3 direction, ILineTrajectory[] borders, float length, float width)
{
var position = trajectory.Position((startT + endT) / 2);
var dashTrajectory = new StraightTrajectory(position, position + direction, false);
var intersects = MarkupIntersect.Calculate(dashTrajectory, borders, true);
intersects = intersects.OrderBy(i => i.FirstT).ToList();
var halfLength = length / 2;
var halfWidth = width / 2;
for (var i = 1; i < intersects.Count; i += 2)
{
var startOffset = GetOffset(intersects[i - 1], halfWidth);
var endOffset = GetOffset(intersects[i], halfWidth);
var start = Mathf.Clamp(intersects[i - 1].FirstT + startOffset, -halfLength, halfLength);
var end = Mathf.Clamp(intersects[i].FirstT - endOffset, -halfLength, halfLength);
var delta = end - start;
if (delta < 0.9 * length && delta < 0.67 * width)
{
continue;
}
var startPosition = position + direction * start;
var endPosition = position + direction * end;
yield return(new MarkupStyleDash(startPosition, endPosition, direction, width, Color));
}
}
private bool FindIntersects(List <ITrajectory> A, List <ITrajectory> B, bool invert)
{
var x = !invert ? A.Count - 1 : 0;
var xPart = new StraightTrajectory(A[x]);
for (var y = !invert ? 1 : B.Count - 2; !invert ? y < B.Count : y >= 0; y += !invert ? 1 : -1)
{
var yPart = new StraightTrajectory(B[y]);
var intersect = MarkupIntersect.CalculateSingle(xPart, yPart);
if (intersect.IsIntersect)
{
if (!invert)
{
A[x] = xPart.Cut(0f, intersect.FirstT);
B[y] = yPart.Cut(intersect.SecondT, 1f);
B.RemoveRange(0, y);
}
else
{
A[x] = xPart.Cut(intersect.FirstT, 1f);
B[y] = yPart.Cut(0f, intersect.SecondT);
B.RemoveRange(y + 1, B.Count - (y + 1));
}
return(true);
}
}
return(false);
}
public static bool CalculateSolidPart(LineBorders borders, ITrajectory trajectory, float offset, float width, Color32 color, out MarkupStylePart part)
{
part = CalculateSolidPart(trajectory, offset, width, color);
if (borders.IsEmpty)
{
return(true);
}
var vertex = borders.GetVertex(part);
var from = 0f;
var to = 1f;
foreach (var border in borders)
{
for (var i = 0; i < vertex.Length; i += 2)
{
var start = Intersection.CalculateSingle(border, vertex[i]);
var end = Intersection.CalculateSingle(border, vertex[i + 1]);
if (start.IsIntersect && end.IsIntersect)
{
return(false);
}
if (!start.IsIntersect && !end.IsIntersect)
{
continue;
}
var intersect = Intersection.CalculateSingle(border, new StraightTrajectory(vertex[i].EndPosition, vertex[i + 1].EndPosition));
if (intersect.IsIntersect)
{
if (start.IsIntersect)
{
from = Mathf.Max(from, intersect.SecondT);
}
else if (end.IsIntersect)
{
to = Mathf.Min(to, intersect.SecondT);
}
}
}
}
if (from != 0f || to != 1f)
{
var dir = part.Angle.Direction();
var line = new StraightTrajectory(part.Position + dir * (part.Length / 2), part.Position - dir * (part.Length / 2)).Cut(from, to);
part = new MarkupStylePart(line.StartPosition, line.EndPosition, line.Direction, part.Width, part.Color);
}
return(true);
}
private static void Connect(List <FillerContour.Part> parts, List <FillerContour.Part> originalParts, float offset, float medianOffset)
{
var count = 0;
for (var i = 0; i < parts.Count; i += 1)
{
var j = (i + 1) % parts.Count;
var first = parts[i];
var second = parts[j];
if ((first.IsEnter ? medianOffset : offset) != 0)
{
if ((second.IsEnter ? medianOffset : offset) != 0)
{
if ((first.Trajectory.EndPosition - second.Trajectory.StartPosition).sqrMagnitude > 0.0001f)
{
var nextCount = (count + 1) % originalParts.Count;
var firstTrajectory = new StraightTrajectory(first.Trajectory.EndPosition, originalParts[count].Trajectory.EndPosition);
var secondTrajectory = new StraightTrajectory(originalParts[nextCount].Trajectory.StartPosition, second.Trajectory.StartPosition);
AddToList(parts, i + 1, new FillerContour.Part(firstTrajectory));
AddToList(parts, i + 2, new FillerContour.Part(secondTrajectory));
i += 2;
}
}
else if (Intersection.CalculateSingle(first.Trajectory, second.Trajectory, out var firstT, out var secondT))
{
if (first.Trajectory.Length * (1f - firstT) < 0.1f || second.Trajectory.Length * secondT < 0.1f)
{
first = new FillerContour.Part(first.Trajectory.Cut(0f, firstT), first.IsEnter);
parts[i] = first;
second = new FillerContour.Part(second.Trajectory.Cut(secondT, 1f), second.IsEnter);
parts[j] = second;
}
else
{
Add(parts, ref i, first.Trajectory.EndPosition, second.Trajectory.StartPosition);
}
}
else if (Intersection.CalculateSingle(new StraightTrajectory(first.Trajectory.EndPosition, first.Trajectory.EndPosition - first.Trajectory.EndDirection), second.Trajectory, out _, out var t))
{
second = new FillerContour.Part(second.Trajectory.Cut(t, 1f), second.IsEnter);
parts[j] = second;
Add(parts, ref i, first.Trajectory.EndPosition, second.Trajectory.StartPosition);
}
else
{
Add(parts, ref i, first.Trajectory.EndPosition, second.Trajectory.StartPosition);
}
}
public ITrajectory[] SetMedianOffset(MarkupFiller filler)
{
var lineParts = filler.Contour.Parts.ToArray();
var trajectories = filler.Contour.TrajectoriesRaw.ToArray();
for (var i = 0; i < lineParts.Length; i += 1)
{
if (trajectories[i] == null)
{
continue;
}
var line = lineParts[i].Line;
if (line is MarkupEnterLine)
{
continue;
}
var prevI = i == 0 ? lineParts.Length - 1 : i - 1;
if (lineParts[prevI].Line is MarkupEnterLine && trajectories[prevI] != null)
{
trajectories[i] = Shift(trajectories[i]);
trajectories[prevI] = new StraightTrajectory(trajectories[prevI].StartPosition, trajectories[i].StartPosition);
}
var nextI = i + 1 == lineParts.Length ? 0 : i + 1;
if (lineParts[nextI].Line is MarkupEnterLine && trajectories[nextI] != null)
{
trajectories[i] = Shift(trajectories[i].Invert()).Invert();
trajectories[nextI] = new StraightTrajectory(trajectories[i].EndPosition, trajectories[nextI].EndPosition);
}
ITrajectory Shift(ITrajectory trajectory)
{
var newT = trajectory.Travel(0, MedianOffset);
return(trajectory.Cut(newT, 1));
}
}
return(trajectories.Where(t => t != null).Select(t => t).ToArray());
}
private void CalculatePosition(NetSegment segment)
{
if (DriveLanes.FirstOrDefault() is DriveLane driveLane)
{
var position = driveLane.NetLane.CalculatePosition(T);
var coef = Mathf.Sin(CornerAndNormalAngle);
RoadHalfWidth = segment.Info.m_halfWidth - segment.Info.m_pavementWidth;
RoadHalfWidthTransform = RoadHalfWidth / coef;
Position = position + (IsLaneInvert ? -CornerDir : CornerDir) * driveLane.Position / coef;
FirstPointSide = Position.Value - RoadHalfWidthTransform * CornerDir;
LastPointSide = Position.Value + RoadHalfWidthTransform * CornerDir;
Line = new StraightTrajectory(FirstPointSide, LastPointSide);
}
else
{
Position = null;
}
}
private static IEnumerable <PartT> CalculateDashesStraightT(StraightTrajectory straightTrajectory, float dashLength, float spaceLength)
{
var length = straightTrajectory.Length;
var partCount = (int)(length / (dashLength + spaceLength));
var startSpace = (length + spaceLength - (dashLength + spaceLength) * partCount) / 2;
var startT = startSpace / length;
var partT = dashLength / length;
var spaceT = spaceLength / length;
for (var i = 0; i < partCount; i += 1)
{
var tStart = startT + (partT + spaceT) * i;
var tEnd = tStart + partT;
yield return(new PartT {
Start = tStart, End = tEnd
});
}
}
public void Update()
{
var segment = GetSegment();
var segmentId = GetSegmentId();
Vector3 leftPos;
Vector3 rightPos;
Vector3 leftDir;
Vector3 rightDir;
if (IsStartSide)
{
segment.CalculateCorner(segmentId, true, true, true, out leftPos, out leftDir, out _);
segment.CalculateCorner(segmentId, true, true, false, out rightPos, out rightDir, out _);
}
else
{
segment.CalculateCorner(segmentId, true, false, true, out leftPos, out leftDir, out _);
segment.CalculateCorner(segmentId, true, false, false, out rightPos, out rightDir, out _);
}
CornerDir = (rightPos - leftPos).normalized;
CornerAngle = CornerDir.AbsoluteAngle();
NormalDir = NormalSign * (leftDir + rightDir).normalized;
NormalAngle = NormalDir.AbsoluteAngle();
var angle = Vector3.Angle(NormalDir, CornerDir);
CornerAndNormalAngle = (angle > 90 ? 180 - angle : angle) * Mathf.Deg2Rad;
TranformCoef = Mathf.Sin(CornerAndNormalAngle);
Position = (leftPos + rightPos) / 2f;
RoadHalfWidth = segment.Info.m_halfWidth - segment.Info.m_pavementWidth;
FirstPointSide = GetPosition(-RoadHalfWidth);
LastPointSide = GetPosition(RoadHalfWidth);
Line = new StraightTrajectory(FirstPointSide, LastPointSide);
}
