orbit_utils.c

卫星仿真软件 卫星仿真软件 卫星仿真软件

 *
 * Convert geocentric latitude (NOTE: NOT geodetic latitude. Will need to
 * account for oblateness of earth later.) and longitude coordinates to
 * NORMALIZED (length=1) geocentric equatorial spherical coordinates
 * (r, theta, phi).
 *
 * We assume that longitude equals geocentric equatorial theta at t=0.
 */
void
lat_lon_to_spherical(const double lat, const double lon, const double t,
		     const CentralBody * pcb, SphericalCoordinates * px)
{
  px->r = 1.0;			/* Geocentric equatorial spherical */
  px->theta = lon * DEG_TO_RAD + pcb->rotation_rate * t;	/* coordinates. */
  px->phi = (90.0 - lat) * DEG_TO_RAD;
}

/*
 * lat_lon_to_cartesian
 *
 * Convert geocentric latitude (NOTE: NOT geodetic latitude. Will need to
 * account for oblateness of earth later.) and longitude coordinates to
 * NORMALIZED (length=1) geocentric equatorial cartesian coordinates.
 *
 * We assume that longitude equals geocentric equatorial theta at t=0.
 */
void
lat_lon_to_cartesian(const double lat, const double lon, const double t,
		     const CentralBody * pcb, CartesianCoordinates * px)
{
  SphericalCoordinates y;

  lat_lon_to_spherical(lat, lon, t, pcb, &y);
  spherical_to_cartesian(px, &y);
}

/*
 * spherical_to_lat_lon
 *
 * Converts spherical (geocentric equatorial) coordinates to
 * the latitude and longitude of the point on earth it is over
 */
void
spherical_to_lat_lon(LatLon * pl, const SphericalCoordinates * px,
		     const double t, const CentralBody * pcb)
{
  double x;
  x = (px->theta - pcb->rotation_rate * t) * RAD_TO_DEG - 180.0;
  if (x > 0)
    pl->lon = fmod(x, 360.0) - 180.0;
  else
    pl->lon = fmod(x, 360.0) + 180.0;
  pl->lat = 90.0 - px->phi * RAD_TO_DEG;
}

/*
 * Given a time, return true anomaly.
 */
double
time_to_true_anomaly(const double t, const CentralBody * pcb,
		     const double a, const double e, const double T)
{
  double M, M_n, E, incr;
/*
 * E is the eccentric anomaly, M = E-e*sinE is the mean anomaly.  The
 * time of flight equation, t-T = Msqrt(a^3/mu), gives M in terms of
 * time, and we solve for E in terms of M by Newton's method (the Kepler
 * problem).  The calculation is trivial for circular orbits, and is handled
 * separately.
 */

  if (e == 0.0) {
    /* mean anomaly = eccentric anomaly = true anomaly */
    return mean_anomaly(t, pcb->mu, a, T);
  }

  if (e < 0.1) {
    /* mean anomaly is a good seed for Newton's method */
    M = mean_anomaly(t, pcb->mu, a, T);
    E = M;
    M_n = M - e * sin(M);
    incr = (M - M_n) / (1 - e * cos(E));
    while (fabs(incr) > ANOMALY_COMPUTATION_TOLERANCE) {
      E += incr;
      M_n = E - e * sin(E);
      incr = (M - M_n) / (1 - e * cos(E));
    }
  } else {
    /* will force each iterate of Newton's method to remain in 0 to 2pi */
    M = mean_anomaly(t, pcb->mu, a, T);
    E = M;
    M_n = M - e * sin(M);
    incr = (M - M_n) / (1 - e * cos(E));
    while (fabs(incr) > ANOMALY_COMPUTATION_TOLERANCE) {
      E += incr;
      if (E < 0) {
	E = (E - incr) / 2.0;
      } else if (E > TWOPI) {
	E = (E - incr + TWOPI) / 2.0;
      }
      M_n = E - e * sin(E);
      incr = (M - M_n) / (1 - e * cos(E));
    }
  }

  return eccentric_anomaly_to_true_anomaly(E, e);
}

