/// <summary>
/// 生成轨迹线, TobeModified
/// </summary>
public void CreatePathLine(ref LineInfo currentLineDrawer, Color lineColor, List <MotionInfo> motion_info_data)
{
currentLineDrawer.Clear();
// PathLineDraw_Script.lineOriginalDrawer.Clear();
List <ToolChangeInfo> tool_motion_line_list = new List <ToolChangeInfo>();
ToolChangeInfo tempToolData = new ToolChangeInfo();
for (int i = 0; i < tool_motion_list.Count; i++)
{
tempToolData = new ToolChangeInfo();
tempToolData.ToolDataCopy(tool_motion_list[i]);
tool_motion_line_list.Add(tempToolData);
}
//将画线参考体移动到主轴指定位置
PathLineDraw_Script.lineRef.parent = GameObject.Find("main axle_4").transform;
PathLineDraw_Script.lineRef.localPosition = new Vector3(0, -0.731137f, 0.0003082752f);
PathLineDraw_Script.lineRef.localEulerAngles = new Vector3(90, 270, 0);
PathLineDraw_Script.lineRef.parent = GameObject.Find("workbench_1").transform;
//本次轨迹线的起始点
Vector3 orinPoint = Auto_Script.CurrentVirtualPos() / 1000;
for (int i = 0; i < motion_info_data.Count; i++)
{
if (motion_info_data[i].Motion_Type != -1 && motion_info_data[i].Motion_Type != (int)MotionType.Pause)
{
//圆弧
if (motion_info_data[i].Motion_Type == (int)MotionType.Circular02 || motion_info_data[i].Motion_Type == (int)MotionType.Circular03)
{
Vector3 centre_point = motion_info_data[i].VirtualTarget - motion_info_data[i].DisplayTarget + motion_info_data[i].Center_Point;
centre_point /= 1000;
Vector3 start_vector = motion_info_data[i].VirtualStart / 1000 - centre_point;
Vector3 end_vector = motion_info_data[i].VirtualTarget / 1000 - centre_point;
Vector3 axis_vector = Vector3.zero;
//半圆弧或者圆弧时关于旋转轴的处理
if (motion_info_data[i].Rotate_Degree % 180 == 0)
{
//顺时针
if (motion_info_data[i].Motion_Type == (int)MotionType.Circular02)
{
if (motion_info_data[i].Current_Plane == (int)CheckInformation.XYPlane)
{
axis_vector = new Vector3(0, 0, -1);
}
else if (motion_info_data[i].Current_Plane == (int)CheckInformation.ZXPlane)
{
axis_vector = new Vector3(0, -1, 0);
}
else
{
axis_vector = new Vector3(-1, 0, 0);
}
}
//逆时针
else
{
if (motion_info_data[i].Current_Plane == (int)CheckInformation.XYPlane)
{
axis_vector = new Vector3(0, 0, 1);
}
else if (motion_info_data[i].Current_Plane == (int)CheckInformation.ZXPlane)
{
axis_vector = new Vector3(0, 1, 0);
}
else
{
axis_vector = new Vector3(1, 0, 0);
}
}
}
else
{
axis_vector = Vector3.Cross(start_vector, end_vector).normalized;
if (motion_info_data[i].Rotate_Degree > 180f)
{
axis_vector = -1 * axis_vector;
}
}
// float radius = (motion_info_list[i].VirtualTarget / 1000 - centre_point).magnitude;
float angle = motion_info_data[i].Rotate_Degree * Mathf.PI / 180;
//r旋转theta弧度后的向量
Vector3 rotate_point = new Vector3(0f, 0f, 0f);
//圆弧精度计算
int slices = (int)(SystemArguments.CirclePrecision * angle / (2 * Mathf.PI));
if (slices <= 2)
{
slices = 3;
}
motion_info_data[i].Slices = slices;
// Debug.Log(slices + ": angle = " + angle + "; radius = " + radius);
//每次旋转的弧度数
float theta = angle / slices;
float calTheta = 0;
//运用了旋转矩阵，等价于Rodrigues旋转公式
Vector3 firstPoint = new Vector3(0, 0, 0); //折线起始点
Vector3 secondPoint = new Vector3(0f, 0f, 0f); //折线终点
for (int j = 0; j <= slices; ++j)
{
if (j != 0)
{
calTheta += theta;
}
rotate_point = Mathf.Cos(calTheta) * start_vector + Vector3.Cross(axis_vector, start_vector) * Mathf.Sin(calTheta) + Vector3.Dot(axis_vector, start_vector) * axis_vector * (1 - Mathf.Cos(calTheta));
secondPoint = centre_point + rotate_point;
if (j != 0)
{
currentLineDrawer.Add(i, j, firstPoint - orinPoint, secondPoint - orinPoint, lineColor);
}
firstPoint = secondPoint;
}
}
//从参考点返回，两点，即两条线段
else if (motion_info_data[i].Motion_Type == (int)MotionType.BackFromRP || motion_info_data[i].Motion_Type == (int)MotionType.AutoReturnRP)
{
motion_info_data[i].Slices = 2;
currentLineDrawer.Add(i, 1, motion_info_data[i].VirtualStart / 1000 - orinPoint, motion_info_data[i].VirtualTarget / 1000 - orinPoint, lineColor);
currentLineDrawer.Add(i, 2, motion_info_data[i].VirtualTarget / 1000 - orinPoint, motion_info_data[i].VirtualTarget2 / 1000 - orinPoint, lineColor);
}
//直线情况
else
{
currentLineDrawer.Add(i, -1, motion_info_data[i].VirtualStart / 1000 - orinPoint, motion_info_data[i].VirtualTarget / 1000 - orinPoint, lineColor);
}
}
string toolchange_str = "" + (char)ImmediateMotionType.M06;
//有换刀程序
if (motion_info_data[i].Immediate_Motion.Contains(toolchange_str))
{
if (tool_motion_line_list[0].TimeValue > 0)
{
currentLineDrawer.Add(motion_info_data.Count + tool_motion_line_list.Count, -1, tool_motion_line_list[0].VirtualStart / 1000 - orinPoint, tool_motion_line_list[0].VirtualTarget / 1000 - orinPoint, lineColor);
}
tool_motion_line_list.RemoveAt(0);
}
}
// Debug.Log(PathLineDraw_Script.lineDrawer.Count());
}