Display_D_HP/TeamAAS-VM/Core/PalletTool.cs

using MathNet.Numerics.LinearAlgebra;
using NPOI.SS.Formula.Functions;
using OpenCvSharp;
using OpenCvSharp.Flann;
using System;
using System.Collections.Generic;
using System.Data.Common;
using System.Data.Entity.Core.Common.CommandTrees.ExpressionBuilder;
using System.IO;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Controls;
using TeamAAS_VP.Models;
using TeamAAS_VP.Models.PLC;
using TeamAAS_VP.Models.Robot;
using Unity;

namespace TeamAAS_VP.Core
{
    public class PalletTool
    {
        private List<List<RPoint>> rPoints=new List<List<RPoint>>();
        private List<List<PlcPoint>> plcPoints = new List<List<PlcPoint>>();
        public RPoint[] Points { get; set; }
        public PlcPoint[] PlcPoints { get; set; }
        Pallet pallet;
        PalletEx _pallet1;
        public PalletTool(Pallet pallet1) 
        {
            pallet= pallet1;

            //----------------------------------------计算旋转矩阵-----------------------------------------------------------------
            Vector<double> p1 = Vector<double>.Build.Dense(new double[] { pallet.P0.X, pallet.P0.Y });
            Vector<double> p2 = Vector<double>.Build.Dense(new double[] { pallet.P1.X, pallet.P1.Y });
            Vector<double> p3 = Vector<double>.Build.Dense(new double[] { pallet.P2.X, pallet.P2.Y });
            //计算正交时的X与Y轴的单位向量
            Vector<double> E1 = (p2 - p1) / (p2 - p1).L2Norm();
            Vector<double> E2 = (p3 - p1 - E1.DotProduct(p3 - p1) * E1) / (p3 - p1 - E1.DotProduct(p3 - p1) * E1).L2Norm();
            //得到旋转矩阵
            Matrix<double> R= Matrix<double>.Build.DenseOfColumnVectors(E1, E2);

            //--------------------------------------计算切变矩阵--------------------------------------------------------
            double a = (E1.DotProduct(p3 - p1) * E1).L2Norm() / (p3 - p1-(E1.DotProduct(p3 - p1) * E1))[1];
            Matrix<double> Shear = Matrix<double>.Build.DenseOfArray(new double[,] { {1,a },{0,1 } });

            //--------------------------------------计算托盘行列间距---------------------------------------------------------
            double distanceX = (p2 - p1).L2Norm() / (pallet.Column-1);
            double distanceY = (p3 - p1 - E1.DotProduct(p3 - p1) * E1).L2Norm() / (pallet.Row-1);

            //------------------------------------生成切变前的行列坐标集合-------------------------------------------------------
            List<double> cols = new List<double>();
            for (int i = 0; i < pallet.Column; i++)
            {
                cols.Add(distanceX*i);
            }
            List<double> rows = new List<double>();
            for (int i = 0; i < pallet.Row; i++)
            {
                rows.Add(distanceY * i);
            }
            //-------------------------------------计算转换后的机器人绝对坐标系下的所有托盘点---------------------------------------
            rPoints.Clear();
            for (int i = 0;i<rows.Count; i++)
            {
                rPoints.Add(new List<RPoint>());
                for (int j = 0;j<cols.Count; j++)
                {
                    //切变前的坐标
                    Vector<double> q = Vector<double>.Build.Dense(new double[] { cols[j], rows[i] });
                    //切变后的坐标
                    Vector<double> h = Shear * q;
                    //转换后的机器人坐标
                    Vector<double> rp = R * h + p1;
                    //输出完整的机器人坐标
                    rPoints[rPoints.Count-1].Add(new RPoint() { X = (float)rp[0], Y = (float)rp[1], Z = pallet.P0.Z, U = pallet.P0.U, V = pallet.P0.V, W = pallet.P0.W, Hand = pallet.P0.Hand, Local = pallet.P0.Local, Tool = pallet.P0.Tool });
                }
            }
            //-----------------------------------------------排序---------------------------------------------------------------
            List<RPoint> newPoints= new List<RPoint>();
            if (pallet.Arrangement== Enums.PalletArrangement.Z)
            {
                foreach (var item in rPoints)
                {
                    foreach (var item1 in item)
                    {
                        newPoints.Add(item1);
                    }
                }
                Points = newPoints.ToArray();
            }
            else
            {
                for (int i = 0; i < rows.Count; i++)
                {
                    if (i % 2 == 0) //偶数
                    {
                        for (int j = 0; j < cols.Count; j++)
                        {
                            newPoints.Add(rPoints[i][j]);
                        }
                    }
                    else
                    {
                        for (int j = cols.Count - 1; j > -1; j--)
                        {
                            newPoints.Add(rPoints[i][j]);
                        }
                    }
                }
                Points = newPoints.ToArray();
            }

            //组合托盘
            if (pallet.Extend!=null)
            {
                //-------------------------------------计算转换后的用户坐标系中的机器人点位---------------------------------------
                rPoints.Clear();
                for (int i = 0; i < rows.Count; i++)
                {
                    rPoints.Add(new List<RPoint>());
                    for (int j = 0; j < cols.Count; j++)
                    {
                        //切变前的坐标
                        Vector<double> q = Vector<double>.Build.Dense(new double[] { cols[j], rows[i] });
                        //切变后的坐标
                        Vector<double> h = Shear * q;
                        //未转换前的托盘点
                        rPoints[rPoints.Count - 1].Add(new RPoint() { X = (float)h[0], Y = (float)h[1], Z = pallet.P0.Z, U = pallet.P0.U, V = pallet.P0.V, W = pallet.P0.W, Hand = pallet.P0.Hand, Local = pallet.P0.Local, Tool = pallet.P0.Tool });
                    }
                }

                //-----------------------------------------------排序---------------------------------------------------------------
                newPoints.Clear();
                if (pallet.Arrangement == Enums.PalletArrangement.Z)
                {
                    foreach (var item in rPoints)
                    {
                        foreach (var item1 in item)
                        {
                            newPoints.Add(item1);
                        }
                    }
                }
                else
                {
                    for (int i = 0; i < rows.Count; i++)
                    {
                        if (i % 2 == 0) //偶数
                        {
                            for (int j = 0; j < cols.Count; j++)
                            {
                                newPoints.Add(rPoints[i][j]);
                            }
                        }
                        else
                        {
                            for (int j = cols.Count - 1; j > -1; j--)
                            {
                                newPoints.Add(rPoints[i][j]);
                            }
                        }
                    }
                }

                //一个大托盘中，包含每个托盘的点位（大托盘<一行托盘>-<一行托盘中所有列托盘>-<一个托盘中所有的点位>）
                List<List<List<RPoint>>> nPoints = new List<List<List<RPoint>>>();
                //遍历所有的行
                for (int i = 0;i < pallet.Extend.Pallet_Row; i++)
                {
                    nPoints.Add(new List<List<RPoint>>());
                    //遍历每行中所有列
                    for (int j = 0; j < pallet.Extend.Pallet_Column; j++)
                    {
                        nPoints[nPoints.Count-1].Add(new List<RPoint>());
                        double dx = pallet.Extend.Pallet_X_Interval * i;
                        double dy = pallet.Extend.Pallet_Y_Interval * j;

                        //如果是切变矩阵模式
                        if (pallet.Extend.CoordinateModel== Enums.CoordinateModel.Shear)
                        {
                            //切变前的坐标
                            Vector<double> q = Vector<double>.Build.Dense(new double[] { 0, dx });
                            //切变后的坐标
                            Vector<double> h = Shear * q;
                            dy += h[0];
                            dx = h[1];
                        }

                        foreach (var item in newPoints)
                        {
                            //偏移的向量
                            Vector<double> q = Vector<double>.Build.Dense(new double[] { dy, dx });
                            Vector<double> q1 = Vector<double>.Build.Dense(new double[] { item.X, item.Y });
                            Vector<double> rp = R * (q + q1) + p1;
                            nPoints[nPoints.Count - 1][nPoints[nPoints.Count - 1].Count-1].Add(new RPoint() { X = (float)rp[0], Y = (float)rp[1], Z = item.Z, U = item.U, V = item.V, W = item.W, Hand = item.Hand, Local = item.Local, Tool = item.Tool });
                        }
                    }
                }
                newPoints = new List<RPoint>();
                //对组合的所有托盘排序
                if (pallet.Arrangement == Enums.PalletArrangement.Z)
                {
                    foreach (var item in nPoints)
                    {
                        foreach (var item1 in item)
                        {
                            foreach (var item2 in item1)
                            {
                                newPoints.Add(item2);
                            }
                        }
                    }
                    Points = newPoints.ToArray();
                }
                else
                {
                    for (int i = 0; i < nPoints.Count; i++)
                    {
                        if (i % 2 == 0) //偶数
                        {
                            for (int j = 0; j < nPoints[i].Count; j++)
                            {
                                foreach (var item2 in nPoints[i][j])
                                {
                                    newPoints.Add(item2);
                                }
                            }
                        }
                        else
                        {
                            for (int j = nPoints[i].Count - 1; j > -1; j--)
                            {
                                foreach (var item2 in nPoints[i][j])
                                {
                                    newPoints.Add(item2);
                                }
                            }
                        }
                    }
                    Points = newPoints.ToArray();
                }
            }
        }

