直线数据离散密化

最新推荐文章于 2022-06-14 17:21:09 发布

MetaSmiles

最新推荐文章于 2022-06-14 17:21:09 发布

阅读量406

点赞数 1

分类专栏： Robotics

本文链接：https://blog.csdn.net/weixin_39752599/article/details/83042086

版权

Robotics 专栏收录该内容

14 篇文章 5 订阅

订阅专栏

//*****************************************直线数据离散密化***************************************************************
   //先定义起始点和终点
   double start_point[3]={1,2,3};
    double end_point[3]={2,5,5};

   double error=0.01;

//直线相关参数
   //小臂长度
   double r=0.01;
   double length=sqrt((end_point[0]-start_point[0])*(end_point[0]-start_point[0])+(end_point[1]-start_point[1])*(end_point[1]-start_point[1])+(end_point[2]-start_point[2])*(end_point[2]-start_point[2]));
   double step_length= 2*sqrt(r*r-(r-error)*(r-error));
   double step_nums=ceil(length/step_length);
   step_length=length/step_nums;

   double direction[3]={(end_point[0]-start_point[0])*step_length/length,(end_point[1]-start_point[1])*step_length/length,(end_point[2]-start_point[2])*step_length/length};

   double middle_point[3]={0} ;

   remove("data.txt");
std::ofstream outf("data.txt",std::ios::app);
   if (!outf)
{
std::cout<<"can not open file "<<std::endl;
       return 0;
}

   for(int i= 0;i<=step_nums;i++){
       middle_point[0]=start_point[0]+i*direction[0];
       middle_point[1]=start_point[1]+i*direction[1];
       middle_point[2]=start_point[2]+i*direction[2];
       outf<<middle_point[0]<<" "<<middle_point[1]<<" "<<middle_point[2]<<std::endl;
   }

   outf.close();