基础函数-圆弧路径生成-圆弧插值

圆弧轨迹的生成在泊车几何路径规划的领域中出现，在其他的路径规划的领域中应用不多，在泊车规划中常常是已知车辆的行驶方向、方向盘转角方向、方向盘转角大小、行驶距离这几个量，求出车辆行驶的圆弧上的离散点坐标或者行驶轨迹，这里面涉及的内容有如何获取车辆行驶的圆心坐标、如何将方向盘转角转为前轮转角以及如何插值出圆弧上的每个点，第二个问题单独讨论，本文介绍其余两个问题。

根据方向盘转角方向、大小计算转向中心

由下图可以得到，左转前进或者后退时:
o.x=p.x-|op|*sin(t);
o.y=p.y+|op|*cos(t);

右转前进或者后退时:
o.x=p.x+|op|*sin(t);
o.y=p.y-|op|*cos(t);

一点绕一点旋转一定角度后得到新的点

cos(α+β)=(p0.x-o.x)/R;
sin(α+β)=(p0.y-o.y)/R;
cos(β)=(p1.x-o.x)/R;
sin(β)=(p1.y-o.y)/R;
p1.x=o.x+(p0.x-o.x)*cos(α)-(p0.y-o.y)*sin(α);
p1.y=o.y+(p0.x-o.x)*sin(α)+(p0.y-o.y)*cos(α);

角度α的正负代表旋转的方向，逆时针为正，顺时针为负值。

泊车圆弧轨迹生成

圆弧轨迹的生成主要是根据车辆的行驶方向和方向盘转角方向、行驶的曲率（转弯半径）以及行驶距离或者角度变化来确定圆弧轨迹的中间点的过程。主要的计算逻辑是根据行驶方向和方向盘转角的方向确定旋转角度的正负，例如左转前进是逆时针旋转，左转后退是顺时针旋转，右转前进是顺时针旋转，右转后退时逆时针旋转，这样我们就确定了旋转角度的正负了。
例如左转前进，是逆时针方向，假设已知行驶方向和方向盘转角方向、转弯半径r以及行驶距离，那么就可将行驶距离按照步长step_size均分为num个点，每两个个点之间的旋转角度为step_size/r，那么就可以根据一点绕一点旋转一定角度后得到新的点的公式得到中间所有点的坐标。

// Steering direction
#define STEERING_LEFT (1)
#define STEERING_RIGHT (-1)

// Running direction
#define FORWARD (1)
#define BACKWARD (-1)
     typedef struct _Point3d_
    {
   
        double x;
        double y;
        double yaw;
        _Point3d_() : x(0), y(0), yaw(0) {
   }
        _Point3d_(double a, double b) : x(a), y(b) {
   }
        _Point3d_(double a, double b,