圆弧点计算方式-c++代码

轨迹规划中，常用到圆弧点的计算，已知一个轨迹点的坐标(x,y,theta)，求这个点以R为半径，向前走len米的距离，产生的新数据点的坐标。

好，接下来确认下输入输出：

输入为：起点坐标x，y，theta，半径R，向前走len距离

输出为：终点坐标：X,Y,Thetas

#include <iostream>
#include <math.h>
#include <cmath>
#include <vector>
//角度归一 将角度转化为 -M_PI~M_PI之间
double NormalizeAngle(const double angle) {
  double a = std::fmod(angle + M_PI, 2.0 * M_PI);
  if (a < 0.0) {
    a += (2.0 * M_PI);
  }
  return a - M_PI;
}

struct PointXYZ{
    PointXYZ() = default;
    PointXYZ(float x_,float y_,float theta_):x(x_),y(y_),theta(theta){}
    float x{0.0};
    float y{0.0};
    float theta{0.0};
};
//当旋转半径处于起始点的左侧时
void left_segment(const PointXYZ & start_point,const double R,const double len,PointXYZ* result){
    float t = len/R;//归一化 将半径看成单位1
    float d_x = sin(start_point.theta + t) - sin(start_point.theta);
    float d_y = -cos(start_point.theta + t) + cos(start_point.theta);
    float d_head = NormalizeAngle(start_point.theta + t);
    //将半径反乘回来，变成真实值
    result->x = d_x * R + start_point.x;
    result->y = d_y * R + start_point.y;
    result->theta = d_head;
}
//当旋转半径处于起始点的右侧时
void right_segment(const PointXYZ & start_point,const double R,const double len,PointXYZ* result){
    float t = len/R;//归一化 将半径看成单位1
    float d_x = sin(start_point.theta) - sin(start_point.theta - t);
    float d_y = cos(start_point.theta - t) - cos(start_point.theta);
    float d_head = NormalizeAngle(start_point.theta - t);
    //将半径反乘回来，变成真实值
    result->x = d_x * R + start_point.x;
    result->y = d_y * R + start_point.y;
    result->theta = d_head;
}
//直线延长时
void straight_segment(const PointXYZ & start_point,const double len,PointXYZ* result){
    float d_x = cos(start_point.theta) * len;
    float d_y = sin(start_point.theta) * len;
    float d_head = NormalizeAngle(start_point.theta);
    //加上偏置
    result->x = d_x  + start_point.x;
    result->y = d_y  + start_point.y;
    result->theta = d_head;
}

int main(){
    std::vector<PointXYZ> trajectory;
    PointXYZ start(5,6,0);
    PointXYZ next_point;
    float R = 20;//转弯半径为20米
    for(float len = 0;len < 20*M_PI*2;len+=0.5)
    {
        left_segment(start,R,len,&next_point);
        trajectory.push_back(next_point);
        std::cout<<next_point.x<<","<<next_point.y<<std::endl;
    }
    return 0;
}

输出结果为：

