C#实现坐标系转换

已知坐标系的向量线段AB，旋转指定角度后平移到达坐标A'B'

获取旋转角度以及新的其他坐标转换。

新建窗体应用程序CoordinateTransDemo，将默认的Form1重命名为FormCoordinateTrans，窗体设计如图：

窗体设计代码如下：

部分类文件FormCoordinateTrans.Designer.cs

namespace CoordinateTransDemo
{
    partial class FormCoordinateTrans
    {
        /// <summary>
        /// 必需的设计器变量。
        /// </summary>
        private System.ComponentModel.IContainer components = null;

        /// <summary>
        /// 清理所有正在使用的资源。
        /// </summary>
        /// <param name="disposing">如果应释放托管资源，为 true；否则为 false。</param>
        protected override void Dispose(bool disposing)
        {
            if (disposing && (components != null))
            {
                components.Dispose();
            }
            base.Dispose(disposing);
        }

        #region Windows 窗体设计器生成的代码

        /// <summary>
        /// 设计器支持所需的方法 - 不要修改
        /// 使用代码编辑器修改此方法的内容。
        /// </summary>
        private void InitializeComponent()
        {
            System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle5 = new System.Windows.Forms.DataGridViewCellStyle();
            System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle6 = new System.Windows.Forms.DataGridViewCellStyle();
            this.rtxtMessage = new System.Windows.Forms.RichTextBox();
            this.groupBox2 = new System.Windows.Forms.GroupBox();
            this.txtCrd2Mark2_Y = new System.Windows.Forms.TextBox();
            this.label9 = new System.Windows.Forms.Label();
            this.label10 = new System.Windows.Forms.Label();
            this.txtCrd2Mark2_X = new System.Windows.Forms.TextBox();
            this.txtCrd2Mark1_Y = new System.Windows.Forms.TextBox();
            this.label11 = new System.Windows.Forms.Label();
            this.label12 = new System.Windows.Forms.Label();
            this.txtCrd2Mark1_X = new System.Windows.Forms.TextBox();
            this.groupBox1 = new System.Windows.Forms.GroupBox();
            this.txtCrd1Mark2_Y = new System.Windows.Forms.TextBox();
            this.label7 = new System.Windows.Forms.Label();
            this.label8 = new System.Windows.Forms.Label();
            this.txtCrd1Mark2_X = new System.Windows.Forms.TextBox();
            this.txtCrd1Mark1_Y = new System.Windows.Forms.TextBox();
            this.label6 = new System.Windows.Forms.Label();
            this.label5 = new System.Windows.Forms.Label();
            this.txtCrd1Mark1_X = new System.Windows.Forms.TextBox();
            this.dgvData = new System.Windows.Forms.DataGridView();
            this.btnTransform = new System.Windows.Forms.Button();
            this.dgvcPoleSequence = new System.Windows.Forms.DataGridViewTextBoxColumn();
            this.dgvcPoleX = new System.Windows.Forms.DataGridViewTextBoxColumn();
            this.dgvcPoleY = new System.Windows.Forms.DataGridViewTextBoxColumn();
            this.dgvcPoleAngle = new System.Windows.Forms.DataGridViewTextBoxColumn();
            this.dgvcTransformX = new System.Windows.Forms.DataGridViewTextBoxColumn();
            this.dgvcTransformY = new System.Windows.Forms.DataGridViewTextBoxColumn();
            this.groupBox2.SuspendLayout();
            this.groupBox1.SuspendLayout();
            ((System.ComponentModel.ISupportInitialize)(this.dgvData)).BeginInit();
            this.SuspendLayout();
            // 
            // rtxtMessage
            // 
            this.rtxtMessage.Location = new System.Drawing.Point(727, 12);
            this.rtxtMessage.Name = "rtxtMessage";
            this.rtxtMessage.Size = new System.Drawing.Size(491, 520);
            this.rtxtMessage.TabIndex = 22;
            this.rtxtMessage.Text = "";
            // 
            // groupBox2
            // 
            this.groupBox2.Controls.Add(this.txtCrd2Mark2_Y);
            this.groupBox2.Controls.Add(this.label9);
            this.groupBox2.Controls.Add(this.label10);
            this.groupBox2.Controls.Add(this.txtCrd2Mark2_X);
            this.groupBox2.Controls.Add(this.txtCrd2Mark1_Y);
            this.groupBox2.Controls.Add(this.label11);
            this.groupBox2.Controls.Add(this.label12);
            this.groupBox2.Controls.Add(this.txtCrd2Mark1_X);
            this.groupBox2.Location = new System.Drawing.Point(255, 4);
            this.groupBox2.Name = "groupBox2";
            this.groupBox2.Size = new System.Drawing.Size(217, 120);
            this.groupBox2.TabIndex = 21;
            this.groupBox2.TabStop = false;
            this.groupBox2.Text = "焊接Mark点坐标";
            // 
            // txtCrd2Mark2_Y
            // 
            this.txtCrd2Mark2_Y.Location = new System.Drawing.Point(108, 95);
            this.txtCrd2Mark2_Y.Name = "txtCrd2Mark2_Y";
            this.txtCrd2Mark2_Y.Size = new System.Drawing.Size(93, 21);
            this.txtCrd2Mark2_Y.TabIndex = 16;
            this.txtCrd2Mark2_Y.Text = "320.5";
            // 
            // label9
            // 
            this.label9.AutoSize = true;
            this.label9.Location = new System.Drawing.Point(10, 97);
            this.label9.Name = "label9";
            this.label9.Size = new System.Drawing.Size(89, 12);
            this.label9.TabIndex = 15;
            this.label9.Text = "焊接Mark2坐标Y";
            // 
            // label10
            // 
            this.label10.AutoSize = true;
            this.label10.Location = new System.Drawing.Point(10, 76);
            this.label10.Name = "label10";
            this.label10.Size = new System.Drawing.Size(89, 12);
            this.label10.TabIndex = 14;
            this.label10.Text = "焊接Mark2坐标X";
            // 
            // txtCrd2Mark2_X
            // 
            this.txtCrd2Mark2_X.Location = new System.Drawing.Point(108, 71);
            this.txtCrd2Mark2_X.Name = "txtCrd2Mark2_X";
            this.txtCrd2Mark2_X.Size = new System.Drawing.Size(93, 21);
            this.txtCrd2Mark2_X.TabIndex = 13;
            this.txtCrd2Mark2_X.Text = "120.5";
            // 
            // txtCrd2Mark1_Y
            // 
            this.txtCrd2Mark1_Y.Location = new System.Drawing.Point(108, 40);
            this.txtCrd2Mark1_Y.Name = "txtCrd2Mark1_Y";
            this.txtCrd2Mark1_Y.Size = new System.Drawing.Size(93, 21);
            this.txtCrd2Mark1_Y.TabIndex = 12;
            this.txtCrd2Mark1_Y.Text = "20.5";
            // 
            // label11
            // 
            this.label11.AutoSize = true;
            this.label11.Location = new System.Drawing.Point(10, 41);
            this.label11.Name = "label11";
            this.label11.Size = new System.Drawing.Size(89, 12);
            this.label11.TabIndex = 11;
            this.label11.Text = "焊接Mark1坐标Y";
            // 
            // label12
            // 
            this.label12.AutoSize = true;
            this.label12.Location = new System.Drawing.Point(10, 20);
            this.label12.Name = "label12";
            this.label12.Size = new System.Drawing.Size(89, 12);
            this.label12.TabIndex = 10;
            this.label12.Text = "焊接Mark1坐标X";
            // 
            // txtCrd2Mark1_X
            // 
            this.txtCrd2Mark1_X.Location = new System.Drawing.Point(108, 15);
            this.txtCrd2Mark1_X.Name = "txtCrd2Mark1_X";
            this.txtCrd2Mark1_X.Size = new System.Drawing.Size(93, 21);
            this.txtCrd2Mark1_X.TabIndex = 9;
            this.txtCrd2Mark1_X.Text = "20.5";
            // 
            // groupBox1
            // 
            this.groupBox1.Controls.Add(this.txtCrd1Mark2_Y);
            this.groupBox1.Controls.Add(this.label7);
            this.groupBox1.Controls.Add(this.label8);
            this.groupBox1.Controls.Add(this.txtCrd1Mark2_X);
            this.groupBox1.Controls.Add(this.txtCrd1Mark1_Y);
            this.groupBox1.Controls.Add(this.label6);
            this.groupBox1.Controls.Add(this.label5);
            this.groupBox1.Controls.Add(this.txtCrd1Mark1_X);
            this.groupBox1.Location = new System.Drawing.Point(9, 4);
            this.groupBox1.Name = "groupBox1";
            this.groupBox1.Size = new System.Drawing.Size(217, 120);
            this.groupBox1.TabIndex = 20;
            this.groupBox1.TabStop = false;
            this.groupBox1.Text = "拍照Mark点坐标";
            // 
            // txtCrd1Mark2_Y
            // 
            this.txtCrd1Mark2_Y.Location = new System.Drawing.Point(108, 95);
            this.txtCrd1Mark2_Y.Name = "txtCrd1Mark2_Y";
            this.txtCrd1Mark2_Y.Size = new System.Drawing.Size(93, 21);
            this.txtCrd1Mark2_Y.TabIndex = 16;
            this.txtCrd1Mark2_Y.Text = "110";
            // 
            // label7
            // 
            this.label7.AutoSize = true;
            this.label7.Location = new System.Drawing.Point(10, 97);
            this.label7.Name = "label7";
            this.label7.Size = new System.Drawing.Size(89, 12);
            this.label7.TabIndex = 15;
            this.label7.Text = "拍照Mark2坐标Y";
            // 
            // label8
            // 
            this.label8.AutoSize = true;
            this.label8.Location = new System.Drawing.Point(10, 76);
            this.label8.Name = "label8";
            this.label8.Size = new System.Drawing.Size(89, 12);
            this.label8.TabIndex = 14;
            this.label8.Text = "拍照Mark2坐标X";
            // 
            // txtCrd1Mark2_X
            // 
            this.txtCrd1Mark2_X.Location = new System.Drawing.Point(108, 71);
            this.txtCrd1Mark2_X.Name = "txtCrd1Mark2_X";
            this.txtCrd1Mark2_X.Size = new System.Drawing.Size(93, 21);
            this.txtCrd1Mark2_X.TabIndex = 13;
            this.txtCrd1Mark2_X.Text = "310";
            // 
            // txtCrd1Mark1_Y
            // 
            this.txtCrd1Mark1_Y.Location = new System.Drawing.Point(108, 40);
            this.txtCrd1Mark1_Y.Name = "txtCrd1Mark1_Y";
            this.txtCrd1Mark1_Y.Size = new System.Drawing.Size(93, 21);
            this.txtCrd1Mark1_Y.TabIndex = 12;
            this.txtCrd1Mark1_Y.Text = "10";
            // 
            // label6
            // 
            this.label6.AutoSize = true;
            this.label6.Location = new System.Drawing.Point(10, 41);
            this.label6.Name = "label6";
            this.label6.Size = new System.Drawing.Size(89, 12);
            this.label6.TabIndex = 11;
            this.label6.Text = "拍照Mark1坐标Y";
            // 
            // label5
            // 
            this.label5.AutoSize = true;
            this.label5.Location = new System.Drawing.Point(10, 20);
            this.label5.Name = "label5";
            this.label5.Size = new System.Drawing.Size(89, 12);
            this.label5.TabIndex = 10;
            this.label5.Text = "拍照Mark1坐标X";
            // 
            // txtCrd1Mark1_X
            // 
            this.txtCrd1Mark1_X.Location = new System.Drawing.Point(108, 15);
            this.txtCrd1Mark1_X.Name = "txtCrd1Mark1_X";
            this.txtCrd1Mark1_X.Size = new System.Drawing.Size(93, 21);
            this.txtCrd1Mark1_X.TabIndex = 9;
            this.txtCrd1Mark1_X.Text = "10";
            // 
            // dgvData
            // 
            this.dgvData.AllowUserToAddRows = false;
            this.dgvData.AllowUserToDeleteRows = false;
            this.dgvData.ColumnHeadersHeightSizeMode = System.Windows.Forms.DataGridViewColumnHeadersHeightSizeMode.AutoSize;
            this.dgvData.Columns.AddRange(new System.Windows.Forms.DataGridViewColumn[] {
            this.dgvcPoleSequence,
            this.dgvcPoleX,
            this.dgvcPoleY,
            this.dgvcPoleAngle,
            this.dgvcTransformX,
            this.dgvcTransformY});
            this.dgvData.Location = new System.Drawing.Point(5, 129);
            this.dgvData.Name = "dgvData";
            this.dgvData.ReadOnly = true;
            this.dgvData.RowHeadersWidth = 25;
            this.dgvData.RowTemplate.Height = 23;
            this.dgvData.Size = new System.Drawing.Size(717, 403);
            this.dgvData.TabIndex = 19;
            // 
            // btnTransform
            // 
            this.btnTransform.Font = new System.Drawing.Font("宋体", 16F);
            this.btnTransform.ForeColor = System.Drawing.Color.Blue;
            this.btnTransform.Location = new System.Drawing.Point(505, 12);
            this.btnTransform.Name = "btnTransform";
            this.btnTransform.Size = new System.Drawing.Size(127, 45);
            this.btnTransform.TabIndex = 23;
            this.btnTransform.Text = "坐标转换";
            this.btnTransform.UseVisualStyleBackColor = true;
            this.btnTransform.Click += new System.EventHandler(this.btnTransform_Click);
            // 
            // dgvcPoleSequence
            // 
            this.dgvcPoleSequence.HeaderText = "极柱序号";
            this.dgvcPoleSequence.Name = "dgvcPoleSequence";
            this.dgvcPoleSequence.ReadOnly = true;
            this.dgvcPoleSequence.Width = 80;
            // 
            // dgvcPoleX
            // 
            this.dgvcPoleX.HeaderText = "极柱X坐标";
            this.dgvcPoleX.Name = "dgvcPoleX";
            this.dgvcPoleX.ReadOnly = true;
            // 
            // dgvcPoleY
            // 
            this.dgvcPoleY.HeaderText = "极柱Y坐标";
            this.dgvcPoleY.Name = "dgvcPoleY";
            this.dgvcPoleY.ReadOnly = true;
            // 
            // dgvcPoleAngle
            // 
            this.dgvcPoleAngle.HeaderText = "极柱角度Z";
            this.dgvcPoleAngle.Name = "dgvcPoleAngle";
            this.dgvcPoleAngle.ReadOnly = true;
            // 
            // dgvcTransformX
            // 
            dataGridViewCellStyle5.ForeColor = System.Drawing.Color.Red;
            this.dgvcTransformX.DefaultCellStyle = dataGridViewCellStyle5;
            this.dgvcTransformX.HeaderText = "转换后X坐标";
            this.dgvcTransformX.Name = "dgvcTransformX";
            this.dgvcTransformX.ReadOnly = true;
            this.dgvcTransformX.Width = 130;
            // 
            // dgvcTransformY
            // 
            dataGridViewCellStyle6.ForeColor = System.Drawing.Color.Red;
            this.dgvcTransformY.DefaultCellStyle = dataGridViewCellStyle6;
            this.dgvcTransformY.HeaderText = "转换后Y坐标";
            this.dgvcTransformY.Name = "dgvcTransformY";
            this.dgvcTransformY.ReadOnly = true;
            this.dgvcTransformY.Width = 130;
            // 
            // FormCoordinateTrans
            // 
            this.AutoScaleDimensions = new System.Drawing.SizeF(6F, 12F);
            this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.Font;
            this.ClientSize = new System.Drawing.Size(1238, 544);
            this.Controls.Add(this.btnTransform);
            this.Controls.Add(this.rtxtMessage);
            this.Controls.Add(this.groupBox2);
            this.Controls.Add(this.groupBox1);
            this.Controls.Add(this.dgvData);
            this.Name = "FormCoordinateTrans";
            this.Text = "坐标转换-斯内科";
            this.Load += new System.EventHandler(this.FormCoordinateTrans_Load);
            this.groupBox2.ResumeLayout(false);
            this.groupBox2.PerformLayout();
            this.groupBox1.ResumeLayout(false);
            this.groupBox1.PerformLayout();
            ((System.ComponentModel.ISupportInitialize)(this.dgvData)).EndInit();
            this.ResumeLayout(false);

        }

