Yubeshi/EcefCoordinate.cs

   1 /*\r
   2  *      Yubeshi GPS Parser\r
   3  *\r
   4  *      This software is distributed under a zlib-style license.\r
   5  *      See license.txt for more information.\r
   6  */\r
   7 \r
   8 using System;\r
   9 using System.Collections.Generic;\r
  10 using System.Text;\r
  11 using Const = Yubeshi.Constants.Wgs84;\r
  12 \r
  13 namespace Yubeshi\r
  14 {\r
  15     public class EcefCoordinate\r
  16     {\r
  17         #region fields\r
  18         private const double a = Const.SemiMajorAxisA;\r
  19         private const double b = Const.SemiMajorAxisB;\r
  20         private const double e1sq = Const.FirstEccentricitySquared;\r
  21         private const double e2sq = Const.SecondEccentricitySquared;\r
  22         private const double pi = Const.Pi;\r
  23         #endregion\r
  24 \r
  25         #region constructors\r
  26 \r
  27         public EcefCoordinate(double x, double y, double z)\r
  28             : this(x, y, z, Double.NaN)\r
  29         {\r
  30         }\r
  31 \r
  32         public EcefCoordinate(double x, double y, double z, double accuracy)\r
  33         {\r
  34             X = x;\r
  35             Y = y;\r
  36             Z = z;\r
  37             Accuracy = accuracy;\r
  38         }\r
  39         #endregion\r
  40 \r
  41         #region properties\r
  42         public double X\r
  43         {\r
  44             get;\r
  45             set;\r
  46         }\r
  47 \r
  48         public double Y\r
  49         {\r
  50             get;\r
  51             set;\r
  52         }\r
  53 \r
  54         public double Z\r
  55         {\r
  56             get;\r
  57             set;\r
  58         }\r
  59 \r
  60         public double Accuracy\r
  61         {\r
  62             get;\r
  63             set;\r
  64         }\r
  65         #endregion\r
  66 \r
  67         #region public methods\r
  68         public GeodeticCoordinate ToGeodeticCoordinate()\r
  69         {\r
  70             // approximation\r
  71             double lambda = Math.Atan2(Y, X);\r
  72             double p = Math.Sqrt(X * X + Y * Y);\r
  73             double theta = Math.Atan2(Z * a, p * b);\r
  74             double sin = Math.Sin(theta);\r
  75             double cos = Math.Cos(theta);\r
  76 \r
  77             double phi = Math.Atan2(Z + e2sq * b * sin * sin * sin,\r
  78                                             p - e1sq * a * cos * cos * cos);\r
  79             double sinPhi = Math.Sin(phi);\r
  80             double cosPhi = Math.Cos(phi);\r
  81             double khi = Math.Sqrt(1.0 - e1sq * sinPhi * sinPhi);\r
  82             double n = a / khi;\r
  83             double ht;\r
  84             if (cosPhi < 0.5)\r
  85             {\r
  86                 ht = Z / sinPhi - n * (1.0 - e1sq);\r
  87             }\r
  88             else\r
  89             { \r
  90                 ht = p / Math.Cos(phi) - n;\r
  91             }\r
  92             Height h = new Height(ht, Height.Base.Wgs84Ellipsoid);\r
  93             return new GeodeticCoordinate(\r
  94                                         phi * 180 / pi, lambda * 180 / pi, h);\r
  95         }\r
  96         #endregion\r
  97     }\r
  98 }\r