已知坐标系的向量线段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();
}));
}
}
}
运行如图：