        #endregion

        private System.Windows.Forms.RichTextBox rtxtMessage;
        private System.Windows.Forms.GroupBox groupBox2;
        private System.Windows.Forms.TextBox txtCrd2Mark2_Y;
        private System.Windows.Forms.Label label9;
        private System.Windows.Forms.Label label10;
        private System.Windows.Forms.TextBox txtCrd2Mark2_X;
        private System.Windows.Forms.TextBox txtCrd2Mark1_Y;
        private System.Windows.Forms.Label label11;
        private System.Windows.Forms.Label label12;
        private System.Windows.Forms.TextBox txtCrd2Mark1_X;
        private System.Windows.Forms.GroupBox groupBox1;
        private System.Windows.Forms.TextBox txtCrd1Mark2_Y;
        private System.Windows.Forms.Label label7;
        private System.Windows.Forms.Label label8;
        private System.Windows.Forms.TextBox txtCrd1Mark2_X;
        private System.Windows.Forms.TextBox txtCrd1Mark1_Y;
        private System.Windows.Forms.Label label6;
        private System.Windows.Forms.Label label5;
        private System.Windows.Forms.TextBox txtCrd1Mark1_X;
        private System.Windows.Forms.DataGridView dgvData;
        private System.Windows.Forms.Button btnTransform;
        private System.Windows.Forms.DataGridViewTextBoxColumn dgvcPoleSequence;
        private System.Windows.Forms.DataGridViewTextBoxColumn dgvcPoleX;
        private System.Windows.Forms.DataGridViewTextBoxColumn dgvcPoleY;
        private System.Windows.Forms.DataGridViewTextBoxColumn dgvcPoleAngle;
        private System.Windows.Forms.DataGridViewTextBoxColumn dgvcTransformX;
        private System.Windows.Forms.DataGridViewTextBoxColumn dgvcTransformY;
    }
}

