新增1.0和2.0两种协议车型车型
This commit is contained in:
ykkokluo
@@ -0,0 +1,226 @@
|
||||
using SimpleCore;
|
||||
using SimpleCore.PropType;
|
||||
using StandardScene.Magnetic.Protocol;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
|
||||
namespace StandardScene.Magnetic.Tasking
|
||||
{
|
||||
/// <summary>
|
||||
/// 将站点路径与站点/边 fields 合并为 FASS 2.0 <c>0xB1</c> 节点序列。
|
||||
/// 对齐 backend <c>CarRequestService.GetSendNodes</c> 的组包规则。
|
||||
/// </summary>
|
||||
public static class Fass2TaskBuilder
|
||||
{
|
||||
public const byte StartStopPass = 1;
|
||||
public const byte StartStopStop = 2;
|
||||
public const byte StartStopPrecision = 22;
|
||||
|
||||
public static Fass2TaskPlan BuildPath(int startSiteId, int goalSiteId, Fass2TaskBuildOptions options,
|
||||
Func<int, ushort> resolveNodeId)
|
||||
{
|
||||
if (resolveNodeId == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(resolveNodeId));
|
||||
}
|
||||
|
||||
options ??= new Fass2TaskBuildOptions();
|
||||
var siteIds = Fass2PathFinder.GetSitesBetween(startSiteId, goalSiteId);
|
||||
if (siteIds.Count == 0)
|
||||
{
|
||||
throw new InvalidOperationException($"no path from site {startSiteId} to {goalSiteId}");
|
||||
}
|
||||
|
||||
var nodes = BuildNodesForSites(siteIds, options, resolveNodeId);
|
||||
return new Fass2TaskPlan
|
||||
{
|
||||
StartSiteId = startSiteId,
|
||||
GoalSiteId = goalSiteId,
|
||||
SiteIds = siteIds,
|
||||
Nodes = nodes,
|
||||
Batches = SplitBatches(nodes, options.MaxNodesPerFrame),
|
||||
FieldsSignature = Fass2SiteFieldReader.BuildFieldsSignature(siteIds, 0, SimpleLib.GetSite)
|
||||
};
|
||||
}
|
||||
|
||||
public static Fass2NodeMessage[] BuildSegment(int srcSiteId, int dstSiteId, Fass2TaskBuildOptions options,
|
||||
Func<int, ushort> resolveNodeId)
|
||||
{
|
||||
return BuildPath(srcSiteId, dstSiteId, options, resolveNodeId).Nodes.ToArray();
|
||||
}
|
||||
|
||||
public static IReadOnlyList<Fass2NodeMessage[]> SplitBatches(IReadOnlyList<Fass2NodeMessage> nodes, int maxPerFrame = 10)
|
||||
{
|
||||
if (nodes == null || nodes.Count == 0)
|
||||
{
|
||||
return Array.Empty<Fass2NodeMessage[]>();
|
||||
}
|
||||
|
||||
if (maxPerFrame <= 0 || maxPerFrame > 10)
|
||||
{
|
||||
maxPerFrame = 10;
|
||||
}
|
||||
|
||||
var batches = new List<Fass2NodeMessage[]>();
|
||||
for (var offset = 0; offset < nodes.Count; offset += maxPerFrame)
|
||||
{
|
||||
var count = Math.Min(maxPerFrame, nodes.Count - offset);
|
||||
var batch = new Fass2NodeMessage[count];
|
||||
for (var i = 0; i < count; i++)
|
||||
{
|
||||
batch[i] = nodes[offset + i];
|
||||
}
|
||||
|
||||
batches.Add(batch);
|
||||
}
|
||||
|
||||
return batches;
|
||||
}
|
||||
|
||||
private static List<Fass2NodeMessage> BuildNodesForSites(IReadOnlyList<int> siteIds, Fass2TaskBuildOptions options,
|
||||
Func<int, ushort> resolveNodeId)
|
||||
{
|
||||
var nodes = new List<Fass2NodeMessage>(siteIds.Count);
|
||||
for (var i = 0; i < siteIds.Count; i++)
|
||||
{
|
||||
var siteId = siteIds[i];
|
||||
var site = SimpleLib.GetSite(siteId);
|
||||
var siteAction = Fass2SiteFieldReader.Read(site);
|
||||
var isLast = i == siteIds.Count - 1;
|
||||
var prevSiteId = i > 0 ? siteIds[i - 1] : siteId;
|
||||
|
||||
Fass2TrackMotionData trackMotion = null;
|
||||
Site prevSite = null;
|
||||
if (i > 0)
|
||||
{
|
||||
prevSite = SimpleLib.GetSite(prevSiteId);
|
||||
var track = FindTrack(prevSiteId, siteId);
|
||||
trackMotion = Fass2TrackFieldReader.Read(track);
|
||||
}
|
||||
|
||||
var node = new Fass2NodeMessage
|
||||
{
|
||||
Node = resolveNodeId(siteId),
|
||||
StartStop = ResolveStartStop(siteAction, isLast),
|
||||
Distance = i > 0 ? ComputeEdgeDistance(prevSite, site) : (ushort)0,
|
||||
Speed = SpeedToProtocol(trackMotion?.Speed ?? options.DefaultSpeed, options.CarSpeed),
|
||||
Orientation = ResolveOrientation(prevSite, site, trackMotion),
|
||||
Byroad = siteAction.Byroad ?? trackMotion?.Byroad ?? 0,
|
||||
Direction = siteAction.Direction ?? trackMotion?.Direction ?? 0,
|
||||
Lift = siteAction.Lift ?? 0,
|
||||
Roll = siteAction.Roll ?? 0,
|
||||
Charge = siteAction.Charge ?? 0,
|
||||
Obstacle = siteAction.Obstacle ?? 0,
|
||||
Audio = siteAction.Audio ?? 0,
|
||||
Light = siteAction.Light ?? 0,
|
||||
Rest = siteAction.Rest ?? 0,
|
||||
Clamp = siteAction.Clamp ?? 0,
|
||||
Tray = siteAction.Tray ?? 0,
|
||||
Shutdown = siteAction.Shutdown ?? 0
|
||||
};
|
||||
|
||||
if (siteAction.Orientation.HasValue)
|
||||
{
|
||||
node.Orientation = siteAction.Orientation.Value;
|
||||
}
|
||||
|
||||
nodes.Add(node);
|
||||
}
|
||||
|
||||
return nodes;
|
||||
}
|
||||
|
||||
private static byte ResolveStartStop(Fass2SiteActionData siteAction, bool isLast)
|
||||
{
|
||||
if (siteAction.StartStop.HasValue)
|
||||
{
|
||||
return siteAction.StartStop.Value;
|
||||
}
|
||||
|
||||
if (isLast)
|
||||
{
|
||||
return siteAction.PrecisionStop ? StartStopPrecision : StartStopStop;
|
||||
}
|
||||
|
||||
return StartStopPass;
|
||||
}
|
||||
|
||||
private static byte ResolveOrientation(Site src, Site dst, Fass2TrackMotionData trackMotion)
|
||||
{
|
||||
if (trackMotion?.Orientation != null)
|
||||
{
|
||||
return trackMotion.Orientation.Value;
|
||||
}
|
||||
|
||||
if (src == null || dst == null)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
var angle = trackMotion != null && trackMotion.EnableCarAbsoluteDirection
|
||||
? trackMotion.CarAbsoluteDirection
|
||||
: Math.Atan2(dst.y - src.y, dst.x - src.x) / Math.PI * 180.0;
|
||||
|
||||
if (trackMotion?.Reverse == true)
|
||||
{
|
||||
angle += 180;
|
||||
}
|
||||
|
||||
angle += trackMotion?.CarDirectionBias ?? 0;
|
||||
angle %= 360;
|
||||
if (angle < 0)
|
||||
{
|
||||
angle += 360;
|
||||
}
|
||||
|
||||
return (byte)((int)Math.Round(angle / 360.0 * 256) % 256);
|
||||
}
|
||||
|
||||
private static ushort ComputeEdgeDistance(Site src, Site dst)
|
||||
{
|
||||
var dx = dst.x - src.x;
|
||||
var dy = dst.y - src.y;
|
||||
var length = Math.Sqrt(dx * dx + dy * dy);
|
||||
return (ushort)Math.Min(ushort.MaxValue, Math.Max(1, Math.Round(length)));
|
||||
}
|
||||
|
||||
private static ushort SpeedToProtocol(double trackSpeed, double carSpeed)
|
||||
{
|
||||
var value = trackSpeed <= 0 ? carSpeed : trackSpeed;
|
||||
var protocolSpeed = value <= 1
|
||||
? (int)Math.Round(value * 600)
|
||||
: (int)Math.Round(value * 10);
|
||||
return (ushort)Math.Max(1, Math.Min(10000, protocolSpeed));
|
||||
}
|
||||
|
||||
private static Track FindTrack(int fromSiteId, int toSiteId)
|
||||
{
|
||||
foreach (var track in SimpleLib.GetAllTracks())
|
||||
{
|
||||
if (track.direction == 0)
|
||||
{
|
||||
if ((track.siteA == fromSiteId && track.siteB == toSiteId) ||
|
||||
(track.siteB == fromSiteId && track.siteA == toSiteId))
|
||||
{
|
||||
return track;
|
||||
}
|
||||
|
||||
continue;
|
||||
}
|
||||
|
||||
if (track.direction == 1 && track.siteA == fromSiteId && track.siteB == toSiteId)
|
||||
{
|
||||
return track;
|
||||
}
|
||||
|
||||
if (track.direction == 2 && track.siteB == fromSiteId && track.siteA == toSiteId)
|
||||
{
|
||||
return track;
|
||||
}
|
||||
}
|
||||
|
||||
return null;
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user