postGISを使ったシミュレータのテストプログラム(既出)「とりあえず、適当な地図を作って、自動車を走らせよう」

2022年5月6日

http://kobore.net/test1031-2.cpp

/////////////////////////////////////////
// 
//
// 適当な2つの座標を指定して、そのルートをトレースする
// 
//  ST_Distance, ST_DWithinの使い方
//



/*

gcc -g -I"D:\PostgreSQL\pg96\include" test1031-2.cpp -o test1031-2.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 PERSON{  
  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 (){

  PERSON test_person;

  // 適当な2つの座標
  double dep_x = 139.46507, dep_y = 35.59577;
  double arr_x = 139.47627, arr_y = 35.60358;

  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));
  }
  
  /////////////////////// startのもっとも近いノード番号を検索する ////////////////////////
  
  char stringSQL[1024] = {0};  
  
  // 半径300メートルくらいで検索
  sprintf(stringSQL,"SELECT source, ST_Distance('SRID=4326;POINT(%f %f)'::GEOGRAPHY, the_geom) as dist FROM ways WHERE ST_DWithin(the_geom, ST_GeographyFromText('SRID=4326;POINT(%f %f)'), 300.0) ORDER BY dist;", dep_x, dep_y, dep_x, dep_y);
  
  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);
  
  // そして行を表示する。
  int i_count = 0;
  
  while( atof(PQgetvalue(res, i_count, 1)) == 0.0){
	i_count += 1;
  }
  
  double dist = atof(PQgetvalue(res, i_count, 1));
  int start_source = atoi(PQgetvalue(res, i_count, 0));
  
  PQclear(res); // SQL文の発行に対して必ずクリアする
  
  /////////////////////// startとstartのもっとも近いノードの距離を計測する ////////////////////////  
  
  memset( stringSQL, 0, sizeof(stringSQL)); // 念の為クリア

  sprintf(stringSQL,"SELECT ST_Distance('SRID=4326;POINT(%f %f)'::GEOGRAPHY, the_geom) from ways where source = %d;", dep_x, dep_y, start_source );

  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);
  }
  
  nFields = PQnfields(res);

  double dep_dist = atof(PQgetvalue(res, 0, 0));  

  PQclear(res); // SQL文の発行に対して必ずクリアする


  ////////////////////// endのもっとも近いノード番号を検索する///////////////////////////

  memset( stringSQL, 0, sizeof(stringSQL)); // 念の為クリア
  
  // 半径300メートルくらいで検索
  sprintf(stringSQL,"SELECT source, ST_Distance('SRID=4326;POINT(%f %f)'::GEOGRAPHY, the_geom) as dist FROM ways WHERE ST_DWithin(the_geom, ST_GeographyFromText('SRID=4326;POINT(%f %f)'), 300.0) ORDER BY dist;", arr_x, arr_y, arr_x, arr_y);

  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);
  }
  
  nFields = PQnfields(res);
  
  // そして行を表示する。
  i_count = 0;
  
  while( atof(PQgetvalue(res, i_count, 1)) == 0.0){
	i_count += 1;
  }
  
  dist = atof(PQgetvalue(res, i_count, 1));
  int end_source = atoi(PQgetvalue(res, i_count, 0));
  
  PQclear(res); // SQL文の発行に対して必ずクリアする

  /////////////////////// endとendのもっとも近いノードの距離を計測する ////////////////////////  
  
  memset( stringSQL, 0, sizeof(stringSQL)); // 念の為クリア

  sprintf(stringSQL,"SELECT ST_Distance('SRID=4326;POINT(%f %f)'::GEOGRAPHY, the_geom) from ways where source = %d;", arr_x, arr_y, end_source );

  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);
  }
  
  nFields = PQnfields(res);

  double arr_dist = atof(PQgetvalue(res, 0, 0));  

  PQclear(res); // SQL文の発行に対して必ずクリアする

  printf("start_source:%d end_source:%d\n",start_source, end_source);
  printf("dep_dist:%f arr_dist:%f\n",dep_dist, arr_dist);


  
  ////////////////// start_sourceとend_sourceのノードをダイクストラで計算する //////////////////////

  printf("%-15f,%-15f\n",dep_x, dep_y); // 出発点を書き込んでおく

  memset( stringSQL, 0, sizeof(stringSQL)); // 念の為クリア

  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 );",start_source, end_source);

  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);
  }
  
  nFields = PQnfields(res);

  // start_sourceとend_sourceのノードのノード番号の一つをゲットする処理
  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_person.p_x = start_x;
	test_person.p_y = start_y;
	
	// ここから0.1m単位でグルグル回す

	int do_flag = 0;
	do{
	  // printf("x=%-15f,y=%-15f\n",test_person.p_x, test_person.p_y);
	  printf("%-15f,%-15f\n",test_person.p_x, test_person.p_y);
	  
	  // 以下の0.1は1サンプリング0.1メートルを擬制
	  test_person.p_x += diff_longitude(0.1 * cos(rad_up1), test_person.p_y);	
	  test_person.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_person.p_y),(end_x - test_person.p_x));
	  
	  // ここは、http://kobore.net/over90.jpg で解説してある
	  
	  if (fabs(rad0 - rad1) >= 3.141592654 * 0.5){
		// 終点越えの場合、終点に座標を矯正する
		test_person.p_x = end_x;
		test_person.p_y = end_y;
		
		do_flag = 1; // フラグを上げろ
	  }
	  
	}while(do_flag == 0);
	
  } // バス停とバス停の間のノードを全部出す
  
  PQclear(res); // SQL文の発行に対して必ずクリアする
  
  printf("%-15f,%-15f\n",arr_x, arr_y); // 終着点を書き込んでおく

}

上記のプログラムと同じような振舞をするプログラムをGolangで書いてみました。

// go get github.com/lib/pq を忘れずに
// go run main7.go

/*
	前提は、https://ebata-books.booth.pm/items/3351408 の環境

	(1)スタート地点  139.91804656152837 36.57246810341353,
	(2)ゴール地点 139.93515104919825 36.55883778950532
	(3)とした時、この2点に最も近いノードを探し出して、
	(4)その2点をダイクストラ法で、最短距離探索をするプログラム

	分かったこと
	上記(3)のノードが、孤立ノードになっている場合があり、この場合(4)が実施できないので、
	ダイクストラに失敗する場合は、別のノードでトライアルする必要あり
	(このコードでは展開しないけど)
*/

