2018-07-20 ワーシャル-フロイド法(応用) [長年日記]
/* gcc -g wf4.cpp -o wf4 ワーシャル-フロイド法(その4) 参考：ゼオスTTのブログ http://zeosutt.hatenablog.com/entry/2015/05/05/045943 */ #include <stdio.h> #include <math.h> #include <stdlib.h> typedef struct location{ // ちなみに X,Y 軸座標は、→に＋ ↑に＋ int num; char name[10]; double longitude; // 経度 東経 139.691 X軸 double latitude; // 緯度 北緯 35.698 Y軸 } LOCATION; LOCATION location[] = { {0,"A",35.896889, 139.966776}, {1,"B",35.911303, 139.950609}, {2,"C",35.902413, 139.950861}, {3,"D",35.905665, 139.945430}, {4,"E",35.891797, 139.959714}, {5,"F",35.888019, 139.949214}, {6,"G",35.887682, 139.945090}, {7,"H",35.900693, 139.938919}, {8,"I",35.898430, 139.932503}, {9,"J",35.899640, 139.919513}, {10,"K",35.893257, 139.934952}, {11,"L",35.892819, 139.943374}, {12,"M",35.887158, 139.922399}, {13,"N",35.893778, 139.952453} }; #define rad2deg(a) ((a)/M_PI * 180.0) /* rad を deg に換算するマクロ関数 */ #define deg2rad(a) ((a)/180.0 * M_PI) /* deg を rad に換算するマクロ関数 */ double distance_km(LOCATION* A, LOCATION* B, double *rad_up) { double earth_r = 6378.137; double loRe = deg2rad(B->longitude - A->longitude); // 東西 double laRe = deg2rad(B->latitude - A->latitude); // 南北 double EWD = cos(deg2rad(A->latitude))*earth_r*loRe; // 東西距離 double NSD = earth_r*laRe; //南北距離 double distance = sqrt(pow(NSD,2)+pow(EWD,2)); *rad_up = atan2(NSD, EWD); return distance; } LOCATION* new_location(LOCATION* D, double diff_p_x, double diff_p_y) { double earth_r = 6378.137; double loRe = diff_p_x / earth_r / cos(deg2rad(D->latitude)); // 東西 double laRe = diff_p_y / earth_r; // 南北 double diff_lo = rad2deg(loRe); // 東西 double diff_la = rad2deg(laRe); // 南北 static LOCATION C; C.longitude = D->longitude + diff_lo; // 東西 C.latitude = D->latitude + diff_la; // 南北 return &C; } double diff_longitude(double diff_p_x, double latitude) { double earth_r = 6378.137; // ↓ これが正解だけど、 double loRe = diff_p_x / earth_r / cos(deg2rad(latitude)); // 東西 // 面倒なので、これで統一しよう(あまり差が出ないしね) //double loRe = diff_p_x / earth_r / cos(deg2rad(35.700759)); // 東西 double diff_lo = rad2deg(loRe); // 東西 return diff_lo; // 東西 } double diff_latitude(double diff_p_y) { double earth_r = 6378.137; double laRe = diff_p_y / earth_r; // 南北 double diff_la = rad2deg(laRe); // 南北 return diff_la; // 南北 } double d[14][14]; // d[i][k]：ノードiからノードkへの距離 int next[14][14]; // ノードi から ノードj への最短経路における i の1つ後のノード int main(void) { // 変数の初期化 for(int i = 0; i < 14; i++){ for(int j = 0; j < 14; j++){ d[i][j] = 99999999.9; // 距離の初期化(でっかい値を入力しておく(INT_MAXは足し算の時に桁上がりが起こるので使わない) next[i][j] = j; // ノードi から ノードj への最短経路における i の1つ後のノード } } // 確認用の表示 printf("\n[STEP1]\n"); for(int i = 0; i < 14; i++){ for(int k = 0; k < 14; k++){ printf("d[%d][%d]):%f\t",i,k,d[i][k]); printf("next[%d][%d]):%d\n",i,k,next[i][k]); } } //// ここからは実際の距離を手書き for(int i = 0; i < 14; i++){ d[i][i] = 0; //// 同じノードへの距離は0になるので、上書き } double rad_up; d[0][1] = distance_km(&location[0], &location[1], &rad_up); d[0][4] = distance_km(&location[0], &location[4], &rad_up); d[1][0] = distance_km(&location[1], &location[0], &rad_up); d[1][3] = distance_km(&location[1], &location[3], &rad_up); d[2][3] = distance_km(&location[2], &location[3], &rad_up); d[2][4] = distance_km(&location[2], &location[4], &rad_up); d[2][13] = distance_km(&location[2], &location[13], &rad_up); d[3][1] = distance_km(&location[3], &location[1], &rad_up); d[3][2] = distance_km(&location[3], &location[2], &rad_up); d[3][7] = distance_km(&location[3], &location[7], &rad_up); d[4][0] = distance_km(&location[4], &location[0], &rad_up); d[4][2] = distance_km(&location[4], &location[2], &rad_up); d[4][13] = distance_km(&location[4], &location[13], &rad_up); d[5][4] = distance_km(&location[5], &location[4], &rad_up); d[5][6] = distance_km(&location[5], &location[6], &rad_up); d[5][13] = distance_km(&location[5], &location[13], &rad_up); d[6][5] = distance_km(&location[6], &location[5], &rad_up); d[6][11] = distance_km(&location[6], &location[11], &rad_up); d[6][12] = distance_km(&location[6], &location[12], &rad_up); d[7][3] = distance_km(&location[7], &location[3], &rad_up); d[7][8] = distance_km(&location[7], &location[8], &rad_up); d[7][11] = distance_km(&location[7], &location[11], &rad_up); d[8][7] = distance_km(&location[8], &location[7], &rad_up); d[8][9] = distance_km(&location[8], &location[9], &rad_up); d[8][10] = distance_km(&location[8], &location[10], &rad_up); d[9][8] = distance_km(&location[9], &location[8], &rad_up); d[10][8] = distance_km(&location[10], &location[8], &rad_up); d[10][11] = distance_km(&location[10], &location[11], &rad_up); d[11][6] = distance_km(&location[11], &location[6], &rad_up); d[11][7] = distance_km(&location[11], &location[7], &rad_up); d[11][10] = distance_km(&location[11], &location[10], &rad_up); d[12][6] = distance_km(&location[12], &location[6], &rad_up); d[13][2] = distance_km(&location[13], &location[2], &rad_up); d[13][4] = distance_km(&location[13], &location[4], &rad_up); d[13][5] = distance_km(&location[13], &location[5], &rad_up); // 確認用の表示 printf("\n[STEP2]\n"); for(int i = 0; i < 14; i++){ for(int k = 0; k < 14; k++){ printf("d[%d][%d]):%f\t",i,k,d[i][k]); printf("next[%d][%d]):%d\n",i,k,next[i][k]); } } // 経路長計算 for (int k =0; k < 14; k++){ for (int i =0; i < 14; i++){ for(int j = 0; j < 14; j++){ if(d[i][j] > d[i][k] + d[k][j]){ d[i][j] = d[i][k] + d[k][j]; next[i][j] = next[i][k]; //更新処理 } } } } // 計算結果 printf("\n[STEP3]\n"); for(int i = 0; i < 14; i++){ for(int k = 0; k < 14; k++){ printf("d[%d][%d]):%f\t",i,k,d[i][k]); printf("next[%d][%d]):%d\n",i,k,next[i][k]); } } // 経路パス表示 printf("\n[Path]\n"); for(int i = 0; i < 14; i++){ for(int k = 0; k < 14; k++){ int cur; printf("d[%d][%d]: ",i,k); for (cur = i; cur != k; cur = next[cur][k]){ printf("%d -> ", cur); } printf("%d\n", k); } } }
[ツッコミを入れる]