/*
 * Convert eccentric anomaly to true anomaly.
 * E is the eccentric anomaly, e is the eccentricity of the orbit.
 *
 * Note:  If e=0, then eccentric anomaly equals true anomaly and
 *   there is no reason to call this routine!
 */
double
eccentric_anomaly_to_true_anomaly(const double E, const double e)
{
  if (sin(E) >= 0.0)
    return acos((e - cos(E)) / (e * cos(E) - 1));
  else
    return 2 * PI - acos((e - cos(E)) / (e * cos(E) - 1));
}

/*
 * sat_to_fisheye.c
 *
 * Given a satellite position vector (cartesian geocentric-equatorial
 * coordinates) and time, compute the fisheye coordinates of that satellite as
 * viewed from a given latitude/longitude on the earth.
 *
 * If the spherical coordinates of the satellite as viewed from the
 * ground are (phi, theta), where phi is measured from vertical, and
 * theta is measured from east, then the fisheye coordinates are given
 * by 1/(PI/2)*(-phi*cos(theta), phi*sin(theta)).  Thus coordinates
 * lie in the unit disk, and are oriented so that north is up, south is
 * down, east is LEFT, west is RIGHT. (This is the correct orientation
 * when looking up.
 *
 * Value of function is TRUE if satellite is in view, FALSE otherwise.
 */
int
sat_to_fisheye(const CartesianCoordinates * ps, const double lat, const double lon,
	       const CentralBody * pcb, const double t, double sky[2])
{
  CartesianCoordinates p, sp, e, n;
  double phi, alpha, radius;

  lat_lon_to_cartesian(lat, lon, t, pcb, &p);

  /* sp is the vector from point on earth to satellite.
     Scale satellite vector by earth radius since we assume |p|=1.  */

  radius = pcb->radius;
  sp.x = ps->x / radius - p.x;
  sp.y = ps->y / radius - p.y;
  sp.z = ps->z / radius - p.z;

  normalize(&sp);
  normalize(&p);

  phi = acos(dot(&p, &sp));	/* The view angle from vertical. */

  if (debug) fprintf(stderr, "Angle from vertical: %f\n", phi * RAD_TO_DEG);

  if (phi > PI / 2.0)
    return FALSE;

  /* Compute "east" and "north" vectors spanning the tangent plane at p.
     For the north pole, take east to be direction of 0 degrees long.
     For the south pole, take east to be direction of 180 degrees long.   */

  if (fabs(lat) == 90.0) {
    lat_lon_to_cartesian(0.0, lon + 90.0, t, pcb, &e);
  } else {
    e.x = -p.y;			/* The "east" vector is the projection of p onto the    */
    e.y = p.x;			/* equatorial plane, then rotated counterclockwise pi/2 */
    e.z = 0.0;			/* and normalized.                                      */
  }
  normalize(&e);
  cross_product(&n, &p, &e);	/* "North" is then p x e.  */

  /* project sp onto plane defined by n,e by subtracting out components of p */
  alpha = dot(&p, &sp);
  sp.x -= alpha * p.x;
  sp.y -= alpha * p.y;
  sp.z -= alpha * p.z;
  normalize(&sp);

  /* Cos(theta) equals the projection of the new sp onto e,
     or the dot product of sp and e. Sin(theta) equals the
     projection of sp onto n. */

  sky[0] = -2.0 / PI * phi * dot(&sp, &e);
  sky[1] = 2.0 / PI * phi * dot(&sp, &n);

  if (debug) {
    print_vec("sp", &sp);
    print_vec("p", &p);
    print_vec("e", &e);
    print_vec("n", &n);
    fprintf(stderr, "Norms: sp %f p %f e %f n %f\n", norm(&sp), norm(&p),
	    norm(&e), norm(&n));
    fprintf(stderr, "sky = (%f , %f )\n", sky[0], sky[1]);
  }

  return TRUE;
}

