You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

grtcirc.cc 10KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409
  1. /*
  2. Great Circle utility functions
  3. Copyright (C) 2002 Robert Lipe, robertlipe+source@gpsbabel.org
  4. This program is free software; you can redistribute it and/or modify
  5. it under the terms of the GNU General Public License as published by
  6. the Free Software Foundation; either version 2 of the License, or
  7. (at your option) any later version.
  8. This program is distributed in the hope that it will be useful,
  9. but WITHOUT ANY WARRANTY; without even the implied warranty of
  10. MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  11. GNU General Public License for more details.
  12. You should have received a copy of the GNU General Public License
  13. along with this program; if not, write to the Free Software
  14. Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111 USA
  15. */
  16. #include "defs.h"
  17. #include "grtcirc.h"
  18. #include <errno.h>
  19. #include <math.h>
  20. #include <stdio.h>
  21. static const double EARTH_RAD = 6378137.0;
  22. static void crossproduct(double x1, double y1, double z1,
  23. double x2, double y2, double z2,
  24. double* xa, double* ya, double* za)
  25. {
  26. *xa = y1*z2-y2*z1;
  27. *ya = z1*x2-z2*x1;
  28. *za = x1*y2-y1*x2;
  29. }
  30. static double dotproduct(double x1, double y1, double z1,
  31. double x2, double y2, double z2)
  32. {
  33. return (x1*x2+y1*y2+z1*z2);
  34. }
  35. /*
  36. * Note: this conversion to miles uses the WGS84 value for the radius of
  37. * the earth at the equator.
  38. * (radius in meters)*(100cm/m) -> (radius in cm)
  39. * (radius in cm) / (2.54 cm/in) -> (radius in in)
  40. * (radius in in) / (12 in/ft) -> (radius in ft)
  41. * (radius in ft) / (5280 ft/mi) -> (radius in mi)
  42. * If the compiler is half-decent, it'll do all the math for us at compile
  43. * time, so why not leave the expression human-readable?
  44. */
  45. double radtomiles(double rads)
  46. {
  47. const double radmiles = EARTH_RAD*100.0/2.54/12.0/5280.0;
  48. return (rads*radmiles);
  49. }
  50. double radtometers(double rads)
  51. {
  52. return (rads * EARTH_RAD);
  53. }
  54. double gcdist(double lat1, double lon1, double lat2, double lon2)
  55. {
  56. double res;
  57. double sdlat, sdlon;
  58. errno = 0;
  59. sdlat = sin((lat1 - lat2) / 2.0);
  60. sdlon = sin((lon1 - lon2) / 2.0);
  61. res = sqrt(sdlat * sdlat + cos(lat1) * cos(lat2) * sdlon * sdlon);
  62. if (res > 1.0) {
  63. res = 1.0;
  64. } else if (res < -1.0) {
  65. res = -1.0;
  66. }
  67. res = asin(res);
  68. if (
  69. #if defined isnan
  70. /* This is a C99-ism. */
  71. (isnan(res)) ||
  72. #endif
  73. (errno == EDOM)) { /* this should never happen: */
  74. errno = 0; /* Math argument out of domain of
  75. function, */
  76. return 0; /* or value returned is not a number */
  77. }
  78. return 2.0 * res;
  79. }
  80. /* This value is the heading you'd leave point 1 at to arrive at point 2.
  81. * Inputs and outputs are in radians.
  82. */
  83. double heading(double lat1, double lon1, double lat2, double lon2)
  84. {
  85. double v1, v2;
  86. v1 = sin(lon1 - lon2) * cos(lat2);
  87. v2 = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(lon1 - lon2);
  88. /* rounding error protection */
  89. if (fabs(v1) < 1e-15) {
  90. v1 = 0.0;
  91. }
  92. if (fabs(v2) < 1e-15) {
  93. v2 = 0.0;
  94. }
  95. return atan2(v1, v2);
  96. }
  97. /* As above, but outputs is in degrees from 0 - 359. Inputs are still radians. */
  98. double heading_true_degrees(double lat1, double lon1, double lat2, double lon2)
  99. {
  100. double h = 360.0 - DEG(heading(lat1, lon1, lat2, lon2));
  101. if (h >= 360.0) {
  102. h -= 360.0;
  103. }
  104. return h;
  105. }
  106. double linedistprj(double lat1, double lon1,
  107. double lat2, double lon2,
  108. double lat3, double lon3,
  109. double* prjlat, double* prjlon,
  110. double* frac)
  111. {
  112. static double _lat1 = -9999;
  113. static double _lat2 = -9999;
  114. static double _lon1 = -9999;
  115. static double _lon2 = -9999;
  116. static double x1,y1,z1;
  117. static double x2,y2,z2;
  118. static double xa,ya,za,la;
  119. double x3,y3,z3;
  120. double xp,yp,zp,lp;
  121. double xa1,ya1,za1;
  122. double xa2,ya2,za2;
  123. double d1, d2;
  124. double c1, c2;
  125. double dot;
  126. int newpoints;
  127. *prjlat = lat1;
  128. *prjlon = lon1;
  129. *frac = 0;
  130. /* degrees to radians */
  131. lat1 = RAD(lat1);
  132. lon1 = RAD(lon1);
  133. lat2 = RAD(lat2);
  134. lon2 = RAD(lon2);
  135. lat3 = RAD(lat3);
  136. lon3 = RAD(lon3);
  137. newpoints = 1;
  138. if (lat1 == _lat1 && lat2 == _lat2 && lon1 == _lon1 && lon2 == _lon2) {
  139. newpoints = 0;
  140. } else {
  141. _lat1 = lat1;
  142. _lat2 = lat2;
  143. _lon1 = lon1;
  144. _lon2 = lon2;
  145. }
  146. /* polar to ECEF rectangular */
  147. if (newpoints) {
  148. x1 = cos(lon1)*cos(lat1);
  149. y1 = sin(lat1);
  150. z1 = sin(lon1)*cos(lat1);
  151. x2 = cos(lon2)*cos(lat2);
  152. y2 = sin(lat2);
  153. z2 = sin(lon2)*cos(lat2);
  154. }
  155. x3 = cos(lon3)*cos(lat3);
  156. y3 = sin(lat3);
  157. z3 = sin(lon3)*cos(lat3);
  158. if (newpoints) {
  159. /* 'a' is the axis; the line that passes through the center of the earth
  160. * and is perpendicular to the great circle through point 1 and point 2
  161. * It is computed by taking the cross product of the '1' and '2' vectors.*/
  162. crossproduct(x1, y1, z1, x2, y2, z2, &xa, &ya, &za);
  163. la = sqrt(xa*xa+ya*ya+za*za);
  164. if (la) {
  165. xa /= la;
  166. ya /= la;
  167. za /= la;
  168. }
  169. }
  170. if (la) {
  171. /* dot is the component of the length of '3' that is along the axis.
  172. * What's left is a non-normalized vector that lies in the plane of
  173. * 1 and 2. */
  174. dot = dotproduct(x3,y3,z3,xa,ya,za);
  175. xp = x3-dot*xa;
  176. yp = y3-dot*ya;
  177. zp = z3-dot*za;
  178. lp = sqrt(xp*xp+yp*yp+zp*zp);
  179. if (lp) {
  180. /* After this, 'p' is normalized */
  181. xp /= lp;
  182. yp /= lp;
  183. zp /= lp;
  184. crossproduct(x1,y1,z1,xp,yp,zp,&xa1,&ya1,&za1);
  185. d1 = dotproduct(xa1, ya1, za1, xa, ya, za);
  186. crossproduct(xp,yp,zp,x2,y2,z2,&xa2,&ya2,&za2);
  187. d2 = dotproduct(xa2, ya2, za2, xa, ya, za);
  188. if (d1 >= 0 && d2 >= 0) {
  189. /* rather than call gcdist and all its sines and cosines and
  190. * worse, we can get the angle directly. It's the arctangent
  191. * of the length of the component of vector 3 along the axis
  192. * divided by the length of the component of vector 3 in the
  193. * plane. We already have both of those numbers.
  194. *
  195. * atan2 would be overkill because lp and fabs(dot) are both
  196. * known to be positive. */
  197. *prjlat = DEG(asin(yp));
  198. if (xp == 0 && zp == 0) {
  199. *prjlon = 0;
  200. } else {
  201. *prjlon = DEG(atan2(zp, xp));
  202. }
  203. *frac = d1/(d1 + d2);
  204. return atan(fabs(dot)/lp);
  205. }
  206. /* otherwise, get the distance from the closest endpoint */
  207. c1 = dotproduct(x1,y1,z1,xp,yp,zp);
  208. c2 = dotproduct(x2,y2,z2,xp,yp,zp);
  209. d1 = fabs(d1);
  210. d2 = fabs(d2);
  211. /* This is a hack. d$n$ is proportional to the sine of the angle
  212. * between point $n$ and point p. That preserves orderedness up
  213. * to an angle of 90 degrees. c$n$ is proportional to the cosine
  214. * of the same angle; if the angle is over 90 degrees, c$n$ is
  215. * negative. In that case, we flop the sine across the y=1 axis
  216. * so that the resulting value increases as the angle increases.
  217. *
  218. * This only works because all of the points are on a unit sphere. */
  219. if (c1 < 0) {
  220. d1 = 2 - d1;
  221. }
  222. if (c2 < 0) {
  223. d2 = 2 - d2;
  224. }
  225. if (fabs(d1) < fabs(d2)) {
  226. return gcdist(lat1,lon1,lat3,lon3);
  227. } else {
  228. *prjlat = DEG(lat2);
  229. *prjlon = DEG(lon2);
  230. *frac = 1.0;
  231. return gcdist(lat2,lon2,lat3,lon3);
  232. }
  233. } else {
  234. /* lp is 0 when 3 is 90 degrees from the great circle */
  235. return M_PI/2;
  236. }
  237. } else {
  238. /* la is 0 when 1 and 2 are either the same point or 180 degrees apart */
  239. dot = dotproduct(x1,y1,z1,x2,y2,z2);
  240. if (dot >= 0) {
  241. return gcdist(lat1,lon1,lat3,lon3);
  242. } else {
  243. return 0;
  244. }
  245. }
  246. }
  247. double linedist(double lat1, double lon1,
  248. double lat2, double lon2,
  249. double lat3, double lon3)
  250. {
  251. double dummy;
  252. return linedistprj(lat1, lon1, lat2, lon2, lat3, lon3, &dummy, &dummy, &dummy);
  253. }
  254. /*
  255. * Compute the position of a point partially along the geodesic from
  256. * lat1,lon1 to lat2,lon2
  257. *
  258. * Ref: http://mathworld.wolfram.com/RotationFormula.html
  259. */
  260. void linepart(double lat1, double lon1,
  261. double lat2, double lon2,
  262. double frac,
  263. double* reslat, double* reslon)
  264. {
  265. double x1,y1,z1;
  266. double x2,y2,z2;
  267. double xa,ya,za,la;
  268. double xr, yr, zr;
  269. double xx, yx, zx;
  270. double theta = 0;
  271. double phi = 0;
  272. double cosphi = 0;
  273. double sinphi = 0;
  274. /* result must be in degrees */
  275. *reslat = lat1;
  276. *reslon = lon1;
  277. /* degrees to radians */
  278. lat1 = RAD(lat1);
  279. lon1 = RAD(lon1);
  280. lat2 = RAD(lat2);
  281. lon2 = RAD(lon2);
  282. /* polar to ECEF rectangular */
  283. x1 = cos(lon1)*cos(lat1);
  284. y1 = sin(lat1);
  285. z1 = sin(lon1)*cos(lat1);
  286. x2 = cos(lon2)*cos(lat2);
  287. y2 = sin(lat2);
  288. z2 = sin(lon2)*cos(lat2);
  289. /* 'a' is the axis; the line that passes through the center of the earth
  290. * and is perpendicular to the great circle through point 1 and point 2
  291. * It is computed by taking the cross product of the '1' and '2' vectors.*/
  292. crossproduct(x1, y1, z1, x2, y2, z2, &xa, &ya, &za);
  293. la = sqrt(xa*xa+ya*ya+za*za);
  294. if (la) {
  295. xa /= la;
  296. ya /= la;
  297. za /= la;
  298. }
  299. /* if la is zero, the points are either equal or directly opposite
  300. * each other. Either way, there's no single geodesic, so we punt. */
  301. if (la) {
  302. crossproduct(x1, y1, z1, xa, ya, za, &xx, &yx, &zx);
  303. theta = atan2(dotproduct(xx,yx,zx,x2,y2,z2),
  304. dotproduct(x1,y1,z1,x2,y2,z2));
  305. phi = frac * theta;
  306. cosphi = cos(phi);
  307. sinphi = sin(phi);
  308. /* The second term of the formula from the mathworld reference is always
  309. * zero, because r (lat1,lon1) is always perpendicular to n (a here) */
  310. xr = x1*cosphi + xx * sinphi;
  311. yr = y1*cosphi + yx * sinphi;
  312. zr = z1*cosphi + zx * sinphi;
  313. if (xr > 1) {
  314. xr = 1;
  315. }
  316. if (xr < -1) {
  317. xr = -1;
  318. }
  319. if (yr > 1) {
  320. yr = 1;
  321. }
  322. if (yr < -1) {
  323. yr = -1;
  324. }
  325. if (zr > 1) {
  326. zr = 1;
  327. }
  328. if (zr < -1) {
  329. zr = -1;
  330. }
  331. *reslat = DEG(asin(yr));
  332. if (xr == 0 && zr == 0) {
  333. *reslon = 0;
  334. } else {
  335. *reslon = DEG(atan2(zr, xr));
  336. }
  337. }
  338. }