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 \r
  12 namespace Yubeshi\r
  13 {\r
  14     public class EcefCoordinate\r
  15     {\r
  16         #region fields\r
  17         private const double a = Constants.SemiMajorAxisA;\r
  18         private const double b = Constants.SemiMajorAxisB;\r
  19         private const double e1sq = Constants.FirstEccentricitySquared;\r
  20         private const double e2sq = Constants.SecondEccentricitySquared;\r
  21         private const double pi = Constants.Pi;\r
  22         #endregion\r
  23 \r
  24         #region constructors\r
  25 \r
  26         public EcefCoordinate(double x, double y, double z)\r
  27             : this(x, y, z, Double.NaN)\r
  28         {\r
  29         }\r
  30 \r
  31         public EcefCoordinate(double x, double y, double z, double accuracy)\r
  32         {\r
  33             X = x;\r
  34             Y = y;\r
  35             Z = z;\r
  36             Accuracy = accuracy;\r
  37         }\r
  38 \r
  39         /// <summary>\r
  40         ///     \r
  41         /// </summary>\r
  42         /// <param name="x">ECEF X coordinate in cm</param>\r
  43         /// <param name="y">ECEF Y coordinate in cm</param>\r
  44         /// <param name="z">ECEF Z coordinate in cm</param>\r
  45         /// <param name="accuracy">3D Position Accuracy Estimate in cm</param>\r
  46         public EcefCoordinate(int x, int y, int z, uint accuracy)\r
  47             : this(x * 0.01, y * 0.01, z * 0.01, accuracy * 0.01)\r
  48         {\r
  49         }\r
  50         #endregion\r
  51 \r
  52         #region properties\r
  53         public double X\r
  54         {\r
  55             get;\r
  56             set;\r
  57         }\r
  58 \r
  59         public double Y\r
  60         {\r
  61             get;\r
  62             set;\r
  63         }\r
  64 \r
  65         public double Z\r
  66         {\r
  67             get;\r
  68             set;\r
  69         }\r
  70 \r
  71         public double Accuracy\r
  72         {\r
  73             get;\r
  74             set;\r
  75         }\r
  76         #endregion\r
  77 \r
  78         #region public methods\r
  79         public GeodeticCoordinate ToGeodeticCoordinate()\r
  80         {\r
  81             // approximation\r
  82             double lambda = Math.Atan2(Y, X);\r
  83             double p = Math.Sqrt(X * X + Y * Y);\r
  84             double theta = Math.Atan2(Z * a, p * b);\r
  85             double sin = Math.Sin(theta);\r
  86             double cos = Math.Cos(theta);\r
  87 \r
  88             double phi = Math.Atan2(Z + e2sq * b * sin * sin * sin,\r
  89                                             p - e1sq * a * cos * cos * cos);\r
  90             double sinPhi = Math.Sin(phi);\r
  91             double khi = Math.Sqrt(1.0 - e1sq * sinPhi * sinPhi);\r
  92             double n = a / khi;\r
  93             double h = p / Math.Cos(phi) - n;\r
  94             return new GeodeticCoordinate(phi * 180 / pi, lambda * 180 / pi, h);\r
  95         }\r
  96         #endregion\r
  97     }\r
  98 }\r