人工势场法路径规划c语言程序

应用人工势场法，进行二维路径规划。上传的是整个解决方案，用vs2010可以直接打开，简单好用

#include 
#include 
#include
float k = 1;
float m = 15;
float w = 1;
float step = （float）1;
int iter = 2000;
float n = 8;

float r[8] = {751086455};
float Po[8];

float a = 2;
float Goal[2000][2]={0};
float Theta[9];
float Fat[2];
float Fatx;
float Faty;
float Freall[4];
float Frexsum;
float Freysum;
float Freaxsum;
float Freaysum;
float Fsumxj;
float Fsumyj;
float Theta_Fsum[1500];
float pi = （float）3.1415927;
float Xsum[9][2] = {{（float）100（float）100}{（float）25（float）30}{（float）48（float）22}{（float）50（float）29}{（float）45（float）52}{（float）57（float）53}
	{（float）80（float）65}{（float）70（float）70}{（float）93（float）97}};//终止点和障碍
float Xj[2] = {00};//起点
float ZH_angel（float A_xfloat A_yfloat B_xfloat B_y）;//计算角度
float ZH_Fat（float A_xfloat A_yfloat B_xfloat B_yfloat k）;//计算引力

int main （）//主函数
{
	float Xsum_x1;
	float Xsum_y1;
	float Xj_x1;
	float Xj_y1;
	float Xsum_x11;
	float Xsum_y11;
	float Po_i1;
	float Theta_i1;

	float Fre;
	float Frea;

	float Frex;
	float Frey;
	float Freax;
	float Freay;

	float rre;
	float rat;
	int j;

	float Xnext;
	float Ynext;
	int K;
	int Ki;

	Po[0] = （r[0]）*（float）（5）;//障碍影响区域
	Po[1] = （r[1]）*（float）（5）;
	Po[2] = （r[2]）*（float）（5）;
	Po[3] = （r[3]）*（float）（5）;
	Po[4] = （r[4]）*（float）（5）;
	Po[5] = （r[5]）*（float）（5）;
	Po[6] = （r[6]）*（float）（5）;
	Po[7] = （r[7]）*（float）（5）;

for （j=0;j{
	float Xsum_x;
	float Xsum_y;
	float Xj_x;
	float Xj_y;
	float Theta_i;
	int n = 8;
	int i;
	float Fat_i;
	float Fat[2] = {00};
	Goal[j][0] = Xj[0];
	Goal[j][1] = Xj[1];

	for （i=0;i<9;i++）//计算角度
	{
	    Xsum_x = Xsum[i][0];
	    Xsum_y = Xsum[i][1];
	    Xj_x = Xj[0];
	    Xj_y = Xj[1];

	    Theta_i = ZH_angel（Xsum_xXsum_yXj_xXj_y）;

	    Theta[i] = Theta_i;
    }

	for （i=0;i<1;i++）//计算引力
	{
		Xsum_x = Xsum[i][0];
		Xsum_y = Xsum[i][1];
		Xj_x = Xj[0];
		Xj_y = Xj[1];
		Theta_i = Theta[i];
		Fat_i = ZH_Fat（Xsum_xXsum_yXj_xXj_yk）;

		Fatx = Fat_i * cos（Theta_i）;
        Faty = Fat_i * sin（Theta_i）;
		Fat[0] = Fatx;
		Fat[1] = Faty;
	}

	Frexsum = 0;//对合力进行初始化
	Freysum = 0;
	Freaxsum = 0;
	Freaysum = 0;


	for （i=0;i<8;i++）//计算斥力
	{

		Xsum_x1 = Xsum[i+1][0];//障碍中心点
		Xsum_y1 = Xsum[i+1][1];
		Xj_x1 = Xj[0];//飞行器当前位置
		Xj_y1 = Xj[1];
		Xsum_x11 = Xsum[0][0];//终止点
		Xsum_y11 = Xsum[0][1];
		Po_i1 = Po[i];//斥力影响范围
	    Theta_i1 = Theta[i+1];//起始点与障碍点之间的角度


		rat = sqrt（（（（float）Xsum_x11-（float）Xj_x1）*（（float）Xsum_x11-（float）Xj_x1））+（（float）Xsum_y11-（float）Xj_y1）*（（float）Xsum_y11-（float）Xj_y1））;

		rre = sqrt（（（float）Xsum_x1-（float）Xj_x1）*（（float）Xsum_x1-（float）Xj_x1）+（（float）Xsum_y1-（float）Xj_y1）*（（float）Xsum_y1-（float）Xj_y1））;


		if（rre <= Po_i1）//在斥力影响范围内
		{
			Fre = m *（（float）（1/rre）-（float）（1/Po_i1））*（float）（（1/（rre*rre））） * （float）pow（（float）rat（float）a）;
            Frea = （float）（a/2） * m *（（（float）（1/rre）-（float）（1/Po

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2018-12-26 10:44  APF_test\
     目录           0  2018-12-26 10:44  APF_test\APFtest4.0\
     文件     1921024  2018-12-26 10:44  APF_test\APFtest4.0.sdf
     文件         897  2018-08-20 20:05  APF_test\APFtest4.0.sln
     文件       11776  2018-12-26 10:44  APF_test\APFtest4.0.suo
     文件        4877  2018-08-20 19:58  APF_test\APFtest4.0\APFtest4.0.cpp
     文件        2148  2018-08-17 10:25  APF_test\APFtest4.0\APFtest4.0.vcxproj
     文件         948  2018-08-17 10:25  APF_test\APFtest4.0\APFtest4.0.vcxproj.filters
     文件         143  2018-08-13 16:07  APF_test\APFtest4.0\APFtest4.0.vcxproj.user
     目录           0  2018-12-26 10:44  APF_test\APFtest4.0\Debug\
     文件        1363  2018-08-20 19:15  APF_test\APFtest4.0\Debug\APFtest4.0.Build.CppClean.log
     文件         381  2018-08-20 19:58  APF_test\APFtest4.0\Debug\APFtest4.0.exe.intermediate.manifest
     文件          90  2018-08-20 19:58  APF_test\APFtest4.0\Debug\APFtest4.0.lastbuildstate
     文件        2134  2018-08-20 19:58  APF_test\APFtest4.0\Debug\APFtest4.0.log
     文件       22133  2018-08-20 19:58  APF_test\APFtest4.0\Debug\APFtest4.0.obj
     文件         698  2018-08-20 19:58  APF_test\APFtest4.0\Debug\cl.command.1.tlog
     文件        1320  2018-08-20 19:58  APF_test\APFtest4.0\Debug\CL.read.1.tlog
     文件         558  2018-08-20 19:58  APF_test\APFtest4.0\Debug\CL.write.1.tlog
     文件        1458  2018-08-20 19:58  APF_test\APFtest4.0\Debug\link.command.1.tlog
     文件        2368  2018-08-20 19:58  APF_test\APFtest4.0\Debug\link.read.1.tlog
     文件         602  2018-08-20 19:58  APF_test\APFtest4.0\Debug\link.write.1.tlog
     文件         592  2018-08-20 19:58  APF_test\APFtest4.0\Debug\mt.command.1.tlog
     文件         874  2018-08-20 19:58  APF_test\APFtest4.0\Debug\mt.read.1.tlog
     文件         410  2018-08-20 19:58  APF_test\APFtest4.0\Debug\mt.write.1.tlog
     文件       44032  2018-08-20 19:58  APF_test\APFtest4.0\Debug\vc100.idb
     文件       69632  2018-08-20 19:58  APF_test\APFtest4.0\Debug\vc100.pdb
     目录           0  2018-12-26 10:44  APF_test\Debug\
     文件       17408  2018-08-20 19:58  APF_test\Debug\APFtest4.0.exe
     目录           0  2018-12-26 10:44  APF_test\ipch\
     目录           0  2018-12-26 10:44  APF_test\ipch\apftest4.0-a129446d\
     文件     2162688  2018-12-26 10:44  APF_test\ipch\apftest4.0-a129446d\apftest4.ipch
............此处省略0个文件信息