PageRenderTime 27ms CodeModel.GetById 1ms RepoModel.GetById 1ms app.codeStats 0ms

/source/library/Interlace/Geo/Point.cs

https://bitbucket.org/VahidN/interlace
C# | 304 lines | 214 code | 54 blank | 36 comment | 15 complexity | 9806549e8213cc6e9a3086dac913c371 MD5 | raw file
  1. #region Using Directives and Copyright Notice
  2. // Copyright (c) 2007-2010, Computer Consultancy Pty Ltd
  3. // All rights reserved.
  4. //
  5. // Redistribution and use in source and binary forms, with or without
  6. // modification, are permitted provided that the following conditions are met:
  7. // * Redistributions of source code must retain the above copyright
  8. // notice, this list of conditions and the following disclaimer.
  9. // * Redistributions in binary form must reproduce the above copyright
  10. // notice, this list of conditions and the following disclaimer in the
  11. // documentation and/or other materials provided with the distribution.
  12. // * Neither the name of the Computer Consultancy Pty Ltd nor the
  13. // names of its contributors may be used to endorse or promote products
  14. // derived from this software without specific prior written permission.
  15. //
  16. // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  17. // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  18. // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  19. // ARE DISCLAIMED. IN NO EVENT SHALL COMPUTER CONSULTANCY PTY LTD BE LIABLE
  20. // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  21. // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
  22. // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  23. // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  24. // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  25. // OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
  26. // DAMAGE.
  27. using System;
  28. #endregion
  29. namespace Interlace.Geo
  30. {
  31. [Serializable]
  32. public struct Position
  33. {
  34. public double Longitude, Latitude;
  35. public static Position FromDegrees(double longitude, double latitude)
  36. {
  37. return new Position(Math.PI * longitude / 180.0,
  38. Math.PI * latitude / 180.0);
  39. }
  40. public void ToDegrees(out double longitude, out double latitude)
  41. {
  42. longitude = 180.0 * this.Longitude / Math.PI;
  43. latitude = 180.0 * this.Latitude / Math.PI;
  44. }
  45. public Position ToDegrees()
  46. {
  47. return new Position(180.0 * this.Longitude / Math.PI, 180.0 * this.Latitude / Math.PI);
  48. }
  49. public Position ToRadians()
  50. {
  51. return new Position(Math.PI * this.Longitude / 180.0, Math.PI * this.Latitude / 180.0);
  52. }
  53. public Position(double longitude, double latitide)
  54. {
  55. this.Longitude = longitude;
  56. this.Latitude = latitide;
  57. }
  58. public double X {
  59. get
  60. {
  61. return Longitude;
  62. }
  63. set
  64. {
  65. Longitude = value;
  66. }
  67. }
  68. public double Y {
  69. get
  70. {
  71. return Latitude;
  72. }
  73. set
  74. {
  75. Latitude = value;
  76. }
  77. }
  78. public static Position operator - (Position lhs, Position rhs)
  79. {
  80. return new Position(lhs.X - rhs.X, lhs.Y - rhs.Y);
  81. }
  82. public static Position operator + (Position lhs, Position rhs)
  83. {
  84. return new Position(lhs.X + rhs.X, lhs.Y + rhs.Y);
  85. }
  86. public static double DotProduct(Position lhs, Position rhs)
  87. {
  88. return lhs.X * rhs.X + lhs.Y * rhs.Y;
  89. }
  90. public static double SquaredDistance(Position lhs, Position rhs)
  91. {
  92. return (lhs.X - rhs.X) * (lhs.X - rhs.X) +
  93. (lhs.Y - rhs.Y) * (lhs.Y - rhs.Y);
  94. }
  95. public double SquaredDistanceFromOrigin()
  96. {
  97. return X * X + Y * Y;
  98. }
  99. public Position GetPointFromTargetAndDistance(Position target, double distance)
  100. {
  101. Position delta = target - this;
  102. double delta_length = Math.Sqrt(delta.SquaredDistanceFromOrigin());
  103. double udx = delta.X / delta_length;
  104. double udy = delta.Y / delta_length;
  105. return new Position(this.X + udx * distance, this.Y + udy * distance);
  106. }
  107. private static double Radians(double d)
  108. {
  109. return Math.PI * d / 180;
  110. }
  111. public static bool PointsEqual(Position a, Position b, double tolerance)
  112. {
  113. return Math.Abs(a.Latitude - b.Latitude) <= tolerance &&
  114. Math.Abs(a.Longitude - b.Longitude) <= tolerance;
  115. }
  116. public static double CartesianDistance(Position a, Position b)
  117. {
  118. return Math.Sqrt((a.X - b.X) * (a.X - b.X) + (a.Y - b.Y) * (a.Y - b.Y));
  119. }
  120. // Calculates the distance between two points, using the method
  121. // described in:
  122. //
  123. // Vincenty, T. Direct and Inverse Solutions of Geodesics of the
  124. // Ellipsoid with Applications of Nested Equations, Survey Review,
  125. // No. 176, 1975.
  126. //
  127. // Points that are nearly antipodal yield no solution; a
  128. // Double.NaN is returned.
  129. public static double CalculateDistance(Position a, Position b, Ellipsoid e)
  130. {
  131. double distance;
  132. Angle bearing;
  133. CalculateDistanceAndBearing(a, b, e, out distance, out bearing);
  134. return distance;
  135. }
  136. public static void CalculateDistanceAndBearing(Position a, Position b, Ellipsoid e, out double distance, out Angle bearing)
  137. {
  138. if (PointsEqual(a, b, 1e-12))
  139. {
  140. distance = 0.0;
  141. bearing = Angle.FromAngleInDegrees(0.0);
  142. }
  143. double lambda = Radians(b.Longitude - a.Longitude);
  144. double last_lambda = 2 * Math.PI;
  145. double U1 = Math.Atan((1 - e.F) * Math.Tan(Radians(a.Latitude)));
  146. double U2 = Math.Atan((1 - e.F) * Math.Tan(Radians(b.Latitude)));
  147. double sin_U1 = Math.Sin(U1);
  148. double sin_U2 = Math.Sin(U2);
  149. double cos_U1 = Math.Cos(U1);
  150. double cos_U2 = Math.Cos(U2);
  151. double alpha, sin_sigma, cos_sigma, sigma, cos_2sigma_m, sqr_cos_2sigma_m;
  152. int loop_limit = 30;
  153. const double threshold = 1e-12;
  154. do {
  155. double sin_lambda = Math.Sin(lambda);
  156. double cos_lambda = Math.Cos(lambda);
  157. sin_sigma =
  158. Math.Sqrt(
  159. Math.Pow(cos_U2 * sin_lambda, 2) +
  160. Math.Pow(cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda, 2)
  161. );
  162. cos_sigma = sin_U1 * sin_U2 + cos_U1 * cos_U2 * cos_lambda;
  163. sigma = Math.Atan2(sin_sigma, cos_sigma);
  164. alpha = Math.Asin(cos_U1 * cos_U2 * sin_lambda / sin_sigma);
  165. double sqr_cos_alpha = Math.Pow(Math.Cos(alpha), 2);
  166. cos_2sigma_m = cos_sigma - 2 * sin_U1 * sin_U2 / sqr_cos_alpha;
  167. sqr_cos_2sigma_m = Math.Pow(cos_2sigma_m, 2);
  168. double C = e.F / 16 * sqr_cos_alpha * (4 + e.F * (4 - 3 * sqr_cos_alpha));
  169. last_lambda = lambda;
  170. lambda =
  171. Radians(b.Longitude - a.Longitude) + (1 - C) * e.F * Math.Sin(alpha) *
  172. (sigma + C * sin_sigma *
  173. (cos_2sigma_m + C * cos_sigma * (-1 + 2 * sqr_cos_2sigma_m))
  174. );
  175. loop_limit -= 1;
  176. } while (Math.Abs(lambda - last_lambda) > threshold && loop_limit > 0);
  177. // As in Vincenty 1975, "The inverse formula may give no
  178. // solution over a line between two nearly antipodal points":
  179. if (loop_limit == 0)
  180. {
  181. distance = double.NaN;
  182. bearing = Angle.FromAngleInDegrees(0.0);
  183. return;
  184. }
  185. double sqr_u = Math.Pow(Math.Cos(alpha), 2) *
  186. (e.A * e.A - e.B * e.B) / (e.B * e.B);
  187. double A = 1 + sqr_u / 16384 * (4096 + sqr_u * (-768 + sqr_u * (320 - 175 * sqr_u)));
  188. double B = sqr_u / 1024 * (256 + sqr_u * (-128 + sqr_u * (74 - 47 * sqr_u)));
  189. double delta_sigma =
  190. B * sin_sigma *
  191. (cos_2sigma_m + B / 4 *
  192. (cos_sigma * (-1 + 2 * sqr_cos_2sigma_m) -
  193. B / 6 * cos_2sigma_m * (-3 + 4 * Math.Pow(sin_sigma, 2)) *
  194. (-3 + 4 * sqr_cos_2sigma_m)
  195. ));
  196. distance = e.B * A * (sigma - delta_sigma);
  197. bearing = Angle.FromHeadingInDegrees(180.0 / Math.PI *
  198. Math.Atan2(cos_U2 * Math.Sin(lambda), cos_U1 * sin_U2 - sin_U1 * cos_U2 * Math.Cos(lambda)));
  199. }
  200. private static void TotalDegreesToDegreesMinutesSeconds(double totalDegrees,
  201. out int degrees, out int minutes, out double seconds)
  202. {
  203. degrees = (int)totalDegrees;
  204. double totalMinutes = (Math.Abs(totalDegrees - degrees)) * 60.0;
  205. minutes = (int)totalMinutes;
  206. seconds = (Math.Abs(totalMinutes - minutes)) * 60.0;
  207. }
  208. private static string TotalDegreesToString(double totalDegrees, string positiveHemisphere,
  209. string negativeHemisphere)
  210. {
  211. int degrees, minutes;
  212. double seconds;
  213. TotalDegreesToDegreesMinutesSeconds(totalDegrees, out degrees, out minutes, out seconds);
  214. return String.Format("{0}° {1}' {2:0.000}\" {3}", Math.Abs(degrees), minutes,
  215. seconds, degrees >= 0.0 ? positiveHemisphere : negativeHemisphere);
  216. }
  217. public string LatitudeString
  218. {
  219. get
  220. {
  221. return TotalDegreesToString(Latitude, "N", "S");
  222. }
  223. }
  224. public string LongitudeString
  225. {
  226. get
  227. {
  228. return TotalDegreesToString(Longitude, "E", "W");
  229. }
  230. }
  231. public override bool Equals(object obj)
  232. {
  233. if (!(obj is Position)) return false;
  234. Position rhs = (Position)obj;
  235. return Longitude == rhs.Longitude && Latitude == rhs.Latitude;
  236. }
  237. public override int GetHashCode()
  238. {
  239. return Longitude.GetHashCode() ^ Latitude.GetHashCode();
  240. }
  241. public static bool operator==(Position lhs, Position rhs)
  242. {
  243. return lhs.Longitude == rhs.Longitude && lhs.Latitude == rhs.Latitude;
  244. }
  245. public static bool operator!=(Position lhs, Position rhs)
  246. {
  247. return lhs.Longitude != rhs.Longitude || lhs.Latitude != rhs.Latitude;
  248. }
  249. }
  250. }