392 lines
9.6 KiB
C#
392 lines
9.6 KiB
C#
using System;
|
||
using System.Collections.Generic;
|
||
|
||
/// <summary>
|
||
///
|
||
/// </summary>
|
||
public class CVT
|
||
{
|
||
/// <summary>
|
||
///
|
||
/// </summary>
|
||
/// <param name="M"></param>
|
||
/// <returns></returns>
|
||
public static double MToMM(object M)
|
||
{
|
||
return Math.Round(Convert.ToDouble(M.ToString()) * 1000, 2);
|
||
}
|
||
/// <summary>
|
||
///
|
||
/// </summary>
|
||
/// <param name="MM"></param>
|
||
/// <returns></returns>
|
||
public static double MMToM(object MM)
|
||
{
|
||
return Math.Round(Convert.ToDouble(MM.ToString()) / 1000, 5);
|
||
}
|
||
|
||
/// <summary>
|
||
///
|
||
/// </summary>
|
||
/// <param name="angle"></param>
|
||
/// <returns></returns>
|
||
public static double AngleToRad(object angle)
|
||
{
|
||
return Math.Round(Convert.ToDouble(angle.ToString()) * Math.PI / 180, 5);
|
||
}
|
||
/// <summary>
|
||
///
|
||
/// </summary>
|
||
/// <param name="rad"></param>
|
||
/// <returns></returns>
|
||
public static double RadToAngle(object rad)
|
||
{
|
||
return Math.Round(Convert.ToDouble(rad.ToString()) * 180 / Math.PI, 5);
|
||
}
|
||
|
||
/// <summary>
|
||
///
|
||
/// </summary>
|
||
/// <param name="M"></param>
|
||
/// <returns></returns>
|
||
public static double MToMM(string M)
|
||
{
|
||
return Math.Round(Convert.ToDouble(M) * 1000, 2);
|
||
}
|
||
/// <summary>
|
||
///
|
||
/// </summary>
|
||
/// <param name="MM"></param>
|
||
/// <returns></returns>
|
||
public static double MMToM(string MM)
|
||
{
|
||
return Math.Round(Convert.ToDouble(MM) / 1000, 5);
|
||
}
|
||
|
||
/// <summary>
|
||
///
|
||
/// </summary>
|
||
/// <param name="angle"></param>
|
||
/// <returns></returns>
|
||
public static double AngleToRad(string angle)
|
||
{
|
||
return Math.Round(Convert.ToDouble(angle) * Math.PI / 180, 5);
|
||
}
|
||
/// <summary>
|
||
///
|
||
/// </summary>
|
||
/// <param name="rad"></param>
|
||
/// <returns></returns>
|
||
public static double RadToAngle(string rad)
|
||
{
|
||
return Math.Round(Convert.ToDouble(rad) * 180 / Math.PI, 5);
|
||
}
|
||
|
||
|
||
|
||
/// <summary>
|
||
///
|
||
/// </summary>
|
||
/// <param name="M"></param>
|
||
/// <returns></returns>
|
||
public static double MToMM(double M)
|
||
{
|
||
return Math.Round(M * 1000, 2);
|
||
}
|
||
/// <summary>
|
||
///
|
||
/// </summary>
|
||
/// <param name="MM"></param>
|
||
/// <returns></returns>
|
||
public static double MMToM(double MM)
|
||
{
|
||
return Math.Round(MM / 1000, 5);
|
||
}
|
||
/// <summary>
|
||
///
|
||
/// </summary>
|
||
/// <param name="angle"></param>
|
||
/// <returns></returns>
|
||
public static double AngleToRad(double angle)
|
||
{
|
||
return Math.Round(angle * Math.PI / 180, 5);
|
||
}
|
||
/// <summary>
|
||
///
|
||
/// </summary>
|
||
/// <param name="rad"></param>
|
||
/// <returns></returns>
|
||
public static double RadToAngle(double rad)
|
||
{
|
||
return Math.Round(rad * 180 / Math.PI, 5);
|
||
}
|
||
|
||
|
||
|
||
|
||
/// <summary>
|
||
/// 坐标系的转换
|
||
/// ThreeJsToMatlab
|
||
/// 原则：Y、Z互换，且Y取负方向
|
||
/// 2020.10.19
|
||
/// </summary>
|
||
/// <param name="threePosition"></param>
|
||
/// <returns></returns>
|
||
public static double[] ThreeJsToMatlab__Y_Z(double[] threePosition)
|
||
{
|
||
double[] matlabPosition = new double[6];
|
||
matlabPosition[0] = threePosition[0];
|
||
matlabPosition[1] = threePosition[2] * (-1);
|
||
matlabPosition[2] = threePosition[1];
|
||
matlabPosition[3] = threePosition[3];
|
||
matlabPosition[4] = threePosition[4];
|
||
matlabPosition[5] = threePosition[5];
|
||
return matlabPosition;
|
||
}
|
||
|
||
|
||
|
||
/// <summary>
|
||
/// 坐标系的转换
|
||
/// ThreeJsToMatlab
|
||
/// </summary>
|
||
/// <param name="threePosition"></param>
|
||
/// <returns></returns>
|
||
public static double[] ThreeJsToKdl(double[] threePosition)
|
||
{
|
||
double[] matlabPosition = new double[6];
|
||
|
||
//// 2020.10.19 原则：当时好用的变换
|
||
//matlabPosition[0] = threePosition[2] * (-1);
|
||
//matlabPosition[1] = threePosition[0] * (-1);
|
||
//matlabPosition[2] = threePosition[1];
|
||
//matlabPosition[3] = threePosition[5] * (-1);
|
||
//matlabPosition[4] = threePosition[3] * (-1);
|
||
//matlabPosition[5] = threePosition[4];
|
||
|
||
//// 2020.11.24 原则：不做变换
|
||
//matlabPosition[0] = threePosition[0];
|
||
//matlabPosition[1] = threePosition[1];
|
||
//matlabPosition[2] = threePosition[2];
|
||
//matlabPosition[3] = threePosition[3];
|
||
//matlabPosition[4] = threePosition[4];
|
||
//matlabPosition[5] = threePosition[5];
|
||
|
||
//// 2020.11.25 原则：
|
||
//matlabPosition[0] = threePosition[2] ;
|
||
//matlabPosition[1] = threePosition[0] * (-1);
|
||
//matlabPosition[2] = threePosition[1];
|
||
//matlabPosition[3] = threePosition[5] ;
|
||
//matlabPosition[4] = threePosition[3] * (-1);
|
||
//matlabPosition[5] = threePosition[4];
|
||
|
||
// 2020.11.25 原则：现在好用的
|
||
matlabPosition[0] = threePosition[2];
|
||
matlabPosition[1] = threePosition[0];
|
||
matlabPosition[2] = threePosition[1];
|
||
matlabPosition[3] = threePosition[5];
|
||
matlabPosition[4] = threePosition[3];
|
||
matlabPosition[5] = threePosition[4];
|
||
|
||
return matlabPosition;
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
/// <summary>
|
||
/// 字符串 转换 双浮点
|
||
/// </summary>
|
||
/// <param name="arrayStr"></param>
|
||
/// <returns></returns>
|
||
public static double[] StringArrayToDoubleArray(string[] arrayStr)
|
||
{
|
||
double[] arrayDouble = new double[arrayStr.Length];
|
||
for (int i = 0; i < arrayStr.Length; i++)
|
||
arrayDouble[i] = Convert.ToDouble(arrayStr[i]);
|
||
return arrayDouble;
|
||
}
|
||
|
||
|
||
|
||
/// <summary>
|
||
/// obj 转换 双浮点
|
||
/// </summary>
|
||
/// <param name="arrayStr"></param>
|
||
/// <returns></returns>
|
||
public static double[] ObjectArrayToDoubleArray(object[] arrayObj)
|
||
{
|
||
double[] arrayDouble = new double[arrayObj.Length];
|
||
for (int i = 0; i < arrayObj.Length; i++)
|
||
arrayDouble[i] = Convert.ToDouble(arrayObj[i].ToString());
|
||
return arrayDouble;
|
||
}
|
||
|
||
|
||
|
||
/// <summary>
|
||
/// 毫米转米
|
||
/// 角度转弧度
|
||
/// 10个值
|
||
/// 2021.2.26
|
||
/// </summary>
|
||
/// <param name="p_Ten"></param>
|
||
public static double[] MMToM_AngleToRad_Ten(double[] p_Ten)
|
||
{
|
||
return new double[] {
|
||
CVT.MMToM(p_Ten[0]),
|
||
CVT.MMToM(p_Ten[1]),
|
||
CVT.MMToM(p_Ten[2]),
|
||
p_Ten[3],
|
||
p_Ten[4],
|
||
p_Ten[5],
|
||
p_Ten[6],
|
||
CVT.AngleToRad(p_Ten[7]),
|
||
CVT.AngleToRad(p_Ten[8]),
|
||
CVT.AngleToRad(p_Ten[9]) };
|
||
|
||
}
|
||
/// <summary>
|
||
/// 毫米转米
|
||
/// 角度转弧度
|
||
/// 7个值
|
||
/// 2021.2.26
|
||
/// </summary>
|
||
/// <param name="p_Seven"></param>
|
||
public static double[] MMToM_AngleToRad_Seven(double[] p_Seven)
|
||
{
|
||
return new double[] {
|
||
CVT.MMToM(p_Seven[0]),
|
||
CVT.MMToM(p_Seven[1]),
|
||
CVT.MMToM(p_Seven[2]),
|
||
p_Seven[3],
|
||
p_Seven[4],
|
||
p_Seven[5],
|
||
p_Seven[6] };
|
||
|
||
}
|
||
/// <summary>
|
||
/// 毫米转米
|
||
/// 角度转弧度
|
||
/// 6个值
|
||
/// 2021.2.26
|
||
/// </summary>
|
||
/// <param name="p_Six"></param>
|
||
public static double[] MMToM_AngleToRad_Six(double[] p_Six)
|
||
{
|
||
return new double[]{
|
||
CVT.MMToM(p_Six[0]),
|
||
CVT.MMToM(p_Six[1]),
|
||
CVT.MMToM(p_Six[2]),
|
||
CVT.AngleToRad(p_Six[3]),
|
||
CVT.AngleToRad(p_Six[4]),
|
||
CVT.AngleToRad(p_Six[5])};
|
||
|
||
}
|
||
|
||
|
||
/// <summary>
|
||
/// 米转毫米
|
||
/// 弧度转角度
|
||
/// 10个值
|
||
/// 2021.2.26
|
||
/// </summary>
|
||
/// <param name="p_Ten"></param>
|
||
public static double[] MToMM_RadToAngle_Ten(double[] p_Ten)
|
||
{
|
||
return new double[] {
|
||
CVT.MToMM(p_Ten[0]),
|
||
CVT.MToMM(p_Ten[1]),
|
||
CVT.MToMM(p_Ten[2]),
|
||
p_Ten[3],
|
||
p_Ten[4],
|
||
p_Ten[5],
|
||
p_Ten[6],
|
||
CVT.RadToAngle(p_Ten[7]),
|
||
CVT.RadToAngle(p_Ten[8]),
|
||
CVT.RadToAngle(p_Ten[9])
|
||
};
|
||
|
||
}
|
||
/// <summary>
|
||
/// 米转毫米
|
||
/// 弧度转角度
|
||
/// 7个值
|
||
/// 2021.2.26
|
||
/// </summary>
|
||
/// <param name="p_Seven"></param>
|
||
public static double[] MToMM_RadToAngle_Seven(double[] p_Seven)
|
||
{
|
||
return new double[] {
|
||
CVT.MToMM(p_Seven[0]),
|
||
CVT.MToMM(p_Seven[1]),
|
||
CVT.MToMM(p_Seven[2]),
|
||
p_Seven[3],
|
||
p_Seven[4],
|
||
p_Seven[5],
|
||
p_Seven[6] };
|
||
|
||
}
|
||
/// <summary>
|
||
/// 米转毫米
|
||
/// 弧度转角度
|
||
/// 6个值
|
||
/// 2021.2.26
|
||
/// </summary>
|
||
/// <param name="p_Six"></param>
|
||
public static double[] MToMM_RadToAngle_Six(double[] p_Six)
|
||
{
|
||
return new double[]{
|
||
CVT.MToMM(p_Six[0]),
|
||
CVT.MToMM(p_Six[1]),
|
||
CVT.MToMM(p_Six[2]),
|
||
CVT.RadToAngle(p_Six[3]),
|
||
CVT.RadToAngle(p_Six[4]),
|
||
CVT.RadToAngle(p_Six[5])
|
||
};
|
||
|
||
}
|
||
|
||
|
||
/// <summary>
|
||
///
|
||
/// </summary>
|
||
/// <param name="radList"></param>
|
||
/// <returns></returns>
|
||
public static List<double> RadToAngle(List<double> radList)
|
||
{
|
||
var angleList = new List<double>();
|
||
|
||
foreach (double rad in radList)
|
||
{
|
||
angleList.Add(Math.Round(rad * 180 / Math.PI, 5));
|
||
}
|
||
|
||
return angleList;
|
||
}
|
||
/// <summary>
|
||
///
|
||
/// </summary>
|
||
/// <param name="angleList"></param>
|
||
/// <returns></returns>
|
||
public static List<double> AngleToRad(List<double> angleList)
|
||
{
|
||
var radList = new List<double>();
|
||
|
||
foreach (double angle in angleList)
|
||
{
|
||
radList.Add(Math.Round(angle * Math.PI / 180, 5));
|
||
}
|
||
|
||
return radList;
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
}
|
||
|