From fa6e0acceb3bd52a83a11f20e9853425968d858b Mon Sep 17 00:00:00 2001
From: kimikage <kimikage_ceo@hotmail.com>
Date: Mon, 28 Feb 2011 22:53:16 +0900
Subject: [PATCH] =?utf8?q?Enu=E5=BA=A7=E6=A8=99=E3=81=8B=E3=82=89Ecef?=
 =?utf8?q?=E5=BA=A7=E6=A8=99=E3=81=B8=E3=81=AE=E5=A4=89=E6=8F=9B=E3=81=AB?=
 =?utf8?q?=E5=AF=BE=E5=BF=9C?=
MIME-Version: 1.0
Content-Type: text/plain; charset=utf8
Content-Transfer-Encoding: 8bit

---
 Yubeshi/EnuCoordinate.cs | 35 ++++++++++++++++++++++++++++++++---
 1 file changed, 32 insertions(+), 3 deletions(-)

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