C++最短距離解(2021年1月17日)

C++は1つの点から曲線までの最近の距離の解を実現する(遍歴法)

#include
#include
#include
#include
#include
#include
using namespace std;
int main ()
{
     
    //     ，         、  （           ）
    struct vechicleState
    {
     
        public:
            double x;
            double y;
            double yaw;
            double v;
    };

    struct vechicleState state={
     7.0,-3.0,0.0,0.0};
    vector<double> cx={
     };
    vector<double> cy={
     };
    for(int i=1;i<50;i++)
    {
     
        cx.push_back(i);
        cy.push_back((i/5.0)*i/2.0);
        
    }
    
    double L=0.0;
    double Lf;
    Lf=0.1*state.v+2.0;
    double min=abs(sqrt(pow(state.x-cx[10],2)+pow(state.y-cy[10],2)));
    cout<<min<<endl;
    int index=0;
    //                 
    for(int j=0;j<cx.size();j++)
    {
     
        double d=abs(sqrt(pow(state.x-cx[j],2)+pow(state.y-cy[j],2)));
        if(d<min)
        {
     
            min=d;
            index=j;   
        }
           
    }
    cout<<index<<endl;//           
    cout<<min<<endl;//       
    //           
    while(Lf>L && (index+1)<50)
    {
        
        double dx=cx[index+1]-cx[index];
        double dy=cy[index+1]-cy[index];
        L+=sqrt(pow(dx,2)+pow(dy,2));
        index+=1;

        
        
    }
    cout<<index<<endl;
}