圆弧等分点的坐标

最新推荐文章于 2023-10-03 10:33:37 发布

Dong冬先生

最新推荐文章于 2023-10-03 10:33:37 发布

阅读量1.9k

点赞数

分类专栏： c++ 图形学文章标签： c++ 几何学算法

原文链接：https://blog.csdn.net/yue597215286/article/details/114642049

版权

c++ 同时被 2 个专栏收录

2 篇文章 0 订阅

订阅专栏

图形学

1 篇文章 0 订阅

订阅专栏

轨迹规划中，常用到圆弧点的计算，已知一个轨迹点的坐标(x,y,theta)，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;
      
      
     
     

     
     
      
      
     
     
     
     
      
      
       
       }

输出结果为：

画图结果如下：

Dong冬先生

关注

0
点赞
踩
3

收藏

觉得还不错? 一键收藏
0
评论
圆弧等分点的坐标

轨迹规划中，常用到圆弧点的计算，已知一个轨迹点的坐标(x,y,theta)，theta是角度的弧度制表示，求这个点以R为半径，向前走len米的距离，产生的新数据点的坐标。接下来确认下输入输出：输入为：起点坐标x，y，theta，半径R，向前走len距离输出为：终点坐标：X,Y,Thetas #include <iostream>#include <math.h>#include...
复制链接

扫一扫

专栏目录