// /////////////////////////////////////////////////////////////////////
// Calculate the angle between this vector and another
public double Angle(Vector vec)
{
return Math.Acos(Dot(vec) / (Magnitude() * vec.Magnitude()));
}
// /////////////////////////////////////////////////////////////////////
protected Eci FinalPosition(double incl, double omega, double e,
double a, double xl, double xnode,
double xn, double tsince)
{
if ((e * e) > 1.0)
{
throw new Exception("Error in satellite data");
}
double beta = Math.Sqrt(1.0 - e * e);
// Long period periodics
double axn = e * Math.Cos(omega);
double temp = 1.0 / (a * beta * beta);
double xll = temp * m_xlcof * axn;
double aynl = temp * m_aycof;
double xlt = xl + xll;
double ayn = e * Math.Sin(omega) + aynl;
// Solve Kepler's Equation
double capu = Globals.Fmod2p(xlt - xnode);
double temp2 = capu;
double temp3 = 0.0;
double temp4 = 0.0;
double temp5 = 0.0;
double temp6 = 0.0;
double sinepw = 0.0;
double cosepw = 0.0;
bool fDone = false;
for (int i = 1; (i <= 10) && !fDone; i++)
{
sinepw = Math.Sin(temp2);
cosepw = Math.Cos(temp2);
temp3 = axn * sinepw;
temp4 = ayn * cosepw;
temp5 = axn * cosepw;
temp6 = ayn * sinepw;
double epw = (capu - temp4 + temp3 - temp2) /
(1.0 - temp5 - temp6) + temp2;
if (Math.Abs(epw - temp2) <= Globals.E6A)
fDone = true;
else
temp2 = epw;
}
// Short period preliminary quantities
double ecose = temp5 + temp6;
double esine = temp3 - temp4;
double elsq = axn * axn + ayn * ayn;
temp = 1.0 - elsq;
double pl = a * temp;
double r = a * (1.0 - ecose);
double temp1 = 1.0 / r;
double rdot = Globals.XKE * Math.Sqrt(a) * esine * temp1;
double rfdot = Globals.XKE * Math.Sqrt(pl) * temp1;
temp2 = a * temp1;
double betal = Math.Sqrt(temp);
temp3 = 1.0 / (1.0 + betal);
double cosu = temp2 * (cosepw - axn + ayn * esine * temp3);
double sinu = temp2 * (sinepw - ayn - axn * esine * temp3);
double u = Globals.AcTan(sinu, cosu);
double sin2u = 2.0 * sinu * cosu;
double cos2u = 2.0 * cosu * cosu - 1.0;
temp = 1.0 / pl;
temp1 = Globals.CK2 * temp;
temp2 = temp1 * temp;
// Update for short periodics
double rk = r * (1.0 - 1.5 * temp2 * betal * m_x3thm1) +
0.5 * temp1 * m_x1mth2 * cos2u;
double uk = u - 0.25 * temp2 * m_x7thm1 * sin2u;
double xnodek = xnode + 1.5 * temp2 * m_cosio * sin2u;
double xinck = incl + 1.5 * temp2 * m_cosio * m_sinio * cos2u;
double rdotk = rdot - xn * temp1 * m_x1mth2 * sin2u;
double rfdotk = rfdot + xn * temp1 * (m_x1mth2 * cos2u + 1.5 * m_x3thm1);
// Orientation vectors
double sinuk = Math.Sin(uk);
double cosuk = Math.Cos(uk);
double sinik = Math.Sin(xinck);
double cosik = Math.Cos(xinck);
double sinnok = Math.Sin(xnodek);
double cosnok = Math.Cos(xnodek);
double xmx = -sinnok * cosik;
double xmy = cosnok * cosik;
double ux = xmx * sinuk + cosnok * cosuk;
double uy = xmy * sinuk + sinnok * cosuk;
double uz = sinik * sinuk;
double vx = xmx * cosuk - cosnok * sinuk;
double vy = xmy * cosuk - sinnok * sinuk;
double vz = sinik * cosuk;
// Position
double x = rk * ux;
double y = rk * uy;
double z = rk * uz;
Vector vecPos = new Vector(x, y, z);
// Validate on altitude
double altKm = (vecPos.Magnitude() * (Globals.XKMPER / Globals.AE));
if ((altKm < Globals.EARTH_RAD) || (altKm > (2 * Globals.GEOSYNC_ALT)))
{
throw new Exception("Satellite orbit may have decayed");
}
// Velocity
double xdot = rdotk * ux + rfdotk * vx;
double ydot = rdotk * uy + rfdotk * vy;
double zdot = rdotk * uz + rfdotk * vz;
Vector vecVel = new Vector(xdot, ydot, zdot);
DateTime gmt = m_Orbit.EpochTime;
gmt = gmt.AddMinutes(tsince);
return new Eci(vecPos, vecVel, new Julian(gmt), true);
}