using MathNet.Numerics.LinearAlgebra;
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
namespace TeamAAS_VP.Core
{
///
/// 三点建立坐标系(或者原点及角度)，并实现Local与Word坐标系直接的坐标互转换
///
public class CoordinateTransformer
{
///
/// 三个点建立坐标系时确定是以X轴作为基准还是Y轴作为基准
///
public enum Axis { X, Y };
///
/// P1 作为新坐标系的原点
///
public Vector P1 { get; }
///
/// 坐标系的基向量
///
public Vector E1 { get; }
///
/// 坐标系的基向量
///
public Vector E2 { get; }
public Matrix R { get; }
///
/// 旋转矩阵
///
public Matrix InvR { get; }
///
/// 平移矩阵
///
public Matrix T { get; }
///
/// 坐标系的旋转角度
///
public double Angle
{
get
{
//Vector2 e1 = new Vector2(Math.Cos(radians), Math.Sin(radians)); // 新x轴向量
//Vector2 e2 = new Vector2(-Math.Sin(radians), Math.Cos(radians)); // 新y轴向量
double thetaRand1 = Math.Asin(E1[1]);
double thetaDeg1 = thetaRand1 * 180 / Math.PI; // 将弧度值转换为角度值
return Math.Round(thetaDeg1, 3);
}
}
///
/// 三点建立坐标系，并实现Local与Word坐标系直接的坐标互转换
///
/// 原点X坐标
/// 原点Y坐标
/// X轴上的点X坐标
/// X轴上的点Y坐标
/// Y轴上的点X坐标
/// Y轴上的点Y坐标
/// 三个点建立坐标系时确定是以X轴作为基准还是Y轴作为基准
public CoordinateTransformer(double x1, double y1, double x2, double y2, double x3, double y3, Axis axis = Axis.X)
{
//假设我们有三个点 P1(x1, y1), P2(x2, y2), P3(x3, y3)，它们是笛卡尔坐标系的三个点。我们要基于这三个点创建一个经过旋转和平移的坐标系，并且要求两个坐标系中的点可以互相转换。
//1.新坐标系的原点：我们可以将 P1 作为新坐标系的原点。因此，新坐标系的原点坐标为 (x1, y1)。
P1 = Vector.Build.Dense(new double[] { x1, y1 });
// Calculate basis vectors
Vector p2 = Vector.Build.Dense(new double[] { x2, y2 });
Vector p3 = Vector.Build.Dense(new double[] { x3, y3 });
//2.计算坐标系的基向量：
if (axis == Axis.X)
{
E1 = (p2 - P1) / (p2 - P1).L2Norm();
E2 = (p3 - P1 - E1.DotProduct(p3 - P1) * E1) / (p3 - P1 - E1.DotProduct(p3 - P1) * E1).L2Norm();
}
else
{
E2 = (p3 - P1) / (p3 - P1).L2Norm();
E1 = (p2 - P1 - E2.DotProduct(p2 - P1) * E2) / (p2 - P1 - E2.DotProduct(p2 - P1) * E2).L2Norm();
}
//3.计算旋转矩阵：我们可以使用基向量组成的矩阵作为旋转矩阵。具体地，我们可以将基向量 e1 和 e2 分别作为新坐标系下的 $x$ 和 $y$ 轴，然后组成一个矩阵，再对这个矩阵求逆即可得到旋转矩阵。代码实现：
// Compute rotation matrix
R = Matrix.Build.DenseOfColumnVectors(E1, E2);
InvR = R.Inverse();
//4.平移矩阵：由于我们选择的原点是 P1，因此我们需要对所有点进行平移，使得新坐标系的原点与笛卡尔坐标系的原点重合。具体地，我们可以使用以下平移矩阵：
// Compute translation matrix
T = Matrix.Build.DenseIdentity(3);
T[0, 2] = -P1[0];
T[1, 2] = -P1[1];
}
///
/// 三点建立坐标系，并实现Local与Word坐标系直接的坐标互转换
///
/// 原点坐标
/// X轴上的点坐标
/// Y轴上的点坐标
/// 三个点建立坐标系时确定是以X轴作为基准还是Y轴作为基准
public CoordinateTransformer(PointF Po, PointF Px, PointF Py, Axis axis = Axis.X)
{
//假设我们有三个点 P1(x1, y1), P2(x2, y2), P3(x3, y3)，它们是笛卡尔坐标系的三个点。我们要基于这三个点创建一个经过旋转和平移的坐标系，并且要求两个坐标系中的点可以互相转换。
//1.新坐标系的原点：我们可以将 P1 作为新坐标系的原点。因此，新坐标系的原点坐标为 (x1, y1)。
P1 = Vector.Build.Dense(new double[] { Po.X, Po.Y });
// Calculate basis vectors
Vector p2 = Vector.Build.Dense(new double[] { Px.X, Px.Y });
Vector p3 = Vector.Build.Dense(new double[] { Py.X, Py.Y });
//2.计算坐标系的基向量：
if (axis == Axis.X)
{
E1 = (p2 - P1) / (p2 - P1).L2Norm();
E2 = (p3 - P1 - E1.DotProduct(p3 - P1) * E1) / (p3 - P1 - E1.DotProduct(p3 - P1) * E1).L2Norm();
}
else
{
E2 = (p3 - P1) / (p3 - P1).L2Norm();
E1 = (p2 - P1 - E2.DotProduct(p2 - P1) * E2) / (p2 - P1 - E2.DotProduct(p2 - P1) * E2).L2Norm();
}
//3.计算旋转矩阵：我们可以使用基向量组成的矩阵作为旋转矩阵。具体地，我们可以将基向量 e1 和 e2 分别作为新坐标系下的 $x$ 和 $y$ 轴，然后组成一个矩阵，再对这个矩阵求逆即可得到旋转矩阵。代码实现：
// Compute rotation matrix
R = Matrix.Build.DenseOfColumnVectors(E1, E2);
InvR = R.Inverse();
//4.平移矩阵：由于我们选择的原点是 P1，因此我们需要对所有点进行平移，使得新坐标系的原点与笛卡尔坐标系的原点重合。具体地，我们可以使用以下平移矩阵：
// Compute translation matrix
T = Matrix.Build.DenseIdentity(3);
T[0, 2] = -P1[0];
T[1, 2] = -P1[1];
}
///
/// 原点及角度建立坐标系
///
/// 原点坐标
/// 角度
public CoordinateTransformer(PointF Po, double theta)
{
double radians = theta * Math.PI / 180; // 将角度值转换为弧度值
//1.新坐标系的原点：我们可以将 P1 作为新坐标系的原点。因此，新坐标系的原点坐标为 (x1, y1)。
P1 = Vector.Build.Dense(new double[] { Po.X, Po.Y });
//2.计算坐标系的基向量：
E1 = Vector.Build.Dense(new double[] { Math.Cos(radians), Math.Sin(radians) }); // 新x轴向量
E2 = Vector.Build.Dense(new double[] { -Math.Sin(radians), Math.Cos(radians) }); // 新y轴向量
//3.计算旋转矩阵：我们可以使用基向量组成的矩阵作为旋转矩阵。具体地，我们可以将基向量 e1 和 e2 分别作为新坐标系下的 $x$ 和 $y$ 轴，然后组成一个矩阵，再对这个矩阵求逆即可得到旋转矩阵。代码实现：
// Compute rotation matrix
R = Matrix.Build.DenseOfColumnVectors(E1, E2);
InvR = R.Inverse();
//4.平移矩阵：由于我们选择的原点是 P1，因此我们需要对所有点进行平移，使得新坐标系的原点与笛卡尔坐标系的原点重合。具体地，我们可以使用以下平移矩阵：
// Compute translation matrix
T = Matrix.Build.DenseIdentity(3);
T[0, 2] = -P1[0];
T[1, 2] = -P1[1];
}
///
/// Word坐标系下的点转换成Local坐标系下的点
///
/// Word坐标系下的点X坐标
/// Word坐标系下的点Y坐标
/// 返回Local坐标系下的点
public Vector ToNewCoord(double x, double y)
{
Vector pOld = Vector.Build.Dense(new double[] { x, y });
Vector pNew = InvR * (pOld - P1);
return pNew;
}
///
/// Local坐标系下的点转换成Word坐标系下的点
///
/// Local坐标系下的点X坐标
/// Local坐标系下的点Y坐标
/// 返回Word坐标系下的点
public Vector ToOldCoord(double x, double y)
{
Vector pNew = Vector.Build.Dense(new double[] { x, y });
Vector pOld = R * pNew + P1;
return pOld;
}
}
}