1 // Copyright (c) 2018-2021 Robert J. Hijmans
2 //
3 // This file is part of the "spat" library.
4 //
5 // spat is free software: you can redistribute it and/or modify it
6 // under the terms of the GNU General Public License as published by
7 // the Free Software Foundation, either version 2 of the License, or
8 // (at your option) any later version.
9 //
10 // spat is distributed in the hope that it will be useful, but
11 // WITHOUT ANY WARRANTY; without even the implied warranty of
12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 // GNU General Public License for more details.
14 //
15 // You should have received a copy of the GNU General Public License
16 // along with spat. If not, see <http://www.gnu.org/licenses/>.
17
18
19 #ifndef M_PI
20 #define M_PI (3.14159265358979323846)
21 #endif
22
23 #include <vector>
24 //#include <math.h>
25 #include <cmath>
26 #include "geodesic.h"
27 #include "recycle.h"
28
29
distance_lonlat(const double & lon1,const double & lat1,const double & lon2,const double & lat2)30 double distance_lonlat(const double &lon1, const double &lat1, const double &lon2, const double &lat2) {
31 double a = 6378137.0;
32 double f = 1/298.257223563;
33 double s12, azi1, azi2;
34 struct geod_geodesic g;
35 geod_init(&g, a, f);
36 geod_inverse(&g, lat1, lon1, lat2, lon2, &s12, &azi1, &azi2);
37 return s12;
38 }
39
40
distance_lonlat(std::vector<double> & lon1,std::vector<double> & lat1,std::vector<double> & lon2,std::vector<double> & lat2)41 std::vector<double> distance_lonlat(std::vector<double> &lon1, std::vector<double> &lat1, std::vector<double> &lon2, std::vector<double> &lat2) {
42 double a = 6378137.0;
43 double f = 1/298.257223563;
44
45 recycle(lon1, lon2);
46 recycle(lat1, lat2);
47 std::vector<double> r (lon1.size());
48 double azi1, azi2;
49 struct geod_geodesic g;
50 geod_init(&g, a, f);
51 size_t n = lat1.size();
52 for (size_t i=0; i < n; i++) {
53 geod_inverse(&g, lat1[i], lon1[i], lat2[i], lon2[i], &r[i], &azi1, &azi2);
54 }
55 return r;
56 }
57
58
distance_lonlat_vd(std::vector<double> & lon1,std::vector<double> & lat1,double lon2,double lat2)59 std::vector<double> distance_lonlat_vd(std::vector<double> &lon1, std::vector<double> &lat1, double lon2, double lat2) {
60 std::vector<double> vlon2(lon1.size(), lon2);
61 std::vector<double> vlat2(lat1.size(), lat2);
62 return distance_lonlat(lon1, lat1, vlon2, vlat2);
63 }
64
65
66
distance_plane(const double & x1,const double & y1,const double & x2,const double & y2)67 double distance_plane(const double &x1, const double &y1, const double &x2, const double &y2) {
68 return( sqrt(pow((x2-x1),2) + pow((y2-y1), 2)) );
69 }
70
71
distance_plane(std::vector<double> & x1,std::vector<double> & y1,std::vector<double> & x2,std::vector<double> & y2)72 std::vector<double> distance_plane(std::vector<double> &x1, std::vector<double> &y1, std::vector<double> &x2, std::vector<double> &y2) {
73 recycle(x1, x2);
74 recycle(y1, y2);
75 std::vector<double> r (x1.size());
76 size_t n = x1.size();
77 for (size_t i=0; i < n; i++) {
78 r[i] = distance_plane(x1[i], y1[i], x2[i], y2[i]);
79 }
80 return r;
81 }
82
distance_plane_vd(std::vector<double> & x1,std::vector<double> & y1,double x2,double y2)83 std::vector<double> distance_plane_vd(std::vector<double> &x1, std::vector<double> &y1, double x2, double y2) {
84 std::vector<double> vx2(x1.size(), x2);
85 std::vector<double> vy2(y1.size(), y2);
86 return distance_plane(x1, y1, vx2, vy2);
87 }
88
89
90 /*
91 double distPlane(double x1, double y1, double x2, double y2) {
92 return( sqrt(pow((x2-x1),2) + pow((y2-y1), 2)) );
93 }
94 */
95
96 // Convert degrees to radians
toRad(double & deg)97 double toRad(double °) {
98 return( deg * 0.0174532925199433 );
99 }
100
101
toDeg(double & rad)102 double toDeg(double &rad) {
103 return( rad * 57.2957795130823 );
104 }
105
106
107
108
direction_lonlat(double lon1,double lat1,double lon2,double lat2,bool degrees)109 double direction_lonlat(double lon1, double lat1, double lon2, double lat2, bool degrees) {
110 double a = 6378137.0;
111 double f = 1/298.257223563;
112
113 double s12, azi1, azi2;
114 struct geod_geodesic g;
115 geod_init(&g, a, f);
116 geod_inverse(&g, lat1, lon1, lat2, lon2, &s12, &azi1, &azi2);
117 if (!degrees) {
118 return(toRad(azi1));
119 }
120 return( azi1) ;
121 }
122
direction_lonlat(std::vector<double> lon1,std::vector<double> lat1,std::vector<double> lon2,std::vector<double> lat2,bool degrees)123 std::vector<double> direction_lonlat(std::vector<double> lon1, std::vector<double> lat1, std::vector<double> lon2, std::vector<double> lat2, bool degrees) {
124 double a = 6378137.0;
125 double f = 1/298.257223563;
126
127 // lonlat1 and lonlat2 should have the same length
128
129 std::vector<double> azi1(lon1.size());
130 double s12, azi2;
131 struct geod_geodesic g;
132 geod_init(&g, a, f);
133 size_t n = lat1.size();
134 if (degrees) {
135 for (size_t i=0; i < n; i++) {
136 geod_inverse(&g, lat1[i], lon1[i], lat2[i], lon2[i], &s12, &azi1[i], &azi2);
137 }
138 } else {
139 for (size_t i=0; i < n; i++) {
140 geod_inverse(&g, lat1[i], lon1[i], lat2[i], lon2[i], &s12, &azi1[i], &azi2);
141 azi1[i] = toRad(azi1[i]);
142 }
143 }
144 return azi1;
145 }
146
147
directionToNearest_lonlat(std::vector<double> lon1,std::vector<double> lat1,std::vector<double> lon2,std::vector<double> lat2,bool degrees,bool from)148 std::vector<double> directionToNearest_lonlat(std::vector<double> lon1, std::vector<double> lat1, std::vector<double> lon2, std::vector<double> lat2, bool degrees, bool from) {
149
150 double a = 6378137.0;
151 double f = 1/298.257223563;
152
153 double azi1, azi2, s12, dist;
154 size_t n = lon1.size();
155 size_t m = lon2.size();
156 std::vector<double> azi(n);
157
158 struct geod_geodesic g;
159 geod_init(&g, a, f);
160
161 if (from) {
162 for (size_t i=0; i < n; i++) {
163 geod_inverse(&g, lat2[0], lon2[0], lat1[i], lon1[i], &dist, &azi1, &azi2);
164 azi[i] = azi1;
165 for (size_t j=1; j<m; j++) {
166 geod_inverse(&g, lat2[j], lon2[j], lat1[i], lon1[i], &s12, &azi1, &azi2);
167 if (s12 < dist) {
168 azi[i] = azi1;
169 }
170 }
171 if (!degrees) {
172 azi[i] = toRad(azi[i]);
173 }
174 }
175
176 } else {
177 for (size_t i=0; i < n; i++) {
178 geod_inverse(&g, lat1[i], lon1[i], lat2[0], lon2[0], &dist, &azi1, &azi2);
179 azi[i] = azi1;
180 for (size_t j=1; j<m; j++) {
181 geod_inverse(&g, lat1[i], lon1[i], lat2[j], lon2[j], &s12, &azi1, &azi2);
182 if (s12 < dist) {
183 azi[i] = azi1;
184 }
185 }
186 if (!degrees) {
187 azi[i] = toRad(azi[i]);
188 }
189 }
190 }
191 return azi;
192 }
193
194
195
direction_plane(double x1,double y1,double x2,double y2,bool degrees)196 double direction_plane(double x1, double y1, double x2, double y2, bool degrees) {
197 double a;
198 double pi2 = M_PI * 2;
199 a = fmod(atan2( x2 - x1, y2 - y1), pi2);
200 a = (a < 0 ? a + pi2 : a );
201 return (degrees ? toDeg(a) : a);
202 }
203
204
205
direction_plane(std::vector<double> x1,std::vector<double> y1,std::vector<double> x2,std::vector<double> y2,bool degrees)206 std::vector<double> direction_plane(std::vector<double> x1, std::vector<double> y1, std::vector<double> x2, std::vector<double> y2, bool degrees) {
207 // xy1 and xy2 should have the same length
208 std::vector<double> r (x1.size());
209 //double a;
210 size_t n = x1.size();
211 for (size_t i=0; i < n; i++) {
212 r[i] = direction_plane(x1[i], y1[i], x2[i], y2[i], degrees);
213 }
214 return r;
215 }
216
217
218
directionToNearest_plane(std::vector<double> x1,std::vector<double> y1,std::vector<double> x2,std::vector<double> y2,bool degrees,bool from)219 std::vector<double> directionToNearest_plane(std::vector<double> x1, std::vector<double> y1, std::vector<double> x2, std::vector<double> y2, bool degrees, bool from) {
220 size_t n = x1.size();
221 size_t m = x2.size();
222 std::vector<double> r(n);
223 double d, mind;
224 size_t minj;
225 if (from) {
226 for (size_t i = 0; i < n; i++) {
227 mind = distance_plane(x1[i], y1[i], x2[0], y2[0]);
228 minj = 0;
229 for (size_t j = 1; j < m; j++) {
230 d = distance_plane(x1[i], y1[i], x2[j], y2[j]);
231 if (d < mind) {
232 mind = d;
233 minj = j;
234 }
235 }
236 r[i] = direction_plane(x2[minj], y2[minj], x1[i], y1[i], degrees);
237 }
238 } else {
239 for (size_t i = 0; i < n; i++) {
240 mind = distance_plane(x1[i], y1[i], x2[0], y2[0]);
241 minj = 0;
242 for (size_t j = 1; j < m; j++) {
243 d = distance_plane(x1[i], y1[i], x2[j], y2[j]);
244 if (d < mind) {
245 mind = d;
246 minj = j;
247 }
248 }
249 r[i] = direction_plane(x1[i], y1[i], x2[minj], y2[minj], degrees);
250 }
251 }
252 return r;
253 }
254
255
256
257
258
destpoint_lonlat(double longitude,double latitude,double bearing,double distance)259 std::vector<double> destpoint_lonlat(double longitude, double latitude, double bearing, double distance) {
260 double a = 6378137.0;
261 double f = 1/298.257223563;
262
263 struct geod_geodesic g;
264 geod_init(&g, a, f);
265 double lat2, lon2, azi2;
266 geod_direct(&g, latitude, longitude, bearing, distance, &lat2, &lon2, &azi2);
267 std::vector<double> out = {lon2, lat2, azi2 };
268 return out;
269 }
270
271
destpoint_lonlat(const std::vector<double> & longitude,const std::vector<double> & latitude,const std::vector<double> & bearing,const std::vector<double> & distance)272 std::vector<std::vector<double> > destpoint_lonlat(const std::vector<double> &longitude, const std::vector<double> &latitude, const std::vector<double> &bearing, const std::vector<double> &distance) {
273 double a = 6378137.0;
274 double f = 1/298.257223563;
275
276 struct geod_geodesic g;
277 geod_init(&g, a, f);
278 size_t n = longitude.size();
279 std::vector<std::vector<double> > out(3, std::vector<double>(n));
280 double lat2, lon2, azi2;
281 for (size_t i=0; i < n; i++) {
282 geod_direct(&g, latitude[i], longitude[i], bearing[i], distance[i], &lat2, &lon2, &azi2);
283 out[0][i] = lon2;
284 out[1][i] = lat2;
285 out[2][i] = azi2;
286 }
287 return out;
288 }
289
290
destpoint_lonlat(const double & longitude,const double & latitude,const std::vector<double> & bearing,const double & distance,bool wrap)291 std::vector<std::vector<double> > destpoint_lonlat(const double &longitude, const double &latitude, const std::vector<double> &bearing, const double& distance, bool wrap) {
292 double a = 6378137.0;
293 double f = 1/298.257223563;
294
295 struct geod_geodesic g;
296 geod_init(&g, a, f);
297 size_t n = bearing.size();
298 std::vector<std::vector<double> > out(3, std::vector<double>(n));
299 double lat2, lon2, azi2;
300 if (wrap) {
301 for (size_t i=0; i < n; i++) {
302 geod_direct(&g, latitude, longitude, bearing[i], distance, &lat2, &lon2, &azi2);
303 out[0][i] = lon2;
304 out[1][i] = lat2;
305 out[2][i] = azi2;
306 }
307 } else {
308 for (size_t i=0; i < n; i++) {
309 geod_direct(&g, latitude, 0, bearing[i], distance, &lat2, &lon2, &azi2);
310 out[0][i] = lon2 + longitude;
311 out[1][i] = lat2;
312 out[2][i] = azi2;
313 }
314 }
315 return out;
316 }
317
318
319
destpoint_plane(double x,double y,double bearing,double distance)320 std::vector<double> destpoint_plane(double x, double y, double bearing, double distance) {
321 bearing = bearing * M_PI / 180;
322 x += distance * cos(bearing);
323 y += distance * sin(bearing);
324 std::vector<double> out = {x, y};
325 return(out);
326 }
327
328
destpoint_plane(std::vector<double> x,std::vector<double> y,std::vector<double> bearing,std::vector<double> distance)329 std::vector<std::vector<double> > destpoint_plane(std::vector<double> x, std::vector<double> y, std::vector<double> bearing, std::vector<double> distance) {
330 size_t n = x.size();
331 std::vector<std::vector<double> > out;
332 out.reserve(n);
333 double xd, yd, b;
334 for (size_t i=0; i < n; i++) {
335 b = bearing[i] * M_PI / 180;
336 xd = x[i] + distance[i] * cos(b);
337 yd = y[i] + distance[i] * sin(b);
338 out.push_back( {xd, yd });
339 }
340 return(out);
341 }
342
343
distanceToNearest_lonlat(std::vector<double> & d,const std::vector<double> & lon1,const std::vector<double> & lat1,const std::vector<double> & lon2,const std::vector<double> & lat2)344 void distanceToNearest_lonlat(std::vector<double> &d, const std::vector<double> &lon1, const std::vector<double> &lat1, const std::vector<double> &lon2, const std::vector<double> &lat2) {
345 int n = lon1.size();
346 int m = lon2.size();
347 double a = 6378137.0;
348 double f = 1/298.257223563;
349 double azi1, azi2, s12;
350 struct geod_geodesic g;
351 geod_init(&g, a, f);
352
353 for (int i=0; i < n; i++) {
354 if (std::isnan(lat1[i])) {
355 continue;
356 }
357 geod_inverse(&g, lat1[i], lon1[i], lat2[0], lon2[0], &d[i], &azi1, &azi2);
358 for (int j=1; j<m; j++) {
359 geod_inverse(&g, lat1[i], lon1[i], lat2[j], lon2[j], &s12, &azi1, &azi2);
360 if (s12 < d[i]) {
361 d[i] = s12;
362 }
363 }
364 }
365 }
366
367
distCosine(const double & lon1,const double & lat1,const double & lon2,const double & lat2)368 double distCosine(const double &lon1, const double &lat1, const double &lon2, const double &lat2) {
369 return 6378137 * acos((sin(lat1) * sin(lat2) + cos(lat1) * cos(lat2) * cos(lon1-lon2)));
370 }
371
372 // input lon lat in radians
distanceCosineToNearest_lonlat(std::vector<double> & d,const std::vector<double> & lon1,const std::vector<double> & lat1,const std::vector<double> & lon2,const std::vector<double> & lat2)373 void distanceCosineToNearest_lonlat(std::vector<double> &d, const std::vector<double> &lon1, const std::vector<double> &lat1, const std::vector<double> &lon2, const std::vector<double> &lat2) {
374 int n = lon1.size();
375 int m = lon2.size();
376 double s12;
377 for (int i=0; i < n; i++) {
378 if (std::isnan(lat1[i])) {
379 continue;
380 }
381 d[i] = distCosine(lat1[i], lon1[i], lat2[0], lon2[0]);
382 for (int j=1; j<m; j++) {
383 s12 = distCosine(lat1[i], lon1[i], lat2[j], lon2[j]);
384 if (s12 < d[i]) {
385 d[i] = s12;
386 }
387 }
388 }
389 }
390
391
392
393
distanceToNearest_plane(std::vector<double> & d,const std::vector<double> & x1,const std::vector<double> & y1,const std::vector<double> & x2,const std::vector<double> & y2,const double & lindist)394 void distanceToNearest_plane(std::vector<double> &d, const std::vector<double> &x1, const std::vector<double> &y1, const std::vector<double> &x2, const std::vector<double> &y2, const double& lindist) {
395 int n = x1.size();
396 int m = x2.size();
397 for (int i=0; i < n; i++) {
398 if (std::isnan(x1[i])) continue;
399 d[i] = sqrt(pow((x2[0]-x1[i]),2) + pow((y2[0]-y1[i]), 2)) * lindist;
400 for (int j=1; j < m; j++) {
401 double r = sqrt(pow((x2[j]-x1[i]),2) + pow((y2[j]-y1[i]), 2));
402 if (r < d[i]) {
403 d[i] = r * lindist;
404 }
405 }
406 }
407 }
408
409
410
nearest_lonlat(std::vector<long> & id,std::vector<double> & d,std::vector<double> & nlon,std::vector<double> & nlat,const std::vector<double> & lon1,const std::vector<double> & lat1,const std::vector<double> & lon2,const std::vector<double> & lat2)411 void nearest_lonlat(std::vector<long> &id, std::vector<double> &d, std::vector<double> &nlon, std::vector<double> &nlat, const std::vector<double> &lon1, const std::vector<double> &lat1, const std::vector<double> &lon2, const std::vector<double> &lat2) {
412 size_t n = lon1.size();
413 size_t m = lon2.size();
414 double a = 6378137.0;
415 double f = 1/298.257223563;
416 double azi1, azi2, s12;
417 struct geod_geodesic g;
418 geod_init(&g, a, f);
419 nlon.resize(n);
420 nlat.resize(n);
421 id.resize(n);
422 d.resize(n);
423
424 for (size_t i=0; i < n; i++) {
425 if (std::isnan(lat1[i])) {
426 nlon[i] = NAN;
427 nlat[i] = NAN;
428 id[i] = -1;
429 d[i] = NAN;
430 continue;
431 }
432 geod_inverse(&g, lat1[i], lon1[i], lat2[0], lon2[0], &d[i], &azi1, &azi2);
433 nlon[i] = lon2[0];
434 nlat[i] = lat2[0];
435 id[i] = 0;
436 for (size_t j=1; j<m; j++) {
437 geod_inverse(&g, lat1[i], lon1[i], lat2[j], lon2[j], &s12, &azi1, &azi2);
438 if (s12 < d[i]) {
439 d[i] = s12;
440 id[i] = j;
441 nlon[i] = lon2[j];
442 nlat[i] = lat2[j];
443 }
444 }
445 }
446 }
447
448
nearest_lonlat_self(std::vector<long> & id,std::vector<double> & d,std::vector<double> & nlon,std::vector<double> & nlat,const std::vector<double> & lon,const std::vector<double> & lat)449 void nearest_lonlat_self(std::vector<long> &id, std::vector<double> &d, std::vector<double> &nlon, std::vector<double> &nlat, const std::vector<double> &lon, const std::vector<double> &lat) {
450 size_t n = lon.size();
451 if (n <= 1) {
452 nlon = lon;
453 nlat = lat;
454 if (nlon.size() == 1) {
455 id.resize(1);
456 id[0] = 0;
457 }
458 return;
459 }
460 double a = 6378137.0;
461 double f = 1/298.257223563;
462 double azi1, azi2, s12;
463 struct geod_geodesic g;
464 geod_init(&g, a, f);
465 nlon.resize(n);
466 nlat.resize(n);
467 id.resize(n);
468 d.resize(n);
469 for (size_t i=0; i < n; i++) {
470 if (std::isnan(lat[i])) {
471 id[i] = -1;
472 d[i] = NAN;
473 nlon[i] = NAN;
474 nlat[i] = NAN;
475 continue;
476 }
477 if (i>0) {
478 geod_inverse(&g, lat[i], lon[i], lat[0], lon[0], &d[i], &azi1, &azi2);
479 nlon[i] = lon[0];
480 nlat[i] = lat[0];
481 id[i] = 0;
482 } else {
483 geod_inverse(&g, lat[1], lon[1], lat[0], lon[0], &d[i], &azi1, &azi2);
484 nlon[i] = lon[1];
485 nlat[i] = lat[1];
486 id[i] = 1;
487 }
488
489 for (size_t j=1; j < n; j++) {
490 if (j == i) continue;
491 geod_inverse(&g, lat[i], lon[i], lat[j], lon[j], &s12, &azi1, &azi2);
492 if (s12 < d[i]) {
493 d[i] = s12;
494 id[i] = j;
495 nlon[i] = lon[j];
496 nlat[i] = lat[j];
497 }
498 }
499 }
500 }
501
502
503
distHaversine(double lon1,double lat1,double lon2,double lat2)504 double distHaversine(double lon1, double lat1, double lon2, double lat2) {
505 double r = 6378137;
506 double dLat, dLon, a;
507 lon1 = toRad(lon1);
508 lon2 = toRad(lon2);
509 lat1 = toRad(lat1);
510 lat2 = toRad(lat2);
511
512 dLat = lat2-lat1;
513 dLon = lon2-lon1;
514 a = sin(dLat/2.) * sin(dLat/2.) + cos(lat1) * cos(lat2) * sin(dLon/2.) * sin(dLon/2.);
515 return 2. * atan2(sqrt(a), sqrt(1.-a)) * r;
516 }
517
518