        public PalletTool(PalletEx pallet1)
        {
            _pallet1 = pallet1;

            //----------------------------------------计算旋转矩阵-----------------------------------------------------------------
            Vector<double> p1 = Vector<double>.Build.Dense(new double[] { _pallet1.P0.X_Position, _pallet1.P0.Y_Position });
            Vector<double> p2 = Vector<double>.Build.Dense(new double[] { _pallet1.P1.X_Position, _pallet1.P1.Y_Position });
            Vector<double> p3 = Vector<double>.Build.Dense(new double[] { _pallet1.P2.X_Position, _pallet1.P2.Y_Position });
            //计算正交时的X与Y轴的单位向量
            Vector<double> E1 = (p2 - p1) / (p2 - p1).L2Norm();
            Vector<double> E2 = (p3 - p1 - E1.DotProduct(p3 - p1) * E1) / (p3 - p1 - E1.DotProduct(p3 - p1) * E1).L2Norm();
            //得到旋转矩阵
            Matrix<double> R = Matrix<double>.Build.DenseOfColumnVectors(E1, E2);

            //--------------------------------------计算切变矩阵--------------------------------------------------------
            double a = (E1.DotProduct(p3 - p1) * E1).L2Norm() / (p3 - p1 - (E1.DotProduct(p3 - p1) * E1))[1];
            Matrix<double> Shear = Matrix<double>.Build.DenseOfArray(new double[,] { { 1, a }, { 0, 1 } });

            //--------------------------------------计算托盘行列间距---------------------------------------------------------
            double distanceX = (p2 - p1).L2Norm() / (_pallet1.Column - 1);
            double distanceY = (p3 - p1 - E1.DotProduct(p3 - p1) * E1).L2Norm() / (_pallet1.Row - 1);

            //------------------------------------生成切变前的行列坐标集合-------------------------------------------------------
            List<double> cols = new List<double>();
            for (int i = 0; i < _pallet1.Column; i++)
            {
                cols.Add(distanceX * i);
            }
            List<double> rows = new List<double>();
            for (int i = 0; i < _pallet1.Row; i++)
            {
                rows.Add(distanceY * i);
            }
            //-------------------------------------计算转换后的机器人绝对坐标系下的所有托盘点---------------------------------------
            plcPoints.Clear();
            for (int i = 0; i < rows.Count; i++)
            {
                plcPoints.Add(new List<PlcPoint>());
                for (int j = 0; j < cols.Count; j++)
                {
                    //切变前的坐标
                    Vector<double> q = Vector<double>.Build.Dense(new double[] { cols[j], rows[i] });
                    //切变后的坐标
                    Vector<double> h = Shear * q;
                    //转换后的机器人坐标
                    Vector<double> rp = R * h + p1;
                    //输出完整的机器人坐标
                    plcPoints[plcPoints.Count - 1].Add(new PlcPoint() { X_Position = (float)rp[0], Y_Position = (float)rp[1], Z_Position_Start = _pallet1.P0.Z_Position_Start,Z_Position_Stop= _pallet1.P0.Z_Position_Stop, U_Position = _pallet1.P0.U_Position});
                }
            }
            //-----------------------------------------------排序---------------------------------------------------------------
            List<PlcPoint> newPoints = new List<PlcPoint>();
            if (_pallet1.Arrangement == Enums.PalletArrangement.Z)
            {
                foreach (var item in plcPoints)
                {
                    foreach (var item1 in item)
                    {
                        newPoints.Add(item1);
                    }
                }
                PlcPoints = newPoints.ToArray();
            }
            else
            {
                for (int i = 0; i < rows.Count; i++)
                {
                    if (i % 2 == 0) //偶数
                    {
                        for (int j = 0; j < cols.Count; j++)
                        {
                            newPoints.Add(plcPoints[i][j]);
                        }
                    }
                    else
                    {
                        for (int j = cols.Count - 1; j > -1; j--)
                        {
                            newPoints.Add(plcPoints[i][j]);
                        }
                    }
                }
                PlcPoints = newPoints.ToArray();
            }

            //组合托盘
            if (_pallet1.Extend != null)
            {
                //-------------------------------------计算转换后的用户坐标系中的机器人点位---------------------------------------
                plcPoints.Clear();
                for (int i = 0; i < rows.Count; i++)
                {
                    plcPoints.Add(new List<PlcPoint>());
                    for (int j = 0; j < cols.Count; j++)
                    {
                        //切变前的坐标
                        Vector<double> q = Vector<double>.Build.Dense(new double[] { cols[j], rows[i] });
                        //切变后的坐标
                        Vector<double> h = Shear * q;
                        //未转换前的托盘点
                        plcPoints[plcPoints.Count - 1].Add(new PlcPoint() { X_Position = (float)h[0], Y_Position = (float)h[1], Z_Position_Start = _pallet1.P0.Z_Position_Start,Z_Position_Stop= pallet1.P0.Z_Position_Stop, U_Position = _pallet1.P0.U_Position});
                    }
                }

                //-----------------------------------------------排序---------------------------------------------------------------
                newPoints.Clear();
                if (_pallet1.Arrangement == Enums.PalletArrangement.Z)
                {
                    foreach (var item in plcPoints)
                    {
                        foreach (var item1 in item)
                        {
                            newPoints.Add(item1);
                        }
                    }
                }
                else
                {
                    for (int i = 0; i < rows.Count; i++)
                    {
                        if (i % 2 == 0) //偶数
                        {
                            for (int j = 0; j < cols.Count; j++)
                            {
                                newPoints.Add(plcPoints[i][j]);
                            }
                        }
                        else
                        {
                            for (int j = cols.Count - 1; j > -1; j--)
                            {
                                newPoints.Add(plcPoints[i][j]);
                            }
                        }
                    }
                }

                //一个大托盘中，包含每个托盘的点位（大托盘<一行托盘>-<一行托盘中所有列托盘>-<一个托盘中所有的点位>）
                List<List<List<PlcPoint>>> nPoints = new List<List<List<PlcPoint>>>();
                //遍历所有的行
                for (int i = 0; i < _pallet1.Extend.Pallet_Row; i++)
                {
                    nPoints.Add(new List<List<PlcPoint>>());
                    //遍历每行中所有列
                    for (int j = 0; j < _pallet1.Extend.Pallet_Column; j++)
                    {
                        nPoints[nPoints.Count - 1].Add(new List<PlcPoint>());
                        double dx = _pallet1.Extend.Pallet_X_Interval * i;
                        double dy = _pallet1.Extend.Pallet_Y_Interval * j;

                        //如果是切变矩阵模式
                        if (_pallet1.Extend.CoordinateModel == Enums.CoordinateModel.Shear)
                        {
                            //切变前的坐标
                            Vector<double> q = Vector<double>.Build.Dense(new double[] { 0, dx });
                            //切变后的坐标
                            Vector<double> h = Shear * q;
                            dy += h[0];
                            dx = h[1];
                        }

                        foreach (var item in newPoints)
                        {
                            //偏移的向量
                            Vector<double> q = Vector<double>.Build.Dense(new double[] { dy, dx });
                            Vector<double> q1 = Vector<double>.Build.Dense(new double[] { item.X_Position, item.Y_Position });
                            Vector<double> rp = R * (q + q1) + p1;
                            nPoints[nPoints.Count - 1][nPoints[nPoints.Count - 1].Count - 1].Add(new PlcPoint() { X_Position = (float)rp[0], Y_Position = (float)rp[1], Z_Position_Start = item.Z_Position_Start,Z_Position_Stop=item.Z_Position_Stop, U_Position = item.U_Position});
                        }
                    }
                }
                newPoints = new List<PlcPoint>();
                //对组合的所有托盘排序
                if (_pallet1.Arrangement == Enums.PalletArrangement.Z)
                {
                    foreach (var item in nPoints)
                    {
                        foreach (var item1 in item)
                        {
                            foreach (var item2 in item1)
                            {
                                newPoints.Add(item2);
                            }
                        }
                    }
                    PlcPoints = newPoints.ToArray();
                }
                else
                {
                    for (int i = 0; i < nPoints.Count; i++)
                    {
                        if (i % 2 == 0) //偶数
                        {
                            for (int j = 0; j < nPoints[i].Count; j++)
                            {
                                foreach (var item2 in nPoints[i][j])
                                {
                                    newPoints.Add(item2);
                                }
                            }
                        }
                        else
                        {
                            for (int j = nPoints[i].Count - 1; j > -1; j--)
                            {
                                foreach (var item2 in nPoints[i][j])
                                {
                                    newPoints.Add(item2);
                                }
                            }
                        }
                    }
                    PlcPoints = newPoints.ToArray();
                }
            }
        }