新建坐标结构Coord

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace CoordinateTransDemo
{
    /// <summary>
    /// 表示三维坐标系的一个坐标(x,y,z)
    /// </summary>
    public struct Coord
    {
        /// <summary>
        /// X坐标，一般是左右方向的轴X
        /// </summary>
        public double X;
        /// <summary>
        /// Y坐标，一般是前后方向的轴Y
        /// </summary>
        public double Y;
        /// <summary>
        /// Z坐标，一般是垂直高度【上下】方向的轴Z，有时也用角度Angle表示
        /// </summary>
        public double Z;
        public Coord(double x = 0, double y = 0, double z = 0) 
        {
            X = x;
            Y = y;
            Z = z;
        }

        /// <summary>
        /// 判断两个坐标是否重合【是否是同一个点】
        /// </summary>
        /// <param name="coord1"></param>
        /// <param name="coord2"></param>
        /// <returns></returns>
        public static bool operator ==(Coord coord1, Coord coord2) 
        {
            return coord1.X == coord2.X && coord1.Y == coord2.Y && coord1.Z == coord2.Z;
        }

        /// <summary>
        /// 判断两个坐标是否不重合【不同的坐标点】
        /// </summary>
        /// <param name="coord1"></param>
        /// <param name="coord2"></param>
        /// <returns></returns>
        public static bool operator !=(Coord coord1, Coord coord2)
        {
            return coord1.X != coord2.X || coord1.Y != coord2.Y || coord1.Z != coord2.Z;
        }

        /// <summary>
        /// 打印坐标，返回坐标字符串
        /// </summary>
        /// <returns></returns>
        public override string ToString()
        {
            return $"({X},{Y},{Z})";
        }

        public override bool Equals(object obj)
        {
            return base.Equals(obj);
        }

        public override int GetHashCode()
        {
            return base.GetHashCode();
        }
    }
}

