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;
}
bool cOrbit::TrueAnomalyAtRadius(double r, double timeOfStart, double &tAOut) const
{
double e = Eccentricity();
double a = SemiMajorAxis();
bool status = false;
double arg = (a - a*e*e - r)/(e*r);
if(-1 <= arg && arg <= 1)
{
double tA = TrueAnomalyAt(timeOfStart, status);
tAOut = acos(arg);
tAOut = min(
fmod(tAOut - tA + 4*M_PI, 2*M_PI),
fmod(-tAOut - tA + 4*M_PI, 2*M_PI)
) + tA;
return status;
}
else
{
return false;
}
}
Vector3d cOrbit::VelocityAtTrueAnomaly(double tA) const
{
double mu = ReferenceBody().GravitationalParameter();
double e = Eccentricity();
double h = std::sqrt(mu * SemiMajorAxis() * (1.0 - e * e));
double r = RadiusAtTrueAnomaly(tA);
double sin = std::sin(tA);
double cos = std::cos(tA);
double vr = mu * e * sin / h;
double vtA = h / r;
return RotationToReferenceFrame()._transformVector(Vector3d(vr * cos - vtA * sin, vr * sin + vtA * cos, 0));
}
double cOrbit::PhaseAngle(cOrbit &orbit, double t, bool &status) const
{
Vector3d n = NormalVector();
Vector3d p1 = PositionAtTrueAnomaly(TrueAnomalyAt(t, status));
Vector3d p2 = orbit.PositionAtTrueAnomaly(orbit.TrueAnomalyAt(t, status));
p2 = p2 - (n * p2.dot(n)); // Project p2 onto our orbital plane
double r1 = p1.norm();
double r2 = p2.norm();
double phaseAngle = std::acos(p1.dot(p2) / (r1 * r2));
if(p1.cross(p2).dot(n) < 0.0)
{
phaseAngle = TWO_PI - phaseAngle;
}
if (orbit.SemiMajorAxis() < SemiMajorAxis())
{
phaseAngle = phaseAngle - TWO_PI;
}
return phaseAngle;
}
double _stdcall PrimeVerticalCurvature( double dAngle )
{
return SemiMajorAxis() / sqrt(1.0 - eSq() * (sin(dAngle) * sin(dAngle)));
}
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;
}
double cOrbit::SpeedAtTrueAnomaly(double tA) const
{
return std::sqrt(ReferenceBody().GravitationalParameter() * (2.0 / RadiusAtTrueAnomaly(tA) - 1.0 / SemiMajorAxis()));
}
double cOrbit::RadiusAtTrueAnomaly(double tA) const
{
double e = Eccentricity();
return SemiMajorAxis() * (1 - e * e) / (1 + e * std::cos(tA));
}