        /// <summary>
        /// 绘制
        /// </summary>
        /// <param name="filePath"></param>
        public void DrawCoordinatesAndSaveImage(string filePath)
        {
            string flo = Path.GetExtension(filePath);
            string pa = filePath.Replace(flo,".csv");
            StreamWriter writer = new StreamWriter(pa);
            foreach (var item in Points)
            {
                writer.WriteLine($"{item.X},{item.Y}");
            }
            writer.Close();
            writer.Dispose();
            int width = 800;
            int height = 600;
            //X方向 点位 最小值
            double minx= Points.Min(p => p.X);
            //X方向 点位 最大值
            double maxx = Points.Max(p => p.X);
            //Y方向 点位 最小值
            double miny = Points.Max(p => p.Y);
            //Y方向 点位 最小值
            double maxy = Points.Max(p => p.Y);

            double xoffset = minx <0? Math.Abs(minx)+10 :10;
            double yoffset = miny < 0 ? Math.Abs(miny)+10 : 10;
            double xZoom = minx < 0? width / (maxx - minx): width / maxx;
            double yZoom = miny < 0 ? height / (maxy - miny) : height / maxy;

            // 创建一个空白图像

            Mat image = new Mat(height+50, width+50,MatType.CV_8UC3); // 3通道（BGR），8位深度
            image.SetTo(new Scalar(255, 255, 255)); // 设置为白色背景

            // 绘制十字叉
            foreach (var point in Points)
            {
                OpenCvSharp.Point point1=new Point(point.X* xZoom+ xoffset, point.Y * yZoom + yoffset);
                Cv2.DrawMarker(image, point1, Scalar.Blue,MarkerTypes.Cross,20,2); // 红色十字叉
            }


            // 定义坐标系的原点
            Point origin = new Point(xoffset, yoffset);

            // 画 X 轴
            Cv2.Line(image, new Point(0, origin.Y), new Point(width, origin.Y), Scalar.Green, 2);
            Cv2.PutText(image, "X", new Point(width - 20, origin.Y + 10), HersheyFonts.HersheySimplex, 1, Scalar.Blue, 2);

            // 画 Y 轴
            Cv2.Line(image, new Point(origin.X, 0), new Point(origin.X, height), Scalar.Red, 2);
            Cv2.PutText(image, "Y", new Point(origin.X + 10, height-20), HersheyFonts.HersheySimplex, 1, Scalar.Blue, 2);

            Cv2.Flip(image, image,FlipMode.X);
            // 保存图像
            Cv2.ImWrite(filePath, image);
            image.Dispose();
        }
    }
}