//跟随点显示
private void RealPathReview()
{
//数据读取
StreamReader pathFile = File.OpenText(RecieveRealPointInf);
List <string> tempString = new List <string>();
while (!pathFile.EndOfStream)
{
tempString.Add(pathFile.ReadLine());
}
pathFile.Close();
//如果点数小于2，不能规划出轨迹
if (tempString.Count() < 2)
{
return;
}
Graphics myGra = viewControl.CreateGraphics();
Pen myPen = new Pen(System.Drawing.Color.Red, 1);
foreach (string str in tempString)
{
string[] sArray = str.Split(',');
PointF pnt;
pnt = ViewControl.GetPanelAxes(float.Parse(sArray[0]), float.Parse(sArray[1]));
myGra.DrawEllipse(myPen, pnt.X, pnt.Y, 1, 1);
}
}
//坐标点接收函数
private void PointRecieve(string tempStr, string DataPath)
{
try
{
StreamWriter velFile = new StreamWriter(VelInf, true);
StreamWriter keyPointFile = new StreamWriter(DataPath, true);
//轨迹实时显示
Graphics myGra = viewControl.CreateGraphics();
Pen myPen = new Pen(System.Drawing.Color.Red, 1);
string[] sArray = tempStr.Split('\n');
for (int i = 0; i < sArray.Length - 1; i++)
{
if (i == 0)
{
continue;
}
string[] tempArray = sArray[i].Split('\t');
if (tempArray.Length < 2)
{
return;
}
PointF pnt;
pnt = ViewControl.GetPanelAxes(float.Parse(tempArray[0]), float.Parse(tempArray[1]));
myGra.DrawEllipse(myPen, pnt.X, pnt.Y, 1, 1);
if (DataPath == RecieveRealPointInf)
{
keyPointFile.Write(tempArray[0] + "," + tempArray[1] + "\r\n");
velFile.WriteLine(tempArray[2] + "," + tempArray[3] + "," + tempArray[4] + "," + tempArray[5] + "," + tempArray[6] + "," +
tempArray[7] + "," + tempArray[8] + "," + tempArray[9]);
}
else //示教关键点信息
{
keyPointFile.Write(tempArray[0] + "," + tempArray[1] + "," + tempArray[2] + "\r\n");
}
}
keyPointFile.Close();
velFile.Close();
}
catch
{
return;
}
}
/// <summary>
/// 首末端点有方向的B样条曲线绘制
/// </summary>
/// <param name="gra">外部声明调用GDI</param>
/// <param name="point1">点1</param>
/// <param name="point2">点2</param>
/// <param name="angle1">点1的方向</param>
/// <param name="angle2">点2的方向</param>
static public void DrawBspline2(Graphics gra, PointF point1, PointF point2, float angle1, float angle2)
{
PointF[] finalDataPoint = new PointF[6];
PointF[] dataPoint = new PointF[2];
float length = 0.0f;
const float CHANGE_TO_RADIAN = (3.1415927f / 180.0f);
PointF[] lines = new PointF[2];
length = 0.6f * (float)Math.Sqrt((point1.X - point2.X) * (point1.X - point2.X) + (point1.Y - point2.Y) * (point1.Y - point2.Y));
dataPoint[0].X = (int)((3 * point1.X + length * Math.Cos((angle1) * CHANGE_TO_RADIAN)) / 3.0f);
dataPoint[0].Y = (int)((3 * point1.Y + length * Math.Sin((angle1) * CHANGE_TO_RADIAN)) / 3.0f);
dataPoint[1].X = (int)((3 * point2.X - length * Math.Cos((angle2) * CHANGE_TO_RADIAN)) / 3.0f);
dataPoint[1].Y = (int)((3 * point2.Y - length * Math.Sin((angle2) * CHANGE_TO_RADIAN)) / 3.0f);
//复制最终控制点坐标
finalDataPoint[0] = point1;
finalDataPoint[1] = point1;
finalDataPoint[2] = dataPoint[0];
finalDataPoint[3] = dataPoint[1];
finalDataPoint[4] = point2;
finalDataPoint[5] = point2;
//从初始点开始
lines[0].X = (int)finalDataPoint[0].X;
lines[0].Y = (int)finalDataPoint[0].Y;
PointF pnt1 = new PointF();
PointF pnt2 = new PointF();
for (int i = 0; i < 3; i++)
{
for (float u = 0.01f; u <= 1; u += 0.01f)
{
float b0 = 1.0f / 6 * (1 - u) * (1 - u) * (1 - u);
float b1 = 1.0f / 6 * (3 * u * u * u - 6 * u * u + 4);
float b2 = 1.0f / 6 * (-3 * u * u * u + 3 * u * u + 3 * u + 1);
float b3 = 1.0f / 6 * u * u * u;
lines[1].X = (b0 * finalDataPoint[i].X + b1 * finalDataPoint[i + 1].X + b2 * finalDataPoint[i + 2].X + b3 * finalDataPoint[i + 3].X);
lines[1].Y = (b0 * finalDataPoint[i].Y + b1 * finalDataPoint[i + 1].Y + b2 * finalDataPoint[i + 2].Y + b3 * finalDataPoint[i + 3].Y);
//转换到panel像素坐标系
pnt1 = ViewControl.GetPanelAxes(lines[0].X, lines[0].Y);
pnt2 = ViewControl.GetPanelAxes(lines[1].X, lines[1].Y);
gra.DrawLine(Pens.Yellow, pnt1, pnt2);
lines[0] = lines[1];
}
}
pnt2 = ViewControl.GetPanelAxes(point2.X, point2.Y);
gra.DrawLine(Pens.Red, pnt1, pnt2);
}
/// <summary>
/// @name DrowBspline1
/// @brief 首末端点为自由条件的B样条曲线绘制
/// @inPoint 示教点
/// </summary>
static public void DrawBspline1(int num, Graphics gra, Pen pen, PointF[] inPoint)
{
//系数矩阵对角列
float[] a = new float[num];
//系数矩阵对角上列
float[] b = new float[num];
//系数矩阵对角下列
float[] c = new float[num];
//定义soluctionX、soluctionY为线性方程的解
float[] soluctionX = new float[num];
float[] soluctionY = new float[num];
//定义dataX和dataY,用来存放inPoint里的X和Y坐标
float[] dataX = new float[num];
float[] dataY = new float[num];
//定义controlPoint用来存放控制点
PointF[] controlPoint = new PointF[num + 4];
//存放画线的两个使用点
PointF[] lines = new PointF[2];
//初始化 a,b,c
a[0] = 18;
a[num - 1] = 18;
for (int i = 1; i < num - 1; i++)
{
a[i] = 4;
}
for (int i = 1; i < num - 1; i++)
{
b[i] = 1;
c[i] = 1;
}
c[num - 1] = -9;
b[0] = -9;
for (int i = 0; i < num; i++)
{
dataX[i] = 6.0f * inPoint[i].X;
dataY[i] = 6.0f * inPoint[i].Y;
}
dataX[0] *= 1.5f;
dataY[0] *= 1.5f;
dataX[num - 1] *= 1.5f;
dataY[num - 1] *= 1.5f;
//计算outdataX,outdataY;
Calculate math = new Calculate();
//调用Matrix用追赶法求解线性方程
math.Matrix(dataX, num, ref a, ref b, ref c, ref soluctionX);
math.Matrix(dataY, num, ref a, ref b, ref c, ref soluctionY);
controlPoint[num + 3].X = dataX[num - 1] / 9;
controlPoint[num + 2].X = dataX[num - 1] / 9;
controlPoint[0].X = dataX[0] / 9;
controlPoint[1].X = dataX[0] / 9;
for (int i = 0; i < num; i++)
{
controlPoint[i + 2].X = soluctionX[i];
}
controlPoint[num + 3].Y = dataY[num - 1] / 9;
controlPoint[num + 2].Y = dataY[num - 1] / 9;
controlPoint[0].Y = dataY[0] / 9;
controlPoint[1].Y = dataY[0] / 9;
for (int i = 0; i < num; i++)
{
controlPoint[i + 2].Y = soluctionY[i];
}
//计算型值点，画出曲线
//从初始点开始
lines[0].X = (int)controlPoint[0].X;
lines[0].Y = (int)controlPoint[0].Y;
for (int i = 0; i < num + 1; i++)
{
for (float u = 0.01f; u <= 1; u += 0.01f)
{
float b0 = 1.0f / 6 * (1 - u) * (1 - u) * (1 - u);
float b1 = 1.0f / 6 * (3 * u * u * u - 6 * u * u + 4);
float b2 = 1.0f / 6 * (-3 * u * u * u + 3 * u * u + 3 * u + 1);
float b3 = 1.0f / 6 * u * u * u;
lines[1].X = (b0 * controlPoint[i].X + b1 * controlPoint[i + 1].X + b2 * controlPoint[i + 2].X + b3 * controlPoint[i + 3].X);
lines[1].Y = (b0 * controlPoint[i].Y + b1 * controlPoint[i + 1].Y + b2 * controlPoint[i + 2].Y + b3 * controlPoint[i + 3].Y);
PointF pnt1 = new PointF();
PointF pnt2 = new PointF();
//转换到panel像素坐标系
pnt1 = ViewControl.GetPanelAxes(lines[0].X, lines[0].Y);
pnt2 = ViewControl.GetPanelAxes(lines[1].X, lines[1].Y);
gra.DrawLine(Pens.Yellow, pnt1, pnt2);
lines[0] = lines[1];
}
}
}