//通过配置的轮子最大加速度进行降速
//适当比例的降速，算完后记得把最新速度数据放在ringbuffer里
//wheelOne 一号轮速度数组首地址
//wheelTwo 二号轮速度数组首地址
//wheelThree 三号轮速度数组首地址
static public void DynamicalAjusting(float[] wheelOne, float[] wheelTwo, float[] wheelThree)
{
float time = 0.0f;
int n = PointsInfo.pnts.Count();
float tempAcc = 0.0f;
//每次加速度降低至上次的百分值
float percent = 0.9f;
//先正向削减速度
for (int i = 2; i < n + 1; i++)
{
//粗略计算每两示教点之间的运动的时间
time = (PointsInfo.pnts[i - 1].length - PointsInfo.pnts[i - 2].length) / (PointsInfo.pnts[i - 1].velMax + PointsInfo.pnts[i - 2].velMax) * 2;
//轮1
//只处理速度同向的情况
if (wheelOne[i - 1] * wheelOne[i - 2] > 0)
{
tempAcc = (Math.Abs(wheelOne[i - 1]) - Math.Abs(wheelOne[i - 2])) / time;
}
else
{
tempAcc = 0.0f;
}
if (tempAcc > MotionCardParameter.GetAccMax())
{
//每次削减0.05的加速度
wheelOne[i - 1] = wheelOne[i - 1] > 0 ? wheelOne[i - 2] + tempAcc * percent * time : wheelOne[i - 2] - tempAcc * percent * time;
}
//轮2
//只处理速度同向的情况
if (wheelTwo[i - 1] * wheelTwo[i - 2] > 0)
{
tempAcc = (Math.Abs(wheelTwo[i - 1]) - Math.Abs(wheelTwo[i - 2])) / time;
}
else
{
tempAcc = 0.0f;
}
if (tempAcc > MotionCardParameter.GetAccMax())
{
//每次削减0.05的加速度
wheelTwo[i - 1] = wheelTwo[i - 1] > 0 ? wheelTwo[i - 2] + tempAcc * percent * time : wheelTwo[i - 2] - tempAcc * percent * time;
}
//轮3
//只处理速度同向的情况
if (wheelThree[i - 1] * wheelThree[i - 2] > 0)
{
tempAcc = (Math.Abs(wheelThree[i - 1]) - Math.Abs(wheelThree[i - 2])) / time;
}
else
{
tempAcc = 0.0f;
}
if (tempAcc > MotionCardParameter.GetAccMax())
{
//每次削减0.05的加速度
wheelThree[i - 1] = wheelThree[i - 1] > 0 ? wheelThree[i - 2] + tempAcc * percent * time : wheelThree[i - 2] - tempAcc * percent * time;
}
}
//反向削减速度
for (int i = n; i > 1; i--)
{
//粗略计算每两示教点之间的运动的时间
time = (PointsInfo.pnts[i - 1].length - PointsInfo.pnts[i - 2].length) / (PointsInfo.pnts[i - 1].velMax + PointsInfo.pnts[i - 2].velMax) * 2;
//轮1
//只处理速度同向的情况
if (wheelOne[i - 1] * wheelOne[i - 2] > 0)
{
tempAcc = (Math.Abs(wheelOne[i - 1]) - Math.Abs(wheelOne[i - 2])) / time;
}
else
{
tempAcc = 0.0f;
}
if (tempAcc < -MotionCardParameter.GetAccMax())
{
//每次削减0.05的加速度
wheelOne[i - 2] = wheelOne[i - 2] > 0 ? wheelOne[i - 1] - tempAcc * percent * time : wheelOne[i - 1] + tempAcc * percent * time;
}
//轮2
//只处理速度同向的情况
if (wheelTwo[i - 1] * wheelTwo[i - 2] > 0)
{
tempAcc = (Math.Abs(wheelTwo[i - 1]) - Math.Abs(wheelTwo[i - 2])) / time;
}
else
{
tempAcc = 0.0f;
}
if (tempAcc < -MotionCardParameter.GetAccMax())
{
//每次削减0.05的加速度
wheelTwo[i - 2] = wheelTwo[i - 2] > 0 ? wheelTwo[i - 1] - tempAcc * percent * time : wheelTwo[i - 1] + tempAcc * percent * time;
}
//轮3
//只处理速度同向的情况
if (wheelThree[i - 1] * wheelThree[i - 2] > 0)
{
tempAcc = (Math.Abs(wheelThree[i - 1]) - Math.Abs(wheelThree[i - 2])) / time;
}
else
{
tempAcc = 0.0f;
}
if (tempAcc < -MotionCardParameter.GetAccMax())
{
//每次削减0.05的加速度
wheelThree[i - 2] = wheelThree[i - 2] > 0 ? wheelThree[i - 1] - tempAcc * percent * time : wheelThree[i - 1] + tempAcc * percent * time;
}
}
for (int i = 0; i < n - 1; i++)
{
Calculate.TriWheelVel2_t tempVel2;
tempVel2 = Calculate.TriWheelVel2ResultantVel(wheelOne[i], wheelTwo[i], wheelThree[i], PointsInfo.pnts[i].posAngle);
//存入新计算的速度
PointsInfo.pnts[i].velMax = tempVel2.speed;
}
}
//绘制合速度图像
void ResultantVelCurveDrawing()
{
//数据读取
StreamReader pathFile = File.OpenText(VelInf);
List <string> tempString = new List <string>();
while (!pathFile.EndOfStream)
{
tempString.Add(pathFile.ReadLine());
}
pathFile.Close();
//如果点数小于2，不能规划出轨迹
if (tempString.Count() < 2)
{
return;
}
Graphics myGra = VelDisplay.CreateGraphics();
Pen myPen = new Pen(Color.Red, 1);
PointF[] velWheel1 = new PointF[tempString.Count()];
PointF[] velWheel2 = new PointF[tempString.Count()];
PointF[] velWheel3 = new PointF[tempString.Count()];
PointF[] targetVelWheel1 = new PointF[tempString.Count()];
PointF[] targetVelWheel2 = new PointF[tempString.Count()];
PointF[] targetVelWheel3 = new PointF[tempString.Count()];
float[] zAngle = new float[tempString.Count()];
int i = 0;
foreach (string str in tempString)
{
string[] sArray = str.Split(',');
velWheel1[i].X = velWheel2[i].X = velWheel3[i].X
= targetVelWheel1[i].X = targetVelWheel2[i].X = targetVelWheel3[i].X
= float.Parse(sArray[0]);
velWheel1[i].Y = float.Parse(sArray[1]);
velWheel2[i].Y = float.Parse(sArray[2]);
velWheel3[i].Y = float.Parse(sArray[3]);
targetVelWheel1[i].Y = float.Parse(sArray[4]);
targetVelWheel2[i].Y = float.Parse(sArray[5]);
targetVelWheel3[i].Y = float.Parse(sArray[6]);
i++;
}
PointF[] trueVel = new PointF[tempString.Count()];
PointF[] targetVel = new PointF[tempString.Count()];
PointF[] trueDir = new PointF[tempString.Count()];
PointF[] targetDir = new PointF[tempString.Count()];
PointF[] trueRotateVel = new PointF[tempString.Count()];
PointF[] targetRotateVel = new PointF[tempString.Count()];
PointF[] DirectionErr = new PointF[tempString.Count()];
PointF[] binaryzation = new PointF[tempString.Count()];
//由三个轮子的速度计算合成速度
for (int j = 0; j < targetVelWheel1.Count(); j++)
{
Calculate.TriWheelVel2_t tempVel;
tempVel = Calculate.TriWheelVel2ResultantVel(velWheel1[j].Y, velWheel2[j].Y, velWheel3[j].Y, zAngle[j]);
trueVel[j].Y = tempVel.speed;
trueVel[j].X = velWheel1[j].X;
trueDir[j].Y = tempVel.direction;
trueDir[j].X = velWheel1[j].X;
trueRotateVel[j].Y = tempVel.rotationVel;
trueRotateVel[j].X = velWheel1[j].X;
tempVel = Calculate.TriWheelVel2ResultantVel(targetVelWheel1[j].Y, targetVelWheel2[j].Y, targetVelWheel3[j].Y, zAngle[j]);
targetVel[j].Y = tempVel.speed;
targetVel[j].X = targetVelWheel1[j].X;
targetDir[j].Y = tempVel.direction;
targetDir[j].X = targetVelWheel1[j].X;
targetRotateVel[j].Y = tempVel.rotationVel;
targetRotateVel[j].X = targetVelWheel1[j].X;
}
pathMax = FindMaxAbs(targetDir).X;
zoom = (float)(VelDisplay.Bottom / 2) / FindMaxAbs(targetDir).Y;
//设置滚动条最大值
hScrollBar1.Maximum = (int)(PERCENT * pathMax / (VelDisplay.Right / zoomL)) + 1;
for (int j = 0; j < targetVelWheel1.Count(); j++)
{
DirectionErr[j].X = targetDir[j].X;
DirectionErr[j].Y = Math.Abs(targetDir[j].Y - trueDir[j].Y);
if (DirectionErr[j].Y > 180)
{
DirectionErr[j].Y = 360 - DirectionErr[j].Y;
}
else if (DirectionErr[j].Y < -180)
{
DirectionErr[j].Y = 360 + DirectionErr[j].Y;
}
}
//进行滤波处理
for (int m = 0; m < 4; m++)
{
for (int j = 1; j < DirectionErr.Count() - 1; j++)
{
DirectionErr[j].Y = (DirectionErr[j - 1].Y + DirectionErr[j + 1].Y) / 2;
}
}
//进行阈值判断，二值化
for (int j = 0; j < DirectionErr.Count(); j++)
{
if (DirectionErr[j].Y < 8)
{
binaryzation[j].Y = 0;
binaryzation[j].X = DirectionErr[j].X;
}
else
{
binaryzation[j].Y = DirectionErr[j].Y;
binaryzation[j].X = DirectionErr[j].X;
}
}
//将二值化的文件存入txt文档中
StreamWriter SpeedFile = new StreamWriter(SpeedReplanInfo, false);
for (int m = 0; m < binaryzation.Count(); m++)
{
SpeedFile.WriteLine("{0:f0},{1:f0}", binaryzation[m].X, binaryzation[m].Y);
}
SpeedFile.Close();
//进行阈值判断，二值化
for (int j = 0; j < DirectionErr.Count(); j++)
{
binaryzation[j] = GetPanelAxes(binaryzation[j].X, binaryzation[j].Y);
}
myPen.Color = Color.Black;
myGra.DrawLines(myPen, binaryzation);
}
//计算所绘制B样条曲线关键点路径长度，并存在txt文件中,用于切分姿态角度所用
static public void CalculateBsplineInfo(string txtAdress)
{
//创建临时文件
StreamWriter tempFile = new StreamWriter(Directory.GetCurrentDirectory() + "\\eeww.txt", true);
//////////////////////////////////从TXT文档读取数据点/////////////////////////////////////
//数据读取
StreamReader pathFile = File.OpenText(txtAdress);
List <string> tempString = new List <string>();
while (!pathFile.EndOfStream)
{
tempString.Add(pathFile.ReadLine());
}
pathFile.Close();
//如果点数小于2，不能规划出轨迹
if (tempString.Count() < 2)
{
return;
}
List <PointF> tempPnts = new List <PointF>();
Pen myPen = new Pen(Color.Black, 1);
foreach (string str in tempString)
{
string[] sArray = str.Split(',');
tempPnts.Add(new PointF(float.Parse(sArray[0]), float.Parse(sArray[1])));
}
System.Drawing.PointF[] tempPoint = tempPnts.ToArray();
//////////////////////////////////////////////////////////////////////////////////////////////
int num = tempPoint.Count();
float[] pntLen = new float[num];
//系数矩阵对角列
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 * tempPnts[i].X;
dataY[i] = 6.0f * tempPnts[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];
}
float threshold = 1.0f;
while (true)
{
//计算型值点，画出曲线
//从初始点开始
lines[0] = tempPoint[0];
pntLen[0] = 0.0f;
int count = 1;
float length = 0.0f;
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);
length += Bspline.CalculatePnt2Pnt(lines[0], lines[1]);
if (Bspline.CalculatePnt2Pnt(lines[1], tempPnts[count]) < threshold)
{
pntLen[count++] = length;
}
lines[0] = lines[1];
}
}
if (count == num - 1)
{
length += Bspline.CalculatePnt2Pnt(lines[0], tempPoint[num - 1]);
pntLen[num - 1] = length;
break;
}
else
{
threshold *= 2.0f;
}
}
int ii = 0;
foreach (string str in tempString)
{
tempFile.WriteLine(str + "," + pntLen[ii++].ToString());
}
tempFile.Close();
//清空txt文档
FileStream fs = new FileStream(txtAdress, FileMode.Create, FileAccess.Write);
fs.Close();
FileTxt.FileTxtCopy(Directory.GetCurrentDirectory() + "\\eeww.txt", txtAdress);
//删除临时文件
System.IO.File.Delete(Directory.GetCurrentDirectory() + "\\eeww.txt");
}
//将姿态角度没15cm计算一个值
static public void Segment2(PointF[] inPoint, StreamWriter file, StreamReader lengthSegmentFileR)
{
int num = inPoint.Count();
//系数矩阵对角列
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];
}
List <string> segmentStr = new List <string>();
while (!lengthSegmentFileR.EndOfStream)
{
segmentStr.Add(lengthSegmentFileR.ReadLine());
}
List <float> lenthSegments = new List <float>();
foreach (string str in segmentStr)
{
string[] sArray = str.Split(',');
lenthSegments.Add(float.Parse(sArray[0]));
}
int count = 0;
file.WriteLine(inPoint[0].Y);
for (int i = 0; i < num + 1; i++)
{
for (float u = 0.00001f; u <= 1; u += 0.00001f)
{
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);
float temp = 0.0f;
if (lines[1].X > lenthSegments[count])
{
temp = lines[1].Y;
if (lines[1].Y < 0.0f)
{
int tempI = -1;
while (true)
{
tempI++;
if (temp > tempI * (-360) - 180)
{
break;
}
}
temp += tempI * 360;
file.WriteLine(temp);
}
else if (lines[1].Y > 0.0f)
{
int tempI = -1;
while (true)
{
tempI++;
if (temp < tempI * (360) + 180)
{
break;
}
}
temp -= tempI * 360;
file.WriteLine(temp);
}
else
{
file.WriteLine(lines[1].Y);
}
count++;
if (count == lenthSegments.Count())
{
break;
}
}
}
if (count == lenthSegments.Count())
{
break;
}
}
//对最后一点进行判断
if (inPoint[num - 1].Y < 0.0f)
{
float temp1 = inPoint[num - 1].Y;
int tempI = -1;
while (true)
{
tempI++;
if (temp1 > tempI * (-360) - 180)
{
break;
}
}
temp1 += tempI * 360;
file.WriteLine(temp1);
}
else if (lines[1].Y > 0.0f)
{
float temp1 = inPoint[num - 1].Y;
int tempI = -1;
while (true)
{
tempI++;
if (temp1 < tempI * (360) + 180)
{
break;
}
}
temp1 -= tempI * 360;
file.WriteLine(temp1);
}
else
{
file.WriteLine(lines[1].Y);
}
}
//进行整体分段处理，仅用于BsplineSegment,计算每段15cm处切线方向
static public float Segment(PointF[] inPoint, string BsplineWithDirTxt, StreamWriter lengthSegmentFile)
{
StreamWriter file = new StreamWriter(BsplineWithDirTxt, false);
int num = inPoint.Count();
//系数矩阵对角列
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 = controlPoint[0].X;
//lines[0].Y = controlPoint[0].Y;
float length = 0.0f;
float lengthOrigin = 0.0f;
const float CHANGE_TO_ANGLE = (180.0f / 3.1415927f);
int count = 0;
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);
lengthOrigin += Bspline.CalculatePnt2Pnt(lines[0], lines[1]);
length += Bspline.CalculatePnt2Pnt(lines[0], lines[1]);
if (length > 300.0)
{
length = 0;
float Db0 = -1.0f / 2 * (u - 1) * (u - 1);
float Db1 = 1.0f / 2 * (3 * u * u - 4 * u);
float Db2 = 1.0f / 2 * (-3 * u * u + u * 2 + 1);
float Db3 = 1.0f / 2 * u * u;
float angle = CHANGE_TO_ANGLE * (float)Math.Atan2((Db0 * controlPoint[i].Y + Db1 * controlPoint[i + 1].Y
+ Db2 * controlPoint[i + 2].Y + Db3 * controlPoint[i + 3].Y),
(Db0 * controlPoint[i].X + Db1 * controlPoint[i + 1].X
+ Db2 * controlPoint[i + 2].X + Db3 * controlPoint[i + 3].X));
if (count == 0)
{
file.Write(inPoint[0].X.ToString() + "," + inPoint[0].Y.ToString() + "," + (Math.Atan2(lines[1].Y - inPoint[0].Y, lines[1].X - inPoint[0].X) * 180.0f / 3.14159f).ToString() + "\r\n");
file.Write(lines[1].X.ToString() + "," + lines[1].Y.ToString() + "," + angle.ToString() + "\r\n");
}
else
{
file.Write(lines[1].X.ToString() + "," + lines[1].Y.ToString() + "," + angle.ToString() + "\r\n");
}
count++;
//并没有记录第一点和最后一点
lengthSegmentFile.WriteLine(lengthOrigin);
}
lines[0] = lines[1];
}
}
lengthOrigin += Bspline.CalculatePnt2Pnt(inPoint[num - 1], lines[1]);
file.Write(inPoint[num - 1].X.ToString() + "," + inPoint[num - 1].Y.ToString() + "," + (Math.Atan2(inPoint[num - 1].Y - lines[1].Y, inPoint[num - 1].X - lines[1].X) * 180.0f / 3.14159f).ToString() + "\r\n");
file.Close();
return(lengthOrigin);
}
/// <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];
}
}
}
//计算b样条长度，端点条件为自由端点条件
static public float CalculateBsplineLenWithoutDir(PointF[] inPoint)
{
int num = inPoint.Count();
//系数矩阵对角列
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] = inPoint[0];
float length = 0.0f;
const float CHANGE_TO_ANGLE = (180.0f / 3.1415927f);
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);
length += Bspline.CalculatePnt2Pnt(lines[0], lines[1]);
float Db0 = -1.0f / 2 * (u - 1) * (u - 1);
float Db1 = 1.0f / 2 * (3 * u * u - 4 * u);
float Db2 = 1.0f / 2 * (-3 * u * u + u * 2 + 1);
float Db3 = 1.0f / 2 * u * u;
float angle = CHANGE_TO_ANGLE * (float)Math.Atan2((Db0 * controlPoint[i].Y + Db1 * controlPoint[i + 1].Y
+ Db2 * controlPoint[i + 2].Y + Db3 * controlPoint[i + 3].Y),
(Db0 * controlPoint[i].X + Db1 * controlPoint[i + 1].X
+ Db2 * controlPoint[i + 2].X + Db3 * controlPoint[i + 3].X));
lines[0] = lines[1];
}
}
length += Bspline.CalculatePnt2Pnt(lines[0], inPoint[num - 1]);
return(length);
}