新建关键的坐标转换类CoordinateTransHelper

关键算法代码如下：

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace CoordinateTransDemo
{
    /// <summary>
    /// 坐标转换的算法
    /// 斯内科Snake
    /// </summary>
    public class CoordinateTransHelper
    {
        /// <summary>
        /// 坐标系1是标准坐标系
        /// 坐标系2相对于坐标系1MARK点距离差值的最大值
        /// </summary>
        public static double Max_MarkDistanceDIF = 1;
        /// <summary>
        /// 计算坐标系2相对于坐标系1的旋转角，结果是弧度
        /// </summary>
        public static double Crd2ToCrd1Angle = 0;

        /// <summary>
        /// 坐标转换方法【单个】
        /// </summary>
        /// <param name="Crd2Mark1">坐标系2的Mark1的点坐标</param>
        /// <param name="Crd2Mark2">坐标系2的Mark2的点坐标</param>
        /// <param name="Crd1Mark1">坐标系1的Mark1的点坐标</param>
        /// <param name="Crd1Mark2">坐标系1的Mark2的点坐标</param>
        /// <param name="InPoint">需要转换的初始坐标</param>
        /// <param name="OutPoint">转换后的结果坐标</param>
        /// <returns></returns>
        public static bool GetCalcCoordCoordinate(Coord Crd2Mark1, Coord Crd2Mark2, Coord Crd1Mark1, Coord Crd1Mark2, Coord InPoint, out Coord OutPoint, out string errorMsg)
        {
            errorMsg = string.Empty;
            OutPoint = new Coord();
            OutPoint.X = 0;
            OutPoint.Y = 0;
            try
            {                
                Crd2ToCrd1Angle = GetRotateAngle(Crd2Mark1, Crd2Mark2, Crd1Mark1, Crd1Mark2);
                //计算坐标系2相对于坐标系1的缩放系数
                double Crd2ToCrd1Zoom =
                     Math.Sqrt(Math.Pow((Crd2Mark1.X - Crd2Mark2.X), 2) + Math.Pow((Crd2Mark1.Y - Crd2Mark2.Y), 2))
                    /
                    Math.Sqrt(Math.Pow((Crd1Mark1.X - Crd1Mark2.X), 2) + Math.Pow((Crd1Mark1.Y - Crd1Mark2.Y), 2));

                //输出坐标计算
                OutPoint.X = ((InPoint.X - Crd1Mark1.X) * Math.Cos(Crd2ToCrd1Angle) - (InPoint.Y - Crd1Mark1.Y) * Math.Sin(Crd2ToCrd1Angle)) * Crd2ToCrd1Zoom + Crd2Mark1.X;
                OutPoint.Y = ((InPoint.X - Crd1Mark1.X) * Math.Sin(Crd2ToCrd1Angle) + (InPoint.Y - Crd1Mark1.Y) * Math.Cos(Crd2ToCrd1Angle)) * Crd2ToCrd1Zoom + Crd2Mark1.Y;

                return true;
            }
            catch (Exception ex)
            {
                errorMsg = "计算异常：" + ex.Message + ex.StackTrace;
                return false;
            }
        }

        /// <summary>
        /// 坐标转换方法【批量】
        /// </summary>
        /// <param name="Crd2Mark1">坐标系2的Mark1的点坐标</param>
        /// <param name="Crd2Mark2">坐标系2的Mark2的点坐标</param>
        /// <param name="Crd1Mark1">坐标系1的Mark1的点坐标</param>
        /// <param name="Crd1Mark2">坐标系1的Mark2的点坐标</param>
        /// <param name="InPoint">需要转换的初始坐标集合</param>
        /// <param name="OutPoint">转换后的结果坐标集合</param>
        public static bool GetCalcCoordCoordinate(Coord Crd2Mark1, Coord Crd2Mark2, Coord Crd1Mark1, Coord Crd1Mark2, Coord[] InPointArray, out Coord[] OutPointArray,out string errorMessage) 
        {
            errorMessage = string.Empty;
            OutPointArray = new Coord[0];
            if (InPointArray == null || InPointArray.Length == 0) 
            {
                errorMessage = "需要转化的源坐标数组不能为空";
                return false;
            }
            OutPointArray = new Coord[InPointArray.Length];
            Crd2ToCrd1Angle = GetRotateAngle(Crd2Mark1, Crd2Mark2, Crd1Mark1, Crd1Mark2);
            //计算坐标系2相对于坐标系1的缩放系数
            double Crd2ToCrd1Zoom =
                 Math.Sqrt(Math.Pow((Crd2Mark1.X - Crd2Mark2.X), 2) + Math.Pow((Crd2Mark1.Y - Crd2Mark2.Y), 2))
                /
                Math.Sqrt(Math.Pow((Crd1Mark1.X - Crd1Mark2.X), 2) + Math.Pow((Crd1Mark1.Y - Crd1Mark2.Y), 2));
            Coord OutPoint;
            for (int i = 0; i < OutPointArray.Length; i++)
            {
                Coord InPoint = InPointArray[i];
                OutPoint = new Coord();

                //输出坐标计算
                OutPoint.X = ((InPoint.X - Crd1Mark1.X) * Math.Cos(Crd2ToCrd1Angle) - (InPoint.Y - Crd1Mark1.Y) * Math.Sin(Crd2ToCrd1Angle)) * Crd2ToCrd1Zoom + Crd2Mark1.X;
                OutPoint.Y = ((InPoint.X - Crd1Mark1.X) * Math.Sin(Crd2ToCrd1Angle) + (InPoint.Y - Crd1Mark1.Y) * Math.Cos(Crd2ToCrd1Angle)) * Crd2ToCrd1Zoom + Crd2Mark1.Y;
                OutPointArray[i] = OutPoint;
            }
            return true;
        }

        /// <summary>
        /// 计算坐标系2相对于坐标系1的旋转角，结果是弧度
        /// </summary>
        /// <param name="Crd2Mark1"></param>
        /// <param name="Crd2Mark2"></param>
        /// <param name="Crd1Mark1"></param>
        /// <param name="Crd1Mark2"></param>
        /// <returns></returns>
        public static double GetRotateAngle(Coord Crd2Mark1, Coord Crd2Mark2, Coord Crd1Mark1, Coord Crd1Mark2) 
        {
            if (Crd1Mark1 == Crd1Mark2 || Crd2Mark1 == Crd2Mark2)
            {
                throw new Exception("请检查参数配置，Mark点不能重合");
            }
            //计算坐标系2相对于坐标系1MARK点距离差值
            double MarkDistanceDIF = 0;//线段的长度进行比较
            MarkDistanceDIF = Math.Sqrt(Math.Pow((Crd2Mark1.X - Crd2Mark2.X), 2) + Math.Pow((Crd2Mark1.Y - Crd2Mark2.Y), 2))
               - Math.Sqrt(Math.Pow((Crd1Mark1.X - Crd1Mark2.X), 2) + Math.Pow((Crd1Mark1.Y - Crd1Mark2.Y), 2));

            if (Math.Abs(MarkDistanceDIF) > Math.Abs(Max_MarkDistanceDIF))
            {
                throw new Exception($"坐标系2相对于坐标系1MARK点距离差值:{MarkDistanceDIF}，大于限制值：{Max_MarkDistanceDIF}");
            }
            //类似于将线段AB平移到原点开始变成线段OP，与 X轴正方向的夹角∠POX
            //线段向量OP共有8种可能：第一象限，第二象限，第三象限，第四象限，X+，Y+，X-，Y-
            double Crd1DertaX = Crd1Mark2.X - Crd1Mark1.X;
            double Crd1DertaY = Crd1Mark2.Y - Crd1Mark1.Y;
            double Crd2DertaX = Crd2Mark2.X - Crd2Mark1.X;
            double Crd2DertaY = Crd2Mark2.Y - Crd2Mark1.Y;

            double Crd1Angle = 0;
            //坐标系1的MARK点连成的直线指向第一象限
            if (Crd1DertaX > 0 && Crd1DertaY > 0)
            {
                Crd1Angle = Math.Abs(Math.Atan((Crd1Mark2.Y - Crd1Mark1.Y) / (Crd1Mark2.X - Crd1Mark1.X)));
            }
            else
            {
                
            }

            //坐标系1的MARK点连成的直线指向第二象限
            if (Crd1DertaX < 0 && Crd1DertaY > 0)
            {
                Crd1Angle = (180 * Math.PI / 180) - Math.Abs(Math.Atan((Crd1Mark2.Y - Crd1Mark1.Y) / (Crd1Mark2.X - Crd1Mark1.X)));
            }
            else
            {

            }

            //坐标系1的MARK点连成的直线指向第三象限
            if (Crd1DertaX < 0 && Crd1DertaY < 0)
            {
                Crd1Angle = (180 * Math.PI / 180) + Math.Abs(Math.Atan((Crd1Mark2.Y - Crd1Mark1.Y) / (Crd1Mark2.X - Crd1Mark1.X)));
            }
            else
            {

            }

            //坐标系1的MARK点连成的直线指向第四象限
            if (Crd1DertaX > 0 && Crd1DertaY < 0)
            {
                Crd1Angle = (360 * Math.PI / 180) - Math.Abs(Math.Atan((Crd1Mark2.Y - Crd1Mark1.Y) / (Crd1Mark2.X - Crd1Mark1.X)));
            }
            else
            {

            }
            
            double Crd2Angle = 0;
            //坐标系2的MARK点连成的直线指向第一象限
            if (Crd2DertaX > 0 && Crd2DertaY > 0)
            {
                Crd2Angle = Math.Abs(Math.Atan((Crd2Mark2.Y - Crd2Mark1.Y) / (Crd2Mark2.X - Crd2Mark1.X)));
            }
            else
            {

            }

            //坐标系2的MARK点连成的直线指向第二象限
            if (Crd2DertaX < 0 && Crd2DertaY > 0)
            {
                Crd2Angle = (180 * Math.PI / 180) - Math.Abs(Math.Atan((Crd2Mark2.Y - Crd2Mark1.Y) / (Crd2Mark2.X - Crd2Mark1.X)));
            }
            else
            {

            }

            //坐标系2的MARK点连成的直线指向第三象限
            if (Crd2DertaX < 0 && Crd2DertaY < 0)
            {
                Crd2Angle = (180 * Math.PI / 180) + Math.Abs(Math.Atan((Crd2Mark2.Y - Crd2Mark1.Y) / (Crd2Mark2.X - Crd2Mark1.X)));
            }
            else
            {

            }

            //坐标系2的MARK点连成的直线指向第四象限
            if (Crd2DertaX > 0 && Crd2DertaY < 0)
            {
                Crd2Angle = (360 * Math.PI / 180) - Math.Abs(Math.Atan((Crd2Mark2.Y - Crd2Mark1.Y) / (Crd2Mark2.X - Crd2Mark1.X)));
            }
            else
            {

            }

            //坐标系1的MARK点连成的直线倾角是0度，即X+
            if (Crd1DertaX > 0 && Crd1DertaY == 0)
            {
                Crd1Angle = 0;
            }

            //坐标系1的MARK点连成的直线倾角是90度，即Y+
            if (Crd1DertaX == 0 && Crd1DertaY > 0)
            {
                Crd1Angle = (90 * Math.PI / 180);
            }

            //坐标系1的MARK点连成的直线倾角是180度，即X-
            if (Crd1DertaX < 0 && Crd1DertaY == 0)
            {
                Crd1Angle = (180 * Math.PI / 180);
            }

            //坐标系1的MARK点连成的直线倾角是270度，即Y-
            if (Crd1DertaX == 0 && Crd1DertaY < 0)
            {
                Crd1Angle = (270 * Math.PI / 180);
            }
            
            //坐标系2的MARK点连成的直线倾角是0度
            if (Crd2DertaX > 0 && Crd2DertaY == 0)
            {
                Crd2Angle = 0;
            }

            //坐标系2的MARK点连成的直线倾角是90度
            if (Crd2DertaX == 0 && Crd2DertaY > 0)
            {
                Crd2Angle = (90 * Math.PI / 180);
            }

            //坐标系2的MARK点连成的直线倾角是180度
            if (Crd2DertaX < 0 && Crd2DertaY == 0)
            {
                Crd2Angle = (180 * Math.PI / 180);
            }

            //坐标系2的MARK点连成的直线倾角是270度
            if (Crd2DertaX == 0 && Crd2DertaY < 0)
            {
                Crd2Angle = (270 * Math.PI / 180);
            }

            //计算坐标系2相对于坐标系1的旋转角，结果是弧度
            Crd2ToCrd1Angle = Crd2Angle - Crd1Angle;
            return Crd2ToCrd1Angle;
        }
    }
}

