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)
}
}
