Files
terrain-utilities/src/main/java/pl/wat/ms4ds/terrain/Coord.java

527 lines
19 KiB
Java

package pl.wat.ms4ds.terrain;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;
public class Coord {
private static final Logger logger = LoggerFactory.getLogger(Coord.class);
public static class Geo {
public double lat;
public double lon;
public Geo() {
}
public Geo(double lat, double lon) {
this.lat = lat;
this.lon = lon;
}
public Geo(GeoCoord other) {
lat = other.lat;
lon = other.lon;
}
@Override
public String toString() {
return "Geo{" +
"lat=" + lat +
", lon=" + lon +
'}';
}
}
/**
* Współrzędne punktu odwzorowania kartograficznego PUWG 1992.
* <p>
* Wartości współrzędnych [metry].
*
*/
public static class Puwg {
/**
* Współrzędna X (oś odcietych) odwzorowania kartograficznego [metry].
*/
public double easting;
/**
* Współrzędna Y (oś rzędnych) odwzorowania kartograficznego [metry].
*/
public double northing;
public Puwg() {
this.easting = 0;
this.northing = 0;
}
public Puwg(double easting, double northing) {
this.easting = easting;
this.northing = northing;
}
@Override
public String toString() {
return "Puwg{" +
"easting=" + easting +
", northing=" + northing +
'}';
}
}
public static class Grid {
/**
* Współrzędna pozioma (oś OX) w siatce kwadratów. Indeks kolumny.
*/
public int x;
/**
* Współrzędna pionowa (oś OY) w siatce kwadratów. Indeks wiersza.
*/
public int y;
/**
* Konstruktor klasy na bazie współrzędnych geograficznych.
*
* @param lon długość geograficzna
* @param lat szerokość geograficzna
*/
public Grid(double lon, double lat) {
double xms_f = (lon + 180) * MapConsts.DEG_MS;
long xms = (long) xms_f;
x = zamienWspXmsNaIdKwadratuX(xms);
double yms_f = (lat + 90) * MapConsts.DEG_MS;
long yms = (long) yms_f;
y = zamienWspYmsNaIdKwadratuY(yms);
}
@Override
public String toString() {
return "Grid{" +
"x=" + x +
", y=" + y +
'}';
}
private static final double ODWROT_SS_DX_MS = 1.0 / MapConsts.SS_DX_MS;
private static final double ODWROT_SS_DY_MS = 1.0 / MapConsts.SS_DY_MS;
public static double distance(Grid a, Grid b) {
int dx = a.x - b.x;
int dy = a.y - b.y;
return Math.sqrt(dx * dx + dy * dy) * MapConsts.DL_MK;
}
public static double distance(int x1, int y1, int x2, int y2) {
int dx = x2 - x1;
int dy = y2 - y1;
return Math.sqrt(dx * dx + dy * dy) * MapConsts.DL_MK;
}
private static final float DL_MK2 = MapConsts.DL_MK * 0.0009765625f;
/**
* Funkcja zwraca aproksymowaną odległość między środkami małych kwadratów [m].
*
* @param x1 Wsp X id malego kwadratu pocz.
* @param y1 Wsp Y id malego kwadratu pocz.
* @param x2 Wsp X id malego kwadratu konc.
* @param y2 Wsp Y id malego kwadratu konc.
* @return Odległość miedzy srodkami malych kwadratow [m].
*/
public static float distanceApprox(int x1, int y1, int x2, int y2) {
int dx = x2 - x1;
int dy = y2 - y1;
int min, max, approx;
if (dx < 0) dx = -dx;
if (dy < 0) dy = -dy;
if (dx < dy) {
min = dx;
max = dy;
} else {
min = dy;
max = dx;
}
approx = (max * 1007) + (min * 441);
if (max < (min << 4)) {
approx -= (max * 40);
}
float odl = approx * DL_MK2;
return odl;
}
public Grid() {
x = -1;
y = -1;
}
public Grid(int x, int y) {
this.x = x;
this.y = y;
}
public Grid(Grid orig) {
this.x = orig.x;
this.y = orig.y;
}
public int getX() {
return x;
}
public void setX(int x) {
this.x = x;
}
public int getY() {
return y;
}
public void setY(int y) {
this.y = y;
}
public void set(Grid orig) {
x = orig.x;
y = orig.y;
}
public void set(int x, int y) {
this.x = x;
this.y = y;
}
@Override
public final boolean equals(Object o) {
if (!(o instanceof Grid grid)) return false;
return x == grid.x && y == grid.y;
}
@Override
public final int hashCode() {
int result = x;
result = 31 * result + y;
return result;
}
}
/**
* Zamienia współrzęną GridCoord.x na długość geograficzną środka małego kwadratu
*
* @param idX współrzęna x GridCoord
* @return długość geograficzna
*/
public static float zamienIdKwadratuXNaDlugoscGeo(int idX) {
long xms = zamienIdKwadratuXNaWspXms(idX);
double lon = (double) xms / (double) MapConsts.DEG_MS - 180;
return (float) lon;
}
/**
* Zamienia współrzęną GridCoord.y na szerokość geograficzną środka małego kwadratu
*
* @param idY współrzęna y GridCoord
* @return szerokość geograficzna
*/
public static float zamienIdKwadratuYNaSzerokoscGeo(int idY) {
long yms = zamienIdKwadratuYNaWspYms(idY);
double lat = (double) yms / (double) MapConsts.DEG_MS - 90;
return (float) lat;
}
/**
* Funkcja zamienia wsp. X GridCoord na wsp. geo xms w milisekundach.
* Zwracana wspolrzedna lezy w srodku kwadratu.
*/
public static long zamienIdKwadratuXNaWspXms(int idKwX) {
// indeksowanie kwadratow pola walki zaczyna sie od (0, 0)
// przesuniecie wspolrzednych do srodka kwadratu
long xms = MapConsts.X_REF_MS + (long) ((idKwX + 0.5) * MapConsts.SS_DX_MS);
xms %= MapConsts.ANGLE_360_MS;
return xms;
}
/**
* Funkcja zamienia wsp. Y GridCoord na wsp. geo yms w milisekundach.
* Zwracana wspolrzedna lezy w srodku kwadratu.
*/
public static long zamienIdKwadratuYNaWspYms(int idKwY) {
// indeksowanie kwadratow pola walki zaczyna sie od (0, 0)
// przesuniecie wspolrzednych do srodka kwadratu
long yms = MapConsts.Y_REF_MS + (long) ((idKwY + 0.5) * MapConsts.SS_DY_MS);
return yms;
}
/**
* Zamienia długość geograficzną na współrzędna x GridCoord.
*
* @param lon długość geograficzna
* @return współrzędna x klasy GridCoord
*/
public static int zamienDlugoscGeoNaIdKwadratuX(double lon) {
double xms_f = (lon + 180) * MapConsts.DEG_MS;
return zamienWspXmsNaIdKwadratuX((long) xms_f);
}
/**
* Zamienia szerokość geograficzną na współrzędna y GridCoord.
*
* @param lat szerokość geograficzna
* @return współrzędna y klasy GridCoord
*/
public static int zamienSzerokoscGeoNaIdKwadratuY(double lat) {
double yms_f = (lat + 90) * MapConsts.DEG_MS;
return zamienWspYmsNaIdKwadratuY((long) yms_f);
}
/**
* Funkcja zamienia dlugosc geog. xms w milisekundach na IdKwadrat.x.
*
* @param xms długość geograficzna (lon) w milisekundach
* @return współrzędna GridCoord.x
*/
public static int zamienWspXmsNaIdKwadratuX(long xms) {
// wspolrzedne geograficzne w milisekundach zawieraja sie w zakresie:
// 0 <= x < 360 dlugosc geograficzna
// 0 <= y <= 180 szerokosc geograficzna
if ((xms < 0) || (xms >= 360 * MapConsts.DEG_MS)) {
// poza zakresem
return -1;
}
long x = xms;
if (x < MapConsts.X_REF_MS) {
// // poza zakresem
long dx = x + MapConsts.ANGLE_360_MS - MapConsts.X_REF_MS;
if (dx > MapConsts.DX_REF_MS) {
return -1;
}
x += MapConsts.ANGLE_360_MS;
}
// if (x > MapConsts.X_REF_MS + MapConsts.DX_REF_MS) {
// // poza zakresem
// return -1;
// }
// indeksowanie kwadratow pola walki zaczyna sie od (0, 0)
double xx = (x - MapConsts.X_REF_MS) * ODWROT_SS_DX_MS;
// x = (x - MapConsts.X_REF_MS) / MapConsts.SS_DX_MS;
return (int) xx;
}
/**
* Funkcja zamienia szerokosc geog. yms w milisekundach na IdKwadrat.y.
*
* @param yms szerokosc geograficzna (lat) w milisekundach
* @return współrzędna GridCoord.y
*/
public static int zamienWspYmsNaIdKwadratuY(long yms) {
// wspolrzedne geograficzne w milisekundach zawieraja sie w zakresie:
// 0 <= x < 360 dlugosc geograficzna
// 0 <= y <= 180 szerokosc geograficzna
if (yms < MapConsts.Y_REF_MS) {
// poza zakresem
return -1;
}
// indeksowanie kwadratow pola walki zaczyna sie od (0, 0)
double yy = (yms - MapConsts.Y_REF_MS) * ODWROT_SS_DY_MS;
// long y = (yms - MapConsts.Y_REF_MS) / MapConsts.SS_DY_MS;
return (int) yy;
}
private static final double ODWROT_SS_DX_MS = 1.0 / MapConsts.SS_DX_MS;
private static final double ODWROT_SS_DY_MS = 1.0 / MapConsts.SS_DY_MS;
/**
* Funkcja służy do konwersji współrzednych elipsoidalnych WGS84 (lat/lon) na płaskie X-northing, Y-easting
* odwzorowania kartograficznego 1992.
*
* @param puwgCoord współrzędne PUWG1992 (easting, northing) [metry]
* @param lat szerokość geograficzna WSG-84 [stopnie dziesiętnie]
* @param lon długość geograficzna WSG-84 [stopnie dziesiętnie]
*/
public static void convertWGS84ToPUWG1992(double lat, double lon, Coord.Puwg puwgCoord) {
if (lon < 13.5 || lon > 25.5) {
//Błędna wartość długości geograficznej (zwracana wartość 999999999999999)
puwgCoord.easting = 999999999999999.0;
puwgCoord.northing = 999999999999999.0;
return;
}
double latRad = lat * DEG_2_RAD;
double dlam = (lon - 19.0) * DEG_2_RAD;
double dlam_pow_2 = dlam * dlam;
double dlam_pow_3 = dlam_pow_2 * dlam;
double dlam_pow_4 = dlam_pow_3 * dlam;
double s = Math.sin(latRad);
double c = Math.cos(latRad);
double c_pow_2 = c * c;
double c_pow_3 = c_pow_2 * c;
double c_pow_4 = c_pow_3 * c;
double t = s / c;
double t_pow_2 = t * t;
double t_pow_3 = t_pow_2 * t;
double t_pow_4 = t_pow_3 * t;
double t_pow_5 = t_pow_4 * t;
double eta = E2_SQUARED * c_pow_2;
double eta_pow_2 = eta * eta;
double eta_pow_3 = eta_pow_2 * eta;
double eta_pow_4 = eta_pow_3 * eta;
double sn = sphsn(latRad);
double tmd = sphtmd(latRad);
double t1, t2, t3, t4, t5;
t1 = tmd * OK;
double sns = sn * s;
t2 = sns * c * OK / 2.0;
t3 = sns * c_pow_3 * OK * (5.0 - t_pow_2 + 9.0 * eta + 4.0 * eta_pow_2) / 24.0;
t4 = sns * c_pow_4 * c * OK * (61.0 - 58.0 * t_pow_2 + t_pow_4
+ 270.0 * eta - 330.0 * t_pow_2 * eta + 445.0 * eta_pow_2
+ 324.0 * eta_pow_3 - 680.0 * t_pow_2 * eta_pow_2
+ 88.0 * eta_pow_4 - 600.0 * t_pow_2 * eta_pow_3 - 192.0 * t_pow_2 * eta_pow_4) / 720.0;
t5 = sns * c_pow_4 * c_pow_3 * OK * (1385.0 - 3111.0 * t_pow_2
+ 543.0 * t_pow_4 - t_pow_5 * t) / 40320.0;
puwgCoord.northing = -5300000.0 + t1 + dlam_pow_2 * t2 + dlam_pow_4 * t3
+ dlam_pow_4 * dlam_pow_2 * t4 + dlam_pow_4 * dlam_pow_4 * t5;
t1 = sn * c * OK;
t2 = sn * c_pow_3 * OK * (1.0 - t_pow_2 + eta) / 6.0;
t3 = sn * c_pow_4 * c * OK * (5.0 - 18.0 * t_pow_2 + t_pow_4 + 14.0 * eta
- 58.0 * t_pow_2 * eta + 13.0 * eta_pow_2 + 4.0 * eta_pow_3
- 64.0 * t_pow_2 * eta_pow_2 - 24.0 * t_pow_2 * eta_pow_3) / 120.0;
t4 = sn * c_pow_4 * c_pow_3 * OK * (61.0 - 479.0 * t_pow_2 + 179.0 * t_pow_4 - t_pow_5 * t) / 5040.0;
puwgCoord.easting = 500000.0 + dlam * t1 + dlam_pow_3 * t2
+ dlam_pow_4 * dlam * t3 + dlam_pow_4 * dlam_pow_3 * t4;// + 0.5;
}
/**
* Funkcja do konwersji współrzędnych płaskich X/Y odwzorowania kartograficznego 1992 na elipsoidalne lat/lon elipsoide WGS84.
* <p>
* PUWGCoord.proj: odwzorowanie kartograficzne (proj = 1 odpowiada odwzorowaniu 1992, natomiast każda inna odwzorowaniu 2000)
*
* @param northing współrzędne na osi OY odwzorowania kartograficznego PUWG-1992 do konwersji [metry]
* @param easting współrzędne na osi OX odwzorowania kartograficznego PUWG-1992 do konwersji [metry]
* @param geoCoord współrzędne geograficzne odwzorowania WGS-84 po konwersji [stopnie]
*/
public static void convertPUWG1992ToWGS84(double northing, double easting, Coord.Geo geoCoord) {
double tmd = (northing + 5300000.0) / OK;
double sr = sphsr(0.0);
double ftphi = tmd / sr;
for (int i = 0; i < 5; i++) {
ftphi += (tmd - sphtmd(ftphi)) / sphsr(ftphi);
}
sr = sphsr(ftphi);
double sn = sphsn(ftphi);
double sn_pow_2 = sn * sn;
double sn_pow_3 = sn_pow_2 * sn;
double sn_pow_4 = sn_pow_3 * sn;
double sn_pow_5 = sn_pow_4 * sn;
double sn_pow_7 = sn_pow_5 * sn_pow_2;
double s = Math.sin(ftphi);
double c = Math.cos(ftphi);
double t = s / c;
double t_pow_2 = t * t;
double t_pow_4 = t_pow_2 * t_pow_2;
double t_pow_6 = t_pow_4 * t_pow_2;
double eta = E2_SQUARED * (c * c);
double eta_pow_2 = eta * eta;
double eta_pow_3 = eta_pow_2 * eta;
double eta_pow_4 = eta_pow_2 * eta_pow_2;
double de = easting - 500000.0;
double de_pow_2 = de * de;
double de_pow_3 = de_pow_2 * de;
double de_pow_4 = de_pow_3 * de;
double t0, t1, t2, t3;
t0 = t / (2.0 * sr * sn * OK_POW_2);
t1 = t * (5.0 + 3.0 * t_pow_2 + eta - 4.0 * eta_pow_2 - 9.0 * t_pow_2 * eta) / (24.0 * sr * sn_pow_3 * OK_POW_4);
t2 = t * (61.0 + 90.0 * t_pow_2 + 46.0 * eta + 45.0 * t_pow_4 - 252.0 * t_pow_2 * eta - 3.0 * eta_pow_2
+ 100.0 * eta_pow_3 - 66.0 * t_pow_2 * eta_pow_2 - 90.0 * t_pow_4 * eta + 88.0 * eta_pow_4
+ 225.0 * t_pow_4 * eta_pow_2 + 84.0 * t_pow_2 * eta_pow_3 - 192.0 * t_pow_2 * eta_pow_4) / (720.0 * sr * sn_pow_5 * OK_POW_6);
t3 = t * (1385.0 + 3633 * t_pow_2 + 4095.0 * t_pow_4 + 1575.0 * t_pow_6) / (40320 * sr * sn_pow_7 * (OK_POW_8));
geoCoord.lat = ftphi - de_pow_2 * t0 + de_pow_4 * t1 - de_pow_3 * de_pow_3 * t2 + de_pow_4 * de_pow_3 * t3;
t0 = 1.0 / (sn * c * OK);
t1 = (1.0 + 2.0 * t_pow_2 + eta) / (6.0 * sn_pow_3 * c * (OK_POW_3));
t2 = (5.0 + 6.0 * eta + 28.0 * t_pow_2 - 3.0 * eta_pow_2 + 8.0 * t_pow_2 * eta
+ 24.0 * t_pow_4 - 4.0 * eta_pow_3 + 4.0 * t_pow_2 * eta_pow_2
+ 24.0 * t_pow_2 * eta_pow_3) / (120.0 * sn_pow_5 * c * (OK_POW_5));
t3 = (61.0 + 662.0 * t_pow_2 + 1320.0 * t_pow_4 + 720.0 * t_pow_6) / (5040.0 * sn_pow_7 * c * OK_POW_7);
double dlam = de * t0 - de_pow_3 * t1 + de_pow_3 * de_pow_2 * t2 - de_pow_3 * de_pow_4 * t3;
// 19.0 * DEG_2_RAD == 0.33161255787892263;
// geoCoord.lon = 0.33161255787892263 + dlam;
// geoCoord.lon *= RAD_2_DEG;
geoCoord.lon = dlam * RAD_2_DEG;
geoCoord.lon += 19.0;
geoCoord.lat *= RAD_2_DEG;
}
////////////////////////////////////////////////////////////////////////////////
// Funkcje pomocnicze i stałe
/// /////////////////////////////////////////////////////////////////////////////
static double calculateESquared(double a, double b) {
a *= a;
b *= b;
return (a - b) / a;
}
static double calculateE2Squared(double a, double b) {
a *= a;
b *= b;
return (a - b) / b;
}
static double denom(double sphi) {
double sinSphi = Math.sin(sphi);
return Math.sqrt(1.0 - E_SQUARED * (sinSphi * sinSphi));
}
static double sphsr(double sphi) {
double dn = denom(sphi);
return A * (1.0 - E_SQUARED) / (dn * dn * dn);
}
static double sphsn(double sphi) {
double sinSphi = Math.sin(sphi);
return A / Math.sqrt(1.0 - E_SQUARED * (sinSphi * sinSphi));
}
static double sphtmd(double sphi) {
return (AP * sphi) - (BP * Math.sin(2.0 * sphi)) + (CP * Math.sin(4.0 * sphi))
- (DP * Math.sin(6.0 * sphi)) + (EP * Math.sin(8.0 * sphi));
}
private static final double DEG_2_RAD = Math.PI / 180.0;
private static final double RAD_2_DEG = 180.0 / Math.PI;
/**
* Dlługość dużej półsi, w metrach dla elipsoidy WGS-84.
*/
private static final double A = 6378137.0;
/**
* double f: spłaszczenie elipsoidalne dla elipsoidy WGS-84, 1 / 298.257223563
*/
// private static final double F = 1.0 / 298.257223563;
private static final double B = A * (1.0 - 1.0 / 298.257223563);
private static final double E_SQUARED = calculateESquared(A, B);
private static final double E2_SQUARED = calculateE2Squared(A, B);
private static final double TN = (A - B) / (A + B);
private static final double AP = A * (1.0 - TN + 5.0 * (TN * TN - TN * TN * TN) / 4.0 + 81.0 * TN * TN * TN * TN - TN * TN * TN * TN * TN / 64.0);
private static final double BP = 3.0 * A * (TN - TN * TN + 7.0 * (TN * TN * TN - TN * TN * TN * TN) / 8.0 + 55.0 * TN * TN * TN * TN * TN / 64.0) / 2.0;
private static final double CP = 15.0 * A * (TN * TN - TN * TN * TN + 3.0 * (TN * TN * TN * TN - TN * TN * TN * TN * TN) / 4.0) / 16.0;
private static final double DP = 35.0 * A * (TN * TN * TN - TN * TN * TN * TN + 11.0 * TN * TN * TN * TN * TN / 16.0) / 48.0;
private static final double EP = 315.0 * A * (TN * TN * TN * TN - TN * TN * TN * TN * TN) / 512.0;
/**
* Współczynnik zniekształcenia skali mapy w południku osiowym dla odwzorowania kartograficznego PUWG-1992.
*/
private static final double OK = 0.9993;
private static final double OK_POW_2 = OK * OK;
private static final double OK_POW_3 = OK_POW_2 * OK;
private static final double OK_POW_4 = OK_POW_3 * OK;
private static final double OK_POW_5 = OK_POW_4 * OK;
private static final double OK_POW_6 = OK_POW_5 * OK;
private static final double OK_POW_7 = OK_POW_6 * OK;
private static final double OK_POW_8 = OK_POW_7 * OK;
}