LGES-2170-ACW_CCW-Welder/AuroraVision/VisionAligner/NativeAligner.cs

using Aurora.Api;
using VisionCommon;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Runtime.InteropServices;
using System.Text;
using System.Threading.Tasks;

namespace VisionAligner
{
    [StructLayout(LayoutKind.Sequential)]
    internal struct AlignData
    {
        public double X;
        public double Y;
        public double T;
    };

    [Serializable]
    [StructLayout(LayoutKind.Sequential)]
    internal struct PointData
    {
        public double X;
        public double Y;
    }

    [Serializable]
    [StructLayout(LayoutKind.Sequential)]
    internal struct PointsData
    {
        public int Count;
        public IntPtr Points;
    }

    internal static class NativeAligner
    {
        private const string FILE_NAME_VISION_DLL = "VisionCore.dll";
        private static Logger _Logger = new Logger("NativeAligner");

        [DllImport(FILE_NAME_VISION_DLL)]
        extern private static AlignData AlignT(ref PointsData sourcePoints, ref PointsData targetPoint);

        [DllImport(FILE_NAME_VISION_DLL)]
        extern private static AlignData AlignT_ICP(ref PointsData sourcePoints, ref PointsData targetPoint);

        [DllImport(FILE_NAME_VISION_DLL)]
        extern private static AlignData AlignXY_MultiPoints(ref PointsData sourcePoints, ref PointsData targetPoint);

        [DllImport(FILE_NAME_VISION_DLL)]
        extern private static AlignData AlignXY_SinglePoint(ref PointData sourcePoint, ref PointData targetPoint);

        [DllImport(FILE_NAME_VISION_DLL)]
        extern private static AlignData HeadAlignXY(
            double angleRadian, ref PointData sourcePoints, ref PointData targetPoint);

        /// <summary>
        /// Source(이동)를 Target(고정)에 맞추는 얼라인 T
        /// </summary>
        /// <param name="srcPoints">이동 포인트 리스트</param>
        /// <param name="tgtPoints">고정 포인트 리스트</param>
        /// <returns>보정량 T</returns>
        public static AlignDelta AlignT(
            List<DoublePoint> srcPoints,
            List<DoublePoint> tgtPoints)
        {
            var sPoints = ConvertPointsData(srcPoints);
            var tPoint = ConvertPointsData(tgtPoints);
            AlignData alignDataT = NativeAligner.AlignT(ref sPoints, ref tPoint);

            return new AlignDelta(
                        new StaticPoint(0, 0),
                        Angle.CreateWithRadian(alignDataT.T));
        }

        public static AlignDelta AlignT(
            Angle theta)
        {
            return new AlignDelta(
                        new StaticPoint(0, 0), theta);
        }

        /// <summary>
        /// ICP 알고리즘을 이용하여 Source(이동)를 Target(고정)에 맞추는 얼라인 T
        /// </summary>
        /// <param name="srcPoints">이동 포인트 리스트</param>
        /// <param name="tgtPoints">고정 포인트 리스트</param>
        /// <returns>보정량 T</returns>
        public static AlignDelta AlignT_ICP(
            List<DoublePoint> srcPoints,
            List<DoublePoint> tgtPoints) {
            var sPoints = ConvertPointsData(srcPoints);
            var tPoint = ConvertPointsData(tgtPoints);
            AlignData alignDataT = NativeAligner.AlignT_ICP(ref sPoints, ref tPoint);

            return new AlignDelta(
                        new StaticPoint(0, 0),
                        Angle.CreateWithRadian(alignDataT.T));
        }

        /// <summary>
        /// Source(이동)를 Target(고정)에 맞추는 얼라인 XY
        /// </summary>
        /// <param name="srcPoints">이동 포인트 리스트</param>
        /// <param name="tgtPoints">고정 포인트 리스트</param>
        /// <returns>보정량 XY</returns>
        public static AlignDelta AlignXY(
            List<DoublePoint> srcPoints,
            List<DoublePoint> tgtPoints)
        {
            var sPoints = ConvertPointsData(srcPoints);
            var tPoints = ConvertPointsData(tgtPoints);
            AlignData alignDataT = NativeAligner.AlignXY_MultiPoints(ref sPoints, ref tPoints);

            return new AlignDelta(
                        new StaticPoint(alignDataT.X, alignDataT.Y),
                        Angle.CreateWithRadian(0));
        }

        /// <summary>
        /// Source(이동)를 Target(고정)에 맞추는 얼라인 XY
        /// </summary>
        /// <param name="srcPoint">이동 포인트</param>
        /// <param name="tgtPoint">고정 포인트</param>
        /// <returns>보정량 XY</returns>
        public static AlignDelta AlignXY(
            DoublePoint srcPoint,
            DoublePoint tgtPoint) {
            var sPoint = ConvertPointData(srcPoint);
            var tPoint = ConvertPointData(tgtPoint);
            AlignData alignDataT = NativeAligner.AlignXY_SinglePoint(ref sPoint, ref tPoint);

            return new AlignDelta(
                        new StaticPoint(alignDataT.X, alignDataT.Y),
                        Angle.CreateWithRadian(0));
        }

        public static AlignDelta HeadAlignXY(
            Angle angle,
            DoublePoint glassMarkPosition,
            DoublePoint glassMarkOffset)
        {
            var glassMarkPointData = ConvertPointData(glassMarkPosition);
            var glassMarkOffsetPointData = ConvertPointData(glassMarkOffset);
            AlignData alignData = HeadAlignXY(
                angle.Radian, ref glassMarkPointData, ref glassMarkOffsetPointData);

            return new AlignDelta(
                new StaticPoint(alignData.X, alignData.Y),
                Angle.CreateWithRadian(0));
        }

        private static PointsData ConvertPointsData(List<DoublePoint> points)
        {
            var pointsData = new PointsData();
            pointsData.Count = points.Count;
            pointsData.Points = Marshal.AllocHGlobal(Marshal.SizeOf<IntPtr>() * pointsData.Count);

            try
            {
                for (int i = 0; i < pointsData.Count; i++)
                {
                    var pointData = new PointData()
                    {
                        X = points[i].X,
                        Y = points[i].Y
                    };

                    // PointData Marshaling
                    IntPtr ptr = Marshal.AllocHGlobal(Marshal.SizeOf(pointData));
                    Marshal.StructureToPtr(pointData, ptr, false);

                    // Copy PointData to Array
                    Marshal.StructureToPtr(ptr, (IntPtr)((long)pointsData.Points + Marshal.SizeOf<IntPtr>() * i), false);
                }
            }
            catch (Exception e)
            {
                _Logger.FuncE(e.Message);
            }

            return pointsData;
        }

        private static PointData ConvertPointData(DoublePoint point)
        {
            var pointData = new PointData()
            {
                X = point.X,
                Y = point.Y
            };

            return pointData;
        }
    }
}