测试坐标转换代码如下：

文件FormCoordinateTrans.cs

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;

namespace CoordinateTransDemo
{
    public partial class FormCoordinateTrans : Form
    {
        public FormCoordinateTrans()
        {
            InitializeComponent();

            //转换坐标为红色
            dgvcTransformX.HeaderCell.Style.ForeColor = Color.Red;
            dgvcTransformY.HeaderCell.Style.ForeColor = Color.Red;
            this.dgvData.EnableHeadersVisualStyles = false;
            rtxtMessage.ReadOnly = true;
        }

        private void FormCoordinateTrans_Load(object sender, EventArgs e)
        {
            dgvData.Rows.Clear();
            int poleCount = 10;
            Coord[] photoCoords = new Coord[poleCount];
            photoCoords[0] = new Coord(123, 34.68);
            photoCoords[1] = new Coord(222.1, 222.2);
            photoCoords[2] = new Coord(333.11, 333.22);
            photoCoords[3] = new Coord(-1, -8.23);
            photoCoords[4] = new Coord(234, -345.3);
            photoCoords[5] = new Coord(-80, 125);
            photoCoords[6] = new Coord(-90, 0);
            photoCoords[7] = new Coord(0, 180);
            for (int i = 0; i < poleCount; i++)
            {
                dgvData.Rows.Add(i + 1, photoCoords[i].X, photoCoords[i].Y, photoCoords[i].Z, 0.0F, 0.0F);
            }
        }

