112 lines
4.1 KiB
C#
112 lines
4.1 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.IO;
|
|
using System.Security.Cryptography;
|
|
using System.Text;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;
|
|
|
|
/// <summary>为重复执行结果生成与进程无关的稳定几何 SHA-256 摘要。</summary>
|
|
public static class StableGeometryDigest
|
|
{
|
|
/// <summary>计算正式平滑结果的状态、方法、分段和路径位模式摘要。</summary>
|
|
public static string Compute(PathSmoothingResult result)
|
|
{
|
|
if (result == null) throw new ArgumentNullException(nameof(result));
|
|
return Compute(result.Status, result.Method, result.Path, result.Segments);
|
|
}
|
|
|
|
/// <summary>计算任意已分析路径的稳定摘要。</summary>
|
|
public static string Compute(
|
|
PathSmoothingStatus status,
|
|
SmoothingMethod? method,
|
|
IReadOnlyList<SmoothedPathPoint> path,
|
|
IReadOnlyList<SmoothedPathSegment> segments)
|
|
{
|
|
using (var stream = new MemoryStream())
|
|
{
|
|
WriteInt32(stream, 1);
|
|
WriteInt32(stream, (int)status);
|
|
WriteBoolean(stream, method.HasValue);
|
|
if (method.HasValue) WriteInt32(stream, (int)method.Value);
|
|
|
|
WriteInt32(stream, path == null ? 0 : path.Count);
|
|
if (path != null)
|
|
{
|
|
for (int index = 0; index < path.Count; index++) WritePoint(stream, path[index]);
|
|
}
|
|
|
|
WriteInt32(stream, segments == null ? 0 : segments.Count);
|
|
if (segments != null)
|
|
{
|
|
for (int index = 0; index < segments.Count; index++) WriteSegment(stream, segments[index]);
|
|
}
|
|
|
|
using (SHA256 sha256 = SHA256.Create())
|
|
{
|
|
byte[] hash = sha256.ComputeHash(stream.ToArray());
|
|
var builder = new StringBuilder(hash.Length * 2);
|
|
for (int index = 0; index < hash.Length; index++) builder.Append(hash[index].ToString("x2"));
|
|
return builder.ToString();
|
|
}
|
|
}
|
|
}
|
|
|
|
private static void WritePoint(Stream stream, SmoothedPathPoint point)
|
|
{
|
|
if (point == null) throw new ArgumentException("稳定路径摘要不能包含空点。", nameof(point));
|
|
WriteDouble(stream, point.X);
|
|
WriteDouble(stream, point.Y);
|
|
WriteDouble(stream, point.Heading);
|
|
WriteDouble(stream, point.UnwrappedHeading);
|
|
WriteDouble(stream, point.ArcLength);
|
|
WriteInt32(stream, (int)point.Direction);
|
|
WriteDouble(stream, point.GeometricCurvature);
|
|
WriteDouble(stream, point.VehicleCurvature);
|
|
WriteDouble(stream, point.BodyClearance);
|
|
WriteBoolean(stream, point.IsGearSwitchPoint);
|
|
WriteInt32(stream, (int)point.Source);
|
|
}
|
|
|
|
private static void WriteSegment(Stream stream, SmoothedPathSegment segment)
|
|
{
|
|
if (segment == null) throw new ArgumentException("稳定路径摘要不能包含空方向段。", nameof(segment));
|
|
WriteInt32(stream, segment.SegmentIndex);
|
|
WriteInt32(stream, (int)segment.Direction);
|
|
WriteInt32(stream, segment.StartIndex);
|
|
WriteInt32(stream, segment.EndIndex);
|
|
WriteBoolean(stream, segment.StartsAtGearSwitch);
|
|
WriteBoolean(stream, segment.EndsAtGearSwitch);
|
|
}
|
|
|
|
private static void WriteDouble(Stream stream, double value)
|
|
{
|
|
WriteInt64(stream, BitConverter.DoubleToInt64Bits(value));
|
|
}
|
|
|
|
private static void WriteBoolean(Stream stream, bool value)
|
|
{
|
|
stream.WriteByte(value ? (byte)1 : (byte)0);
|
|
}
|
|
|
|
private static void WriteInt32(Stream stream, int value)
|
|
{
|
|
unchecked
|
|
{
|
|
stream.WriteByte((byte)value);
|
|
stream.WriteByte((byte)(value >> 8));
|
|
stream.WriteByte((byte)(value >> 16));
|
|
stream.WriteByte((byte)(value >> 24));
|
|
}
|
|
}
|
|
|
|
private static void WriteInt64(Stream stream, long value)
|
|
{
|
|
unchecked
|
|
{
|
|
ulong bits = (ulong)value;
|
|
for (int index = 0; index < 8; index++) stream.WriteByte((byte)(bits >> (index * 8)));
|
|
}
|
|
}
|
|
}
|