C++ 最短距离求解（2021年1月17日）

最新推荐文章于 2024-04-28 12:12:08 发布

原创最新推荐文章于 2024-04-28 12:12:08 发布 · 1k 阅读

3 ·

CC 4.0 BY-SA版权

文章标签：

#算法 #c++

puresuit C++实现专栏收录该内容

2 篇文章

订阅专栏

本文介绍了一个使用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;
}