package main

import (
	"database/sql"
	"fmt"
	"log"
	"math"

	_ "github.com/lib/pq"
)

var source int
var longitude float64
var latitude float64
var dist float64

func rad2deg(a float64) float64 {
	return a / math.Pi * 180.0
}

func deg2rad(a float64) float64 {
	return a / 180.0 * math.Pi
}

func distance_km(a_longitude, a_latitude, b_longitude, b_latitude float64) (float64, float64) {
	earth_r := 6378.137

	loRe := deg2rad(b_longitude - a_longitude) // 東西  経度は135度
	laRe := deg2rad(b_latitude - a_latitude)   // 南北  緯度は34度39分

	EWD := math.Cos(deg2rad(a_latitude)) * earth_r * loRe // 東西距離
	NSD := earth_r * laRe                                 //南北距離

	distance_km := math.Sqrt(math.Pow(NSD, 2) + math.Pow(EWD, 2))
	rad_up := math.Atan2(NSD, EWD)

	return distance_km, rad_up
}

func diff_longitude(diff_p_x, latitude float64) float64 {

	earth_r := 6378.137
	// ↓ これが正解だけど、
	loRe := diff_p_x / earth_r / math.Cos(deg2rad(latitude)) // 東西
	// 面倒なので、これで統一しよう(あまり差が出ないしね)
	//loRe := diff_p_x / earth_r / math.Cos(deg2rad(35.700759)) // 東西
	diff_lo := rad2deg(loRe) // 東西

	return diff_lo // 東西
}

func diff_latitude(diff_p_y float64) float64 {
	earth_r := 6378.137
	laRe := diff_p_y / earth_r // 南北
	diff_la := rad2deg(laRe)   // 南北

	return diff_la // 南北
}

func sub_main() {

	x1, y1 := 139.91804656152837, 36.57246810341353
	x2, y2 := 139.93515104919825, 36.55883778950532

	d_km, rad_up := distance_km(x1, y1, x2, y2)

	//x1 += diff_longitude(0.00566*math.Cos(rad_up), y1)
	//y1 += diff_latitude(0.00566 * math.Sin(rad_up))

	x1 += diff_longitude(1.0*math.Cos(rad_up), y1)
	y1 += diff_latitude(1.0 * math.Sin(rad_up))

	fmt.Println(d_km, rad_up)
	fmt.Println(x1, y1)

	d_km, rad_up = distance_km(x1, y1, x2, y2)

	//x1 += diff_longitude(0.00566*math.Cos(rad_up), y1)
	//y1 += diff_latitude(0.00566 * math.Sin(rad_up))

	x1 += diff_longitude(1.0*math.Cos(rad_up), y1)
	y1 += diff_latitude(1.0 * math.Sin(rad_up))

	fmt.Println(d_km, rad_up)
	fmt.Println(x1, y1)

}

