1 #define USING_R 1
2 /* Copyright by Roger Bivand (C) 2005-2009 */
3
4 #include "sp.h"
5
6 #ifdef USING_R
7 # define POWDI(x,i) R_pow_di(x,i)
8 #else
9 # include <math.h>
10 # define POWDI(x,i) pow(x,i)
11 /*# define pythag(a,b) sqrt(a*a+b*b)*/
12 #endif
13
sp_dists(double * u,double * v,double * uout,double * vout,int * n,double * dists,int * lonlat)14 void sp_dists(double *u, double *v, double *uout, double *vout,
15 int *n, double *dists, int *lonlat) {
16 int N = *n, j;
17 double gc[1];
18
19 if (lonlat[0] == 0) {
20 for (j = 0; j < N; j++)
21 dists[j] = hypot(u[j] - uout[0], v[j] - vout[0]);
22 } else {
23 for (j = 0; j < N; j++) {
24 sp_gcdist(u+j, uout, v+j, vout, gc);
25 dists[j] = gc[0];
26 }
27 }
28 }
29
sp_dists_NN(double * u1,double * v1,double * u2,double * v2,int * n,double * dists,int * lonlat)30 void sp_dists_NN(double *u1, double *v1, double *u2, double *v2,
31 int *n, double *dists, int *lonlat) {
32 int N = *n, j;
33 double gc[1];
34
35 if (lonlat[0] == 0)
36 for (j = 0; j < N; j++)
37 dists[j] = hypot(u1[j] - u2[j], v1[j] - v2[j]);
38 else {
39 for (j = 0; j < N; j++) {
40 sp_gcdist(u1+j, u2+j, v1+j, v2+j, gc);
41 dists[j] = gc[0];
42 }
43 }
44 }
45
sp_lengths(double * u,double * v,int * n,double * lengths,int * lonlat)46 void sp_lengths(double *u, double *v, int *n, double *lengths, int *lonlat) {
47 int N = *n, j;
48 double gc[1];
49 if (N < 2) error("N less than 2");
50
51 if (lonlat[0] == 0)
52 for (j=0; j < N-1; j++)
53 lengths[j] = hypot(u[j]-u[j+1], v[j]-v[j+1]);
54 else {
55 for (j=0; j < N-1; j++) {
56 sp_gcdist(u+j, u+j+1, v+j, v+j+1, gc);
57 lengths[j] = gc[0];
58 }
59 }
60
61 }
62
63 /* http://en.wikipedia.org/wiki/World_Geodetic_System#A_new_World_Geodetic_System:_WGS_84 */
64
sp_gcdist(double * lon1,double * lon2,double * lat1,double * lat2,double * dist)65 void sp_gcdist(double *lon1, double *lon2, double *lat1, double *lat2,
66 double *dist) {
67
68 double F, G, L, sinG2, cosG2, sinF2, cosF2, sinL2, cosL2, S, C;
69 double w, R, a, f, D, H1, H2;
70 double lat1R, lat2R, lon1R, lon2R, DE2RA;
71
72 DE2RA = M_PI/180;
73 a = 6378.137; /* WGS-84 equatorial radius in km */
74 f = 1.0/298.257223563; /* WGS-84 ellipsoid flattening factor */
75
76 if (fabs(lat1[0] - lat2[0]) < DOUBLE_EPS) {
77 if (fabs(fmod(lon1[0] - lon2[0], 360.0)) < DOUBLE_EPS) {
78 dist[0] = 0.0;
79 return;
80 }
81 }
82 lat1R = lat1[0]*DE2RA;
83 lat2R = lat2[0]*DE2RA;
84 lon1R = lon1[0]*DE2RA;
85 lon2R = lon2[0]*DE2RA;
86
87 F = ( lat1R + lat2R )/2.0;
88 G = ( lat1R - lat2R )/2.0;
89 L = ( lon1R - lon2R )/2.0;
90
91 /*
92 printf("%g %g %g %g; %g %g %g\n", *lon1, *lon2, *lat1, *lat2, F, G, L);
93 */
94
95 sinG2 = POWDI( sin( G ), 2 );
96 cosG2 = POWDI( cos( G ), 2 );
97 sinF2 = POWDI( sin( F ), 2 );
98 cosF2 = POWDI( cos( F ), 2 );
99 sinL2 = POWDI( sin( L ), 2 );
100 cosL2 = POWDI( cos( L ), 2 );
101
102 S = sinG2*cosL2 + cosF2*sinL2;
103 C = cosG2*cosL2 + sinF2*sinL2;
104
105 w = atan( sqrt( S/C ) );
106 R = sqrt( S*C )/w;
107
108 D = 2*w*a;
109 H1 = ( 3*R - 1 )/( 2*C );
110 H2 = ( 3*R + 1 )/( 2*S );
111
112 dist[0] = D*( 1 + f*H1*sinF2*cosG2 - f*H2*cosF2*sinG2 );
113
114 }
115
116