定数を楕円体モデルごとに整理，Hieghtクラスに対応

[yubeshi/yubeshi.git] / Yubeshi / EcefCoordinate.cs

diff --git a/Yubeshi/EcefCoordinate.cs b/Yubeshi/EcefCoordinate.cs
index d53665f..0efdecf 100755 (executable)
--- a/Yubeshi/EcefCoordinate.cs
+++ b/Yubeshi/EcefCoordinate.cs
@@ -8,17 +8,18 @@
 using System;\r
 using System.Collections.Generic;\r
 using System.Text;\r
+using Const = Yubeshi.Constants.Wgs84;\r
 \r
 namespace Yubeshi\r
 {\r
     public class EcefCoordinate\r
     {\r
         #region fields\r
-        private const double a = Constants.SemiMajorAxisA;\r
-        private const double b = Constants.SemiMajorAxisB;\r
-        private const double e1sq = Constants.FirstEccentricitySquared;\r
-        private const double e2sq = Constants.SecondEccentricitySquared;\r
-        private const double pi = Constants.Pi;\r
+        private const double a = Const.SemiMajorAxisA;\r
+        private const double b = Const.SemiMajorAxisB;\r
+        private const double e1sq = Const.FirstEccentricitySquared;\r
+        private const double e2sq = Const.SecondEccentricitySquared;\r
+        private const double pi = Const.Pi;\r
         #endregion\r
 \r
         #region constructors\r
@@ -76,10 +77,21 @@ namespace Yubeshi
             double phi = Math.Atan2(Z + e2sq * b * sin * sin * sin,\r
                                             p - e1sq * a * cos * cos * cos);\r
             double sinPhi = Math.Sin(phi);\r
+            double cosPhi = Math.Cos(phi);\r
             double khi = Math.Sqrt(1.0 - e1sq * sinPhi * sinPhi);\r
             double n = a / khi;\r
-            double h = p / Math.Cos(phi) - n;\r
-            return new GeodeticCoordinate(phi * 180 / pi, lambda * 180 / pi, h);\r
+            double ht;\r
+            if (cosPhi < 0.5)\r
+            {\r
+                ht = Z / sinPhi - n * (1.0 - e1sq);\r
+            }\r
+            else\r
+            { \r
+                ht = p / Math.Cos(phi) - n;\r
+            }\r
+            Height h = new Height(ht, Height.Base.Wgs84Ellipsoid);\r
+            return new GeodeticCoordinate(\r
+                                        phi * 180 / pi, lambda * 180 / pi, h);\r
         }\r
         #endregion\r
     }\r