OSDN Git Service

LLH座標系に加えECEF座標系を追加
[yubeshi/yubeshi.git] / Yubeshi / EcefCoordinate.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 EcefCoordinate\r
15     {\r
16         #region fields\r
17         private const double a = Constants.SemiMajorAxisA;\r
18         private const double b = Constants.SemiMajorAxisB;\r
19         private const double e1sq = Constants.FirstEccentricitySquared;\r
20         private const double e2sq = Constants.SecondEccentricitySquared;\r
21         private const double pi = Constants.Pi;\r
22         #endregion\r
23 \r
24         #region constructors\r
25 \r
26         public EcefCoordinate(double x, double y, double z)\r
27             : this(x, y, z, Double.NaN)\r
28         {\r
29         }\r
30 \r
31         public EcefCoordinate(double x, double y, double z, double accuracy)\r
32         {\r
33             X = x;\r
34             Y = y;\r
35             Z = z;\r
36             Accuracy = accuracy;\r
37         }\r
38 \r
39         /// <summary>\r
40         ///     \r
41         /// </summary>\r
42         /// <param name="x">ECEF X coordinate in cm</param>\r
43         /// <param name="y">ECEF Y coordinate in cm</param>\r
44         /// <param name="z">ECEF Z coordinate in cm</param>\r
45         /// <param name="accuracy">3D Position Accuracy Estimate in cm</param>\r
46         public EcefCoordinate(int x, int y, int z, uint accuracy)\r
47             : this(x * 0.01, y * 0.01, z * 0.01, accuracy * 0.01)\r
48         {\r
49         }\r
50         #endregion\r
51 \r
52         #region properties\r
53         public double X\r
54         {\r
55             get;\r
56             set;\r
57         }\r
58 \r
59         public double Y\r
60         {\r
61             get;\r
62             set;\r
63         }\r
64 \r
65         public double Z\r
66         {\r
67             get;\r
68             set;\r
69         }\r
70 \r
71         public double Accuracy\r
72         {\r
73             get;\r
74             set;\r
75         }\r
76         #endregion\r
77 \r
78         #region public methods\r
79         public GeodeticCoordinate ToGeodeticCoordinate()\r
80         {\r
81             // approximation\r
82             double lambda = Math.Atan2(Y, X);\r
83             double p = Math.Sqrt(X * X + Y * Y);\r
84             double theta = Math.Atan2(Z * a, p * b);\r
85             double sin = Math.Sin(theta);\r
86             double cos = Math.Cos(theta);\r
87 \r
88             double phi = Math.Atan2(Z + e2sq * b * sin * sin * sin,\r
89                                             p - e1sq * a * cos * cos * cos);\r
90             return new GeodeticCoordinate(phi * 180 / pi, lambda * 180 / pi);\r
91         }\r
92         #endregion\r
93     }\r
94 }\r