Clarke & Wight saving algorithm求解tsp问题

//应用Clarke & Wight saving algorithm （c-w节约算法）求解旅行商问题（tsp）
//参考运筹学第三版pdf 17.2.2
#include<cstdio>
#include<cmath> 
#include<cstring>
double x[31]= {0},y[31]= {0};
long long d[500]= {0},e[500]= {0},s[31][61]= {0},route[31];
int i,j,h=1,n=0,l,o;
double a[31]= {0},b[31][31]= {0},c[31][31]= {0},f[500]= {0},k;
double sx,sy;//基点 
int posj1,posj2,posk1,posk2;
double sum=0;

double rad(double d) {	// 计算弧度
	const double PI=3.1415926535898;
	return d*PI/180.0;
}

double CalcDistance(double fLati1,double fLong1,double fLati2,double fLong2) {// 已知两个gps坐标点（纬度：fLati，经度：fLong）获得两点的直线距离，单位是千米
	const float EARTH_RADIUS=6378.137;
	double radLat1=rad(fLati1);
	double radLat2=rad(fLati2);
	double a=radLat1-radLat2;
	double b=rad(fLong1)-rad(fLong2);
	double s_=2*asin(sqrt(pow(sin(a/2),2)+cos(radLat1)*cos(radLat2)*pow(sin(b/2),2)));
	s_=s_*EARTH_RADIUS;
//	s = (int)(s*10000000) / 10000;//转换为m 
	return s_;//单位：km 
}

