直线数据离散密化

//*****************************************直线数据离散密化***************************************************************
    //先定义起始点和终点
    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();