/*
 * oe_to_period
 *
 * Returns the period of a classical orbit given its orbital elements
 *
 */
double
oe_to_period(const OrbitalElements * poe, const CentralBody * pcb)
{
  return (TWOPI * pow(poe->a, 1.5) / sqrt(pcb->mu));
}

/*
 * oe_info
 *
 * Writes to file information about the orbital elements
 *
 */
void
oe_info(FILE * out, const OrbitalElements * poe, const CentralBody * pcb)
{
  double period = oe_to_period(poe, pcb);

  /* output orbital elements */
  fprintf(out, "a=%f e=%f i=%f Om=%f om=%f T=%f ",
	  poe->a, poe->e, poe->i, poe->Omega, poe->omega, poe->T);

  /* output period of orbit in hours */
  fprintf(out, " (%f=%f hrs)\n", period, period / 3600.0);
}

/*
 * oe_to_nodal_precession
 *
 * Returns the nodal precession for the J2 model
 *
 */
double
oe_to_nodal_precession(const OrbitalElements * poe, const CentralBody * pcb)
{
  double a = poe->a;

  double R_a_sqr = pcb->radius * pcb->radius / a / a;
  double one_minus_e_sqr = 1 - poe->e * poe->e;
  double factor =
    0.75 * pcb->J2 * R_a_sqr / one_minus_e_sqr / one_minus_e_sqr;
  double n = sqrt(pcb->mu / a / a / a);
  double cosi = cos(poe->i);

  if (debug) fprintf(stderr, "nodalprecession = %f\n", -2.0 * factor * n * cosi);

  return (-2.0 * factor * n * cosi);
}

/*
 * oe_to_apsidal_rotation
 *
 * Returns the apsidal rotation for the J2 model
 *
 */
double
oe_to_apsidal_rotation(const OrbitalElements * poe, const CentralBody * pcb)
{
  double a = poe->a;

  double R_a_sqr = pcb->radius * pcb->radius / a / a;
  double one_minus_e_sqr = 1 - poe->e * poe->e;
  double factor =
    0.75 * pcb->J2 * R_a_sqr / one_minus_e_sqr / one_minus_e_sqr;
  double n = sqrt(pcb->mu / a / a / a);
  double cosi = cos(poe->i);

  if (debug) fprintf(stderr, "apsidal rotation = %f\n",
		     factor * n * (5.0 * cosi * cosi - 1.0));


  return (factor * n * (5.0 * cosi * cosi - 1.0));
}

/*
 * oe_to_apoapsis_altitude
 *
 * Returns the apoapsis altitude, the height from the surface of the central
 * body to the satellite at its furthest point in the orbit.
 *
 */
double
oe_to_apoapsis_altitude(const OrbitalElements * poe, const CentralBody * pcb)
{
  return (poe->a * (1 + poe->e) - pcb->radius);
}

/*
 * oe_to_periapsis_altitude
 *
 * Returns the periapsis altitude, the height from the surface of the central
 * body to the satellite during closest approach.
 *
 */
double
oe_to_periapsis_altitude(const OrbitalElements * poe, const CentralBody * pcb)
{
  return (poe->a * (1 - poe->e) - pcb->radius);
}

/*
 * mean_anomaly
 *
 * Computes the mean anomaly in terms of current time t, gravitational
 * parameter mu, and the orbital elements a and T, and returns it in the
 * range 0 to 2*pi.
 *
 * M = sqrt(mu/(a*a*a))*(t-T) mod 2*pi
 *
 */
double
mean_anomaly(const double t, const double mu, const double a, const double T)
{
  double M = fmod(sqrt(mu / (a * a * a)) * (t - T), TWOPI);

  /* if negative add two pi to put in correct range */
  if (M < 0.0)
    M += TWOPI;

  return M;
}