Enu座標からEcef座標への変換に対応

diff --git a/Yubeshi/EnuCoordinate.cs b/Yubeshi/EnuCoordinate.cs
index 1129628..0ea784e 100755 (executable)
--- a/Yubeshi/EnuCoordinate.cs
+++ b/Yubeshi/EnuCoordinate.cs
@@ -20,11 +20,17 @@ namespace Yubeshi
         #region constructors\r
 \r
         public EnuCoordinate(double e, double n, double u)\r
+            : this(e, n, u, null)\r
+        {\r
+        }\r
+\r
+        public EnuCoordinate(double e, double n, double u, \r
+                                                        EcefCoordinate origin)\r
         {\r
             E = e;\r
             N = n;\r
             U = u;\r
-            Origin = null;\r
+            Origin = origin;\r
         }\r
 \r
         public EnuCoordinate(EcefCoordinate target, EcefCoordinate origin)\r
@@ -46,14 +52,12 @@ namespace Yubeshi
             double dx = target.X - origin.X;\r
             double dy = target.Y - origin.Y;\r
             double dz = target.Z - origin.Z;\r
-            double d = Math.Sqrt(dx * dx + dy * dy + dz * dz);\r
             double longitude = llhOrigin.Longitude.Radian;\r
             double latitude = llhOrigin.Latitude.Radian;\r
             double sinLongitude = Math.Sin(longitude);\r
             double cosLongitude = Math.Cos(longitude);\r
             double sinLatitude = Math.Sin(latitude);\r
             double cosLatitude = Math.Cos(latitude);\r
-\r
             \r
             E = -sinLongitude * dx + \r
                     cosLongitude * dy;\r
@@ -95,5 +99,30 @@ namespace Yubeshi
 \r
         #endregion\r
 \r
+        #region public methods\r
+        public EcefCoordinate ToEcefCoordinate()\r
+        {\r
+            GeodeticCoordinate llhOrigin = Origin.ToGeodeticCoordinate();\r
+            \r
+            double longitude = llhOrigin.Longitude.Radian;\r
+            double latitude = llhOrigin.Latitude.Radian;\r
+            double sinLongitude = Math.Sin(longitude);\r
+            double cosLongitude = Math.Cos(longitude);\r
+            double sinLatitude = Math.Sin(latitude);\r
+            double cosLatitude = Math.Cos(latitude);\r
+\r
+            double dx = -sinLongitude * E -\r
+                            sinLatitude*cosLongitude * N + \r
+                                cosLatitude*cosLongitude * U;\r
+            double dy = cosLongitude * E -\r
+                            sinLatitude*sinLongitude * N + \r
+                                cosLatitude*sinLongitude * U;\r
+            double dz = cosLatitude * N + sinLatitude * U;\r
+            \r
+            return new EcefCoordinate(\r
+                                dx + Origin.X, dy + Origin.Y, + dz + Origin.Z);\r
+        }\r
+\r
+        #endregion\r
     }\r
 }\r