code format
This commit is contained in:
@@ -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);
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user