1 | /************************************************************************************ |
---|
2 | * Copyright (C) 2008 by Politehnica University of Bucharest and Rutgers University |
---|
3 | * All rights reserved. |
---|
4 | * Refer to LICENSE for terms and conditions of use. |
---|
5 | ***********************************************************************************/ |
---|
6 | package vnsim.map.utils; |
---|
7 | |
---|
8 | /** |
---|
9 | * @author Victor-Radu |
---|
10 | */ |
---|
11 | |
---|
12 | |
---|
13 | |
---|
14 | import java.io.RandomAccessFile; |
---|
15 | import java.io.FileNotFoundException; |
---|
16 | import java.io.IOException; |
---|
17 | import java.util.Collections; |
---|
18 | |
---|
19 | import vnsim.map.object.*; |
---|
20 | |
---|
21 | |
---|
22 | public class GPSutil { |
---|
23 | |
---|
24 | /* more information at: |
---|
25 | * http://home.online.no/~sigurdhu/Deg_formats.htm |
---|
26 | * http://home.online.no/~sigurdhu/Grid_1deg.htm |
---|
27 | */ |
---|
28 | |
---|
29 | |
---|
30 | /* mLat[latitude_degree] = 1 minute of latitude in meters |
---|
31 | * latitude_degree is between 0 and 90 |
---|
32 | */ |
---|
33 | static double[] mLat = { |
---|
34 | 1842.90, 1842.91, 1842.93, 1842.96, 1842.99, 1843.05, 1843.11, |
---|
35 | 1843.18, 1843.26, 1843.36, 1843.46, 1843.58, 1843.70, 1843.84, |
---|
36 | 1843.99, 1844.14, 1844.31, 1844.49, 1844.67, 1844.87, 1845.07, |
---|
37 | 1845.28, 1845.50, 1845.73, 1845.97, 1846.21, 1846.47, 1846.73, |
---|
38 | 1846.99, 1847.26, 1847.54, 1847.82, 1848.11, 1848.41, 1848.71, |
---|
39 | 1849.01, 1849.32, 1849.63, 1849.94, 1850.26, 1850.58, 1850.90, |
---|
40 | 1851.22, 1851.55, 1851.87, 1852.20, 1852.52, 1852.85, 1853.17, |
---|
41 | 1853.50, 1853.82, 1854.14, 1854.46, 1854.77, 1855.08, 1855.39, |
---|
42 | 1855.70, 1856.00, 1856.29, 1856.59, 1856.87, 1857.15, 1857.43, |
---|
43 | 1857.69, 1857.96, 1858.21, 1858.46, 1858.70, 1858.93, 1859.15, |
---|
44 | 1859.37, 1859.57, 1859.77, 1859.96, 1860.14, 1860.31, 1860.47, |
---|
45 | 1860.61, 1860.75, 1860.88, 1861.00, 1861.11, 1861.20, 1861.29, |
---|
46 | 1861.36, 1861.42, 1861.47, 1861.51, 1861.54, 1861.56, 1861.57 |
---|
47 | }; |
---|
48 | |
---|
49 | /* mLon[latitude_degree] = 1 minute of longitude in meters |
---|
50 | * latitude_degree is between 0 and 90 |
---|
51 | */ |
---|
52 | static double[] mLon = { |
---|
53 | 1855.32, 1855.04, 1854.20, 1852.80, 1850.84, 1848.31, 1845.23, |
---|
54 | 1841.59, 1837.39, 1832.63, 1827.32, 1821.46, 1815.04, 1808.08, |
---|
55 | 1800.57, 1792.51, 1783.91, 1774.76, 1765.08, 1754.87, 1744.12, |
---|
56 | 1732.84, 1721.04, 1708.71, 1695.86, 1682.50, 1668.63, 1654.25, |
---|
57 | 1639.36, 1623.98, 1608.10, 1591.74, 1574.89, 1557.55, 1539.75, |
---|
58 | 1521.47, 1502.73, 1483.53, 1463.87, 1443.77, 1423.23, 1402.25, |
---|
59 | 1380.85, 1359.02, 1336.77, 1314.11, 1291.05, 1267.60, 1243.76, |
---|
60 | 1219.53, 1194.93, 1169.96, 1144.63, 1118.95, 1092.93, 1066.57, |
---|
61 | 1039.88, 1012.87, 985.55, 957.92, 930.00, 901.79, 873.30, |
---|
62 | 844.55, 815.53, 786.26, 756.75, 727.00, 697.03, 666.84, |
---|
63 | 636.44, 605.85, 575.07, 544.11, 512.99, 481.70, 450.26, |
---|
64 | 418.69, 386.99, 355.16, 323.22, 291.19, 259.06, 226.86, |
---|
65 | 194.58, 162.24, 129.85, 97.43, 64.97, 32.49, 0.00 |
---|
66 | }; |
---|
67 | |
---|
68 | /* return number of Km / latitude degree at current |
---|
69 | * latitude position |
---|
70 | */ |
---|
71 | public static double getKmDegLat(double latitude) |
---|
72 | { |
---|
73 | double km; |
---|
74 | int k; |
---|
75 | if (latitude < 0.0) { |
---|
76 | latitude = -latitude; |
---|
77 | } |
---|
78 | k = (int) latitude; |
---|
79 | if (k >= 90) { |
---|
80 | km = mLat[90] * 0.06; // * 60 minutes / 1000 meters |
---|
81 | } else { |
---|
82 | km = (mLat[k] + (mLat[k + 1] - mLat[k]) * (latitude - k)) * 0.06; |
---|
83 | } |
---|
84 | return km; |
---|
85 | } |
---|
86 | |
---|
87 | /* return number of Km / longitude degree at current |
---|
88 | * _latitude_ position |
---|
89 | */ |
---|
90 | public static double getKmDegLong(double latitude) |
---|
91 | { |
---|
92 | double km; |
---|
93 | int k; |
---|
94 | if (latitude < 0.0) { |
---|
95 | latitude = -latitude; |
---|
96 | } |
---|
97 | k = (int) latitude; |
---|
98 | if (k >= 90) { |
---|
99 | km = mLon[90] * 0.06; // * 60 minutes / 1000 meters |
---|
100 | } else { |
---|
101 | km = (mLon[k] + (mLon[k + 1] - mLon[k]) * (latitude - k)) * 0.06; |
---|
102 | } |
---|
103 | return km; |
---|
104 | } |
---|
105 | |
---|
106 | /* distance [Km] from A to B, length of segment AB */ |
---|
107 | public static double dist(Point a, Point b, |
---|
108 | double KmDegLong, double KmDegLat) |
---|
109 | { |
---|
110 | double x, y; |
---|
111 | x = (a.getLongitude() - b.getLongitude()) * KmDegLong; |
---|
112 | y = (a.getLatitude() - b.getLatitude()) * KmDegLat; |
---|
113 | // System.out.println(x+" "+y +" "+KmDegLong +" "+KmDegLat); |
---|
114 | return java.lang.Math.sqrt(x * x + y * y); |
---|
115 | } |
---|
116 | |
---|
117 | public static double distance(Point a, Point b) |
---|
118 | { |
---|
119 | double dAB = (a.getLatitude() + b.getLatitude()) / 2.0; |
---|
120 | return dist(a, b, getKmDegLong(dAB), getKmDegLat(dAB)); |
---|
121 | } |
---|
122 | |
---|
123 | /* dot product between 2 segments AB and CD */ |
---|
124 | public static double dotProduct(Point a, Point b, |
---|
125 | double KmDegLongAB, double KmDegLatAB, |
---|
126 | Point c, Point d, |
---|
127 | double KmDegLongCD, double KmDegLatCD) |
---|
128 | { |
---|
129 | return (KmDegLongAB * KmDegLongCD * |
---|
130 | (b.getLongitude() - a.getLongitude()) * |
---|
131 | (d.getLongitude() - c.getLongitude()) + |
---|
132 | KmDegLatAB * KmDegLatCD * |
---|
133 | (b.getLatitude() - a.getLatitude()) * |
---|
134 | (d.getLatitude() - c.getLatitude())); |
---|
135 | } |
---|
136 | |
---|
137 | public static double crossProduct(Point a, Point b, |
---|
138 | double KmDegLongAB, double KmDegLatAB, |
---|
139 | Point c, Point d, |
---|
140 | double KmDegLongCD, double KmDegLatCD) |
---|
141 | { |
---|
142 | return (KmDegLatAB * KmDegLongCD * |
---|
143 | (b.getLatitude() - a.getLatitude()) * |
---|
144 | (d.getLongitude() - c.getLongitude()) - |
---|
145 | KmDegLongAB * KmDegLatCD * |
---|
146 | (b.getLongitude() - a.getLongitude()) * |
---|
147 | (d.getLatitude() - c.getLatitude())); |
---|
148 | } |
---|
149 | |
---|
150 | /* cosine of angle between segments AB and CD */ |
---|
151 | public static double cosAngle(Point a, Point b, Point c, Point d) |
---|
152 | { |
---|
153 | double dAB = (a.getLatitude() + b.getLatitude()) / 2.0; |
---|
154 | double dCD = (c.getLatitude() + d.getLatitude()) / 2.0; |
---|
155 | double KmDegLatAB = getKmDegLat(dAB); |
---|
156 | double KmDegLatCD = getKmDegLat(dCD); |
---|
157 | double dLAB = (a.getLongitude() + b.getLongitude()) / 2.0; |
---|
158 | double dLCD = (c.getLongitude() + d.getLongitude()) / 2.0; |
---|
159 | double KmDegLongAB = getKmDegLong(dLAB); |
---|
160 | double KmDegLongCD = getKmDegLong(dLCD); |
---|
161 | return dotProduct(a, b, KmDegLongAB, KmDegLatAB, c, d, |
---|
162 | KmDegLongCD, KmDegLatCD) / |
---|
163 | (dist(a, b, KmDegLongAB, KmDegLatAB) * |
---|
164 | dist(c, d, KmDegLongCD, KmDegLatCD)); |
---|
165 | } |
---|
166 | |
---|
167 | public static double sinAngle(Point a, Point b, Point c, Point d) |
---|
168 | { |
---|
169 | double dAB = (a.getLatitude() + b.getLatitude()) / 2.0; |
---|
170 | double dCD = (c.getLatitude() + d.getLatitude()) / 2.0; |
---|
171 | double KmDegLatAB = getKmDegLat(dAB); |
---|
172 | double KmDegLatCD = getKmDegLat(dCD); |
---|
173 | double dLAB = (a.getLongitude() + b.getLongitude()) / 2.0; |
---|
174 | double dLCD = (c.getLongitude() + d.getLongitude()) / 2.0; |
---|
175 | double KmDegLongAB = getKmDegLong(dLAB); |
---|
176 | double KmDegLongCD = getKmDegLong(dLCD); |
---|
177 | return crossProduct(a, b, KmDegLongAB, KmDegLatAB, c, d, |
---|
178 | KmDegLongCD, KmDegLatCD) / |
---|
179 | (dist(a, b, KmDegLongAB, KmDegLatAB) * |
---|
180 | dist(c, d, KmDegLongCD, KmDegLatCD)); |
---|
181 | } |
---|
182 | |
---|
183 | //returns the distace in meters between the latitude of A and the latitude of B |
---|
184 | public static double getMetersLatitude(Point A,Point B) |
---|
185 | { |
---|
186 | double result=0; |
---|
187 | int i; |
---|
188 | double up, down,j,fraction; |
---|
189 | up=Math.floor(B.getLatitude()); |
---|
190 | down=Math.ceil(A.getLatitude()); |
---|
191 | for(i=(int)down;i<up;i++) |
---|
192 | { |
---|
193 | result=result+GPSutil.getKmDegLat((double)i); |
---|
194 | } |
---|
195 | |
---|
196 | j=getKmDegLat(up); |
---|
197 | fraction=B.getLatitude()-up; |
---|
198 | result=result+fraction*j; |
---|
199 | j=getKmDegLat(down-1); |
---|
200 | fraction=down-1-A.getLatitude(); |
---|
201 | result=result+fraction*j; |
---|
202 | result=result*1000;//returns meters |
---|
203 | return result; |
---|
204 | |
---|
205 | } |
---|
206 | |
---|
207 | /** |
---|
208 | * translates the point p along the longitude with the specified distance |
---|
209 | * |
---|
210 | * @param p |
---|
211 | * @param distance |
---|
212 | * @return the new latitude coordinate |
---|
213 | */ |
---|
214 | public static double addToLatitude(Point p, double distance){ |
---|
215 | //distance < 1 km |
---|
216 | double latitude; |
---|
217 | double up, down; |
---|
218 | double deg; |
---|
219 | double py = p.getLatitude(); |
---|
220 | |
---|
221 | int k = (int) p.getLatitude(); |
---|
222 | int idx = Math.abs(k); |
---|
223 | |
---|
224 | if (idx >= 90) { |
---|
225 | deg = distance / (mLat[90] * 0.06); // * 60 minutes / 1000 meters |
---|
226 | latitude = py + deg; |
---|
227 | } else { |
---|
228 | double a = (mLat[idx+1] - mLat[idx]) / 2; |
---|
229 | double b = (mLat[idx] - k * (mLat[idx+1] - mLat[idx])); |
---|
230 | double c = -mLat[idx] * py - py * py * (mLat[idx + 1] - mLat[idx]) / 2 |
---|
231 | + (mLat[idx + 1] - mLat[idx]) * k * py - distance / 0.06; |
---|
232 | |
---|
233 | latitude = (- b + Math.sqrt(b * b - 4 * a * c)) / (2 * a); |
---|
234 | } |
---|
235 | return latitude; |
---|
236 | } |
---|
237 | |
---|
238 | /** translates the point p along the latitude with the specified distance |
---|
239 | * |
---|
240 | * @param p |
---|
241 | * @param distance |
---|
242 | * @return the new longitude coordinate |
---|
243 | */ |
---|
244 | public static double addToLongitude(Point p, double distance){ |
---|
245 | double longitude; |
---|
246 | double up, down; |
---|
247 | |
---|
248 | double deg; |
---|
249 | double py = p.getLatitude(); |
---|
250 | |
---|
251 | int k = (int) p.getLatitude(); |
---|
252 | int idx = Math.abs(k); |
---|
253 | |
---|
254 | if (idx >= 90) { |
---|
255 | deg = distance / (mLon[90] * 0.06); // * 60 minutes / 1000 meters |
---|
256 | longitude = p.getLongitude() + deg; |
---|
257 | } else { |
---|
258 | longitude = p.getLongitude() + distance |
---|
259 | / ((mLon[idx] + (mLon[idx + 1] - mLon[idx]) * Math.abs(py - k)) * 0.06); |
---|
260 | } |
---|
261 | return longitude; |
---|
262 | } |
---|
263 | |
---|
264 | public static PeanoKey getMaxSearchBoundPK(Point p, double distance){ |
---|
265 | PeanoKey origpk = new PeanoKey(p); |
---|
266 | double longitude = GPSutil.addToLongitude(p,distance); |
---|
267 | double latitude = GPSutil.addToLatitude(p,distance); |
---|
268 | PeanoKey maxpk = new PeanoKey(new Point(longitude, latitude)); |
---|
269 | if (distance > 0){ |
---|
270 | maxpk.setToUpperCorner(origpk); |
---|
271 | }else{ |
---|
272 | maxpk.setToLowerCorner(origpk); |
---|
273 | } |
---|
274 | return maxpk; |
---|
275 | } |
---|
276 | |
---|
277 | //returns the distace in meters between the longitude of A and the longitude of B |
---|
278 | public static double getMetersLongitude(Point A,Point B) |
---|
279 | { |
---|
280 | double result=0; |
---|
281 | double up, down,j,fraction; |
---|
282 | up=Math.floor(B.getLatitude()); |
---|
283 | down=Math.floor(A.getLatitude()); |
---|
284 | j=GPSutil.getKmDegLong(up)+GPSutil.getKmDegLong(down); |
---|
285 | j=j/2; |
---|
286 | fraction=B.getLongitude()-A.getLongitude(); |
---|
287 | result=fraction*j; |
---|
288 | result=result*1000;//returns meters |
---|
289 | return result; |
---|
290 | |
---|
291 | } |
---|
292 | |
---|
293 | |
---|
294 | } |
---|