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             : this(e, n, u, null)\r
  24         {\r
  25         }\r
  26 \r
  27         public EnuCoordinate(double e, double n, double u, \r
  28                                                         EcefCoordinate origin)\r
  29         {\r
  30             E = e;\r
  31             N = n;\r
  32             U = u;\r
  33             Origin = origin;\r
  34         }\r
  35 \r
  36         public EnuCoordinate(EcefCoordinate target, EcefCoordinate origin)\r
  37             : this(target, origin, origin.ToGeodeticCoordinate())\r
  38         {\r
  39         }\r
  40 \r
  41         public EnuCoordinate(GeodeticCoordinate target,\r
  42                                                     GeodeticCoordinate origin)\r
  43             : this(target.ToEcefCoordinate(), \r
  44                                             origin.ToEcefCoordinate(), origin)\r
  45         {\r
  46         }\r
  47 \r
  48         private EnuCoordinate(EcefCoordinate target,\r
  49                         EcefCoordinate origin, GeodeticCoordinate llhOrigin)\r
  50         {\r
  51             Origin = origin;\r
  52             double dx = target.X - origin.X;\r
  53             double dy = target.Y - origin.Y;\r
  54             double dz = target.Z - origin.Z;\r
  55             double longitude = llhOrigin.Longitude.Radian;\r
  56             double latitude = llhOrigin.Latitude.Radian;\r
  57             double sinLongitude = Math.Sin(longitude);\r
  58             double cosLongitude = Math.Cos(longitude);\r
  59             double sinLatitude = Math.Sin(latitude);\r
  60             double cosLatitude = Math.Cos(latitude);\r
  61             \r
  62             E = -sinLongitude * dx + \r
  63                     cosLongitude * dy;\r
  64             N = -sinLatitude * cosLongitude * dx -\r
  65                     sinLatitude * sinLongitude * dy +\r
  66                         cosLatitude * dz;\r
  67             U = cosLatitude * cosLongitude * dx +\r
  68                     cosLatitude * sinLongitude * dy +\r
  69                         sinLatitude * dz;\r
  70         }\r
  71 \r
  72         #endregion\r
  73 \r
  74         #region properties\r
  75 \r
  76         public double E\r
  77         {\r
  78             get;\r
  79             set;\r
  80         }\r
  81 \r
  82         public double N\r
  83         {\r
  84             get;\r
  85             set;\r
  86         }\r
  87 \r
  88         public double U\r
  89         {\r
  90             get;\r
  91             set;\r
  92         }\r
  93 \r
  94         public EcefCoordinate Origin\r
  95         {\r
  96             get;\r
  97             private set;\r
  98         }\r
  99 \r
 100         #endregion\r
 101 \r
 102         #region public methods\r
 103         public EcefCoordinate ToEcefCoordinate()\r
 104         {\r
 105             GeodeticCoordinate llhOrigin = Origin.ToGeodeticCoordinate();\r
 106             \r
 107             double longitude = llhOrigin.Longitude.Radian;\r
 108             double latitude = llhOrigin.Latitude.Radian;\r
 109             double sinLongitude = Math.Sin(longitude);\r
 110             double cosLongitude = Math.Cos(longitude);\r
 111             double sinLatitude = Math.Sin(latitude);\r
 112             double cosLatitude = Math.Cos(latitude);\r
 113 \r
 114             double dx = -sinLongitude * E -\r
 115                             sinLatitude*cosLongitude * N + \r
 116                                 cosLatitude*cosLongitude * U;\r
 117             double dy = cosLongitude * E -\r
 118                             sinLatitude*sinLongitude * N + \r
 119                                 cosLatitude*sinLongitude * U;\r
 120             double dz = cosLatitude * N + sinLatitude * U;\r
 121             \r
 122             return new EcefCoordinate(\r
 123                                 dx + Origin.X, dy + Origin.Y, + dz + Origin.Z);\r
 124         }\r
 125 \r
 126         #endregion\r
 127     }\r
 128 }\r