1 /* 2 * To change this template, choose Tools | Templates 3 * and open the template in the editor. 4 */ 5 package jme3tools.navigation; 6 7 8 9 /** 10 * A utlity class for performing position calculations 11 * 12 * @author Benjamin Jakobus, based on JMarine (by Cormac Gebruers and Benjamin 13 * Jakobus) 14 * @version 1.0 15 * @since 1.0 16 */ 17 public class NavCalculator { 18 19 private double distance; 20 private double trueCourse; 21 22 /* The earth's radius in meters */ 23 public static final int WGS84_EARTH_RADIUS = 6378137; 24 private String strCourse; 25 26 /* The sailing calculation type */ 27 public static final int MERCATOR = 0; 28 public static final int GC = 1; 29 30 /* The degree precision to use for courses */ 31 public static final int RL_CRS_PRECISION = 1; 32 33 /* The distance precision to use for distances */ 34 public static final int RL_DIST_PRECISION = 1; 35 public static final int METERS_PER_MINUTE = 1852; 36 37 /** 38 * Constructor 39 * @param P1 40 * @param P2 41 * @param calcType 42 * @since 1.0 43 */ NavCalculator(Position P1, Position P2, int calcType)44 public NavCalculator(Position P1, Position P2, int calcType) { 45 switch (calcType) { 46 case MERCATOR: 47 mercatorSailing(P1, P2); 48 break; 49 case GC: 50 greatCircleSailing(P1, P2); 51 break; 52 } 53 } 54 55 /** 56 * Constructor 57 * @since 1.0 58 */ NavCalculator()59 public NavCalculator() { 60 } 61 62 /** 63 * Determines a great circle track between two positions 64 * @param p1 origin position 65 * @param p2 destination position 66 */ greatCircleSailing(Position p1, Position p2)67 public GCSailing greatCircleSailing(Position p1, Position p2) { 68 return new GCSailing(new int[0], new float[0]); 69 } 70 71 /** 72 * Determines a Rhumb Line course and distance between two points 73 * @param p1 origin position 74 * @param p2 destination position 75 */ rhumbLineSailing(Position p1, Position p2)76 public RLSailing rhumbLineSailing(Position p1, Position p2) { 77 RLSailing rl = mercatorSailing(p1, p2); 78 return rl; 79 } 80 81 /** 82 * Determines the rhumb line course and distance between two positions 83 * @param p1 origin position 84 * @param p2 destination position 85 */ mercatorSailing(Position p1, Position p2)86 public RLSailing mercatorSailing(Position p1, Position p2) { 87 88 double dLat = computeDLat(p1.getLatitude(), p2.getLatitude()); 89 //plane sailing... 90 if (dLat == 0) { 91 RLSailing rl = planeSailing(p1, p2); 92 return rl; 93 } 94 95 double dLong = computeDLong(p1.getLongitude(), p2.getLongitude()); 96 double dmp = (float) computeDMPClarkeSpheroid(p1.getLatitude(), p2.getLatitude()); 97 98 trueCourse = (float) Math.toDegrees(Math.atan(dLong / dmp)); 99 double degCrs = convertCourse((float) trueCourse, p1, p2); 100 distance = (float) Math.abs(dLat / Math.cos(Math.toRadians(trueCourse))); 101 102 RLSailing rl = new RLSailing(degCrs, (float) distance); 103 trueCourse = rl.getCourse(); 104 strCourse = (dLat < 0 ? "S" : "N"); 105 strCourse += " " + trueCourse; 106 strCourse += " " + (dLong < 0 ? "W" : "E"); 107 return rl; 108 109 } 110 111 /** 112 * Calculate a plane sailing situation - i.e. where Lats are the same 113 * @param p1 114 * @param p2 115 * @return 116 * @since 1.0 117 */ planeSailing(Position p1, Position p2)118 public RLSailing planeSailing(Position p1, Position p2) { 119 double dLong = computeDLong(p1.getLongitude(), p2.getLongitude()); 120 121 double sgnDLong = 0 - (dLong / Math.abs(dLong)); 122 if (Math.abs(dLong) > 180 * 60) { 123 dLong = (360 * 60 - Math.abs(dLong)) * sgnDLong; 124 } 125 126 double redist = 0; 127 double recourse = 0; 128 if (p1.getLatitude() == 0) { 129 redist = Math.abs(dLong); 130 } else { 131 redist = Math.abs(dLong * (float) Math.cos(p1.getLatitude() * 2 * Math.PI / 360)); 132 } 133 recourse = (float) Math.asin(0 - sgnDLong); 134 recourse = recourse * 360 / 2 / (float) Math.PI; 135 136 if (recourse < 0) { 137 recourse = recourse + 360; 138 } 139 return new RLSailing(recourse, redist); 140 } 141 142 /** 143 * Converts a course from cardinal XddY to ddd notation 144 * @param tc 145 * @param p1 position one 146 * @param p2 position two 147 * @return 148 * @since 1.0 149 */ convertCourse(float tc, Position p1, Position p2)150 public static double convertCourse(float tc, Position p1, Position p2) { 151 152 double dLat = p1.getLatitude() - p2.getLatitude(); 153 double dLong = p1.getLongitude() - p2.getLongitude(); 154 //NE 155 if (dLong >= 0 & dLat >= 0) { 156 return Math.abs(tc); 157 } 158 159 //SE 160 if (dLong >= 0 & dLat < 0) { 161 return 180 - Math.abs(tc); 162 } 163 164 //SW 165 if (dLong < 0 & dLat < 0) { 166 return 180 + Math.abs(tc); 167 } 168 169 //NW 170 if (dLong < 0 & dLat >= 0) { 171 return 360 - Math.abs(tc); 172 } 173 return -1; 174 } 175 176 /** 177 * Getter method for the distance between two points 178 * @return distance 179 * @since 1.0 180 */ getDistance()181 public double getDistance() { 182 return distance; 183 } 184 185 /** 186 * Getter method for the true course 187 * @return true course 188 * @since 1.0 189 */ getTrueCourse()190 public double getTrueCourse() { 191 return trueCourse; 192 } 193 194 /** 195 * Getter method for the true course 196 * @return true course 197 * @since 1.0 198 */ getStrCourse()199 public String getStrCourse() { 200 return strCourse; 201 } 202 203 /** 204 * Computes the difference in meridional parts for two latitudes in minutes 205 * (based on Clark 1880 spheroid) 206 * @param lat1 207 * @param lat2 208 * @return difference in minutes 209 * @since 1.0 210 */ computeDMPClarkeSpheroid(double lat1, double lat2)211 public static double computeDMPClarkeSpheroid(double lat1, double lat2) { 212 double absLat1 = Math.abs(lat1); 213 double absLat2 = Math.abs(lat2); 214 215 double m1 = (7915.704468 * (Math.log(Math.tan(Math.toRadians(45 216 + (absLat1 / 2)))) / Math.log(10)) 217 - 23.268932 * Math.sin(Math.toRadians(absLat1)) 218 - 0.052500 * Math.pow(Math.sin(Math.toRadians(absLat1)), 3) 219 - 0.000213 * Math.pow(Math.sin(Math.toRadians(absLat1)), 5)); 220 221 double m2 = (7915.704468 * (Math.log(Math.tan(Math.toRadians(45 222 + (absLat2 / 2)))) / Math.log(10)) 223 - 23.268932 * Math.sin(Math.toRadians(absLat2)) 224 - 0.052500 * Math.pow(Math.sin(Math.toRadians(absLat2)), 3) 225 - 0.000213 * Math.pow(Math.sin(Math.toRadians(absLat2)), 5)); 226 if ((lat1 <= 0 && lat2 <= 0) || (lat1 > 0 && lat2 > 0)) { 227 return Math.abs(m1 - m2); 228 } else { 229 return m1 + m2; 230 } 231 } 232 233 /** 234 * Computes the difference in meridional parts for a perfect sphere between 235 * two degrees of latitude 236 * @param lat1 237 * @param lat2 238 * @return difference in meridional parts between lat1 and lat2 in minutes 239 * @since 1.0 240 */ computeDMPWGS84Spheroid(float lat1, float lat2)241 public static float computeDMPWGS84Spheroid(float lat1, float lat2) { 242 float absLat1 = Math.abs(lat1); 243 float absLat2 = Math.abs(lat2); 244 245 float m1 = (float) (7915.7045 * Math.log10(Math.tan(Math.toRadians(45 + (absLat1 / 2)))) 246 - 23.01358 * Math.sin(absLat1 - 0.05135) * Math.pow(Math.sin(absLat1), 3)); 247 248 float m2 = (float) (7915.7045 * Math.log10(Math.tan(Math.toRadians(45 + (absLat2 / 2)))) 249 - 23.01358 * Math.sin(absLat2 - 0.05135) * Math.pow(Math.sin(absLat2), 3)); 250 251 if (lat1 <= 0 & lat2 <= 0 || lat1 > 0 & lat2 > 0) { 252 return Math.abs(m1 - m2); 253 } else { 254 return m1 + m2; 255 } 256 } 257 258 /** 259 * Predicts the position of a target for a given time in the future 260 * @param time the number of seconds from now for which to predict the future 261 * position 262 * @param speed the miles per minute that the target is traveling 263 * @param currentLat the target's current latitude 264 * @param currentLong the target's current longitude 265 * @param course the target's current course in degrees 266 * @return the predicted future position 267 * @since 1.0 268 */ predictPosition(int time, double speed, double currentLat, double currentLong, double course)269 public static Position predictPosition(int time, double speed, 270 double currentLat, double currentLong, double course) { 271 Position futurePosition = null; 272 course = Math.toRadians(course); 273 double futureLong = currentLong + speed * time * Math.sin(course); 274 double futureLat = currentLat + speed * time * Math.cos(course); 275 try { 276 futurePosition = new Position(futureLat, futureLong); 277 } catch (InvalidPositionException ipe) { 278 ipe.printStackTrace(); 279 } 280 return futurePosition; 281 282 } 283 284 /** 285 * Computes the coordinate of position B relative to an offset given 286 * a distance and an angle. 287 * 288 * @param offset The offset position. 289 * @param bearing The bearing between the offset and the coordinate 290 * that you want to calculate. 291 * @param distance The distance, in meters, between the offset 292 * and point B. 293 * @return The position of point B that is located from 294 * given offset at given distance and angle. 295 * @since 1.0 296 */ computePosition(Position initialPos, double heading, double distance)297 public static Position computePosition(Position initialPos, double heading, 298 double distance) { 299 if (initialPos == null) { 300 return null; 301 } 302 double angle; 303 if (heading < 90) { 304 angle = heading; 305 } else if (heading > 90 && heading < 180) { 306 angle = 180 - heading; 307 } else if (heading > 180 && heading < 270) { 308 angle = heading - 180; 309 } else { 310 angle = 360 - heading; 311 } 312 313 Position newPosition = null; 314 315 // Convert meters into nautical miles 316 distance = distance * 0.000539956803; 317 angle = Math.toRadians(angle); 318 double initialLat = initialPos.getLatitude(); 319 double initialLong = initialPos.getLongitude(); 320 double dlat = distance * Math.cos(angle); 321 dlat = dlat / 60; 322 dlat = Math.abs(dlat); 323 double newLat = 0; 324 if ((heading > 270 && heading < 360) || (heading > 0 && heading < 90)) { 325 newLat = initialLat + dlat; 326 } else if (heading < 270 && heading > 90) { 327 newLat = initialLat - dlat; 328 } 329 double meanLat = (Math.abs(dlat) / 2.0) + newLat; 330 double dep = (Math.abs(dlat * 60)) * Math.tan(angle); 331 double dlong = dep * (1.0 / Math.cos(Math.toRadians(meanLat))); 332 dlong = dlong / 60; 333 dlong = Math.abs(dlong); 334 double newLong; 335 if (heading > 180 && heading < 360) { 336 newLong = initialLong - dlong; 337 } else { 338 newLong = initialLong + dlong; 339 } 340 341 if (newLong < -180) { 342 double diff = Math.abs(newLong + 180); 343 newLong = 180 - diff; 344 } 345 346 if (newLong > 180) { 347 double diff = Math.abs(newLong + 180); 348 newLong = (180 - diff) * -1; 349 } 350 351 if (heading == 0 || heading == 360 || heading == 180) { 352 newLong = initialLong; 353 newLat = initialLat + dlat; 354 } else if (heading == 90 || heading == 270) { 355 newLat = initialLat; 356 // newLong = initialLong + dlong; THIS WAS THE ORIGINAL (IT WORKED) 357 newLong = initialLong - dlong; 358 } 359 try { 360 newPosition = new Position(newLat, 361 newLong); 362 } catch (InvalidPositionException ipe) { 363 ipe.printStackTrace(); 364 System.out.println(newLat + "," + newLong); 365 } 366 return newPosition; 367 } 368 369 /** 370 * Computes the difference in Longitude between two positions and assigns the 371 * correct sign -westwards travel, + eastwards travel 372 * @param lng1 373 * @param lng2 374 * @return difference in longitude 375 * @since 1.0 376 */ computeDLong(double lng1, double lng2)377 public static double computeDLong(double lng1, double lng2) { 378 if (lng1 - lng2 == 0) { 379 return 0; 380 } 381 382 // both easterly 383 if (lng1 >= 0 & lng2 >= 0) { 384 return -(lng1 - lng2) * 60; 385 } 386 //both westerly 387 if (lng1 < 0 & lng2 < 0) { 388 return -(lng1 - lng2) * 60; 389 } 390 391 //opposite sides of Date line meridian 392 393 //sum less than 180 394 if (Math.abs(lng1) + Math.abs(lng2) < 180) { 395 if (lng1 < 0 & lng2 > 0) { 396 return -(Math.abs(lng1) + Math.abs(lng2)) * 60; 397 } else { 398 return Math.abs(lng1) + Math.abs(lng2) * 60; 399 } 400 } else { 401 //sum greater than 180 402 if (lng1 < 0 & lng2 > 0) { 403 return -(360 - (Math.abs(lng1) + Math.abs(lng2))) * 60; 404 } else { 405 return (360 - (Math.abs(lng1) + Math.abs(lng2))) * 60; 406 } 407 } 408 } 409 410 /** 411 * Computes the difference in Longitude between two positions and assigns the 412 * correct sign -westwards travel, + eastwards travel 413 * @param lng1 414 * @param lng2 415 * @return difference in longitude 416 * @since 1.0 417 */ computeLongDiff(double lng1, double lng2)418 public static double computeLongDiff(double lng1, double lng2) { 419 if (lng1 - lng2 == 0) { 420 return 0; 421 } 422 423 // both easterly 424 if (lng1 >= 0 & lng2 >= 0) { 425 return Math.abs(-(lng1 - lng2) * 60); 426 } 427 //both westerly 428 if (lng1 < 0 & lng2 < 0) { 429 return Math.abs(-(lng1 - lng2) * 60); 430 } 431 432 if (lng1 == 0) { 433 return Math.abs(lng2 * 60); 434 } 435 436 if (lng2 == 0) { 437 return Math.abs(lng1 * 60); 438 } 439 440 return (Math.abs(lng1) + Math.abs(lng2)) * 60; 441 } 442 443 /** 444 * Compute the difference in latitude between two positions 445 * @param lat1 446 * @param lat2 447 * @return difference in latitude 448 * @since 1.0 449 */ computeDLat(double lat1, double lat2)450 public static double computeDLat(double lat1, double lat2) { 451 //same side of equator 452 453 //plane sailing 454 if (lat1 - lat2 == 0) { 455 return 0; 456 } 457 458 //both northerly 459 if (lat1 >= 0 & lat2 >= 0) { 460 return -(lat1 - lat2) * 60; 461 } 462 //both southerly 463 if (lat1 < 0 & lat2 < 0) { 464 return -(lat1 - lat2) * 60; 465 } 466 467 //opposite sides of equator 468 if (lat1 >= 0) { 469 //heading south 470 return -(Math.abs(lat1) + Math.abs(lat2)); 471 } else { 472 //heading north 473 return (Math.abs(lat1) + Math.abs(lat2)); 474 } 475 } 476 477 public static class Quadrant { 478 479 private static final Quadrant FIRST = new Quadrant(1, 1); 480 private static final Quadrant SECOND = new Quadrant(-1, 1); 481 private static final Quadrant THIRD = new Quadrant(-1, -1); 482 private static final Quadrant FOURTH = new Quadrant(1, -1); 483 private final int lonMultiplier; 484 private final int latMultiplier; 485 Quadrant(final int xMultiplier, final int yMultiplier)486 public Quadrant(final int xMultiplier, final int yMultiplier) { 487 this.lonMultiplier = xMultiplier; 488 this.latMultiplier = yMultiplier; 489 } 490 getQuadrant(double degrees, boolean invert)491 static Quadrant getQuadrant(double degrees, boolean invert) { 492 if (invert) { 493 if (degrees >= 0 && degrees <= 90) { 494 return FOURTH; 495 } else if (degrees > 90 && degrees <= 180) { 496 return THIRD; 497 } else if (degrees > 180 && degrees <= 270) { 498 return SECOND; 499 } 500 return FIRST; 501 } else { 502 if (degrees >= 0 && degrees <= 90) { 503 return FIRST; 504 } else if (degrees > 90 && degrees <= 180) { 505 return SECOND; 506 } else if (degrees > 180 && degrees <= 270) { 507 return THIRD; 508 } 509 return FOURTH; 510 } 511 } 512 } 513 514 /** 515 * Converts meters to degrees. 516 * 517 * @param meters The meters that you want to convert into degrees. 518 * @return The degree equivalent of the given meters. 519 * @since 1.0 520 */ toDegrees(double meters)521 public static double toDegrees(double meters) { 522 return (meters / METERS_PER_MINUTE) / 60; 523 } 524 525 /** 526 * Computes the bearing between two points. 527 * 528 * @param p1 529 * @param p2 530 * @return 531 * @since 1.0 532 */ computeBearing(Position p1, Position p2)533 public static int computeBearing(Position p1, Position p2) { 534 int bearing; 535 double dLon = computeDLong(p1.getLongitude(), p2.getLongitude()); 536 double y = Math.sin(dLon) * Math.cos(p2.getLatitude()); 537 double x = Math.cos(p1.getLatitude()) * Math.sin(p2.getLatitude()) 538 - Math.sin(p1.getLatitude()) * Math.cos(p2.getLatitude()) * Math.cos(dLon); 539 bearing = (int) Math.toDegrees(Math.atan2(y, x)); 540 return bearing; 541 } 542 543 /** 544 * Computes the angle between two points. 545 * 546 * @param p1 547 * @param p2 548 * @return 549 */ computeAngle(Position p1, Position p2)550 public static int computeAngle(Position p1, Position p2) { 551 // cos (adj / hyp) 552 double adj = Math.abs(p1.getLongitude() - p2.getLongitude()); 553 double opp = Math.abs(p1.getLatitude() - p2.getLatitude()); 554 return (int) Math.toDegrees(Math.atan(opp / adj)); 555 556 // int angle = (int)Math.atan2(p2.getLatitude() - p1.getLatitude(), 557 // p2.getLongitude() - p1.getLongitude()); 558 //Actually it's ATan2(dy , dx) where dy = y2 - y1 and dx = x2 - x1, or ATan(dy / dx) 559 } 560 computeHeading(Position p1, Position p2)561 public static int computeHeading(Position p1, Position p2) { 562 int angle = computeAngle(p1, p2); 563 // NE 564 if (p2.getLongitude() >= p1.getLongitude() && p2.getLatitude() >= p1.getLatitude()) { 565 return angle; 566 } else if (p2.getLongitude() >= p1.getLongitude() && p2.getLatitude() <= p1.getLatitude()) { 567 // SE 568 return 90 + angle; 569 } else if (p2.getLongitude() <= p1.getLongitude() && p2.getLatitude() <= p1.getLatitude()) { 570 // SW 571 return 270 - angle; 572 } else { 573 // NW 574 return 270 + angle; 575 } 576 } 577 main(String[] args)578 public static void main(String[] args) { 579 try { 580 int pos = NavCalculator.computeHeading(new Position(0, 0), new Position(10, -10)); 581 // System.out.println(pos.getLatitude() + "," + pos.getLongitude()); 582 System.out.println(pos); 583 } catch (Exception e) { 584 } 585 586 587 588 589 590 } 591 } 592