函数的功能:在主流层定义路径离散点的数组,调用函数可以将连续曲线化为离散点。
头文件的函数声明;
viod APA_VLC_path_discrete_all(REFER_PATH *path,
VEHICLE_POS point[]);
定义:
viod APA_VLC_path_discrete_all(REFER_PATH *path,
VEHICLE_POS point[])
{
//**************第 0 段 水平直线
point[0].x = path->node[0].vpos.x;
point[0].y = path->node[0].vpos.y;
point[0].theta = path->node[0].vpos.theta;
int i = 0;
int T = 0;
T = path->node[0].node_length / 0.01;
for (i = 1; i < T; i = i + 1)
{
point[i].x = point[i - 1].x - 0.01;
point[i].y = point[i - 1].y;
point[i].theta = point[i - 1].theta;
}
}
int main()
{
VEHICLE_POS point[10000] = { 0 };
APA_VLC_path_discrete_all(path,
point);
}