LLPoint DestVincenty(LLPoint pt, double brng, double dist, double* revAz)
{
double s = dist;
double alpha1 = brng;
double sinAlpha1 = sin(alpha1);
double cosAlpha1 = cos(alpha1);
double tanU1 = (1.0 - Flattening()) * tan(pt.latitude);
double cosU1 = 1.0 / sqrt((1.0 + tanU1*tanU1));
double sinU1 = tanU1 * cosU1;
double sigma1 = atan2(tanU1, cosAlpha1);
double sinAlpha = cosU1 * sinAlpha1;
double cosSqAlpha = 1.0 - sinAlpha * sinAlpha;
double uSq = cosSqAlpha * (SemiMajorAxis() * SemiMajorAxis() - SemiMinorAxis() * SemiMinorAxis()) /
(SemiMinorAxis() * SemiMinorAxis());
double A = 1.0 + uSq / 16384.0 * (4096.0 + uSq * (-768.0 + uSq * (320.0 - 175.0 * uSq)));
double B = uSq / 1024.0 * (256.0 + uSq * (-128.0 + uSq * (74.0 - 47.0 * uSq)));
double sigma = s / (SemiMinorAxis() * A);
double sigmaP = M_2PI;
double sinSigma = sin(sigma);
double cosSigma = cos(sigma);
double cos2SigmaM = cos(2.0 * sigma1 + sigma);
int iterLimit = 0;
while (fabs(sigma-sigmaP) > Eps() && ++iterLimit < 100) {
cos2SigmaM = cos(2.0 * sigma1 + sigma);
sinSigma = sin(sigma);
cosSigma = cos(sigma);
double cos2SigmaSq = cos2SigmaM * cos2SigmaM;
double deltaSigma = B * sinSigma * (cos2SigmaM + B * 0.25 * (cosSigma * (-1.0 + 2.0 * cos2SigmaSq)-
B / 6.0 * cos2SigmaM * (-3.0 + 4.0 * sinSigma * sinSigma) * (-3.0 + 4.0 * cos2SigmaSq)));
sigmaP = sigma;
sigma = s / (SemiMinorAxis() * A) + deltaSigma;
}
double tmp = sinU1 * sinSigma - cosU1 * cosSigma * cosAlpha1;
double lat2 = atan2(sinU1 * cosSigma + cosU1 * sinSigma * cosAlpha1,
(1.0 - Flattening()) * sqrt(sinAlpha * sinAlpha + tmp * tmp));
double lambda = atan2(sinSigma * sinAlpha1, cosU1 * cosSigma - sinU1 * sinSigma * cosAlpha1);
double C = Flattening() / 16.0 * cosSqAlpha * (4.0 + Flattening() * (4.0 -3.0 * cosSqAlpha));
double L = lambda - (1.0 - C) * Flattening() * sinAlpha *
(sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * (-1.0 + 2.0 * cos2SigmaM * cos2SigmaM)));
LLPoint return_value;
return_value.latitude=lat2;
return_value.longitude= pt.longitude + L;
*revAz =atan2(sinAlpha, -tmp);
return return_value;
}
void PureProjection::FromCartesianTGeodetic(const double &X,const double &Y,const double &Z, double &Lat, double &Lng)
{
double E = Flattening() * (2.0 - Flattening());
Lng = atan2(Y, X);
double P = sqrt(X * X + Y * Y);
double Theta = atan2(Z, (P * (1.0 - Flattening())));
double st = sin(Theta);
double ct = cos(Theta);
Lat = atan2(Z + E / (1.0 - Flattening()) * Axis() * st * st * st, P - E * Axis() * ct * ct * ct);
Lat /= (PI / 180);
Lng /= (PI / 180);
}
void PureProjection::FromCartesianTGeodetic(const double &X, const double &Y, const double &Z, double &Lat_D, double &Lng_D)
{
double E = Flattening() * (2.0 - Flattening());
double Lng_R = atan2(Y, X);
double P = sqrt(X * X + Y * Y);
double Theta = atan2(Z, (P * (1.0 - Flattening())));
double st = sin(Theta);
double ct = cos(Theta);
double Lat_R = atan2(Z + E / (1.0 - Flattening()) * Axis() * st * st * st, P - E * Axis() * ct * ct * ct);
Lat_D = Lat_R * DEG2RAD;
Lng_D = Lng_R * DEG2RAD;
}
void PureProjection::FromGeodeticToCartesian(double Lat,double Lng,double Height, double &X, double &Y, double &Z)
{
Lat = (PI / 180) * Lat;
Lng = (PI / 180) * Lng;
double B = Axis() * (1.0 - Flattening());
double ee = 1.0 - (B / Axis()) * (B / Axis());
double N = (Axis() / sqrt(1.0 - ee * sin(Lat) * sin(Lat)));
X = (N + Height) * cos(Lat) * cos(Lng);
Y = (N + Height) * cos(Lat) * sin(Lng);
Z = (N * (B / Axis()) * (B / Axis()) + Height) * sin(Lat);
}
/**
* @brief PureProjection::FromGeodeticToCartesian
* @param Lat_D
* @param Lng_D
* @param Height
* @param X
* @param Y
* @param Z
*/
void PureProjection::FromGeodeticToCartesian(double Lat_D, double Lng_D, double Height, double &X, double &Y, double &Z)
{
double Lat_R = Lat_D * DEG2RAD;
double Lng_R = Lng_D * DEG2RAD;
double B = Axis() * (1.0 - Flattening());
double ee = 1.0 - (B / Axis()) * (B / Axis());
double N = (Axis() / sqrt(1.0 - ee * sin(Lat_R) * sin(Lat_R)));
X = (N + Height) * cos(Lat_R) * cos(Lng_R);
Y = (N + Height) * cos(Lat_R) * sin(Lng_R);
Z = (N * (B / Axis()) * (B / Axis()) + Height) * sin(Lat_R);
}
/**
* <b>DEPRECATED</b>
* @return \e r the inverse flattening of the Airy 1830 ellipsoid.
**********************************************************************/
static Math::real InverseFlattening() throw() { return 1/Flattening(); }
bool DistVincenty(LLPoint pt1, LLPoint pt2, InverseResult * result)
{
double L = pt2.longitude - pt1.longitude;
double U1 = atan((1-Flattening()) * tan(pt1.latitude));
double U2 = atan((1-Flattening()) * tan(pt2.latitude));
double sinU1 = sin(U1);
double cosU1 = cos(U1);
double sinU2 = sin(U2);
double cosU2 = cos(U2);
double dCosU1CosU2 = cosU1 * cosU2;
double dCosU1SinU2 = cosU1 * sinU2;
double dSinU1SinU2 = sinU1 * sinU2;
double dSinU1CosU2 = sinU1 * cosU2;
double lambda = L;
double lambdaP = M_2PI;
int iterLimit = 0;
double cosSqAlpha;
double sinSigma;
double cos2SigmaM;
double cosSigma;
double sigma;
double sinAlpha;
double C;
double sinLambda, cosLambda;
do {
sinLambda = sin(lambda);
cosLambda = cos(lambda);
sinSigma = sqrt((cosU2*sinLambda) * (cosU2*sinLambda) +
(dCosU1SinU2 - dSinU1CosU2 * cosLambda) * (dCosU1SinU2 - dSinU1CosU2 * cosLambda));
if (sinSigma==0)
{
(*result).reverseAzimuth = 0.0;
(*result).azimuth = 0.0;
(*result).distance = 0.0;
return true;
}
cosSigma = dSinU1SinU2 + dCosU1CosU2 * cosLambda;
sigma = atan2(sinSigma, cosSigma);
sinAlpha = dCosU1CosU2 * sinLambda / sinSigma;
cosSqAlpha = 1.0 - sinAlpha * sinAlpha;
cos2SigmaM = cosSigma - 2.0 * dSinU1SinU2 / cosSqAlpha;
if (!IsNumber(cos2SigmaM))
cos2SigmaM = 0.0; // equatorial line: cosSqAlpha=0 (§6)
C = Flattening() / 16.0 * cosSqAlpha * (4.0 + Flattening() * (4.0 - 3.0 * cosSqAlpha));
lambdaP = lambda;
lambda = L + (1.0 - C) * Flattening() * sinAlpha *
(sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * (-1.0 + 2.0 * cos2SigmaM *cos2SigmaM)));
} while (fabs(lambda-lambdaP) > Eps() && ++iterLimit < 100);
double uSq = cosSqAlpha * (SemiMajorAxis() * SemiMajorAxis() - SemiMinorAxis() * SemiMinorAxis()) /
(SemiMinorAxis() * SemiMinorAxis());
double A = 1.0 + uSq / 16384.0 * (4096.0 + uSq * (-768.0 + uSq * (320.0 - 175.0 * uSq)));
double B = uSq / 1024.0 * (256.0 + uSq * (-128.0 + uSq * (74.0 - 47.0 * uSq)));
double deltaSigma = B * sinSigma * (cos2SigmaM + B / 4.0 * (cosSigma * (-1.0 + 2.0 * cos2SigmaM * cos2SigmaM)-
B / 6.0 * cos2SigmaM * (-3.0 + 4.0 * sinSigma * sinSigma) * (-3.0 + 4.0 * cos2SigmaM * cos2SigmaM)));
(*result).distance = SemiMinorAxis() * A * (sigma - deltaSigma);
(*result).azimuth = atan2(cosU2 * sinLambda, dCosU1SinU2 - dSinU1CosU2 * cosLambda);
(*result).reverseAzimuth = M_PI + atan2(cosU1 * sinLambda, -dSinU1CosU2 + dCosU1SinU2 * cosLambda);
if((*result).reverseAzimuth < 0.0)
(*result).reverseAzimuth = M_2PI + (*result).reverseAzimuth;
if((*result).azimuth < 0.0)
(*result).azimuth = M_2PI + (*result).azimuth;
if (iterLimit>98)
return false;
else
return true;
}
