Extensions.cs

using AGVSystem.Models.Map;
using AGVSystemCommonNet6.MAP;
using System.Drawing;

namespace AGVSystem
{
    /// <summary>
    /// 擴充功能
    /// </summary>
    public static class Extensions
    {

        private static List<MapPoint> mapPoints => AGVSMapManager.CurrentMap?.Points.Values.ToList();

        /// <summary>
        /// 取得在同一工作站內另一個地圖點位
        /// </summary>
        /// <param name="point"></param>
        /// <returns></returns>
        public static MapPoint GetAnotherPointOfWorkStation(this MapPoint point)
        {
            if (point.StationType == MapPoint.STATION_TYPE.Normal)
                return point;

            List<int> indexesOfEntryPoints = point.Target.Keys.ToList();

            if (indexesOfEntryPoints.Count == 0)
                return point;

            var pointCandicates = mapPoints.Where(pt => pt.TagNumber != point.TagNumber)
                                           .Where(pt => pt.StationType != MapPoint.STATION_TYPE.Normal)
                                           .Where(pt => pt.Target.Keys.Any(_index => indexesOfEntryPoints.Contains(_index)));
            MapPoint entryPointPt = indexesOfEntryPoints.First().GetMapPointByIndex();
            double forwardAngle = entryPointPt.CalculateForwardAngle(point);
            //取出同向者
            MapPoint anotherPoint = pointCandicates.FirstOrDefault(pt => Math.Abs(entryPointPt.CalculateForwardAngle(pt) - forwardAngle) <= 5);

            return anotherPoint;
        }

        public static MapPoint GetMapPointByIndex(this int ptIndex)
        {
            var pointPair = AGVSMapManager.CurrentMap.Points.FirstOrDefault(pair => pair.Key == ptIndex);
            return pointPair.Value;
        }
        public static MapPoint GetMapPointByTagNumber(this int tagNumber)
        {
            var point = AGVSMapManager.CurrentMap.Points.Values.FirstOrDefault(pair => pair.TagNumber == tagNumber);
            return point;
        }


        public static double CalculateForwardAngle(this MapPoint fromPt, MapPoint toPt)
        {
            return CalculationForwardAngle(new PointF((float)fromPt.X, (float)fromPt.Y), new PointF((float)toPt.X, (float)toPt.Y));
        }

        /// <summary>
        /// 計算兩點之間的前進角度
        /// </summary>
        /// <param name="startPt"></param>
        /// <param name="endPt"></param>
        /// <returns></returns>
        public static double CalculationForwardAngle(PointF startPt, PointF endPt)
        {

            double deltaX = endPt.X - startPt.X;
            double deltaY = endPt.Y - startPt.Y;
            double angleInRadians = Math.Atan2(deltaY, deltaX);
            double angleInDegrees = angleInRadians * (180 / Math.PI);
            // 將角度調整到 -180 至 180 度的範圍
            if (angleInDegrees > 180)
            {
                angleInDegrees -= 360;
            }
            return angleInDegrees;
        }

    }
}