[31] | 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 | } |
---|