int main() {
	printf("C-W节约算法求解TSP问题 \n");
    printf("请输入坐标（30个以内），坐标之间用空格隔开，按回车键结束输入：\n");//坐标格式：(经度，纬度)
	scanf("%lf,%lf",&sx,&sy);
    while(~scanf("%lf,%lf",&x[h],&y[h])) {
		h++;
	}//h为总共点数（输入h-1个点）
//	printf("\nsx=%lf,sy=%lf\n",sx,sy);
//	printf("h=%d\n",h); 
//	for( i=1;i<h;++i) {
//		printf("x[%d]=%lf,y[%d]=%lf\n\n",i,x[i],i,y[i]);
//	}

    for(i=1; i<h; i++) {
		a[i]=CalcDistance(sy,sx,y[i],x[i]); 
//        printf("a[%d]=%lf\n",i,a[i]);
	}

    for(i=1; i<h-1; i++) {
        for(j=i+1; j<h; j++) {
            n++;
			b[i][j]=CalcDistance(y[i],x[i],y[j],x[j]); //b[][] 为距离矩阵
            c[i][j]=a[i]+a[j]-b[i][j];//c[][] 为添加边(i,j)的路程节约值
            d[n]=i;//d[]为索引n对应c[][]的横坐标 
            e[n]=j;//e[]为索引n对应c[][]的纵坐标 
            f[n]=c[i][j];//f[]为索引n对应的路程节约值   //每个n对应一个点对(i,j)且j>i,如(1,2)、(1,3)、...、(1,29)、(2,3)、(2,4)、...、(2,29)、...(28,29) 
//            printf("c[%d][%d]=%lf\n",i,j,c[i][j]); 
        }
    }
//    printf("b[][]:\n");
//    for(i=1;i<h;++i) {
//    	for(j=1;j<h;++j) {
//    		printf("%lf ",b[i][j]);
//		}
//		printf("\n");
//	}
//    printf("n=%d\n",n);n=h-2+h-3+...+1

    for(i=1; i<n; i++) {//按照f[]大小升序对d[]、e[]、f[]排序 
        for(j=1; j<n-i+1; j++)
            if(f[j]>f[j+1]) {
                k=f[j];
                f[j]=f[j+1];
                f[j+1]=k;
                l=d[j];
                d[j]=d[j+1];
                d[j+1]=l;
                o=e[j];
                e[j]=e[j+1];
                e[j+1]=o;
            }
    }
    printf("\n");
//    printf("节约值排序：\n");
//    for(i=n; i>0; i--) {
//        if(f[i]!=0){
//        	printf("%d,%d\n",d[i],e[i]);
//        	printf("%lf(%lf,%lf)--(%lf,%lf)\n",f[i],x[d[i]],y[d[i]],x[e[i]],y[e[i]]);
//		}
//    }
//    for(int i=n-1;i>0;--i) {
//    	printf("d[%d]=%d,e[%d]=%d\n",i,d[i],i,e[i]);

	for(i=1;i<h;++i) {//基点索引为0 s[][]每一行为一个环0->xxx->...->0 
		s[i][1]=0;//由基点发出 
		s[i][2]=i;//初始除基点外每个环只有1个点 
	}
// 	for(i=1; i<h; i++) {
//    	for(j=1;j<h;++j)
//    		if(s[i][j]!=0) {
//    			printf("s[%d][%d]=%ld ",i,j,s[i][j]);
//			}
//    	printf("\n");
//		for(j=1;j<h;++j)
//    		if(s[i][j]!=0) {
//    			printf("x[s[i][j]]=%lf,y[s[i][j]]=%lf",x[s[i][j]],y[s[i][j]]);;
//			}
//		printf("\n");       	
//    }
	for(i=n;i>0;--i) {//从n开始，对于每组点对贪心的选取点使其加入后较其他点节约值最大 
		for(j=1;j<h;++j) {
			for(int k=1;k<h;++k) {
				if(s[j][k]==0&&k!=1) break;
				if(s[j][k]==d[i]) {
					posj1=j;
					posk1=k;
				}
				if(s[j][k]==e[i]) {
					posj2=j;
					posk2=k;
				}
			}
		}
		if(posj1==posj2) {
			
		} else if(s[posj1][posk1-1]==0&&s[posj2][posk2+1]==0) {
			for(int l=1;l<=h;++l) {
				s[posj2][posk2+l]=s[posj1][posk1+l-1];
			}	
			memset(s[posj1],0,sizeof(s[posj1]));
		}else if(s[posj1][posk1+1]==0&&s[posj2][posk2-1]==0) {
			for(int l=1;l<=h;++l) {
				s[posj1][posk1+l]=s[posj2][posk2+l-1];
			}	
			memset(s[posj2],0,sizeof(s[posj2]));
		}else if(s[posj1][posk1+1]==0&&s[posj2][posk2+1]==0) {
			for(int l=1;l<=h&&posk2-l+1;++l) {
				s[posj1][posk1+l]=s[posj2][posk2-l+1];
			}	
			memset(s[posj2],0,sizeof(s[posj2]));
			sum-=f[i];
		}else if(s[posj1][posk1-1]==0&&s[posj2][posk2-1]==0) {
			int len=0;
			for(int l=1;l<=h;++l) {
				if(s[posj1][l]) 
					len++;
			}
			int t=0;
			int r=len-1+1;
			while(t<r){
				int temp;
				temp=s[posj1][t+1];
				s[posj1][t+1]=s[posj1][r+1];
				s[posj1][r+1]=temp;
				t++;
				r--;
			}
			for(int l=1;l<=h;++l) {
				s[posj1][posk1+l]=s[posj2][posk2+l-1];
			}	
			memset(s[posj2],0,sizeof(s[posj2]));
		}
		for(int i=1; i<=h; i++) {
	    	for(int j=1;j<=h;++j)
	    		if(s[i][j]!=0) {
	    			printf("s[%d][%d]=%ld  ",i,j,s[i][j]);
				}
	    	printf("\n");   	
	    } 
	}
//	for(i=1; i<h; i++) {
//    	for(j=1;j<h;++j)
//    		if(s[i][j]!=0) {
//    			printf("s[%d][%d]=%ld  ",i,j,s[i][j]);
//			}
//    	printf("\n");
//		for(j=1;j<h;++j)
//    		if(s[i][j]!=0) {
//    			printf("x[s[i][j]]=%lf,y[s[i][j]]=%lf  ",x[s[i][j]],y[s[i][j]]);;
//			}
//		printf("\n");       	
//    }
    printf("\n旅行商路线：\n");
    printf("数据中心(%lf,%lf)--",sx,sy);
    int num=0;
    for(i=1; i<=h; i++) {
    	for(j=1;j<=h;++j)
        	if(s[i][j]!=0) {
        		printf("传感器%d(%lf,%lf)--",s[i][j],x[s[i][j]],y[s[i][j]]);
        		route[++num]=s[i][j];//route[]为最终路线 
			}
    }
    printf("数据中心(%lf,%lf)",sx,sy);

    for(int i=1;i<num;++i) {
    	if(route[i]<route[i+1])
    		sum+=b[route[i]][route[i+1]];
    	else 
    		sum+=b[route[i+1]][route[i]];
	}
	sum+=a[route[1]]+a[route[num]]; 
    printf("总距离：%lf\n",sum);
	return 0;
}

//数据：经度，纬度 单位：度 
//第一个为数据中心坐标，后29个是传感器坐标 
//求从数据中心出发，经过所有点后回到数据中心的最短路线 
/* 
120.7015202,36.37423
120.6987175,36.37457569
120.6997954,36.37591239
120.70691,36.37579616
120.7056165,36.37248342
120.7031731,36.37753964
120.6928965,36.37800457
120.6943337,36.37521499
120.6973521,36.37876006
120.6962022,36.37643544
120.7011609,36.37905063
120.6939026,36.38021291
120.6983582,36.38056159
120.7025263,36.38120084
120.6914592,36.38201441
120.6960585,36.38247931
120.7005141,36.38276987
120.6998673,36.37079794
120.6928965,36.37079794
120.6964897,36.36824059
120.6969209,36.37143727
120.7052571,36.36899618
120.7088504,36.37021674
120.7087066,36.36731063
120.7130185,36.36829872
120.6896626,36.36661314
120.6937588,36.36242812
120.6993643,36.38741865
120.7129466,36.37201847
120.7002266,36.36428816
*/
//输入完按ctrl+z