放一点写的辅助判断道路的函数，因为太多了就慢慢更吧。

//计算曲率
float process_curvity(int x1, int y1, int x2, int y2, int x3, int y3)
{float K;int S_of_ABC = ((x2 - x1) * (y3 - y1) - (x3 - x1) * (y2 - y1)) / 2;//面积的符号表示方向float q1 = (float)((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1));float AB = sqrt(q1);q1 = (float)((x3 - x2) * (x3 - x2) + (y3 - y2) * (y3 - y2));float BC = sqrt(q1);q1 = (float)((x3 - x1) * (x3 - x1) + (y3 - y1) * (y3 - y1));float AC = sqrt(q1);if (AB * BC * AC == 0){K = 0;}elseK = (float)4 * S_of_ABC / (AB * BC * AC);return K;
}

这个函数是输入三个点，计算曲率，返回值的K就是计算得到的曲率，纯算法就不写注释了，直接就能拿去用。不过最好还是自己写，加深一下了解，也不难。

//最小二乘法拟合,拟合y=kx+b,直线函数
参数（拟合的边线类型，开始的行数（y轴起始点）结束的行数（y轴的结束点））
无返回值，用于用最小二乘法计算从y=startline到y=endline，之间的所选定类型边线的斜率和截距说白了就是k和b，要是不懂最小二乘法的就百度一下，概念很简单
void regression(int type, int startline, int endline)
{int i = 0;int sumlines = endline - startline;int sumX = 0;int sumY = 0;float averageX = 0;float averageY = 0;float sumUp = 0;float sumDown = 0;if (type == 0)      //拟合中线{for (i = startline; i < endline; i++){sumX += i;sumY += centerline[i];}if (sumlines != 0){averageX = sumX / sumlines;     //x的平均值averageY = sumY / sumlines;     //y的平均值}else{averageX = 0;     //x的平均值averageY = 0;     //y的平均值}for (i = startline; i < endline; i++){sumUp += (centerline[i] - averageY) * (i - averageX);sumDown += (i - averageX) * (i - averageX);}if (sumDown == 0) parameterB = 0;else parameterB = sumUp / sumDown;parameterA = averageY - parameterB * averageX;
//b=y-k*x，这么看就会了吧，剩下的都一样，把这两个变量搞成全局变量，直接调用就行，不然你乐意用指针搞两个返回值也行}else if (type == 1)//拟合左线{for (i = startline; i < endline; i++){sumX += i;sumY += leftline[i];}if (sumlines == 0) sumlines = 1;averageX = sumX / sumlines;     //x的平均值averageY = sumY / sumlines;     //y的平均值for (i = startline; i < endline; i++){sumUp += (leftline[i] - averageY) * (i - averageX);sumDown += (i - averageX) * (i - averageX);}if (sumDown == 0) parameterB = 0;else parameterB = sumUp / sumDown;parameterA = averageY - parameterB * averageX;}else if (type == 2)//拟合右线{for (i = startline; i < endline; i++){sumX += i;sumY += rightline[i];}if (sumlines == 0) sumlines = 1;averageX = sumX / sumlines;     //x的平均值averageY = sumY / sumlines;     //y的平均值for (i = startline; i < endline; i++){sumUp += (rightline[i] - averageY) * (i - averageX);sumDown += (i - averageX) * (i - averageX);}if (sumDown == 0) parameterB = 0;else parameterB = sumUp / sumDown;parameterA = averageY - parameterB * averageX;}
}

下边的函数一样的东西，就是拟合的是y=a*x^2+b。先更这些吧，敲累了

//拟合一元二次函数，1组数据版
void LeastSquareCalc_Curve(int StartLine, int EndLine, int type)
{int i = 0;float Sum_X2 = 0, Sum_Y = 0, Sum_X4 = 0, Sum_YX2 = 0, Average_X2 = 0, Average_Y = 0, Average_X4 = 0, Average_YX2 = 0, Sum = 0;Sum = EndLine - StartLine;if (type == 1){for (i = StartLine; i < EndLine; i++){Sum_X2 += i * i;Sum_Y += leftline[i];Sum_X4 += i * i * i * i;Sum_YX2 += leftline[i] * i * i;}}else if (type == 2){for (i = StartLine; i < EndLine; i++){Sum_X2 += i * i;Sum_Y += rightline[i];Sum_X4 += i * i * i * i;Sum_YX2 += rightline[i] * i * i;}}else    //中线{for (i = StartLine; i < EndLine; i++){Sum_X2 += i * i;Sum_Y += centerline[i];Sum_X4 += i * i * i * i;Sum_YX2 += centerline[i] * i * i;}}Average_X2 = Sum_X2 / Sum;Average_Y = Sum_Y / Sum;Average_X4 = Sum_X4 / Sum;Average_YX2 = Sum_YX2 / Sum;curve_a = (Average_YX2 - Average_Y * Average_X2) / (Average_X4 - Average_X2 * Average_X2);//acurve_b = Average_Y - curve_a * Average_X2;//b}