直线连续插补中圆弧过渡的求解思路

我们先用两段直线来说明吧，假设有两段连续运行的轨迹，分别知道起点，交点和终点的坐标。

另外我们可以设置直线进行圆弧过渡的位置，为了更好的说明，我们用如下图来表示：

 M ,A ,N分别表示两条连续插补直线的起点，交点和终点，B点是想要进行圆弧过渡的起点，如果要实现平滑过渡，那么这个圆弧曲线BC必须是要与B点和C点都相切的，图中的圆是△AMN的内切圆，P是圆心。根据内切圆的性质，我们知道AB= AC，△ABC是等腰三角形，D是BC的中垂线，垂直平分BC。

已知M坐标(xm,ym),A点坐标（xa,ya）,N坐标（xn,yn），则根据上述性质可以分别求的B点和C点的坐标。

求解方法如下：

接下来我们求圆心坐标P（xp,yp）,(D点因为是垂直平分BC，所以D的坐标是((xb+xc)/2,(yb+yc)/2)。

我们用向量的思路来。

那么接下来只要求得AD和DP的长度就可以了；

直角三角形△ACP相似于△PDC，

 

上图中，k是CD的长，k的长度因为B，C的坐标都求出来了，所以我们很方便就得出来，r是半径，L是AC的长，PD的长度就等于sqrt(r*r-k*k)，AP的长度是（L*L +r *r）,AD 等于AP的长度-PD的长度，这样就可以求出P点的坐标。

另外半径我们已经求出来了。

这样整个过渡圆的方程我们就知道了， 接下来将该圆弧拆分成小线段的方式，就可以实现两条直线的平滑过渡。

关于圆弧拆分成小线段的方式，我们可以看grbl的代码，里面有详细的说明。

最后还有一点，判断圆弧过渡是逆时针还是顺时针，这个可以通过向量的叉积来判断，如果叉积值为负，则为顺时针，否则为逆时针。

//计算两个向量的叉积
//向量A和B的叉积计算 C =AxBy-AyBx
uint8_t calc_cross_product(float x0, float y0, float x1, float y1)
{
    float c = x0 * y1 - y0 * x1;
    if (c < -1e-5f)
    {
        return 1; //顺时针
    }
    if(c > 1e-5f)
    {
        return 0; //逆时针
    }
    return 0;
}

附求解代码，C语言写的

double distance(Vector p1, Vector p2) {
    double dx =(double)p2.x - (double)p1.x;
    double dy =(double)p2.y - (double)p1.y;
    return sqrt(dx * dx + dy * dy);
}

/*******************************************************************/
/* 等腰三角形ABC，AB=AC，求与B点和C点都相切的圆的圆心坐标P和半径R-**/
/* D点为BC的中点                                                   */
/*                 A*                                              */
/*               / | \                                             */
/*              /  |  \                                            */
/*             /   |   \                                           */
/*            /    |    \                                          */
/*           /     |     \                                         */
/*        B。------D------。C                                      */
/*           \     |       /                                       */
/*             \   |     /                                         */
/*              \  |   /                                           */
/*               \ | /                                             */
/*                 *P                                              */
/*-----------------------------------------------------------------*/
void calculate_circle(float* pt0, float* pt1, float* pt2, float* r, float* center) 
{
    Vector A = { pt0[0], pt0[1] };
    Vector B = { pt1[0], pt1[1] };
    Vector C = { pt2[0], pt2[1] };
    Vector D = { (B.x + C.x) / 2, (B.y + C.y) / 2 }; //D为BC的中点

    double K = distance(C, D);   // 求出向量 CD 的模长
    //由相似三角形计算圆心半径
    //△PDC和△PCA相似，PC长度为圆弧半径r,D为BC中点，AC的长度与AB的长度相等
    //L/r = K/sqrt(r²-K²),K为 CD的长度,r = L*K/sqrt(L²-K²)
    double L = distance(C, A);
    double R = (L * K) / sqrt(L * L - K * K);

    //计算圆心到D的长度 PD
    double PD = sqrt(R * R - K * K);

    //计算比例,AD= AP-PD
    double AD = sqrt(L * L + R * R) - PD;

    double ratio = PD/ AD;
    Vector P; //P为圆心坐标

    P.x = D.x + ratio * (D.x - A.x);
    P.y = D.y + ratio * (D.y - A.y);

    *r = R;
    center[0] = P.x;
    center[1] = P.y;

}
uint8_t intep_line_arc_line( float* pt0,float* pt1, float* pt2,float arc_ratio, float feed_rate,
    uint8_t axis_0, uint8_t axis_1, uint8_t axis_linear, uint8_t isclockwise)
{
    float radius;
   // float pt0[N_AXIS];
    //获取起点位置
   // for (uint8_t idx = 0; idx < N_AXIS; idx++)
   // {
      //  pt0[idx] = sys.position[idx] * settings.steps_per_mm[idx];
   // }
    double x0 = 1.0*pt1[axis_0] - 1.0*pt0[axis_0];
    double y0 = 1.0*pt1[axis_1] - 1.0*pt0[axis_1];

    double x1 = 1.0*pt2[axis_0] - 1.0*pt1[axis_0];
    double y1 = 1.0*pt2[axis_1] - 1.0*pt1[axis_1];

    radius = sqrt(x0 * x0 + y0 * y0) * arc_ratio;  //线条1没有运动的长度
    float rate2 = radius / sqrt(x1 * x1 + y1 * y1); //因为线条1和线条2交点处要构成一个内切圆，因此这里成了一个等腰三角形
    //求解拐点
    float point_arc1[N_AXIS] = { 0.0,0.0,0.0,0.0,0.0,0.0 };
    float point_arc2[N_AXIS] = { 0.0,0.0,0.0,0.0,0.0,0.0 };
    float offset[N_AXIS] = { 0.0,0.0,0.0,0.0,0.0,0.0 };
    for (uint8_t i = 0; i < settings.iaxisnum; i++)
    {
        point_arc1[i] = pt0[i] + (1.0 * pt1[i] - 1.0 * pt0[i])*(1.0-arc_ratio); //等比例求解拐点坐标
        point_arc2[i] = pt1[i] + (1.0 * pt2[i] - 1.0 * pt1[i]) * rate2;
    }
#if 0
    point_arc1[0] = pt0[axis_0] + x0 * (1.0- arc_ratio);//x轴坐标
    point_arc1[1] = pt0[axis_1] + y0 * (1.0 - arc_ratio);//y轴坐标
    point_arc1[2] = 0.0;

    point_arc2[0] = pt1[axis_0] + x1 * rate2;
    point_arc2[1]= pt1[axis_1] + y1 * rate2;
    point_arc2[2] = 0.0;
#endif
    //求解过渡圆弧，BAC为等腰三角形，A是顶点
    float pointA[2] = { pt1[axis_0], pt1[axis_1] };
    float pointB[2] = { point_arc1[axis_0],point_arc1[axis_1] };
    float pointC[2] = { point_arc2[axis_0], point_arc2[axis_1] };

    float inscirb_center[2];    //求解的内切圆心

    calculate_circle(pointA, pointB, pointC,&radius, inscirb_center);