5,6
5.49995,6.00625
5.99958,6.02499
6.49859,6.05622
6.99667,6.09992
7.49349,6.15605
7.98876,6.22458
8.48216,6.30547
8.97339,6.39867
9.46213,6.50412
9.94808,6.62175
10.4309,6.7515
10.9104,6.89327
11.3862,7.04699
11.858,7.21255
12.3255,7.38985
12.7884,7.57878
13.2464,7.77922
13.6993,7.99106
14.1468,8.21415
14.5885,8.44835
15.0243,8.69352
15.4537,8.94951
15.8767,9.21615
16.2929,9.49329
16.7019,9.78074
17.1037,10.0783
17.4979,10.3859
17.8844,10.7032
18.2627,11.03
18.6328,11.3662
18.9943,11.7116
19.3471,12.0659
19.691,12.4289
20.0256,12.8003
20.3509,13.1801
20.6665,13.5678
20.9724,13.9633
21.2683,14.3663
21.554,14.7766
21.8294,15.194
22.0943,15.618
22.3485,16.0486
22.5918,16.4854
22.8241,16.9281
23.0454,17.3765
23.2553,17.8303
23.4538,18.2891
23.6408,18.7528
23.8161,19.2211
23.9797,19.6936
24.1314,20.17
24.2712,20.65
24.3989,21.1334
24.5145,21.6199
24.6179,22.109
24.709,22.6007
24.7878,23.0944
24.8543,23.5899
24.9083,24.087
24.9499,24.5853
24.979,25.0844
24.9957,25.5841
24.9998,26.0841
24.9915,26.584
24.9706,27.0835
24.9373,27.5824
24.8915,28.0803
24.8333,28.5769
24.7627,29.0719
24.6797,29.5649
24.5845,30.0557
24.477,30.544
24.3573,31.0295
24.2255,31.5118
24.0817,31.9907
23.926,32.4658
23.7585,32.9369
23.5792,33.4036
23.3883,33.8657
23.1859,34.3229
22.9722,34.7749
22.7472,35.2215
22.5112,35.6622
22.2642,36.0969
22.0064,36.5253
21.738,36.9472
21.4591,37.3621
21.1699,37.77
20.8707,38.1705
20.5615,38.5635
20.2425,38.9485
19.9141,39.3255
19.5763,39.6942
19.2295,40.0543
18.8737,40.4056
18.5093,40.7479
18.1364,41.081
17.7553,41.4046
17.3662,41.7187
16.9694,42.0229
16.5652,42.3171
16.1537,42.6011
15.7352,42.8747
15.31,43.1378
14.8784,43.3901
14.4406,43.6316
13.9969,43.8621
13.5476,44.0814
13.0929,44.2895
12.6332,44.486
12.1687,44.6711
11.6998,44.8444
11.2266,45.006
10.7496,45.1557
10.2689,45.2935
9.78498,45.4192
9.29806,45.5327
8.80845,45.634
8.31646,45.7231
7.8224,45.7999
7.32657,45.8642
6.82929,45.9162
6.33087,45.9557
5.83161,45.9827
5.33184,45.9972
4.83185,45.9993
4.33198,45.9888
3.83252,45.9659
3.33379,45.9305
2.8361,45.8826
2.33976,45.8223
1.84509,45.7496
1.35238,45.6646
0.861962,45.5672
0.374124,45.4577
-0.110824,45.336
-0.592573,45.2022
-1.07083,45.0564
-1.54529,44.8987
-2.01566,44.7291
-2.48165,44.5479
-2.94296,44.3551
-3.39931,44.1508
-3.85041,43.9352
-4.29598,43.7083
-4.73573,43.4704
-5.1694,43.2216
-5.59672,42.962
-6.01742,42.6918
-6.43123,42.4112
-6.83789,42.1203
-7.23716,41.8194
-7.62878,41.5085
-8.0125,41.188
-8.3881,40.858
-8.75533,40.5186
-9.11395,40.1703
-9.46376,39.813
-9.80453,39.4472
-10.1361,39.0729
-10.4581,38.6904
-10.7705,38.3
-11.073,37.902
-11.3655,37.4965
-11.6478,37.0838
-11.9197,36.6642
-12.181,36.2379
-12.4315,35.8052
-12.6712,35.3664
-12.8998,34.9217
-13.1172,34.4715
-13.3233,34.016
-13.518,33.5554
-13.701,33.0902
-13.8724,32.6205
-14.032,32.1467
-14.1797,31.669
-14.3155,31.1878
-14.4391,30.7033
-14.5506,30.2159
-14.6499,29.7259
-14.7369,29.2335
-14.8115,28.7391
-14.8738,28.2431
-14.9237,27.7456
-14.9611,27.247
-14.986,26.7476
-14.9985,26.2478
-14.9984,25.7478
-14.9859,25.248
-14.9608,24.7486
-14.9233,24.25
-14.8733,23.7525
-14.8109,23.2565
-14.7362,22.7621
-14.6491,22.2698
-14.5497,21.7797
-14.4381,21.2924
-14.3143,20.8079
-14.1785,20.3268
-14.0307,19.8491
-13.871,19.3753
-13.6995,18.9057
-13.5163,18.4404
-13.3215,17.9799
-13.1153,17.5245
-12.8978,17.0743
-12.6691,16.6297
-12.4293,16.1909
-12.1787,15.7583
-11.9173,15.3321
-11.6453,14.9125
-11.363,14.4999
-11.0704,14.0944
-10.7678,13.6965
-10.4553,13.3061
-10.1331,12.9238
-9.80155,12.5496
-9.4607,12.1838
-9.11081,11.8266
-8.7521,11.4783
-8.38479,11.1391
-8.00913,10.8091
-7.62533,10.4887
-7.23364,10.1779
-6.83431,9.87706
-6.42758,9.58627
-6.01371,9.30575
-5.59296,9.03565
-5.16558,8.77615
-4.73185,8.52742
-4.29204,8.28961
-3.84643,8.06287
-3.39528,7.84733
-2.93889,7.64315
-2.47753,7.45043
-2.0115,7.26931
-1.5411,7.09989
-1.0666,6.94229
-0.58831,6.79659
-0.106527,6.6629
0.378448,6.54129
0.866302,6.43185
1.35675,6.33463
1.84947,6.2497
2.34416,6.17712
2.84052,6.11693
3.33821,6.06916
3.83695,6.03385
4.33642,6.01101
4.8363,6.00067

画图结果如下：

非常规整的圆！！

今日理念：代码都是一层一层叠起来的，别看功能小，用的多了，高楼大厦平地起~

【原创】敲代码不易，走过路过给个关注~~~~~