C++ 最短距离求解（2021年1月17日）_c++ 算出点到 一条运动轨迹最近的距离

C++实现一个点到曲线的最近距离求解（遍历法）

#include<iostream>
#include<cstring>
#include<stdlib.h>
#include<vector>
#include<algorithm>
#include<math.h>
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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61