///////////////////////////////////////// // // // postGISを使って、自動車を走らせてみよう // // // /* gcc -g -I"D:\PostgreSQL\pg96\include" test1031.cpp -o test1031.exe -L"D:\PostgreSQL\pg96\lib" -llibpq -lwsock32 -lws2_32 */ #include <stdio.h> #include <stdlib.h> #include <stdint.h> #include <string.h> #include <sys/types.h> #include <math.h> #include "libpq-fe.h" struct BUS{ double p_x; // 現在のX座標 double p_y; // 現在のY座標 }; #define rad2deg(a) ((a)/M_PI * 180.0) /* rad を deg に換算するマクロ関数 */ #define deg2rad(a) ((a)/180.0 * M_PI) /* deg を rad に換算するマクロ関数 */ double distance_km(double a_longitude, double a_latitude, double b_longitude, double b_latitude, double *rad_up) { double earth_r = 6378.137; double loRe = deg2rad(b_longitude - a_longitude); // 東西 経度は135度 double laRe = deg2rad(b_latitude - a_latitude); // 南北 緯度は34度39分 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; } 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; // 南北 } int main (){ // postGISとQGISで調べたノード番号(強制停車するバス停を擬制) int bus_stop_num[] = {115,104, 3, 62, 277,48, 213, 208, -1}; const char *conninfo = "host=localhost user=postgres password=c-anemone dbname=city_routing"; // データベースとの接続を確立する PGconn *conn = PQconnectdb(conninfo); PGresult *res; // バックエンドとの接続確立に成功したかを確認する if (PQstatus(conn) != CONNECTION_OK){ fprintf(stderr, "Connection to database failed: %s", PQerrorMessage(conn)); } int cnt = 0; int initial_flag = 1; BUS test_bus; // バスの生成 while (bus_stop_num[cnt] != -1){ // バス停単位の開始 // printf("%d\n", bus_stop_num[cnt]); // SQL文字列格納用の配列 char stringSQL[1024] = {0}; // バス停とバス停の間のノードをダイクストラで計算する(node(source)とedge(gid)を取る) sprintf(stringSQL, "SELECT node, edge FROM pgr_dijkstra('SELECT gid as id, source, target, cost_s As cost, reverse_cost_s AS reverse_cost FROM ways', %d, %d, true );",bus_stop_num[cnt], bus_stop_num[cnt+1]); res = PQexec(conn, stringSQL); if (res == NULL){ fprintf(stderr, "failed: %s", PQerrorMessage(conn)); } // SELECTの場合戻り値は PGRES_TUPLES_OK. これを確認しておく if (PQresultStatus(res) != PGRES_TUPLES_OK){ PQerrorMessage(conn); } int nFields = PQnfields(res); // バス停からバス停の間のノード番号の一つをゲットする処理 for (int i = 0; i < PQntuples(res)-1 ; i++) { // バス停とバス停の間のノードを全部出す /* ここでは、両端のエッジの座標が必要になる。 pgr_dijkstraでは、エッジ(両端)情報(x1,y1,x2,y2)が取得できるのだけど、 どっちが先端でどっちが終端かが分からない。 そこで(恐しく迂遠だけえど)、まずエッジ情報(x1,y1,x2,y2)を得てから、 ノード情報(x1, y1)を取得する(ちなみにノード情報のx2,y2は、 交差点などの場合は複数出てくるので、信用してはならない)。 で、ノード情報のx1,y1を先端として、エッジ情報のx1,y1とx2,y2と一致していない方を終端とする という処理を取っている。 (もっと簡単な方法があったら、誰か教えて下さい) */ double node[2] = {0.0}; // x1, y1 double edge[4] = {0.0}; // x1, y1, x2, y2 int dummy_int_1 = 0; int dummy_int_2 = 0; for (int j = 0; j < nFields; j++) { // (j=0:node(source)と j=1:edge(gid)の値をゲットする) // まずノードの方 if (j == 0){//(j=0:node(source) memset( stringSQL, 0, sizeof(stringSQL)); // 念の為クリア sprintf(stringSQL, "SELECT x1,y1 from ways where source = %s;", PQgetvalue(res, i, j)); // ノードの座標を得る dummy_int_1 = atof(PQgetvalue(res, i, j)); PGresult *res2 = PQexec(conn, stringSQL); if (res2 == NULL){ fprintf(stderr, "failed: %s", PQerrorMessage(conn)); } int nFields2 = PQnfields(res2); for (int j2 = 0; j2 < nFields2; j2++) { // node(source)のx1,y1の2つの値 // printf("%-15s", PQgetvalue(res2, 0, j2)); node[j2] = atof(PQgetvalue(res2, 0, j2)); } PQclear(res2); // SQL文の発行に対して必ずクリアする } //次にエッジの方 if (j == 1){//(j=1:edge(gid) memset( stringSQL, 0, sizeof(stringSQL)); // 念の為クリア sprintf(stringSQL, "SELECT x1,y1,x2,y2 from ways where gid = %s;", PQgetvalue(res, i, j)); // ノードの座標を得る dummy_int_2 = atof(PQgetvalue(res, i, j)); PGresult *res2 = PQexec(conn, stringSQL); if (res2 == NULL){ fprintf(stderr, "failed: %s", PQerrorMessage(conn)); } int nFields2 = PQnfields(res2); for (int j2 = 0; j2 < nFields2; j2++) { // node(source)のx1,y1の2つの値 // printf("%-15s", PQgetvalue(res2, 0, j2)); edge[j2] = atof(PQgetvalue(res2, 0, j2)); } PQclear(res2); // SQL文の発行に対して必ずクリアする } } double start_x, start_y, end_x, end_y; //出揃ったところで、始点と終点の判定を行う if ((fabs(node[0] - edge[0]) < 1e-6) && (fabs(node[1] - edge[1]) < 1e-6)){ start_x = edge[0]; start_y = edge[1]; end_x = edge[2]; end_y = edge[3]; }else{ start_x = edge[2]; start_y = edge[3]; end_x = edge[0]; end_y = edge[1]; } // printf("両端の確定 %f,%f,%f,%f\n", start_x, start_y, end_x, end_y); //両端の進行方向(ベクトル)を計算する double rad_up1; distance_km(start_x, start_y, end_x, end_y, &rad_up1); // 確定直後にバス位置は、出発点(start_x, start_y)に強制移動する test_bus.p_x = start_x; test_bus.p_y = start_y; // ここから0.1m単位でグルグル回す int do_flag = 0; do{ // printf("x=%-15f,y=%-15f\n",test_bus.p_x, test_bus.p_y); printf("%-15f,%-15f\n",test_bus.p_x, test_bus.p_y); // 以下の0.1は1サンプリング0.1メートルを擬制 test_bus.p_x += diff_longitude(0.1 * cos(rad_up1), test_bus.p_y); test_bus.p_y += diff_latitude(0.1 * sin(rad_up1)); double rad0 = atan2((end_y - start_y),(end_x - start_x)); double rad1 = atan2((end_y - test_bus.p_y),(end_x - test_bus.p_x)); // ここは、http://kobore.net/over90.jpg で解説してある if (fabs(rad0 - rad1) >= 3.141592654 * 0.5){ // 終点越えの場合、終点に座標を矯正する test_bus.p_x = end_x; test_bus.p_y = end_y; do_flag = 1; // フラグを上げろ } }while(do_flag == 0); } // バス停とバス停の間のノードを全部出す PQclear(res); // SQL文の発行に対して必ずクリアする cnt += 1; // カウントアップ }// バス停単位の開始 }