#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）
????????{
?????????

?屬性????????????大小?????日期????時(shí)間???名稱(chēng)
-----------?---------??----------?-----??----
?????文件????????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