SHS/pip
4
3
Fork 0
Code Issues 67 Pull Requests 1 Actions Releases Wiki Activity

code format

Browse Source
This commit is contained in:
Пелипенко Иван
2022-12-14 14:13:52 +03:00
parent 430a41fefc
commit c2b8a8d6da
297 changed files with 27331 additions and 24162 deletions
Download Patch File Download Diff File Expand all files Collapse all files

55
libs/main/geo/piellipsoidmodel.cpp
View File

@@ -1,66 +1,65 @@
/*
PIP - Platform Independent Primitives
Contains geo ellipsoid models
Andrey Bychkov work.a.b@yandex.ru
PIP - Platform Independent Primitives
Contains geo ellipsoid models
Andrey Bychkov work.a.b@yandex.ru
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU Lesser General Public License for more details.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
You should have received a copy of the GNU Lesser General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "piellipsoidmodel.h"
PIEllipsoidModel::PIEllipsoidModel() {
a = 0.0;
flattening = 0.0;
a = 0.0;
flattening = 0.0;
eccentricity = 0.0;
angVelocity = 0.0;
angVelocity = 0.0;
}
PIEllipsoidModel PIEllipsoidModel::WGS84Ellipsoid() {
PIEllipsoidModel v;
v.a = 6378137.0;
v.flattening = 0.335281066475e-2;
v.a = 6378137.0;
v.flattening = 0.335281066475e-2;
v.eccentricity = 8.1819190842622e-2;
v.angVelocity = 7.292115e-5;
v.angVelocity = 7.292115e-5;
return v;
}
PIEllipsoidModel PIEllipsoidModel::PZ90Ellipsoid() {
PIEllipsoidModel v;
v.a = 6378136.0;
v.flattening = 3.35280373518e-3;
v.a = 6378136.0;
v.flattening = 3.35280373518e-3;
v.eccentricity = 8.1819106432923e-2;
v.angVelocity = 7.292115e-5;
v.angVelocity = 7.292115e-5;
return v;
}
PIEllipsoidModel PIEllipsoidModel::GPSEllipsoid() {
PIEllipsoidModel v = WGS84Ellipsoid();
v.angVelocity = 7.2921151467e-5;
v.angVelocity = 7.2921151467e-5;
return v;
}
PIEllipsoidModel PIEllipsoidModel::KrasovskiyEllipsoid() {
PIEllipsoidModel v;
v.a = 6378245.0;
v.flattening = 1.0/298.3;
v.eccentricity = sqrt(v.a*v.a - 6356863.0*6356863.0)/v.a;
v.angVelocity = 7.292115e-5;
v.a = 6378245.0;
v.flattening = 1.0 / 298.3;
v.eccentricity = sqrt(v.a * v.a - 6356863.0 * 6356863.0) / v.a;
v.angVelocity = 7.292115e-5;
return v;
}

37
libs/main/geo/piellipsoidmodel.h
View File

@@ -5,22 +5,22 @@
* \~russian Географическая эллипсоидная модель Земли
*/
/*
PIP - Platform Independent Primitives
Contains geo ellipsoid models
Andrey Bychkov work.a.b@yandex.ru
PIP - Platform Independent Primitives
Contains geo ellipsoid models
Andrey Bychkov work.a.b@yandex.ru
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU Lesser General Public License for more details.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
You should have received a copy of the GNU Lesser General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef PIELLIPSOIDMODEL_H
@@ -32,20 +32,19 @@
class PIP_EXPORT PIEllipsoidModel {
public:
PIEllipsoidModel();
double eccSquared() const {return eccentricity * eccentricity;} // eccentricity squared
double b() const {return a * sqrt(1 - eccSquared());}
double eccSquared() const { return eccentricity * eccentricity; } // eccentricity squared
double b() const { return a * sqrt(1 - eccSquared()); }
static PIEllipsoidModel WGS84Ellipsoid();
static PIEllipsoidModel PZ90Ellipsoid();
static PIEllipsoidModel GPSEllipsoid();
static PIEllipsoidModel KrasovskiyEllipsoid();
double a; /// Major axis of Earth in meters
double flattening; /// Flattening (ellipsoid parameter)
double a; /// Major axis of Earth in meters
double flattening; /// Flattening (ellipsoid parameter)
double eccentricity; /// Eccentricity (ellipsoid parameter)
double angVelocity; /// Angular velocity of Earth in radians/sec
double angVelocity; /// Angular velocity of Earth in radians/sec
};
#endif // PIELLIPSOIDMODEL_H

26
libs/main/geo/pigeomodule.h
View File

@@ -1,20 +1,20 @@
/*
PIP - Platform Independent Primitives
Module includes
Andrey Bychkov work.a.b@yandex.ru
PIP - Platform Independent Primitives
Module includes
Andrey Bychkov work.a.b@yandex.ru
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU Lesser General Public License for more details.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
You should have received a copy of the GNU Lesser General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
//! \defgroup Geo Geo
//! \~\brief

462
libs/main/geo/pigeoposition.cpp
View File

@@ -1,28 +1,28 @@
/*
PIP - Platform Independent Primitives
Class for geo position storage and conversions
Andrey Bychkov work.a.b@yandex.ru
PIP - Platform Independent Primitives
Class for geo position storage and conversions
Andrey Bychkov work.a.b@yandex.ru
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU Lesser General Public License for more details.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
You should have received a copy of the GNU Lesser General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "pigeoposition.h"
const double PIGeoPosition::one_cm_tolerance = 0.01; // One centimeter tolerance.
const double PIGeoPosition::one_mm_tolerance = 0.001; // One milimeter tolerance.
const double PIGeoPosition::one_um_tolerance = 0.000001; // One micron tolerance.
double PIGeoPosition::position_tolerance = PIGeoPosition::one_mm_tolerance; // Default tolerance in meters.
const double PIGeoPosition::one_cm_tolerance = 0.01; // One centimeter tolerance.
const double PIGeoPosition::one_mm_tolerance = 0.001; // One milimeter tolerance.
const double PIGeoPosition::one_um_tolerance = 0.000001; // One micron tolerance.
double PIGeoPosition::position_tolerance = PIGeoPosition::one_mm_tolerance; // Default tolerance in meters.
PIGeoPosition::PIGeoPosition() {
@@ -44,81 +44,84 @@ PIGeoPosition::PIGeoPosition(PIMathVectorT3d v, PIGeoPosition::CoordinateSystem
}
PIGeoPosition &PIGeoPosition::transformTo(PIGeoPosition::CoordinateSystem sys) {
if(sys == Unknown || sys == s) return *this;
PIGeoPosition & PIGeoPosition::transformTo(PIGeoPosition::CoordinateSystem sys) {
if (sys == Unknown || sys == s) return *this;
PIGeoPosition tmp(*this);
switch(s) {
switch (s) {
case Unknown: return *this;
case Geodetic:
switch (sys) {
case Unknown:
return *this;
case Geodetic:
switch(sys) {
case Unknown: case Geodetic: return *this;
case Geocentric:
convertGeodeticToGeocentric(*this, tmp, el);
tmp.s = Geocentric;
break;
case Cartesian:
convertGeodeticToCartesian(*this, tmp, el);
tmp.s = Cartesian;
break;
case Spherical:
convertGeodeticToGeocentric(*this, tmp, el);
tmp[0] = 90 - tmp[0]; // geocen -> sph
tmp.s = Spherical;
break;
}
break;
case Geodetic: return *this;
case Geocentric:
switch(sys) {
case Unknown: case Geocentric: return *this;
case Geodetic:
convertGeocentricToGeodetic(*this, tmp, el);
tmp.s = Geodetic;
break;
case Cartesian:
convertGeocentricToCartesian(*this, tmp);
tmp.s = Cartesian;
break;
case Spherical:
tmp[0] = 90 - tmp[0]; // geocen -> sph
tmp.s = Spherical;
break;
}
break;
convertGeodeticToGeocentric(*this, tmp, el);
tmp.s = Geocentric;
break;
case Cartesian:
switch(sys) {
case Unknown: case Cartesian: return *this;
case Geodetic:
convertCartesianToGeodetic(*this, tmp, el);
tmp.s = Geodetic;
break;
case Geocentric:
convertCartesianToGeocentric(*this, tmp);
tmp.s = Geocentric;
break;
case Spherical:
convertCartesianToSpherical(*this, tmp);
tmp.s = Spherical;
break;
}
break;
convertGeodeticToCartesian(*this, tmp, el);
tmp.s = Cartesian;
break;
case Spherical:
switch(sys) {
case Unknown: case Spherical: return *this;
case Geodetic:
(*this)[0] = 90 - (*this)[0]; // sph -> geocen
convertGeocentricToGeodetic(*this, tmp, el);
tmp.s = Geodetic;
break;
case Geocentric:
tmp[0] = 90 - tmp[0]; // sph -> geocen
tmp.s = Geocentric;
break;
case Cartesian:
convertSphericalToCartesian(*this, tmp);
tmp.s = Cartesian;
break;
}
convertGeodeticToGeocentric(*this, tmp, el);
tmp[0] = 90 - tmp[0]; // geocen -> sph
tmp.s = Spherical;
break;
}
break;
case Geocentric:
switch (sys) {
case Unknown:
case Geocentric: return *this;
case Geodetic:
convertGeocentricToGeodetic(*this, tmp, el);
tmp.s = Geodetic;
break;
case Cartesian:
convertGeocentricToCartesian(*this, tmp);
tmp.s = Cartesian;
break;
case Spherical:
tmp[0] = 90 - tmp[0]; // geocen -> sph
tmp.s = Spherical;
break;
}
break;
case Cartesian:
switch (sys) {
case Unknown:
case Cartesian: return *this;
case Geodetic:
convertCartesianToGeodetic(*this, tmp, el);
tmp.s = Geodetic;
break;
case Geocentric:
convertCartesianToGeocentric(*this, tmp);
tmp.s = Geocentric;
break;
case Spherical:
convertCartesianToSpherical(*this, tmp);
tmp.s = Spherical;
break;
}
break;
case Spherical:
switch (sys) {
case Unknown:
case Spherical: return *this;
case Geodetic:
(*this)[0] = 90 - (*this)[0]; // sph -> geocen
convertGeocentricToGeodetic(*this, tmp, el);
tmp.s = Geodetic;
break;
case Geocentric:
tmp[0] = 90 - tmp[0]; // sph -> geocen
tmp.s = Geocentric;
break;
case Cartesian:
convertSphericalToCartesian(*this, tmp);
tmp.s = Cartesian;
break;
}
break;
}
*this = tmp;
@@ -126,7 +129,7 @@ PIGeoPosition &PIGeoPosition::transformTo(PIGeoPosition::CoordinateSystem sys) {
}
double PIGeoPosition::x() const {
if(s == Cartesian) return (*this)[0];
if (s == Cartesian) return (*this)[0];
PIGeoPosition t(*this);
t.transformTo(Cartesian);
return t[0];
@@ -134,7 +137,7 @@ double PIGeoPosition::x() const {
double PIGeoPosition::y() const {
if(s == Cartesian) return (*this)[1];
if (s == Cartesian) return (*this)[1];
PIGeoPosition t(*this);
t.transformTo(Cartesian);
return t[1];
@@ -142,7 +145,7 @@ double PIGeoPosition::y() const {
double PIGeoPosition::z() const {
if(s == Cartesian) return (*this)[2];
if (s == Cartesian) return (*this)[2];
PIGeoPosition t(*this);
t.transformTo(Cartesian);
return t[2];
@@ -150,7 +153,7 @@ double PIGeoPosition::z() const {
double PIGeoPosition::latitudeGeodetic() const {
if(s == Geodetic) return (*this)[0];
if (s == Geodetic) return (*this)[0];
PIGeoPosition t(*this);
t.transformTo(Geodetic);
return t[0];
@@ -158,7 +161,7 @@ double PIGeoPosition::latitudeGeodetic() const {
double PIGeoPosition::latitudeGeocentric() const {
if(s == Geocentric) return (*this)[0];
if (s == Geocentric) return (*this)[0];
PIGeoPosition t(*this);
t.transformTo(Geocentric);
return t[0];
@@ -166,7 +169,7 @@ double PIGeoPosition::latitudeGeocentric() const {
double PIGeoPosition::longitude() const {
if(s != Cartesian) return (*this)[1];
if (s != Cartesian) return (*this)[1];
PIGeoPosition t(*this);
t.transformTo(Spherical);
return t[1];
@@ -174,7 +177,7 @@ double PIGeoPosition::longitude() const {
double PIGeoPosition::theta() const {
if(s == Spherical) return (*this)[0];
if (s == Spherical) return (*this)[0];
PIGeoPosition t(*this);
t.transformTo(Spherical);
return t[0];
@@ -187,7 +190,7 @@ double PIGeoPosition::phi() const {
double PIGeoPosition::radius() const {
if(s == Spherical || s == Geocentric) return (*this)[2];
if (s == Spherical || s == Geocentric) return (*this)[2];
PIGeoPosition t(*this);
t.transformTo(Spherical);
return t[2];
@@ -195,180 +198,192 @@ double PIGeoPosition::radius() const {
double PIGeoPosition::height() const {
if(s == Geodetic) return (*this)[2];
if (s == Geodetic) return (*this)[2];
PIGeoPosition t(*this);
t.transformTo(Geodetic);
return t[2];
}
PIGeoPosition &PIGeoPosition::setGeodetic(double lat, double lon, double ht, PIEllipsoidModel ell) {
PIGeoPosition & PIGeoPosition::setGeodetic(double lat, double lon, double ht, PIEllipsoidModel ell) {
assertm(lat <= 90 && lat >= -90, "Achtung! Invalid latitude in setGeodetic");
(*this)[0] = lat;
(*this)[1] = lon;
if((*this)[1] < 0) (*this)[1] += 360 * (1 + (unsigned long)((*this)[1]/360));
else if((*this)[1] >= 360) (*this)[1] -= 360 * (unsigned long)((*this)[1]/360);
if ((*this)[1] < 0)
(*this)[1] += 360 * (1 + (unsigned long)((*this)[1] / 360));
else if ((*this)[1] >= 360)
(*this)[1] -= 360 * (unsigned long)((*this)[1] / 360);
(*this)[2] = ht;
el = ell;
s = Geodetic;
el = ell;
s = Geodetic;
return *this;
}
PIGeoPosition &PIGeoPosition::setGeocentric(double lat, double lon, double rad) {
PIGeoPosition & PIGeoPosition::setGeocentric(double lat, double lon, double rad) {
assertm(lat <= 90 && lat >= -90, "Achtung! Invalid latitude in setGeocentric");
assertm(rad >= 0, "Achtung! Invalid radius in setGeocentric");
(*this)[0] = lat;
(*this)[1] = lon;
(*this)[2] = rad;
if((*this)[1] < 0) (*this)[1] += 360*(1+(unsigned long)((*this)[1]/360));
else if((*this)[1] >= 360) (*this)[1] -= 360*(unsigned long)((*this)[1]/360);
if ((*this)[1] < 0)
(*this)[1] += 360 * (1 + (unsigned long)((*this)[1] / 360));
else if ((*this)[1] >= 360)
(*this)[1] -= 360 * (unsigned long)((*this)[1] / 360);
s = Geocentric;
return *this;
}
PIGeoPosition &PIGeoPosition::setSpherical(double theta, double phi, double rad) {
PIGeoPosition & PIGeoPosition::setSpherical(double theta, double phi, double rad) {
assertm(theta <= 180 && theta >= 0, "Achtung! Invalid theta in setSpherical");
assertm(rad >= 0, "Achtung! Invalid radius in setSpherical");
(*this)[0] = theta;
(*this)[1] = phi;
(*this)[2] = rad;
if((*this)[1] < 0) (*this)[1] += 360*(1+(unsigned long)((*this)[1]/360));
else if((*this)[1] >= 360) (*this)[1] -= 360*(unsigned long)((*this)[1]/360);
if ((*this)[1] < 0)
(*this)[1] += 360 * (1 + (unsigned long)((*this)[1] / 360));
else if ((*this)[1] >= 360)
(*this)[1] -= 360 * (unsigned long)((*this)[1] / 360);
s = Spherical;
return *this;
}
PIGeoPosition &PIGeoPosition::setECEF(double x, double y, double z) {
PIGeoPosition & PIGeoPosition::setECEF(double x, double y, double z) {
(*this)[0] = x;
(*this)[1] = y;
(*this)[2] = z;
s = Cartesian;
s = Cartesian;
return *this;
}
void PIGeoPosition::convertSphericalToCartesian(const PIMathVectorT3d &tpr, PIMathVectorT3d &xyz) {
void PIGeoPosition::convertSphericalToCartesian(const PIMathVectorT3d & tpr, PIMathVectorT3d & xyz) {
double st = sin(toRad(tpr[0]));
xyz[0] = tpr[2] * st * cos(toRad(tpr[1]));
xyz[1] = tpr[2] * st * sin(toRad(tpr[1]));
xyz[2] = tpr[2] * cos(toRad(tpr[0]));
xyz[0] = tpr[2] * st * cos(toRad(tpr[1]));
xyz[1] = tpr[2] * st * sin(toRad(tpr[1]));
xyz[2] = tpr[2] * cos(toRad(tpr[0]));
}
void PIGeoPosition::convertCartesianToSpherical(const PIMathVectorT3d &xyz, PIMathVectorT3d &tpr) {
void PIGeoPosition::convertCartesianToSpherical(const PIMathVectorT3d & xyz, PIMathVectorT3d & tpr) {
tpr[2] = xyz.length();
if(tpr[2] <= PIGeoPosition::position_tolerance / 5) { // zero-length Cartesian vector
if (tpr[2] <= PIGeoPosition::position_tolerance / 5) { // zero-length Cartesian vector
tpr[0] = 90.0;
tpr[1] = 0.0;
return;
}
tpr[0] = acos(xyz[2] / tpr[2]) * rad2deg;
if(sqrt(xyz[0] * xyz[0] + xyz[1] * xyz[1]) < PIGeoPosition::position_tolerance / 5) { // pole
if (sqrt(xyz[0] * xyz[0] + xyz[1] * xyz[1]) < PIGeoPosition::position_tolerance / 5) { // pole
tpr[1] = 0.0;
return;
}
tpr[1] = atan2(xyz[1],xyz[0]) * rad2deg;
if(tpr[1] < 0.0) tpr[1] += 360.0;
tpr[1] = atan2(xyz[1], xyz[0]) * rad2deg;
if (tpr[1] < 0.0) tpr[1] += 360.0;
}
void PIGeoPosition::convertCartesianToGeodetic(const PIMathVectorT3d &xyz, PIMathVectorT3d &llh, PIEllipsoidModel ell) {
double p,slat,nn,htold,latold;
void PIGeoPosition::convertCartesianToGeodetic(const PIMathVectorT3d & xyz, PIMathVectorT3d & llh, PIEllipsoidModel ell) {
double p, slat, nn, htold, latold;
p = sqrt(xyz[0] * xyz[0] + xyz[1] * xyz[1]);
if(p < PIGeoPosition::position_tolerance / 5) { // pole or origin
llh[0] = (xyz[2] > 0.0 ? 90.0: -90.0);
llh[1] = 0.0; // lon undefined, really
llh[2] = piAbsd(xyz[2]) - ell.a * sqrt(1.0-ell.eccSquared());
if (p < PIGeoPosition::position_tolerance / 5) { // pole or origin
llh[0] = (xyz[2] > 0.0 ? 90.0 : -90.0);
llh[1] = 0.0; // lon undefined, really
llh[2] = piAbsd(xyz[2]) - ell.a * sqrt(1.0 - ell.eccSquared());
return;
}
llh[0] = atan2(xyz[2], p*(1.0-ell.eccSquared()));
llh[0] = atan2(xyz[2], p * (1.0 - ell.eccSquared()));
llh[2] = 0;
for(int i=0; i<5; i++) {
slat = sin(llh[0]);
nn = ell.a / sqrt(1.0 - ell.eccSquared() * slat * slat);
htold = llh[2];
for (int i = 0; i < 5; i++) {
slat = sin(llh[0]);
nn = ell.a / sqrt(1.0 - ell.eccSquared() * slat * slat);
htold = llh[2];
llh[2] = p / cos(llh[0]) - nn;
latold = llh[0];
llh[0] = atan2(xyz[2], p*(1.0 - ell.eccSquared() * (nn / (nn + llh[2]))));
if(piAbsd(llh[0] - latold) < 1.0e-9 && piAbsd(llh[2] - htold) < 1.0e-9 * ell.a) break;
llh[0] = atan2(xyz[2], p * (1.0 - ell.eccSquared() * (nn / (nn + llh[2]))));
if (piAbsd(llh[0] - latold) < 1.0e-9 && piAbsd(llh[2] - htold) < 1.0e-9 * ell.a) break;
}
llh[1] = atan2(xyz[1], xyz[0]);
if(llh[1] < 0.0) llh[1] += M_2PI;
if (llh[1] < 0.0) llh[1] += M_2PI;
llh[0] *= rad2deg;
llh[1] *= rad2deg;
}
void PIGeoPosition::convertGeodeticToCartesian(const PIMathVectorT3d &llh, PIMathVectorT3d &xyz, PIEllipsoidModel ell) {
void PIGeoPosition::convertGeodeticToCartesian(const PIMathVectorT3d & llh, PIMathVectorT3d & xyz, PIEllipsoidModel ell) {
double slat = sin(toRad(llh[0]));
double clat = cos(toRad(llh[0]));
double nn = ell.a / sqrt(1.0 - ell.eccSquared() * slat * slat);
xyz[0] = (nn + llh[2]) * clat * cos(toRad(llh[1]));
xyz[1] = (nn + llh[2]) * clat * sin(toRad(llh[1]));
xyz[2] = (nn * (1.0 - ell.eccSquared()) + llh[2]) * slat;
double nn = ell.a / sqrt(1.0 - ell.eccSquared() * slat * slat);
xyz[0] = (nn + llh[2]) * clat * cos(toRad(llh[1]));
xyz[1] = (nn + llh[2]) * clat * sin(toRad(llh[1]));
xyz[2] = (nn * (1.0 - ell.eccSquared()) + llh[2]) * slat;
}
void PIGeoPosition::convertCartesianToGeocentric(const PIMathVectorT3d &xyz, PIMathVectorT3d &llr) {
void PIGeoPosition::convertCartesianToGeocentric(const PIMathVectorT3d & xyz, PIMathVectorT3d & llr) {
convertCartesianToSpherical(xyz, llr);
llr[0] = 90.0 - llr[0]; // convert theta to latitude
}
void PIGeoPosition::convertGeocentricToCartesian(const PIMathVectorT3d &llr, PIMathVectorT3d &xyz) {
void PIGeoPosition::convertGeocentricToCartesian(const PIMathVectorT3d & llr, PIMathVectorT3d & xyz) {
PIMathVectorT3d llh(llr);
llh[0] = 90.0 - llh[0]; // convert latitude to theta
convertSphericalToCartesian(llh, xyz);
}
void PIGeoPosition::convertGeocentricToGeodetic(const PIMathVectorT3d &llr, PIMathVectorT3d &llh, PIEllipsoidModel ell) {
void PIGeoPosition::convertGeocentricToGeodetic(const PIMathVectorT3d & llr, PIMathVectorT3d & llh, PIEllipsoidModel ell) {
double cl, p, sl, slat, nn, htold, latold;
llh[1] = llr[1]; // longitude is unchanged
cl = sin(toRad(90.0 - llr[0]));
sl = cos(toRad(90.0 - llr[0]));
if(llr[2] <= PIGeoPosition::position_tolerance / 5) { // radius is below tolerance, hence assign zero-length, arbitrarily set latitude = longitude = 0
cl = sin(toRad(90.0 - llr[0]));
sl = cos(toRad(90.0 - llr[0]));
if (llr[2] <= PIGeoPosition::position_tolerance /
5) { // radius is below tolerance, hence assign zero-length, arbitrarily set latitude = longitude = 0
llh[0] = llh[1] = 0.0;
llh[2] = -ell.a;
llh[2] = -ell.a;
return;
} else if(cl < 1.e-10) { // near pole ... note that 1mm/radius(Earth) = 1.5e-10
if(llr[0] < 0.0) llh[0] = -90.0;
else llh[0] = 90.0;
} else if (cl < 1.e-10) { // near pole ... note that 1mm/radius(Earth) = 1.5e-10
if (llr[0] < 0.0)
llh[0] = -90.0;
else
llh[0] = 90.0;
llh[1] = 0.0;
llh[2] = llr[2] - ell.a * sqrt(1.0 - ell.eccSquared());
return;
}
llh[0] = atan2(sl, cl * (1.0 - ell.eccSquared()));
p = cl * llr[2];
p = cl * llr[2];
llh[2] = 0.0;
for(int i=0; i<5; i++) {
slat = sin(llh[0]);
nn = ell.a / sqrt(1.0 - ell.eccSquared() * slat * slat);
htold = llh[2];
for (int i = 0; i < 5; i++) {
slat = sin(llh[0]);
nn = ell.a / sqrt(1.0 - ell.eccSquared() * slat * slat);
htold = llh[2];
llh[2] = p / cos(llh[0]) - nn;
latold = llh[0];
llh[0] = atan2(sl, cl * (1.0 - ell.eccSquared() * (nn / (nn + llh[2]))));
if(piAbsd(llh[0] - latold) < 1.0e-9 && piAbsd(llh[2] - htold) < 1.0e-9 * ell.a) break;
if (piAbsd(llh[0] - latold) < 1.0e-9 && piAbsd(llh[2] - htold) < 1.0e-9 * ell.a) break;
}
llh[0] *= rad2deg;
}
void PIGeoPosition::convertGeodeticToGeocentric(const PIMathVectorT3d &llh, PIMathVectorT3d &llr, PIEllipsoidModel ell) {
void PIGeoPosition::convertGeodeticToGeocentric(const PIMathVectorT3d & llh, PIMathVectorT3d & llr, PIEllipsoidModel ell) {
double slat = sin(toRad(llh[0]));
double nn = ell.a / sqrt(1.0 - ell.eccSquared() * slat * slat);
llr[1] = llh[1]; // longitude is unchanged
llr[2] = sqrt((nn+llh[2])*(nn+llh[2]) + nn*ell.eccSquared()*(nn*ell.eccSquared()-2*(nn+llh[2]))*slat*slat); // radius
if(llr[2] <= PIGeoPosition::position_tolerance/5) { // radius is below tolerance, hence assign zero-length
llr[0] = llr[1] = llr[2] = 0; // arbitrarily set latitude = longitude = 0
double nn = ell.a / sqrt(1.0 - ell.eccSquared() * slat * slat);
llr[1] = llh[1]; // longitude is unchanged
llr[2] =
sqrt((nn + llh[2]) * (nn + llh[2]) + nn * ell.eccSquared() * (nn * ell.eccSquared() - 2 * (nn + llh[2])) * slat * slat); // radius
if (llr[2] <= PIGeoPosition::position_tolerance / 5) { // radius is below tolerance, hence assign zero-length
llr[0] = llr[1] = llr[2] = 0; // arbitrarily set latitude = longitude = 0
return;
}
if(1 - piAbsd(slat) < 1.e-10) { // at the pole
if(slat < 0) llr[0] = -90.0;
else llr[0] = 90.0;
if (1 - piAbsd(slat) < 1.e-10) { // at the pole
if (slat < 0)
llr[0] = -90.0;
else
llr[0] = 90.0;
llr[1] = 0.0;
return;
}
@@ -380,79 +395,85 @@ void PIGeoPosition::convertGeodeticToGeocentric(const PIMathVectorT3d &llh, PIMa
double PIGeoPosition::radiusEarth(double geolat, PIEllipsoidModel ell) {
double slat = sin(toRad(geolat));
double e = (1.0 - ell.eccSquared());
double f = (1.0 + (e * e - 1.0) * slat * slat) / (1.0 - ell.eccSquared() * slat * slat);
double e = (1.0 - ell.eccSquared());
double f = (1.0 + (e * e - 1.0) * slat * slat) / (1.0 - ell.eccSquared() * slat * slat);
return (ell.a * sqrt(f));
}
PIGeoPosition &PIGeoPosition::operator=(const PIMathVectorT3d &v) {
*((PIMathVectorT3d*)(this)) = v;
PIGeoPosition & PIGeoPosition::operator=(const PIMathVectorT3d & v) {
*((PIMathVectorT3d *)(this)) = v;
return *this;
}
PIGeoPosition &PIGeoPosition::operator-=(const PIGeoPosition &right) {
PIGeoPosition & PIGeoPosition::operator-=(const PIGeoPosition & right) {
PIGeoPosition r(right);
CoordinateSystem saves = s;
transformTo(Cartesian);
r.transformTo(Cartesian);
(*(PIMathVectorT3d*)(this)) -= r;
(*(PIMathVectorT3d *)(this)) -= r;
transformTo(saves);
return *this;
}
PIGeoPosition &PIGeoPosition::operator+=(const PIGeoPosition &right) {
PIGeoPosition & PIGeoPosition::operator+=(const PIGeoPosition & right) {
PIGeoPosition r(right);
CoordinateSystem saves = s;
transformTo(Cartesian);
r.transformTo(Cartesian);
(*(PIMathVectorT3d*)(this)) += r;
(*(PIMathVectorT3d *)(this)) += r;
transformTo(saves);
return *this;
}
bool PIGeoPosition::operator==(const PIGeoPosition &right) const {
if(el.a != right.el.a || el.eccSquared() != right.el.eccSquared()) return false;
if(range(*this, right) < position_tolerance) return true;
else return false;
bool PIGeoPosition::operator==(const PIGeoPosition & right) const {
if (el.a != right.el.a || el.eccSquared() != right.el.eccSquared()) return false;
if (range(*this, right) < position_tolerance)
return true;
else
return false;
}
void PIGeoPosition::initialize(PIMathVectorT3d v, PIGeoPosition::CoordinateSystem sys, PIEllipsoidModel ell) {
double a(v[0]), b(v[1]), c(v[2]);
if(sys == Geodetic || sys==Geocentric) {
if (sys == Geodetic || sys == Geocentric) {
assertm(a <= 90 && a >= -90, "Achtung! Invalid latitude in constructor");
if(b < 0) b += 360*(1+(unsigned long)(b/360));
else if(b >= 360) b -= 360*(unsigned long)(b/360);
if (b < 0)
b += 360 * (1 + (unsigned long)(b / 360));
else if (b >= 360)
b -= 360 * (unsigned long)(b / 360);
}
if(sys==Geocentric || sys==Spherical) {
if (sys == Geocentric || sys == Spherical) {
assertm(c >= 0, "Achtung! Invalid radius in constructor");
}
if(sys==Spherical) {
if (sys == Spherical) {
assertm(a >= 0 && a <= 180, "Achtung! Invalid theta in constructor");
if(b < 0) b += 360*(1+(unsigned long)(b/360));
else if(b >= 360) b -= 360*(unsigned long)(b/360);
if (b < 0)
b += 360 * (1 + (unsigned long)(b / 360));
else if (b >= 360)
b -= 360 * (unsigned long)(b / 360);
}
(*this)[0] = a;
(*this)[1] = b;
(*this)[2] = c;
el = ell;
s = sys;
el = ell;
s = sys;
}
double PIGeoPosition::range(const PIGeoPosition &a, const PIGeoPosition &b) {
PIGeoPosition l(a),r(b);
double PIGeoPosition::range(const PIGeoPosition & a, const PIGeoPosition & b) {
PIGeoPosition l(a), r(b);
l.transformTo(PIGeoPosition::Cartesian);
r.transformTo(PIGeoPosition::Cartesian);
return (l - r).length();
}
double PIGeoPosition::elevation(const PIGeoPosition &p) const {
double PIGeoPosition::elevation(const PIGeoPosition & p) const {
PIGeoPosition r(*this), s(p);
r.transformTo(Cartesian);
s.transformTo(Cartesian);
@@ -460,7 +481,7 @@ double PIGeoPosition::elevation(const PIGeoPosition &p) const {
}
double PIGeoPosition::elevationGeodetic(const PIGeoPosition &p) const {
double PIGeoPosition::elevationGeodetic(const PIGeoPosition & p) const {
PIGeoPosition r(*this), s(p);
double lat = toRad(r.latitudeGeodetic());
double lng = toRad(r.longitude());
@@ -471,47 +492,49 @@ double PIGeoPosition::elevationGeodetic(const PIGeoPosition &p) const {
PIMathVectorT3d z = s - r;
assertm(z.length() > 1e-4, "Positions are within .1 millimeter");
PIMathVectorT3d kv; // Compute k vector in local North-East-Up (NEU) system
kv[0] = cos(lat) * cos(lng);
kv[1] = cos(lat) * sin(lng);
kv[2] = sin(lat);
local_up = z.dot(kv); // Take advantage of dot method to get Up coordinate in local NEU system
cos_up = local_up / z.length(); // Let's get cos(z), being z the angle with respect to local vertical (Up);
kv[0] = cos(lat) * cos(lng);
kv[1] = cos(lat) * sin(lng);
kv[2] = sin(lat);
local_up = z.dot(kv); // Take advantage of dot method to get Up coordinate in local NEU system
cos_up = local_up / z.length(); // Let's get cos(z), being z the angle with respect to local vertical (Up);
return 90.0 - toDeg(acos(cos_up));
}
double PIGeoPosition::azimuth(const PIGeoPosition &p) const {
double PIGeoPosition::azimuth(const PIGeoPosition & p) const {
PIGeoPosition r(*this), s(p);
r.transformTo(Cartesian);
s.transformTo(Cartesian);
double xy, xyz, cosl, sinl, sint, xn1, xn2, xn3, xe1, xe2;
double z1, z2, z3, p1, p2, alpha;
xy = r[0] * r[0] + r[1] * r[1];
xy = r[0] * r[0] + r[1] * r[1];
xyz = xy + r[2] * r[2];
xy = sqrt(xy);
xy = sqrt(xy);
xyz = sqrt(xyz);
assertm(xy > 1e-14 && xyz > 1e-14, "Divide by Zero Error");
cosl = r[0] / xy;
sinl = r[1] / xy;
sint = r[2] / xyz;
xn1 = -sint * cosl;
xn2 = -sint * sinl;
xn3 = xy / xyz;
xe1 = -sinl;
xe2 = cosl;
z1 = s[0] - r[0];
z2 = s[1] - r[1];
z3 = s[2] - r[2];
p1 = (xn1 * z1) + (xn2 * z2) + (xn3 * z3) ;
p2 = (xe1 * z1) + (xe2 * z2) ;
xn1 = -sint * cosl;
xn2 = -sint * sinl;
xn3 = xy / xyz;
xe1 = -sinl;
xe2 = cosl;
z1 = s[0] - r[0];
z2 = s[1] - r[1];
z3 = s[2] - r[2];
p1 = (xn1 * z1) + (xn2 * z2) + (xn3 * z3);
p2 = (xe1 * z1) + (xe2 * z2);
assertm((piAbsd(p1) + piAbsd(p2)) >= 1.0e-16, "azAngle(), failed p1+p2 test");
alpha = 90 - toDeg(atan2(p1, p2));
if (alpha < 0) return alpha + 360;
else return alpha;
if (alpha < 0)
return alpha + 360;
else
return alpha;
}
double PIGeoPosition::azimuthGeodetic(const PIGeoPosition &p) const {
double PIGeoPosition::azimuthGeodetic(const PIGeoPosition & p) const {
PIGeoPosition r(*this), s(p);
double lat = toRad(r.latitudeGeodetic());
double lng = toRad(r.longitude());
@@ -525,21 +548,23 @@ double PIGeoPosition::azimuthGeodetic(const PIGeoPosition &p) const {
iv[1] = -sin(lat) * sin(lng);
iv[2] = cos(lat);
PIMathVectorT3d jv; // Compute j vector in local North-East-Up (NEU) system
jv[0] = -sin(lng);
jv[1] = cos(lng);
jv[2] = 0.0;
double local_n = z.dot(iv) / z.length(); // Now, let's use dot product to get localN unitary vectors
double local_e = z.dot(jv) / z.length(); // Now, let's use dot product to get localE unitary vector
double test = piAbsd(local_n) + piAbsd(local_e); // Let's test if computing azimuth has any sense
if (test < 1.0e-16) return 0.0; // Warning: If elevation is very close to 90 degrees, we will return azimuth = 0.0
jv[0] = -sin(lng);
jv[1] = cos(lng);
jv[2] = 0.0;
double local_n = z.dot(iv) / z.length(); // Now, let's use dot product to get localN unitary vectors
double local_e = z.dot(jv) / z.length(); // Now, let's use dot product to get localE unitary vector
double test = piAbsd(local_n) + piAbsd(local_e); // Let's test if computing azimuth has any sense
if (test < 1.0e-16) return 0.0; // Warning: If elevation is very close to 90 degrees, we will return azimuth = 0.0
double alpha = toDeg(atan2(local_e, local_n));
if (alpha < 0.0) return alpha + 360.0;
else return alpha;
if (alpha < 0.0)
return alpha + 360.0;
else
return alpha;
}
double PIGeoPosition::getCurvMeridian() const {
double slat = sin(toRad(latitudeGeodetic()));
double w = 1.0 / sqrt(1.0 - el.eccSquared() * slat * slat);
double w = 1.0 / sqrt(1.0 - el.eccSquared() * slat * slat);
return el.a * (1.0 - el.eccSquared()) * w * w * w;
}
@@ -548,4 +573,3 @@ double PIGeoPosition::getCurvPrimeVertical() const {
double slat = sin(toRad(latitudeGeodetic()));
return el.a / sqrt(1.0 - el.eccSquared() * slat * slat);
}

180
libs/main/geo/pigeoposition.h
View File

@@ -5,22 +5,22 @@
* \~russian Класс для хранения географической позиции и преобразований
*/
/*
PIP - Platform Independent Primitives
Class for geo position storage and conversions
Andrey Bychkov work.a.b@yandex.ru
PIP - Platform Independent Primitives
Class for geo position storage and conversions
Andrey Bychkov work.a.b@yandex.ru
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU Lesser General Public License for more details.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
You should have received a copy of the GNU Lesser General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef PIGEOPOSITION_H
@@ -29,34 +29,45 @@
#include "piellipsoidmodel.h"
#include "pimathvector.h"
class PIP_EXPORT PIGeoPosition : public PIMathVectorT3d
{
class PIP_EXPORT PIGeoPosition: public PIMathVectorT3d {
public:
enum CoordinateSystem {
Unknown=0, /// Unknown coordinate system
Geodetic, /// Geodetic latitude, longitude, and height above ellipsoid
Geocentric, /// Geocentric (regular spherical coordinates)
Cartesian, /// Cartesian (Earth-centered, Earth-fixed)
Spherical /// Spherical coordinates (theta,phi,radius)
Unknown = 0, /// Unknown coordinate system
Geodetic, /// Geodetic latitude, longitude, and height above ellipsoid
Geocentric, /// Geocentric (regular spherical coordinates)
Cartesian, /// Cartesian (Earth-centered, Earth-fixed)
Spherical /// Spherical coordinates (theta,phi,radius)
};
static const double one_cm_tolerance;/// One centimeter tolerance.
static const double one_mm_tolerance;/// One millimeter tolerance.
static const double one_um_tolerance;/// One micron tolerance.
static double position_tolerance;/// Default tolerance (default 1mm)
static double setPositionTolerance(const double tol) {position_tolerance = tol; return position_tolerance;}
static double getPositionTolerance() {return position_tolerance;}
static const double one_cm_tolerance; /// One centimeter tolerance.
static const double one_mm_tolerance; /// One millimeter tolerance.
static const double one_um_tolerance; /// One micron tolerance.
static double position_tolerance; /// Default tolerance (default 1mm)
static double setPositionTolerance(const double tol) {
position_tolerance = tol;
return position_tolerance;
}
static double getPositionTolerance() { return position_tolerance; }
PIGeoPosition();
PIGeoPosition(double a, double b, double c, CoordinateSystem s = Cartesian, PIEllipsoidModel ell = PIEllipsoidModel::WGS84Ellipsoid());
PIGeoPosition(PIMathVectorT3d v, CoordinateSystem s = Cartesian, PIEllipsoidModel ell = PIEllipsoidModel::WGS84Ellipsoid());
PIGeoPosition &transformTo(CoordinateSystem sys);
PIGeoPosition &asGeodetic() {transformTo(Geodetic); return *this; } /// Convert to geodetic coordinate
PIGeoPosition &asGeodetic(const PIEllipsoidModel &ell) {setEllipsoidModel(ell); transformTo(Geodetic); return *this;} /// Convert to another ell, then to geodetic coordinates
PIGeoPosition &asECEF() {transformTo(Cartesian); return *this; } /// Convert to cartesian coordinates
PIGeoPosition & transformTo(CoordinateSystem sys);
PIGeoPosition & asGeodetic() {
transformTo(Geodetic);
return *this;
} /// Convert to geodetic coordinate
PIGeoPosition & asGeodetic(const PIEllipsoidModel & ell) {
setEllipsoidModel(ell);
transformTo(Geodetic);
return *this;
} /// Convert to another ell, then to geodetic coordinates
PIGeoPosition & asECEF() {
transformTo(Cartesian);
return *this;
} /// Convert to cartesian coordinates
double x() const;
double y() const;
@@ -70,43 +81,51 @@ public:
double height() const;
/// Set the ellipsoid values for this PIGeoPosition given a ellipsoid.
void setEllipsoidModel(const PIEllipsoidModel &ell) {el = ell;}
void setEllipsoidModel(const PIEllipsoidModel & ell) { el = ell; }
/// Set the \a PIGeoPosition given geodetic coordinates in degrees. \a CoordinateSystem is set to \a Geodetic.
PIGeoPosition &setGeodetic(double lat, double lon, double ht, PIEllipsoidModel ell = PIEllipsoidModel::WGS84Ellipsoid());
PIGeoPosition & setGeodetic(double lat, double lon, double ht, PIEllipsoidModel ell = PIEllipsoidModel::WGS84Ellipsoid());
/// Set the \a PIGeoPosition given geocentric coordinates in degrees. \a CoordinateSystem is set to \a Geocentric
PIGeoPosition &setGeocentric(double lat, double lon, double rad);
PIGeoPosition & setGeocentric(double lat, double lon, double rad);
/// Set the \a PIGeoPosition given spherical coordinates in degrees. \a CoordinateSystem is set to \a Spherical
PIGeoPosition &setSpherical(double theta, double phi, double rad);
PIGeoPosition & setSpherical(double theta, double phi, double rad);
/// Set the \a PIGeoPosition given ECEF coordinates in meeters. \a CoordinateSystem is set to \a Cartesian.
PIGeoPosition &setECEF(double x, double y, double z);
PIGeoPosition & setECEF(double x, double y, double z);
/// Fundamental conversion from spherical to cartesian coordinates.
static void convertSphericalToCartesian(const PIMathVectorT3d &tpr, PIMathVectorT3d &xyz);
static void convertSphericalToCartesian(const PIMathVectorT3d & tpr, PIMathVectorT3d & xyz);
/// Fundamental routine to convert cartesian to spherical coordinates.
static void convertCartesianToSpherical(const PIMathVectorT3d &xyz, PIMathVectorT3d &tpr);
static void convertCartesianToSpherical(const PIMathVectorT3d & xyz, PIMathVectorT3d & tpr);
/// Fundamental routine to convert ECEF (cartesian) to geodetic coordinates,
static void convertCartesianToGeodetic(const PIMathVectorT3d &xyz, PIMathVectorT3d &llh, PIEllipsoidModel ell = PIEllipsoidModel::WGS84Ellipsoid());
static void convertCartesianToGeodetic(const PIMathVectorT3d & xyz,
PIMathVectorT3d & llh,
PIEllipsoidModel ell = PIEllipsoidModel::WGS84Ellipsoid());
/// Fundamental routine to convert geodetic to ECEF (cartesian) coordinates,
static void convertGeodeticToCartesian(const PIMathVectorT3d &llh, PIMathVectorT3d &xyz, PIEllipsoidModel ell = PIEllipsoidModel::WGS84Ellipsoid());
static void convertGeodeticToCartesian(const PIMathVectorT3d & llh,
PIMathVectorT3d & xyz,
PIEllipsoidModel ell = PIEllipsoidModel::WGS84Ellipsoid());
/// Fundamental routine to convert cartesian (ECEF) to geocentric
static void convertCartesianToGeocentric(const PIMathVectorT3d &xyz, PIMathVectorT3d &llr);
static void convertCartesianToGeocentric(const PIMathVectorT3d & xyz, PIMathVectorT3d & llr);
/// Fundamental routine to convert geocentric to cartesian (ECEF)
static void convertGeocentricToCartesian(const PIMathVectorT3d &llr, PIMathVectorT3d &xyz);
static void convertGeocentricToCartesian(const PIMathVectorT3d & llr, PIMathVectorT3d & xyz);
/// Fundamental routine to convert geocentric to geodetic
static void convertGeocentricToGeodetic(const PIMathVectorT3d &llr, PIMathVectorT3d &llh, PIEllipsoidModel ell = PIEllipsoidModel::WGS84Ellipsoid());
static void convertGeocentricToGeodetic(const PIMathVectorT3d & llr,
PIMathVectorT3d & llh,
PIEllipsoidModel ell = PIEllipsoidModel::WGS84Ellipsoid());
/// Fundamental routine to convert geodetic to geocentric
static void convertGeodeticToGeocentric(const PIMathVectorT3d &llh, PIMathVectorT3d &llr, PIEllipsoidModel ell = PIEllipsoidModel::WGS84Ellipsoid());
static void convertGeodeticToGeocentric(const PIMathVectorT3d & llh,
PIMathVectorT3d & llr,
PIEllipsoidModel ell = PIEllipsoidModel::WGS84Ellipsoid());
/// Compute the radius of the ellipsoidal Earth, given the geodetic latitude.
static double radiusEarth(double geolat, PIEllipsoidModel ell = PIEllipsoidModel::WGS84Ellipsoid());
@@ -117,22 +136,20 @@ public:
}
/// Compute the range in meters between two PIGeoPositions.
static double range(const PIGeoPosition &a, const PIGeoPosition &b);
double range(const PIGeoPosition &p) const {
return range((*this), p);
}
static double range(const PIGeoPosition & a, const PIGeoPosition & b);
double range(const PIGeoPosition & p) const { return range((*this), p); }
/// Computes the elevation of the input (p) position as seen from this PIGeoPosition.
double elevation(const PIGeoPosition &p) const;
double elevation(const PIGeoPosition & p) const;
/// Computes the elevation of the input (p) position as seen from this PIGeoPosition, using a Geodetic (ellipsoidal) system.
double elevationGeodetic(const PIGeoPosition &p) const;
double elevationGeodetic(const PIGeoPosition & p) const;
/// Computes the azimuth of the input (p) position as seen from this PIGeoPosition.
double azimuth(const PIGeoPosition &p) const;
double azimuth(const PIGeoPosition & p) const;
/// Computes the azimuth of the input (p) position as seen from this PIGeoPosition, using a Geodetic (ellipsoidal) system.
double azimuthGeodetic(const PIGeoPosition &p) const;
double azimuthGeodetic(const PIGeoPosition & p) const;
/// Computes the radius of curvature of the meridian (Rm) corresponding to this PIGeoPosition.
double getCurvMeridian() const;
@@ -141,19 +158,19 @@ public:
double getCurvPrimeVertical() const;
/// Returns as PIMathVectorT3d
const PIMathVectorT3d & vector() const {return *this;}
const PIMathVectorT3d & vector() const { return *this; }
PIGeoPosition &operator=(const PIMathVectorT3d & v);
PIGeoPosition &operator-=(const PIGeoPosition &right);
PIGeoPosition &operator+=(const PIGeoPosition &right);
friend PIGeoPosition operator-(const PIGeoPosition &left, const PIGeoPosition &right);
friend PIGeoPosition operator+(const PIGeoPosition &left, const PIGeoPosition &right);
friend PIGeoPosition operator*(const double &scale, const PIGeoPosition &right);
friend PIGeoPosition operator*(const PIGeoPosition &left, const double &scale);
friend PIGeoPosition operator*(const int &scale, const PIGeoPosition &right);
friend PIGeoPosition operator*(const PIGeoPosition &left, const int &scale);
bool operator==(const PIGeoPosition &right) const;
bool operator!=(const PIGeoPosition &right) const {return !(operator==(right));}
PIGeoPosition & operator=(const PIMathVectorT3d & v);
PIGeoPosition & operator-=(const PIGeoPosition & right);
PIGeoPosition & operator+=(const PIGeoPosition & right);
friend PIGeoPosition operator-(const PIGeoPosition & left, const PIGeoPosition & right);
friend PIGeoPosition operator+(const PIGeoPosition & left, const PIGeoPosition & right);
friend PIGeoPosition operator*(const double & scale, const PIGeoPosition & right);
friend PIGeoPosition operator*(const PIGeoPosition & left, const double & scale);
friend PIGeoPosition operator*(const int & scale, const PIGeoPosition & right);
friend PIGeoPosition operator*(const PIGeoPosition & left, const int & scale);
bool operator==(const PIGeoPosition & right) const;
bool operator!=(const PIGeoPosition & right) const { return !(operator==(right)); }
private:
@@ -161,15 +178,36 @@ private:
PIEllipsoidModel el;
CoordinateSystem s;
};
inline PIGeoPosition operator-(const PIGeoPosition &left, const PIGeoPosition &right) {PIGeoPosition l(left),r(right); l.transformTo(PIGeoPosition::Cartesian); r.transformTo(PIGeoPosition::Cartesian); l -= r; return l;}
inline PIGeoPosition operator+(const PIGeoPosition &left, const PIGeoPosition &right) {PIGeoPosition l(left),r(right); l.transformTo(PIGeoPosition::Cartesian); r.transformTo(PIGeoPosition::Cartesian); l += r; return l;}
inline PIGeoPosition operator*(const double &scale, const PIGeoPosition &right) {PIMathVectorT3d tmp(right); tmp *= scale; return PIGeoPosition(tmp);}
inline PIGeoPosition operator*(const PIGeoPosition &left, const double &scale) {return operator* (scale, left);}
inline PIGeoPosition operator*(const int &scale, const PIGeoPosition &right) {return operator* (double(scale), right);}
inline PIGeoPosition operator*(const PIGeoPosition &left, const int &scale) {return operator* (double(scale), left);}
inline PIGeoPosition operator-(const PIGeoPosition & left, const PIGeoPosition & right) {
PIGeoPosition l(left), r(right);
l.transformTo(PIGeoPosition::Cartesian);
r.transformTo(PIGeoPosition::Cartesian);
l -= r;
return l;
}
inline PIGeoPosition operator+(const PIGeoPosition & left, const PIGeoPosition & right) {
PIGeoPosition l(left), r(right);
l.transformTo(PIGeoPosition::Cartesian);
r.transformTo(PIGeoPosition::Cartesian);
l += r;
return l;
}
inline PIGeoPosition operator*(const double & scale, const PIGeoPosition & right) {
PIMathVectorT3d tmp(right);
tmp *= scale;
return PIGeoPosition(tmp);
}
inline PIGeoPosition operator*(const PIGeoPosition & left, const double & scale) {
return operator*(scale, left);
}
inline PIGeoPosition operator*(const int & scale, const PIGeoPosition & right) {
return operator*(double(scale), right);
}
inline PIGeoPosition operator*(const PIGeoPosition & left, const int & scale) {
return operator*(double(scale), left);
}
#endif // PIGEOPOSITION_H