        private void btnTransform_Click(object sender, EventArgs e)
        {
            Coord[] inPointArray = new Coord[dgvData.Rows.Count];
            for (int i = 0; i < dgvData.Rows.Count; i++)
            {
                inPointArray[i] = new Coord(Convert.ToSingle(dgvData["dgvcPoleX", i].Value),
                Convert.ToSingle(dgvData["dgvcPoleY", i].Value),
                Convert.ToSingle(dgvData["dgvcPoleAngle", i].Value));
            }

            string errorMessage;
            Coord[] outPointArray;
            Coord Crd2Mark1 = new Coord(double.Parse(txtCrd2Mark1_X.Text), double.Parse(txtCrd2Mark1_Y.Text));
            Coord Crd2Mark2 = new Coord(double.Parse(txtCrd2Mark2_X.Text), double.Parse(txtCrd2Mark2_Y.Text));
            Coord Crd1Mark1 = new Coord(double.Parse(txtCrd1Mark1_X.Text), double.Parse(txtCrd1Mark1_Y.Text));
            Coord Crd1Mark2 = new Coord(double.Parse(txtCrd1Mark2_X.Text), double.Parse(txtCrd1Mark2_Y.Text));
            bool result = CoordinateTransHelper.GetCalcCoordCoordinate(Crd2Mark1, Crd2Mark2, Crd1Mark1, Crd1Mark2, inPointArray, out outPointArray, out errorMessage);
            double rotateAngle = CoordinateTransHelper.Crd2ToCrd1Angle * 180 / Math.PI;
            DisplayMessage($"获取转换后的坐标操作结果：【{result}】,旋转角度【{rotateAngle}°】，旋转弧度【{CoordinateTransHelper.Crd2ToCrd1Angle}】");
            for (int i = 0; i < dgvData.Rows.Count; i++)
            {
                float srcPoleX = Convert.ToSingle(dgvData["dgvcPoleX", i].Value);
                float srcPoleY = Convert.ToSingle(dgvData["dgvcPoleY", i].Value);
                if (srcPoleX == 0 && srcPoleY == 0)
                {
                    //如果坐标是(0,0)认为是无效坐标
                    dgvData["dgvcTransformX", i].Value = 0;
                    dgvData["dgvcTransformY", i].Value = 0;
                    continue;
                }
                dgvData["dgvcTransformX", i].Value = outPointArray[i].X;
                dgvData["dgvcTransformY", i].Value = outPointArray[i].Y;
            }
        }
        /// <summary>
        /// 显示推送消息
        /// </summary>
        /// <param name="msg"></param>
        private void DisplayMessage(string msg)
        {
            this.BeginInvoke(new Action(() =>
            {
                if (rtxtMessage.TextLength > 20480)
                {
                    rtxtMessage.Clear();
                }
                rtxtMessage.AppendText($"{DateTime.Now.ToString("HH:mm:ss.fff")}->{msg}\n");
                rtxtMessage.ScrollToCaret();
            }));
        }
    }
}

运行如图：