init commit
This commit is contained in:
zhaowei.huang
@@ -0,0 +1,168 @@
|
||||
using CommonUsage.Mathematics;
|
||||
using LessokajiWeaverUtilities.Utilities;
|
||||
using SimpleCore;
|
||||
using SimpleCore.Compiler;
|
||||
using SimpleCore.Library;
|
||||
using SimpleCore.PropType;
|
||||
using SimpleLite.Props;
|
||||
using StandardScene.CarTypes;
|
||||
using System;
|
||||
using System.Linq;
|
||||
using System.Numerics;
|
||||
using Track = SimpleCore.PropType.Track;
|
||||
|
||||
namespace StandardScene.Magnetic.Coders
|
||||
{
|
||||
/// <summary>
|
||||
/// 唯一磁循迹器（会话1：Kiva 版与 MWL 版去重；NaiveMagGo 采用含 ${track.Speed} 版本，用户确认）。
|
||||
/// C2 物理迁出：原暂置 Kiva.cs（StandardScene.CarTypes），现归 scene.mag 平台插件。
|
||||
/// 触发条件：track.Magnet=true 且非倒车段；分叉/汇流路口按顺时针序计算 magSelect。
|
||||
/// </summary>
|
||||
public class MagneticTrackCoder : ITrackCoder
|
||||
{
|
||||
public bool Code(SegmentPlan plan, Track track, Site src, Site dst, int i)
|
||||
{
|
||||
Diagnosis.Post("enter code", "MagTrackCoder", true);
|
||||
if (!track.fields.TryGetValue("Magnet", out var mStr) || !bool.TryParse(mStr, out var mBool) ||
|
||||
!mBool) return false;
|
||||
|
||||
// do not use magnet navigation for reversing
|
||||
if (track.fields.TryGetValue("ReverseDst", out var reverseStr) && int.TryParse(reverseStr, out var rId) &&
|
||||
rId == dst.id) return false;
|
||||
|
||||
Diagnosis.Post($">>>> checking {track.id}", "MagTrackCoder", true);
|
||||
var magSelect = -1;
|
||||
// choose forking/converging path in clockwise order.
|
||||
// i.e. 0 is the left-most forking. -1 means no choosing
|
||||
|
||||
float CalculateTangent(Track myTrack, bool checkSrc, int segId)
|
||||
{
|
||||
var anchorSite = (Site)(checkSrc ? plan.segments[segId - 1] : plan.segments[segId + 1]);
|
||||
|
||||
if (myTrack is UICircularArcTrack arcTrack)
|
||||
{
|
||||
var angleStart = arcTrack.Arc.AngleStart;
|
||||
var direction = 1;
|
||||
if (Vector2.Distance(anchorSite.v2, arcTrack.Arc.PointEnd) <
|
||||
Vector2.Distance(anchorSite.v2, arcTrack.Arc.PointStart))
|
||||
{
|
||||
angleStart = arcTrack.Arc.AngleEnd;
|
||||
direction = -1;
|
||||
}
|
||||
|
||||
angleStart = CommonMath.RoundTh(angleStart);
|
||||
var myAngle = angleStart + direction * 5;
|
||||
var arcTh = CommonMath.RoundTh(myAngle + 90 * direction);
|
||||
Console.WriteLine($">>>> {checkSrc} id: {myTrack.id} th: {arcTh:0.0}");
|
||||
return arcTh;
|
||||
}
|
||||
|
||||
if (myTrack is UIBezierTrack bTrack)
|
||||
{
|
||||
var add180 = 0f;
|
||||
var controlPoints = bTrack.BezierCurve.ControlPoints;
|
||||
if (Vector2.Distance(anchorSite.v2, controlPoints[0]) >
|
||||
Vector2.Distance(anchorSite.v2, controlPoints.Last())) add180 = 180;
|
||||
var TangentPoint = bTrack.BezierCurve.QueryTangentPoint(anchorSite.v2);
|
||||
Console.WriteLine($">>>>Bezier {checkSrc} id: {myTrack.id} th: {TangentPoint.Angle:0.0} add:{add180} queryid:{anchorSite.id}");
|
||||
return TangentPoint.Angle + add180;
|
||||
}
|
||||
|
||||
if (myTrack is UINurbsTrack nTrack)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
// line track
|
||||
var myDst = SimpleLib.GetSite(myTrack.GetOther(anchorSite.id));
|
||||
var lineTh = (float)(Math.Atan2(myDst.y - anchorSite.y, myDst.x - anchorSite.x) / Math.PI * 180);
|
||||
Console.WriteLine($">>>> {checkSrc} id: {myTrack.id} th: {lineTh:0.0}");
|
||||
return lineTh;
|
||||
}
|
||||
|
||||
bool TestMagSelect(bool checkSrc)
|
||||
{
|
||||
var anchorSite = checkSrc ? src : dst;
|
||||
var myTh = CalculateTangent(track, checkSrc, i);
|
||||
var allTracks = anchorSite.relatedTracks.Select(ii => SimpleLib.GetTrack(ii)).ToList();
|
||||
if (allTracks.Count <= 2) return false;
|
||||
|
||||
var tracksAndThs = allTracks.Select(myTrack => (myTrack, CalculateTangent(myTrack, checkSrc, i))).ToList();
|
||||
var sameHalfTracks = tracksAndThs.Where(item => Math.Abs(LessMath.thDiff(item.Item2, myTh)) < 90)
|
||||
.ToList();
|
||||
if (sameHalfTracks.Count > 1)
|
||||
{
|
||||
sameHalfTracks = checkSrc
|
||||
? sameHalfTracks.OrderByDescending(item => LessMath.thDiff(item.Item2, myTh)).ToList()
|
||||
: sameHalfTracks.OrderBy(item => LessMath.thDiff(item.Item2, myTh)).ToList();
|
||||
Console.WriteLine(
|
||||
$">>>> {checkSrc} {track.id} ({string.Join(", ", sameHalfTracks.Select(tt => tt.myTrack.id))})");
|
||||
magSelect = sameHalfTracks.FindIndex(tt => tt.myTrack.id == track.id);
|
||||
return true;
|
||||
}
|
||||
|
||||
// check if adjacent to forking/converging path
|
||||
Track adjacentTrack;
|
||||
int adjacentSegId;
|
||||
if (checkSrc)
|
||||
{
|
||||
if (i <= 1) return false;
|
||||
adjacentTrack = (Track)plan.segments[i - 2];
|
||||
adjacentSegId = i - 2;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (i >= plan.segments.Count - 2) return false;
|
||||
adjacentTrack = (Track)plan.segments[i + 2];
|
||||
adjacentSegId = i + 2;
|
||||
}
|
||||
var otherHalfTracks = tracksAndThs.Where(item => Math.Abs(LessMath.thDiff(item.Item2, myTh)) >= 90)
|
||||
.ToList();
|
||||
var adjacentTh = CalculateTangent(adjacentTrack, !checkSrc, adjacentSegId);
|
||||
otherHalfTracks = checkSrc
|
||||
? otherHalfTracks.OrderBy(item => LessMath.thDiff(item.Item2, adjacentTh)).ToList()
|
||||
: otherHalfTracks.OrderByDescending(item => LessMath.thDiff(item.Item2, adjacentTh)).ToList();
|
||||
Console.WriteLine($">>>> use adjacentTrack {checkSrc} {adjacentTrack.id} ({string.Join(", ", otherHalfTracks.Select(tt => tt.myTrack.id))})");
|
||||
magSelect = otherHalfTracks.FindIndex(tt => tt.myTrack.id == adjacentTrack.id);
|
||||
return true;
|
||||
}
|
||||
|
||||
if (!TestMagSelect(true)) TestMagSelect(false);
|
||||
|
||||
Console.WriteLine($">>>> {track.id} select {magSelect}");
|
||||
var engine = ProgramCoderHelper.PrepareTrackEngine(plan, track, src, dst, i,
|
||||
carFields: typeof(BasicCarFields),
|
||||
siteFields: typeof(BasicSiteFields),
|
||||
trackFields: typeof(BasicTrackFields),
|
||||
planFields: typeof(BasicPlanFields));
|
||||
var tStr = "agv.MagGo(${src.x},${src.y},${src.id},${dst.x},${dst.y},${dst.id},${track.id}," +
|
||||
$"{magSelect}," +
|
||||
"${track.Speed},${track.Reverse || (track.ReverseDst == dst.id)}," +
|
||||
"${track.CarDirectionBias},${track.EnableCarAbsoluteDirection},${track.CarAbsoluteDirection});";
|
||||
|
||||
if (track.fields.TryGetValue("NaiveMagnet", out var naiveStr) && bool.TryParse(naiveStr, out var naive) &&
|
||||
naive)
|
||||
{
|
||||
var slowDown = i == plan.segments.Count - 2;
|
||||
if (slowDown && (!dst.fields.TryGetValue("TagValue", out var tagValStr) ||
|
||||
!int.TryParse(tagValStr, out _)))
|
||||
throw new Exception($"Destination site {dst.id} must have valid TagValue field!");
|
||||
Console.WriteLine($">>>> Naive magnet!");
|
||||
tStr = "agv.NaiveMagGo(${src.id},${dst.id},${track.id}," +
|
||||
$"{magSelect}," + "${track.Speed}," +
|
||||
$"{slowDown.ToString().ToLower()}," +
|
||||
"${dst.TagValue}," +
|
||||
"${src.x},${src.y},${dst.x},${dst.y},${track.typeInfo});";
|
||||
}
|
||||
|
||||
plan.codeArr[i] += (string)engine.ExecuteExpression($"`{tStr}`", default);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
public bool toBlock()
|
||||
{
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user