func main() {
	db, err := sql.Open("postgres",
		"user=postgres password=password host=localhost port=15432 dbname=utsu_db sslmode=disable")
	if err != nil {
		log.Fatal("OpenError: ", err)
	}
	defer db.Close()

	// スタート地点  139.91804656152837 36.57246810341353
	rows, err := db.Query("SELECT source, x1 as longitude, y1 as latitude, ST_Distance('SRID=4326;POINT(139.91804656152837 36.57246810341353)'::GEOGRAPHY, the_geom) as dist FROM ways WHERE ST_DWithin(the_geom, ST_GeographyFromText('SRID=4326;POINT(139.91804656152837 36.57246810341353)'), 300.0) ORDER BY dist LIMIT 1")
	if err != nil {
		log.Fatal(err)
	}
	defer rows.Close()

	for rows.Next() {

		if err := rows.Scan(&source, &longitude, &latitude, &dist); err != nil {
			fmt.Println(err)
		}
		fmt.Println(source, longitude, latitude, dist)
	}

	o_source := source

	// ゴール地点 139.93515104919825 36.55883778950532
	rows, err = db.Query("SELECT source, x1 as longitude, y1 as latitude, ST_Distance('SRID=4326;POINT(139.93515104919825 36.55883778950532)'::GEOGRAPHY, the_geom) as dist FROM ways WHERE ST_DWithin(the_geom, ST_GeographyFromText('SRID=4326;POINT(139.93515104919825 36.55883778950532)'), 300.0) ORDER BY dist LIMIT 1")
	if err != nil {
		log.Fatal(err)
	}
	defer rows.Close()

	for rows.Next() {

		if err := rows.Scan(&source, &longitude, &latitude, &dist); err != nil {
			fmt.Println(err)
		}
		fmt.Println(source, longitude, latitude, dist)
	}

	d_source := source

	fmt.Println(o_source, d_source)

	//rows, err = db.Query("select x1,y1 from ways where source in (SELECT node as source FROM pgr_dijkstra('SELECT gid as id, source, target,length_m as cost FROM ways', $1::bigint , $2::bigint , false))", o_source, d_source)
	rows, err = db.Query("SELECT node as source FROM pgr_dijkstra('SELECT gid as id, source, target,length_m as cost FROM ways', $1::bigint , $2::bigint , false)", o_source, d_source)
	if err != nil {
		log.Fatal(err)
	}
	defer rows.Close()

	fmt.Println("2")

	x1, y1 := -1.0, -1.0
	_x1, _y1, _x2, _y2 := -1.0, -1.0, -1.0, -1.0
	px, py := -1.0, -1.0
	flag := 0
	f_flag := 0

	for rows.Next() {

		if err := rows.Scan(&source); err != nil {
			fmt.Println(err)
		}

		rows2, err := db.Query("SELECT source, x1, y1 from ways where source = $1::bigint limit 1", source)
		if err != nil {
			log.Fatal(err)
		}

		for rows2.Next() {

			if f_flag == 0 { //初回だけ2居合入力
				if err := rows2.Scan(&source, &x1, &y1); err != nil {
					fmt.Println(err)
				}
				_x1, _y1 = x1, y1
				//fmt.Println(_x1, _y1)
				f_flag = 1
				continue
			}

			if err := rows2.Scan(&source, &x1, &y1); err != nil {
				fmt.Println(err)
			}
			_x2, _y2 = x1, y1

			//fmt.Println(_x2, ",", _y2)
			_, rad_up := distance_km(_x1, _y1, _x2, _y2)
			//fmt.Println("_x1,_y1,_x2,_y2, rad_up:", _x1, _y1, _x2, _y2, rad_up)

			px, py = _x1, _y1

			for {

				//平均時速20km サンプリング時間は、3.6秒  平均速度は 5.56m 20m/3.6秒
				// 悩むところだな、サンプリング時間を1秒にするか3.6秒にするか → 1秒にするか。
				// 距離端数は切り捨てにして、実質は、平均時速を20km以下としよう
				// (カーブでは減速する、と)

				px += diff_longitude(0.00556*math.Cos(rad_up), py)
				py += diff_latitude(0.00556 * math.Sin(rad_up))

				//double rad0 = atan2((end_y - start_y),(end_x - start_x));
				//double rad1 = atan2((end_y - test_person.p_y),(end_x - test_person.p_x));

				rad0 := math.Atan2((_y2 - _y1), (_x2 - _x1))
				rad1 := math.Atan2((_y2 - py), (_x2 - px))
				// ここは、http://kobore.net/over90.jpg で解説してある

				if math.Abs(rad0-rad1) >= math.Pi*0.5 {
					// 終点越えの場合、終点に座標を矯正する
					px, py = _x2, _y2
					flag = 1 // フラグを上げろ
				}

				fmt.Println(px, ",", py)

				if flag == 1 {
					flag = 0
					_x1, _y1 = _x2, _y2
					break
				}
			}
		}
	}

	//平均時速20km サンプリング時間は、3.6秒  平均速度は 5.56m 20m/3.6秒
	// 悩むところだな、サンプリング時間を1秒にするか3.6秒にするか → 1秒にするか。
	// 距離端数は切り捨てにして、実質は、平均時速を20km以下としよう

	if err := db.Ping(); err != nil {
		log.Fatal("PingError: ", err)
	}
}

2022年5月6日2022/04,江端さんの技術メモ

Posted by ebata