http://kobore.net/test1031-2.cpp
#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;
double p_y;
};
#define rad2deg(a) ((a)/M_PI * 180.0)
#define deg2rad(a) ((a)/180.0 * M_PI)
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);
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;
}
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 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;
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));
}
char stringSQL[1024] = {0};
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));
}
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);
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));
}
if (PQresultStatus(res) != PGRES_TUPLES_OK){
PQerrorMessage(conn);
}
nFields = PQnfields(res);
double dep_dist = atof(PQgetvalue(res, 0, 0));
PQclear(res);
memset( stringSQL, 0, sizeof(stringSQL));
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));
}
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);
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));
}
if (PQresultStatus(res) != PGRES_TUPLES_OK){
PQerrorMessage(conn);
}
nFields = PQnfields(res);
double arr_dist = atof(PQgetvalue(res, 0, 0));
PQclear(res);
printf("start_source:%d end_source:%d\n",start_source, end_source);
printf("dep_dist:%f arr_dist:%f\n",dep_dist, arr_dist);
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));
}
if (PQresultStatus(res) != PGRES_TUPLES_OK){
PQerrorMessage(conn);
}
nFields = PQnfields(res);
for (int i = 0; i < PQntuples(res)-1 ; i++) {
double node[2] = {0.0};
double edge[4] = {0.0};
int dummy_int_1 = 0;
int dummy_int_2 = 0;
for (int j = 0; j < nFields; j++) {
if (j == 0){
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[j2] = atof(PQgetvalue(res2, 0, j2));
}
PQclear(res2);
}
if (j == 1){
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++) {
edge[j2] = atof(PQgetvalue(res2, 0, j2));
}
PQclear(res2);
}
}
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];
}
double rad_up1;
distance_km(start_x, start_y, end_x, end_y, &rad_up1);
test_person.p_x = start_x;
test_person.p_y = start_y;
int do_flag = 0;
do{
printf("%-15f,%-15f\n",test_person.p_x, test_person.p_y);
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));
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);
printf("%-15f,%-15f\n",arr_x, arr_y);
}
上記のプログラムと同じような振舞をするプログラムをGolangで書いてみました。
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)
laRe := deg2rad(b_latitude - a_latitude)
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))
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(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(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()
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
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 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 {
if err := rows2.Scan(&source, &x1, &y1); err != nil {
fmt.Println(err)
}
_x1, _y1 = x1, y1
f_flag = 1
continue
}
if err := rows2.Scan(&source, &x1, &y1); err != nil {
fmt.Println(err)
}
_x2, _y2 = x1, y1
_, rad_up := distance_km(_x1, _y1, _x2, _y2)
px, py = _x1, _y1
for {
px += diff_longitude(0.00556*math.Cos(rad_up), py)
py += diff_latitude(0.00556 * math.Sin(rad_up))
rad0 := math.Atan2((_y2 - _y1), (_x2 - _x1))
rad1 := math.Atan2((_y2 - py), (_x2 - px))
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
}
}
}
}
if err := db.Ping(); err != nil {
log.Fatal("PingError: ", err)
}
}
