788 lines
22 KiB
C#
788 lines
22 KiB
C#
using BRS.Common.Model.Helper;
|
||
using BRS.Common.Interface;
|
||
using Newtonsoft.Json;
|
||
using System;
|
||
using System.Collections.Generic;
|
||
using System.ComponentModel;
|
||
using System.Drawing;
|
||
using System.Threading.Tasks;
|
||
using static BRS.Common.Model.Helper.EnumHelper;
|
||
|
||
namespace BRS.Common.Model
|
||
{
|
||
/// <summary>
|
||
/// 自定义的点坐标类型
|
||
/// </summary>
|
||
public class CustomizedPoint : IComplexDisplay, ICSVOutput, INotifyPropertyChanged, IComparable<CustomizedPoint>
|
||
{
|
||
private double x = 0;
|
||
[Category("坐标设置")]
|
||
[Description("X坐标")]
|
||
public double X
|
||
{
|
||
get => x;
|
||
set
|
||
{
|
||
if (value != x)
|
||
{
|
||
x = value;
|
||
PropertyChanged?.Invoke(this, new PropertyChangedEventArgs("X"));
|
||
}
|
||
}
|
||
}
|
||
|
||
private double y = 0;
|
||
[Category("坐标设置")]
|
||
[Description("Y坐标")]
|
||
public double Y
|
||
{
|
||
get => y;
|
||
set
|
||
{
|
||
if (value != y)
|
||
{
|
||
y = value;
|
||
PropertyChanged?.Invoke(this, new PropertyChangedEventArgs("Y"));
|
||
}
|
||
}
|
||
}
|
||
|
||
public CustomizedPoint() { }
|
||
|
||
public CustomizedPoint(double x, double y)
|
||
{
|
||
X = x;
|
||
Y = y;
|
||
}
|
||
|
||
public CustomizedPoint(Point p)
|
||
{
|
||
X = p.X;
|
||
Y = p.Y;
|
||
}
|
||
|
||
public CustomizedPoint(PointF p)
|
||
{
|
||
X = p.X;
|
||
Y = p.Y;
|
||
}
|
||
|
||
public CustomizedPoint(CustomizedPoint p)
|
||
{
|
||
X = p.X;
|
||
Y = p.Y;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 根据PLC的读取数值获取点位坐标
|
||
/// </summary>
|
||
/// <param name="plcValues">0：X低位 1：X高位 2：Y低位 3：Y高位</param>
|
||
public CustomizedPoint(List<int> plcValues)
|
||
{
|
||
if (plcValues == null || plcValues.Count != 4)
|
||
return;
|
||
|
||
var list = plcValues.ParseUnsignShortListToInt();
|
||
|
||
X = list[0];
|
||
Y = list[1];
|
||
}
|
||
|
||
public event PropertyChangedEventHandler PropertyChanged;
|
||
|
||
public static List<CustomizedPoint> GetPoints(List<double> Xs, List<double> Ys)
|
||
{
|
||
List<CustomizedPoint> points = new List<CustomizedPoint>();
|
||
for (int i = 0; i < Xs.Count && i < Ys.Count; i++)
|
||
{
|
||
points.Add(new CustomizedPoint(Xs[i], Ys[i]));
|
||
}
|
||
|
||
return points;
|
||
}
|
||
|
||
public static double GetDistance(CustomizedPoint p1, CustomizedPoint p2)
|
||
{
|
||
return Math.Sqrt(Math.Pow((p1.X - p2.X), 2) + Math.Pow((p1.Y - p2.Y), 2));
|
||
}
|
||
|
||
public override string ToString()
|
||
{
|
||
return GetDisplayText();
|
||
}
|
||
|
||
public virtual string GetDisplayText()
|
||
{
|
||
return string.Format("X:{0};Y:{1}", X, Y);
|
||
}
|
||
|
||
public virtual string GetCSVHead()
|
||
{
|
||
return "X,Y";
|
||
}
|
||
|
||
public virtual string GetCSVData()
|
||
{
|
||
return X.ToString("f3") + ";" + Y.ToString("f3");
|
||
}
|
||
|
||
//public static double GetCustomizedPointDistance(CustomizedPoint startPoint, CustomizedPoint endPoint)
|
||
//{
|
||
// return Math.Sqrt(Math.Pow(endPoint.X - startPoint.X, 2) + Math.Pow(endPoint.Y - startPoint.Y, 2));
|
||
//}
|
||
|
||
public CustomizedPoint OffsetClone(CustomizedPoint point)
|
||
{
|
||
return new CustomizedPoint(X + point.X, Y + point.Y);
|
||
}
|
||
|
||
public void Offset(CustomizedPoint point)
|
||
{
|
||
X += point.X;
|
||
Y += point.Y;
|
||
}
|
||
|
||
public int CompareTo(CustomizedPoint other)
|
||
{
|
||
return (X == other.X && Y == other.Y) ? 0 : 1;
|
||
}
|
||
|
||
public override int GetHashCode()
|
||
{
|
||
//return (int)(X * 10 + Y);
|
||
return (new Tuple<double, double>(X, Y)).GetHashCode();
|
||
}
|
||
|
||
public static CustomizedPoint operator -(CustomizedPoint p1, CustomizedPoint p2)
|
||
{
|
||
return new CustomizedPoint(p1.X - p2.X, p1.Y - p2.Y);
|
||
}
|
||
|
||
public static CustomizedPoint operator +(CustomizedPoint p1, CustomizedPoint p2)
|
||
{
|
||
return new CustomizedPoint(p1.X + p2.X, p1.Y + p2.Y);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 螺丝平面测量点位
|
||
/// </summary>
|
||
public class ScrewPlanPoint : CustomizedPoint
|
||
{
|
||
[Category("坐标设置")]
|
||
[Description("基准校正后的Z坐标")]
|
||
[Browsable(false)]
|
||
[JsonIgnore]
|
||
public float Z { get; set; }
|
||
|
||
[Category("点位设置")]
|
||
[Description("点位索引，用于区分位移传感器的数据来源")]
|
||
public int Index { get; set; }
|
||
|
||
[Category("点位设置")]
|
||
[Description("是否是基准点位，true：是基准点位；false：是测量点位")]
|
||
public bool IsBasePoint { get; set; }
|
||
|
||
[Category("平面度")]
|
||
[Description("Z坐标基准")]
|
||
public float Z_Correction { get; set; }
|
||
|
||
private float? flatness = null;
|
||
[Category("平面度")]
|
||
[Description("平面度/高度值")]
|
||
[Browsable(false)]
|
||
[JsonIgnore]
|
||
public float? Flatness
|
||
{
|
||
get => flatness;
|
||
set
|
||
{
|
||
flatness = value;
|
||
|
||
if (flatness != null && !IsBasePoint)
|
||
{
|
||
IsOK = flatness.Value <= HeightThreshold;
|
||
}
|
||
}
|
||
}
|
||
|
||
private float z_original = 0;
|
||
[Category("坐标设置")]
|
||
[Description("获取的原始Z坐标")]
|
||
[Browsable(false)]
|
||
[JsonIgnore]
|
||
public float Z_Original
|
||
{
|
||
get
|
||
{
|
||
return z_original;
|
||
}
|
||
set
|
||
{
|
||
z_original = value;
|
||
Z = z_original - Z_Correction;
|
||
}
|
||
}
|
||
|
||
[Category("标准设置")]
|
||
[Description("高度阈值。大于该值认为高度超标")]
|
||
public float HeightThreshold { get; set; } = 17000;
|
||
|
||
[Browsable(false)]
|
||
[JsonIgnore]
|
||
public bool IsOK { get; set; } = true;
|
||
|
||
public override string GetCSVData()
|
||
{
|
||
//if (IsBasePoint)
|
||
//{
|
||
// return "NuN";
|
||
//}
|
||
|
||
//return Flatness == null ? "NA" : Flatness.Value.ToString("f3");
|
||
|
||
//return $"{Z.ToString("f2")},{Z_Original.ToString("f2")},{Z_Correction.ToString("f2")},{(Flatness == null ? "NA" : Flatness.Value.ToString("f2"))}";
|
||
return $"{(IsBasePoint ? "NA" : (IsOK ? "OK" : "IsFloating"))},{(Flatness ?? 0).ToString("f2")},{Z.ToString("f2")},{Z_Original.ToString("f2")},{Z_Correction.ToString("f2")}";
|
||
}
|
||
|
||
public override string GetCSVHead()
|
||
{
|
||
//if (IsBasePoint)
|
||
// return "NuN";
|
||
|
||
//return "Screw" + Index + " Flatness";
|
||
|
||
string prefix = "Screw";
|
||
|
||
if (IsBasePoint)
|
||
{
|
||
prefix = "Base";
|
||
}
|
||
|
||
//return $"{prefix}{Index}Height,{prefix}{Index}Measurement,{prefix}{Index}Calibration,{prefix}{Index}Flatness";
|
||
return $"{prefix}{Index}IsFloating,{prefix}{Index}Height,{prefix}{Index}Calib_Height,{prefix}{Index}Measurement,{prefix}{Index}Calibration";
|
||
}
|
||
}
|
||
|
||
public class CustomizedPointWithAngle : CustomizedPoint
|
||
{
|
||
[Category("坐标设置")]
|
||
[Description("角度")]
|
||
[DisplayName("角度")]
|
||
public double Angle { get; set; }
|
||
|
||
public CustomizedPointWithAngle() { }
|
||
|
||
public CustomizedPointWithAngle(double x, double y, double r)
|
||
{
|
||
X = x;
|
||
Y = y;
|
||
Angle = r;
|
||
}
|
||
|
||
//public CustomizedPointWithAngle(CustomizedPointWithAngle point)
|
||
//{
|
||
|
||
//}
|
||
|
||
/// <summary>
|
||
/// 根据PLC的读取数值获取点位坐标
|
||
/// </summary>
|
||
/// <param name="plcValues">0：X低位 1：X高位 2：Y低位 3：Y高位 4：R低位 5：R高位</param>
|
||
public CustomizedPointWithAngle(List<int> plcValues)
|
||
{
|
||
if (plcValues == null || plcValues.Count != 6)
|
||
return;
|
||
|
||
var list = plcValues.ParseUnsignShortListToInt();
|
||
|
||
X = list[0];
|
||
Y = list[1];
|
||
Angle = list[2];
|
||
}
|
||
|
||
public static List<CustomizedPointWithAngle> GetPoints(List<double> Xs, List<double> Ys, List<double> Rs)
|
||
{
|
||
List<CustomizedPointWithAngle> points = new List<CustomizedPointWithAngle>();
|
||
for (int i = 0; i < Xs.Count && i < Ys.Count; i++)
|
||
{
|
||
points.Add(new CustomizedPointWithAngle((float)Xs[i], (float)Ys[i], (float)Rs[i]));
|
||
}
|
||
|
||
return points;
|
||
}
|
||
|
||
public override string ToString()
|
||
{
|
||
return GetDisplayText();
|
||
}
|
||
|
||
public override string GetDisplayText()
|
||
{
|
||
return base.GetDisplayText() + ";Angle:" + Angle.ToString();
|
||
}
|
||
}
|
||
|
||
public class AvailablePoint : CustomizedPointWithAngle, ICloneable
|
||
{
|
||
public bool IsAvailable { get; set; } = false;
|
||
public bool IsUsed { get; set; } = false;
|
||
|
||
public AvailablePoint() { }
|
||
|
||
public override string ToString()
|
||
{
|
||
return GetDisplayText();
|
||
}
|
||
|
||
public override string GetDisplayText()
|
||
{
|
||
return base.GetDisplayText() + (IsAvailable ? "" : " 不可用 ");
|
||
}
|
||
|
||
public object Clone()
|
||
{
|
||
AvailablePoint point = new AvailablePoint
|
||
{
|
||
//point.DataFrom(this);
|
||
X = X,
|
||
Y = Y,
|
||
Angle = Angle,
|
||
IsAvailable = IsAvailable
|
||
};
|
||
|
||
return point;
|
||
}
|
||
}
|
||
|
||
public class CalibrationPoint : IComplexDisplay, INotifyPropertyChanged
|
||
{
|
||
[Category("标定坐标")]
|
||
[Description("平台坐标-X")]
|
||
public double X { get; set; }
|
||
|
||
[Category("标定坐标")]
|
||
[Description("平台坐标-X")]
|
||
public double Y { get; set; }
|
||
|
||
private double u;
|
||
[Category("像素坐标")]
|
||
[Description("像素坐标-U")]
|
||
public double U
|
||
{
|
||
get => u;
|
||
set
|
||
{
|
||
if (u != value)
|
||
{
|
||
u = value;
|
||
PropertyChanged?.Invoke(this, new PropertyChangedEventArgs("U"));
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
private double v;
|
||
[Category("像素坐标")]
|
||
[Description("像素坐标-V")]
|
||
public double V
|
||
{
|
||
get => v;
|
||
set
|
||
{
|
||
if (v != value)
|
||
{
|
||
v = value;
|
||
PropertyChanged?.Invoke(this, new PropertyChangedEventArgs("V"));
|
||
}
|
||
}
|
||
}
|
||
|
||
private bool isBasePoint = false;
|
||
[Category("快捷设置")]
|
||
[Description("是否基准点")]
|
||
[DisplayName("基准点")]
|
||
public bool IsBasePoint
|
||
{
|
||
get => isBasePoint;
|
||
set
|
||
{
|
||
isBasePoint = value;
|
||
PropertyChanged?.Invoke(this, new PropertyChangedEventArgs("IsBasePoint"));
|
||
}
|
||
}
|
||
|
||
public CalibrationPoint() { }
|
||
|
||
public CalibrationPoint(PointF p, bool isImagePoint = true)
|
||
{
|
||
if (isImagePoint)
|
||
{
|
||
U = p.X;
|
||
V = p.Y;
|
||
}
|
||
else
|
||
{
|
||
X = (int)p.X;
|
||
Y = (int)p.Y;
|
||
}
|
||
}
|
||
|
||
public CalibrationPoint(CustomizedPoint imagePoint, CustomizedPoint platPoint)
|
||
{
|
||
U = imagePoint.X;
|
||
V = imagePoint.Y;
|
||
X = (int)platPoint.X;
|
||
Y = (int)platPoint.Y;
|
||
}
|
||
|
||
public event PropertyChangedEventHandler PropertyChanged;
|
||
|
||
public override string ToString()
|
||
{
|
||
return GetDisplayText();
|
||
}
|
||
|
||
public string GetDisplayText()
|
||
{
|
||
return string.Format("X:{0},Y:{1};U:{2},V:{3}", X, Y, U.ToString("f3"), V.ToString("f3"));
|
||
}
|
||
}
|
||
|
||
public class DirectionAidPoint : AvailablePoint
|
||
{
|
||
//private PriorityDirection? direction = null;
|
||
public PriorityDirection? Direction { get; set; }
|
||
//{
|
||
// get => direction;
|
||
// set
|
||
// {
|
||
// if (value != null)
|
||
// {
|
||
// if (value == PriorityDirection.X)
|
||
// {
|
||
// MainAxisValue = X;
|
||
// MinorAxisValue = Y;
|
||
// }
|
||
// else
|
||
// {
|
||
// MainAxisValue = Y;
|
||
// MinorAxisValue = X;
|
||
// }
|
||
// }
|
||
|
||
// direction = value;
|
||
// }
|
||
//}
|
||
|
||
public double MainAxisValue
|
||
{
|
||
get => ((Direction ?? PriorityDirection.X) == PriorityDirection.X) ? X : Y;
|
||
set
|
||
{
|
||
if ((Direction ?? PriorityDirection.X) == PriorityDirection.X)
|
||
{
|
||
X = value;
|
||
}
|
||
else
|
||
{
|
||
Y = value;
|
||
}
|
||
}
|
||
}
|
||
|
||
public double MinorAxisValue
|
||
{
|
||
get => ((Direction ?? PriorityDirection.X) == PriorityDirection.X) ? Y : X;
|
||
set
|
||
{
|
||
if ((Direction ?? PriorityDirection.X) == PriorityDirection.X)
|
||
{
|
||
Y = value;
|
||
}
|
||
else
|
||
{
|
||
X = value;
|
||
}
|
||
}
|
||
}
|
||
|
||
public DirectionAidPoint() { }
|
||
|
||
public DirectionAidPoint(CustomizedPoint p)
|
||
{
|
||
X = p.X;
|
||
Y = p.Y;
|
||
|
||
if (p is CustomizedPointWithAngle temp)
|
||
{
|
||
Angle = temp.Angle;
|
||
}
|
||
}
|
||
|
||
public DirectionAidPoint(float x, float y)
|
||
{
|
||
X = x;
|
||
Y = y;
|
||
}
|
||
}
|
||
|
||
public class ComplexPoint : ICSVOutput, INotifyPropertyChanged, IComplexDisplay
|
||
{
|
||
public DirectionAidPoint ImagePoint { get; set; }
|
||
public DirectionAidPoint ImagePointAfterRotation { get; set; }
|
||
public DirectionAidPoint PlatPoint { get; set; }
|
||
|
||
public bool IsDirectionPositive { get; set; }
|
||
|
||
public bool IsTurnPoint { get; set; } = false;
|
||
|
||
//public bool IsTurnPoint { get; set; } = false;
|
||
//public int X { get; set; }
|
||
//public int Y { get; set; }
|
||
|
||
#region For UI
|
||
private bool? isCurrent = null;
|
||
/// <summary>
|
||
/// 是否时当前输出给PLC的点位，主要是为了方便界面显示时做区分
|
||
/// </summary>
|
||
public bool? IsCurrent
|
||
{
|
||
get => isCurrent;
|
||
set
|
||
{
|
||
if (isCurrent != value)
|
||
{
|
||
isCurrent = value;
|
||
//PropertyChanged?.BeginInvoke(this, new PropertyChangedEventArgs("IsCurrent"), null, null);
|
||
|
||
Task.Run(() =>
|
||
{
|
||
PropertyChanged?.Invoke(this, new PropertyChangedEventArgs("IsCurrent"));
|
||
});
|
||
|
||
}
|
||
}
|
||
}
|
||
|
||
private DateTime updateTime = DateTime.Now;
|
||
public DateTime UpdateTime
|
||
{
|
||
get => updateTime;
|
||
set
|
||
{
|
||
if (updateTime != value)
|
||
{
|
||
updateTime = value;
|
||
PropertyChanged?.BeginInvoke(this, new PropertyChangedEventArgs("UpdateTime"), null, null);
|
||
}
|
||
}
|
||
}
|
||
|
||
public event PropertyChangedEventHandler PropertyChanged;
|
||
#endregion
|
||
|
||
public string GetCSVData()
|
||
{
|
||
if (ImagePoint == null)
|
||
{
|
||
ImagePoint = new DirectionAidPoint(0, 0);
|
||
}
|
||
|
||
return $"{ImagePoint.X.ToString("f3")},{ImagePoint.Y.ToString("f3")},{PlatPoint.X.ToString("f3")},{PlatPoint.Y.ToString("f3")},{PlatPoint.Angle.ToString("f3")},{(PlatPoint.IsAvailable ? "" : "不可用")}";
|
||
|
||
}
|
||
|
||
public string GetCSVHead()
|
||
{
|
||
return "U,V,X,Y,Angle,IsAvailable";
|
||
}
|
||
|
||
public override string ToString()
|
||
{
|
||
return GetDisplayText();
|
||
}
|
||
|
||
public string GetDisplayText()
|
||
{
|
||
return $"U:{ImagePoint.X} V:{ImagePoint.Y} X:{PlatPoint.X} Y:{PlatPoint.Y}";
|
||
}
|
||
}
|
||
|
||
public static class CustomizedPointHelper
|
||
{
|
||
public static string GetCustomizedPointsCSVHead(this List<CustomizedPoint> points, string prefix, int count = 0)
|
||
{
|
||
string head = "";
|
||
|
||
if (count == 0)
|
||
{
|
||
count = points.Count;
|
||
}
|
||
|
||
for (int i = 1; i <= count; i++)
|
||
{
|
||
head += prefix + "_" + i.ToString() + "_X,";
|
||
head += prefix + "_" + i.ToString() + "_Y,";
|
||
}
|
||
|
||
return head.TrimEnd(new char[] { ',' });
|
||
}
|
||
|
||
public static string GetCustomizedPointsCSVData(this List<CustomizedPoint> points, int count = 0, bool isXYInvert = false)
|
||
{
|
||
string data = "";
|
||
if (count == 0)
|
||
{
|
||
count = points.Count;
|
||
}
|
||
|
||
for (int i = 0; i < count; i++)
|
||
{
|
||
if (points != null && i < points.Count)
|
||
{
|
||
if (isXYInvert)
|
||
{
|
||
data += points[i].Y.ToString("f3") + "," + points[i].X.ToString("f3") + ",";
|
||
}
|
||
else
|
||
{
|
||
data += points[i].X.ToString("f3") + "," + points[i].Y.ToString("f3") + ",";
|
||
}
|
||
}
|
||
else
|
||
{
|
||
data += "NA,NA,";
|
||
}
|
||
}
|
||
|
||
return data.TrimEnd(new char[] { ',' });
|
||
//return string.Join(",", points.Select(p => p.X.ToString("f3") + "," + p.Y.ToString("f3")));
|
||
}
|
||
|
||
public static string GetCustomizedPointGGRCSVHead(this List<CustomizedPoint> points)
|
||
{
|
||
string head = "";
|
||
|
||
for (int i = 1; i <= points.Count; i++)
|
||
{
|
||
head += i.ToString() + "_X,";
|
||
head += i.ToString() + "_Y,";
|
||
}
|
||
|
||
if (points.Count >= 2)
|
||
{
|
||
for (int i = 1; i <= points.Count; i++)
|
||
{
|
||
for (int j = i + 1; j <= points.Count; j++)
|
||
{
|
||
head += (i + "_" + j + "_Dist,");
|
||
}
|
||
}
|
||
}
|
||
|
||
return head.TrimEnd(new char[] { ',' });
|
||
}
|
||
public static string GetCustomizedPointsGRRCSVData(this List<CustomizedPoint> points)
|
||
{
|
||
string data = "";
|
||
|
||
for (int i = 0; i < points.Count; i++)
|
||
{
|
||
data += points[i].X.ToString("f3") + "," + points[i].Y.ToString("f3") + ",";
|
||
}
|
||
|
||
if (points.Count >= 2)
|
||
{
|
||
for (int i = 1; i <= points.Count; i++)
|
||
{
|
||
for (int j = i + 1; j <= points.Count; j++)
|
||
{
|
||
data += (CustomizedPoint.GetDistance(points[i - 1], points[j - 1]).ToString("f3") + ",");
|
||
}
|
||
}
|
||
}
|
||
|
||
return data.TrimEnd(new char[] { ',' });
|
||
}
|
||
}
|
||
|
||
public struct StrRobotPoint
|
||
{
|
||
public float X;
|
||
public float Y;
|
||
public float Z;
|
||
public float A;
|
||
public float B;
|
||
public float C;
|
||
}
|
||
public class RobotPoint : IComplexDisplay
|
||
{
|
||
[Category("点位信息")]
|
||
[Description("坐标X")]
|
||
public float X { get; set; }
|
||
|
||
[Category("点位信息")]
|
||
[Description("坐标Y")]
|
||
public float Y { get; set; }
|
||
|
||
[Category("点位信息")]
|
||
[Description("坐标Z")]
|
||
public float Z { get; set; }
|
||
|
||
[Category("点位信息")]
|
||
[Description("角度A")]
|
||
public float A { get; set; }
|
||
|
||
[Category("点位信息")]
|
||
[Description("角度B")]
|
||
public float B { get; set; }
|
||
|
||
[Category("点位信息")]
|
||
[Description("角度C")]
|
||
public float C { get; set; }
|
||
|
||
public string _Description { get; set; } = "点位描述";
|
||
|
||
public string GetDisplayText()
|
||
{
|
||
return $"X:{X.ToString()} Y:{Y.ToString()} Z:{Z.ToString()} A:{A.ToString()} B:{B.ToString()} C:{C.ToString()} Description:{_Description}";
|
||
}
|
||
|
||
//public string GetDisplayText()
|
||
//{
|
||
// return $"X:{X.ToString()} Y:{Y.ToString()} Z:{Z.ToString()} A:{A.ToString()} B:{B.ToString()} C:{C.ToString()}";
|
||
//}
|
||
|
||
public double[] GetArray()
|
||
{
|
||
return new double[] { X, Y, Z, A, B, C };
|
||
}
|
||
|
||
public static RobotPoint GetRobotPointByArray(double[] array)
|
||
{
|
||
if (array.Length != 6)
|
||
{
|
||
return null;
|
||
}
|
||
|
||
RobotPoint point = new RobotPoint
|
||
{
|
||
X = (float)array[0],
|
||
Y = (float)array[1],
|
||
Z = (float)array[2],
|
||
A = (float)array[3],
|
||
B = (float)array[4],
|
||
C = (float)array[5]
|
||
};
|
||
|
||
return point;
|
||
}
|
||
|
||
public static float GetDistance(RobotPoint pointA, RobotPoint pointB)
|
||
{
|
||
return (float)Math.Sqrt(Math.Pow(pointA.X - pointB.X, 2) + Math.Pow(pointA.Y - pointB.Y, 2) + Math.Pow(pointA.Z - pointB.Z, 2));
|
||
}
|
||
}
|
||
}
|