人工势场法C++版及利用opencv（or matlab）实现可视化

最近在学路径规划，学到人工势场法，在参考了matlab版(中文注释)的程序后，自己写了个C++的版本，并利用opencv画出规划好的路径，对于没安装opencv的朋友，也可以把路径打印出来，复制到matlab的命令行窗口，利用plot来查看。里面有５个文件，compute_angle.m，compute_Attract.m，compute_repulsion.m，main.m和C++版的APF.cpp

#include 
#include 
#include 
#include 
#include  
using namespace std;
using namespace cv;
double obs[14]={11.232.544.536625.55.588.5};
double sum（double *p1int n）;
class APF
{
public:
    double Attraction_K;
    double Repulsion_K;
    double Obstacles_dis;
    double a;
    double step;
    double *start_point;
    double *goal_point;
    double *obstacles;
    void APF_init（double Attraction_Kdouble Repulsion_Kdouble Obstacles_dis double adouble stepdouble *start_pointdouble *goal_pointdouble *obstacles）
    {
        this->Attraction_K=Attraction_K;
        this->Repulsion_K=Repulsion_K;
        this->Obstacles_dis=Obstacles_dis;
        this->a=a;
        this->step=step;
        this->start_point=start_point;
        this->goal_point=goal_point;
        this->obstacles=obstacles;
    }
    double * compute_angle（double *start_pointint n）;
    double *compute_attraction（double *start_pointdouble *att_angle）;
    double *compute_repulsion（double *start_pointdouble *angleint n）;
};
double * APF::compute_angle（double *start_pointint n）
{
//     static double Y[8];
    double *Y=new double[n+1];
    double deltaxdeltayr;
    for（int i=0;i    {
        if（i!=0）
        {
            deltax=this->obstacles[（i-1）*2]-start_point[0];
            deltay=this->obstacles[（i-1）*2+1]-start_point[1];
        }
        else
        {
            deltax=this->goal_point[0]-start_point[0];
            deltay=this->goal_point[1]-start_point[1];
        }
        r=sqrt（deltax*deltax+deltay*deltay）;
        if（deltay>0）
            Y[i]=acos（deltax/r）;
        else
            Y[i]=-acos（deltax/r）;
    }
    return Y;
};
double *APF::compute_attraction（double *start_pointdouble *att_angle）
{
    double R=（this->goal_point[0]-start_point[0]）*（this->goal_point[0]-start_point[0]）+（this->goal_point[1]-start_point[1]）*（this->goal_point[1]-start_point[1]）;
    double r=sqrt（R）;
    static double Yatt[2];
    Yatt[0]=this->Attraction_K*r*cos（att_angle[0]）;
    Yatt[1]=this->Attraction_K*r*sin（att_angle[0]）;
    return Yatt;
}
double *APF::compute_repulsion（double *start_pointdouble *angleint n）
{
    double *YY=new double[4];
    double Rat=（start_point[0]-this->goal_point[0]）*（start_point[0]-this->goal_point[0]）+（start_point[1]-this->goal_point[1]）*（start_point[1]-this->goal_point[1]）;
    double rat=sqrt（Rat）;
    double RrerreYrerYata;
    double *Yrerx=new double[n]*Yrery=new double[n]*Yatax=new double[n]*Yatay=new double[n];
    for（int i=0;i    {
        Rre=（start_point[0]-this->obstacles[i*2+0]）*（start_point[0]-this->obstacles[i*2+0]）+（start_point[1]-this->obstacles[i*2+1]）*（start_point[1]-this->obstacles[i*2+1]）;
        rre=sqrt（Rre）;
        if（rre>this->Obstacles_dis）
        {
            Yrerx[i]=0;
            Yrery[i]=0;
            Yatax[i]=0;
            Yatay[i]=0;
        }
        else if（rre>this->Obstacles_dis/2）
        {
         

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件        1285  2018-04-18 13:17  compute_angle.m
     文件         922  2018-04-18 13:17  compute_Attract.m
     文件        3323  2018-04-18 13:17  compute_repulsion.m
     文件        8186  2018-04-18 13:18  main.m
     文件        6531  2018-04-18 13:40  APF.cpp