using System; using System.Drawing; using System.Drawing.Drawing2D; using System.Drawing.Imaging; using System.Globalization; using System.IO; using System.Runtime.InteropServices; using MultiWheelC.TrajectoryPlanning.Mapping; namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Visualization; /// 仅 Windows 运行时使用 GDI+ 将每张共享图形定义渲染为 600 dpi PNG;轨迹仅绘制离散点。 public sealed class SmoothingPngRenderer { public const int WidthPixels = 4296; public const int HeightPixels = 3120; public const uint PixelsPerMeter = 23622u; private const float PixelsPerPoint = 600f / 72f; public byte[] Render(SmoothingFigureModel model, SmoothingFontResolution fonts) { if (model == null) throw new ArgumentNullException(nameof(model)); return Render(new SmoothingFigureSetBuilder().Build(model).Figures[1], fonts); } public byte[] Render(SmoothingFigureDefinition figure, SmoothingFontResolution fonts) { if (figure == null) throw new ArgumentNullException(nameof(figure)); if (fonts == null) throw new ArgumentNullException(nameof(fonts)); using (var bitmap = new Bitmap(WidthPixels, HeightPixels, PixelFormat.Format32bppArgb)) { bitmap.SetResolution(600f, 600f); using (Graphics graphics = Graphics.FromImage(bitmap)) { graphics.Clear(Color.White); graphics.SmoothingMode = SmoothingMode.AntiAlias; graphics.InterpolationMode = InterpolationMode.HighQualityBicubic; graphics.PixelOffsetMode = PixelOffsetMode.HighQuality; DrawFigure(graphics, figure, fonts); } return Encode(bitmap); } } private static void DrawFigure(Graphics graphics, SmoothingFigureDefinition figure, SmoothingFontResolution fonts) { DrawMixedText(graphics, fonts, figure.Title + " — " + figure.Model.ScenarioLabel, PointX(figure.PlotXPoints), PointY(12d), 12f, Color.Black); if (figure.IsCurvatureFigure) DrawCurvatureAxes(graphics, figure, fonts); else DrawOverheadAxes(graphics, figure, fonts); GraphicsState state = graphics.Save(); graphics.SetClip(new RectangleF(PointX(figure.PlotXPoints), PointY(figure.PlotYPoints), PointX(figure.PlotWidthPoints), PointY(figure.PlotHeightPoints))); if (!figure.IsCurvatureFigure && figure.ShowsMapContext) DrawObstacles(graphics, figure); if (figure.IsCurvatureFigure) DrawCurvaturePoints(graphics, figure); else DrawPathPoints(graphics, figure); if (!figure.IsCurvatureFigure && figure.ShowsMapContext) DrawStartGoal(graphics, figure); graphics.Restore(state); DrawLegend(graphics, figure, fonts); } private static void DrawOverheadAxes(Graphics graphics, SmoothingFigureDefinition figure, SmoothingFontResolution fonts) { DrawPlotFrame(graphics, figure); using (var grid = new Pen(Color.FromArgb(230, 230, 230), 0.5f * PixelsPerPoint)) { for (int index = 0; index < figure.XTicks.Count; index++) { float x = WorldX(figure, figure.XTicks[index]); graphics.DrawLine(grid, x, PointY(figure.PlotYPoints), x, PointY(figure.PlotYPoints + figure.PlotHeightPoints)); DrawMixedText(graphics, fonts, Number(figure.XTicks[index]), x - 10f * PixelsPerPoint, PointY(figure.PlotYPoints + figure.PlotHeightPoints + 5d), 8f, Color.Black); } for (int index = 0; index < figure.YTicks.Count; index++) { float y = WorldY(figure, figure.YTicks[index]); graphics.DrawLine(grid, PointX(figure.PlotXPoints), y, PointX(figure.PlotXPoints + figure.PlotWidthPoints), y); DrawMixedText(graphics, fonts, Number(figure.YTicks[index]), PointX(figure.PlotXPoints - 34d), y - 5f * PixelsPerPoint, 8f, Color.Black); } } DrawMixedText(graphics, fonts, "X (m)", PointX(figure.PlotXPoints + figure.PlotWidthPoints / 2d - 11d), PointY(figure.PlotYPoints + figure.PlotHeightPoints + 20d), 10f, Color.Black); DrawVerticalText(graphics, fonts, "Y (m)", PointX(12d), PointY(figure.PlotYPoints + figure.PlotHeightPoints / 2d + 17d), 10f, Color.Black); } private static void DrawCurvatureAxes(Graphics graphics, SmoothingFigureDefinition figure, SmoothingFontResolution fonts) { DrawPlotFrame(graphics, figure); using (var grid = new Pen(Color.FromArgb(230, 230, 230), 0.5f * PixelsPerPoint)) { for (int index = 0; index < figure.CurvatureArcLengthTicks.Count; index++) { float x = CurvatureX(figure, figure.CurvatureArcLengthTicks[index]); graphics.DrawLine(grid, x, PointY(figure.PlotYPoints), x, PointY(figure.PlotYPoints + figure.PlotHeightPoints)); DrawMixedText(graphics, fonts, Number(figure.CurvatureArcLengthTicks[index]), x - 10f * PixelsPerPoint, PointY(figure.PlotYPoints + figure.PlotHeightPoints + 5d), 8f, Color.Black); } for (int index = 0; index < figure.CurvatureTicks.Count; index++) { float y = CurvatureY(figure, figure.CurvatureTicks[index]); graphics.DrawLine(grid, PointX(figure.PlotXPoints), y, PointX(figure.PlotXPoints + figure.PlotWidthPoints), y); DrawMixedText(graphics, fonts, Number(figure.CurvatureTicks[index]), PointX(figure.PlotXPoints - 34d), y - 5f * PixelsPerPoint, 8f, Color.Black); } } if (figure.CurvatureMinimumPerMeter < 0d && figure.CurvatureMaximumPerMeter > 0d) { float y = CurvatureY(figure, 0d); using (var zero = new Pen(Color.FromArgb(77, 77, 77), 0.65f * PixelsPerPoint)) graphics.DrawLine(zero, PointX(figure.PlotXPoints), y, PointX(figure.PlotXPoints + figure.PlotWidthPoints), y); } DrawMixedText(graphics, fonts, "s (m)", PointX(figure.PlotXPoints + figure.PlotWidthPoints / 2d - 10d), PointY(figure.PlotYPoints + figure.PlotHeightPoints + 20d), 10f, Color.Black); DrawVerticalText(graphics, fonts, "κ (m⁻¹)", PointX(12d), PointY(figure.PlotYPoints + figure.PlotHeightPoints / 2d + 22d), 10f, Color.Black); } private static void DrawPlotFrame(Graphics graphics, SmoothingFigureDefinition figure) { using (var border = new Pen(Color.Black, 0.75f * PixelsPerPoint)) graphics.DrawRectangle(border, PointX(figure.PlotXPoints), PointY(figure.PlotYPoints), PointX(figure.PlotWidthPoints), PointY(figure.PlotHeightPoints)); } private static void DrawObstacles(Graphics graphics, SmoothingFigureDefinition figure) { using (var fill = new SolidBrush(Color.FromArgb(217, 217, 217))) using (var outline = new Pen(Color.FromArgb(128, 128, 128), 0.35f * PixelsPerPoint)) { for (int index = 0; index < figure.Model.Obstacles.Count; index++) { SmoothingFigureObstacle obstacle = figure.Model.Obstacles[index]; float x = WorldX(figure, obstacle.X); float y = WorldY(figure, obstacle.Y + obstacle.Height); float width = (float)(obstacle.Width * figure.WorldScalePointsPerMeter * PixelsPerPoint); float height = (float)(obstacle.Height * figure.WorldScalePointsPerMeter * PixelsPerPoint); graphics.FillRectangle(fill, x, y, width, height); graphics.DrawRectangle(outline, x, y, width, height); } } } private static void DrawPathPoints(Graphics graphics, SmoothingFigureDefinition figure) { for (int viewIndex = 0; viewIndex < figure.Series.Count; viewIndex++) { SmoothingFigureSeriesView view = figure.Series[viewIndex]; DrawPoints(graphics, view, point => new PointF(WorldX(figure, point.X), WorldY(figure, point.Y))); DrawViolations(graphics, figure, view); } } private static void DrawCurvaturePoints(Graphics graphics, SmoothingFigureDefinition figure) { for (int viewIndex = 0; viewIndex < figure.Series.Count; viewIndex++) { SmoothingFigureSeriesView view = figure.Series[viewIndex]; DrawPoints(graphics, view, point => new PointF(CurvatureX(figure, point.ArcLength), CurvatureY(figure, point.VehicleCurvature))); } } private static void DrawPoints(Graphics graphics, SmoothingFigureSeriesView view, Func transform) { if (!view.Series.IsCurveVisible) return; using (var fill = new SolidBrush(WithOpacity(ColorFromHex(view.Series.Color), view.Opacity))) { float radius = 1.35f * PixelsPerPoint; for (int index = 0; index < view.Series.Points.Count; index++) { PointF point = transform(view.Series.Points[index]); graphics.FillEllipse(fill, point.X - radius, point.Y - radius, radius * 2f, radius * 2f); } } } private static void DrawViolations(Graphics graphics, SmoothingFigureDefinition figure, SmoothingFigureSeriesView view) { if (view.Series.ViolationMarkers.Count == 0) return; using (var marker = new Pen(ColorFromHex(view.Series.Color), 1.1f * PixelsPerPoint)) { float radius = 3f * PixelsPerPoint; for (int index = 0; index < view.Series.ViolationMarkers.Count; index++) { SmoothingFigurePoint point = view.Series.ViolationMarkers[index]; float x = WorldX(figure, point.X), y = WorldY(figure, point.Y); graphics.DrawLine(marker, x - radius, y - radius, x + radius, y + radius); graphics.DrawLine(marker, x - radius, y + radius, x + radius, y - radius); } } } private static void DrawStartGoal(Graphics graphics, SmoothingFigureDefinition figure) { float startX = WorldX(figure, figure.Model.Start.X), startY = WorldY(figure, figure.Model.Start.Y), radius = 3.2f * PixelsPerPoint; using (var startBrush = new SolidBrush(Color.FromArgb(240, 228, 66))) using (var border = new Pen(Color.Black, 0.8f * PixelsPerPoint)) using (var goalBrush = new SolidBrush(ColorFromHex(IeeeFigureStyle.LimitColor))) { graphics.FillEllipse(startBrush, startX - radius, startY - radius, radius * 2f, radius * 2f); graphics.DrawEllipse(border, startX - radius, startY - radius, radius * 2f, radius * 2f); float goalX = WorldX(figure, figure.Model.Goal.X), goalY = WorldY(figure, figure.Model.Goal.Y), diamond = 4f * PixelsPerPoint; PointF[] points = { new PointF(goalX, goalY - diamond), new PointF(goalX + diamond, goalY), new PointF(goalX, goalY + diamond), new PointF(goalX - diamond, goalY) }; graphics.FillPolygon(goalBrush, points); graphics.DrawPolygon(border, points); } } private static void DrawLegend(Graphics graphics, SmoothingFigureDefinition figure, SmoothingFontResolution fonts) { const double columnWidth = 198d; for (int index = 0; index < figure.LegendEntries.Count; index++) { SmoothingFigureLegendEntry entry = figure.LegendEntries[index]; int column = index % 2, row = index / 2; float x = PointX(figure.PlotXPoints + column * columnWidth); float y = PointY(figure.LegendYPoints + row * 15d - 3d); using (var fill = new SolidBrush(ColorFromHex(entry.Color))) graphics.FillEllipse(fill, x, y - 2.2f * PixelsPerPoint, 4.4f * PixelsPerPoint, 4.4f * PixelsPerPoint); DrawMixedText(graphics, fonts, entry.Label, x + 10f * PixelsPerPoint, y - 5f * PixelsPerPoint, 8.5f, Color.Black); } } private static void DrawVerticalText(Graphics graphics, SmoothingFontResolution fonts, string text, float x, float y, float points, Color color) { GraphicsState state = graphics.Save(); graphics.TranslateTransform(x, y); graphics.RotateTransform(-90f); DrawMixedText(graphics, fonts, text, 0f, 0f, points, color); graphics.Restore(state); } private static void DrawMixedText(Graphics graphics, SmoothingFontResolution fonts, string text, float x, float top, float points, Color color) { var runs = SmoothingFontResolver.SplitRuns(text); float emPixels = points * PixelsPerPoint, maxAscent = 0f; for (int index = 0; index < runs.Count; index++) { FontFamily family = runs[index].IsChinese ? fonts.ChineseFamily : fonts.LatinFamily; maxAscent = Math.Max(maxAscent, family.GetCellAscent(FontStyle.Regular) * emPixels / family.GetEmHeight(FontStyle.Regular)); } float baseline = top + maxAscent; using (var brush = new SolidBrush(color)) using (var format = (StringFormat)StringFormat.GenericTypographic.Clone()) { format.FormatFlags |= StringFormatFlags.MeasureTrailingSpaces; for (int index = 0; index < runs.Count; index++) { FontFamily family = runs[index].IsChinese ? fonts.ChineseFamily : fonts.LatinFamily; using (Font font = fonts.CreateFont(family, points)) { float runTop = baseline - family.GetCellAscent(FontStyle.Regular) * emPixels / family.GetEmHeight(FontStyle.Regular); graphics.DrawString(runs[index].Text, font, brush, x, runTop, format); x += graphics.MeasureString(runs[index].Text, font, PointF.Empty, format).Width; } } } } private static byte[] Encode(Bitmap bitmap) { Rectangle rectangle = new Rectangle(0, 0, bitmap.Width, bitmap.Height); BitmapData data = bitmap.LockBits(rectangle, ImageLockMode.ReadOnly, PixelFormat.Format32bppArgb); try { int sourceLength = checked(data.Stride * bitmap.Height); var source = new byte[sourceLength]; Marshal.Copy(data.Scan0, source, 0, source.Length); var rgba = new byte[checked(bitmap.Width * bitmap.Height * 4)]; for (int y = 0; y < bitmap.Height; y++) { int sourceRow = y * data.Stride, outputRow = y * bitmap.Width * 4; for (int x = 0; x < bitmap.Width; x++) { int sourceOffset = sourceRow + x * 4, outputOffset = outputRow + x * 4; rgba[outputOffset] = source[sourceOffset + 2]; rgba[outputOffset + 1] = source[sourceOffset + 1]; rgba[outputOffset + 2] = source[sourceOffset]; rgba[outputOffset + 3] = source[sourceOffset + 3]; } } using (var output = new MemoryStream()) { ValidatedPngWriter.Write(rgba, bitmap.Width, bitmap.Height, output, PixelsPerMeter); return output.ToArray(); } } finally { bitmap.UnlockBits(data); } } private static Color ColorFromHex(string value) { return ColorTranslator.FromHtml(value); } private static Color WithOpacity(Color color, double opacity) { return Color.FromArgb((int)Math.Round(255d * opacity), color.R, color.G, color.B); } private static float PointX(double points) { return (float)(points * PixelsPerPoint); } private static float PointY(double points) { return (float)(points * PixelsPerPoint); } private static float WorldX(SmoothingFigureDefinition figure, double x) { return PointX(figure.PlotXPoints + (x - figure.WorldXMinMeters) * figure.WorldScalePointsPerMeter); } private static float WorldY(SmoothingFigureDefinition figure, double y) { return PointY(figure.PlotYPoints + figure.PlotHeightPoints - (y - figure.WorldYMinMeters) * figure.WorldScalePointsPerMeter); } private static float CurvatureX(SmoothingFigureDefinition figure, double arcLength) { return PointX(figure.PlotXPoints + arcLength / figure.CurvatureArcLengthMaximumMeters * figure.PlotWidthPoints); } private static float CurvatureY(SmoothingFigureDefinition figure, double curvature) { return PointY(figure.PlotYPoints + figure.PlotHeightPoints - (curvature - figure.CurvatureMinimumPerMeter) / (figure.CurvatureMaximumPerMeter - figure.CurvatureMinimumPerMeter) * figure.PlotHeightPoints); } private static string Number(double value) { return value.ToString("0.###", CultureInfo.InvariantCulture); } }