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); }