Yubeshi/EnuCoordinate.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 EnuCoordinate\r
  15     {\r
  16         #region fields\r
  17         \r
  18         #endregion\r
  19 \r
  20         #region constructors\r
  21 \r
  22         public EnuCoordinate(double e, double n, double u)\r
  23         {\r
  24             E = e;\r
  25             N = n;\r
  26             U = u;\r
  27             Origin = null;\r
  28         }\r
  29 \r
  30         public EnuCoordinate(EcefCoordinate target, EcefCoordinate origin)\r
  31             : this(target, origin, origin.ToGeodeticCoordinate())\r
  32         {\r
  33         }\r
  34 \r
  35         public EnuCoordinate(GeodeticCoordinate target,\r
  36                                                     GeodeticCoordinate origin)\r
  37             : this(target.ToEcefCoordinate(), \r
  38                                             origin.ToEcefCoordinate(), origin)\r
  39         {\r
  40         }\r
  41 \r
  42         private EnuCoordinate(EcefCoordinate target,\r
  43                         EcefCoordinate origin, GeodeticCoordinate llhOrigin)\r
  44         {\r
  45             Origin = origin;\r
  46             double dx = target.X - origin.X;\r
  47             double dy = target.Y - origin.Y;\r
  48             double dz = target.Z - origin.Z;\r
  49             double d = Math.Sqrt(dx * dx + dy * dy + dz * dz);\r
  50             double longitude = llhOrigin.Longitude.Radian;\r
  51             double latitude = llhOrigin.Latitude.Radian;\r
  52             double sinLongitude = Math.Sin(longitude);\r
  53             double cosLongitude = Math.Cos(longitude);\r
  54             double sinLatitude = Math.Sin(latitude);\r
  55             double cosLatitude = Math.Cos(latitude);\r
  56 \r
  57             \r
  58             E = -sinLongitude * dx + \r
  59                     cosLongitude * dy;\r
  60             N = -sinLatitude * cosLongitude * dx -\r
  61                     sinLatitude * sinLongitude * dy +\r
  62                         cosLatitude * dz;\r
  63             U = cosLatitude * cosLongitude * dx +\r
  64                     cosLatitude * sinLongitude * dy +\r
  65                         sinLatitude * dz;\r
  66         }\r
  67 \r
  68         #endregion\r
  69 \r
  70         #region properties\r
  71 \r
  72         public double E\r
  73         {\r
  74             get;\r
  75             set;\r
  76         }\r
  77 \r
  78         public double N\r
  79         {\r
  80             get;\r
  81             set;\r
  82         }\r
  83 \r
  84         public double U\r
  85         {\r
  86             get;\r
  87             set;\r
  88         }\r
  89 \r
  90         public EcefCoordinate Origin\r
  91         {\r
  92             get;\r
  93             private set;\r
  94         }\r
  95 \r
  96         #endregion\r
  97 \r
  98     }\r
  99 }\r