orbit_utils.c

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

/*
 *****************************************************
 *
 *  SaVi by Robert Thurman ([e-mail]) and
 *          Patrick Worfolk ([e-mail]).
 *
 *  Copyright (c) 1997 by The Geometry Center.
 *  This file is part of SaVi.  SaVi is free software;
 *  you can redistribute it and/or modify it only under
 *  the terms given in the file COPYRIGHT which you should
 *  have received along with this file.  SaVi may be
 *  obtained from:
 *  http://savi.sourceforge.net/
 *  http://www.geom.uiuc.edu/locate/SaVi
 *
 *****************************************************
 *
 * orbit_utils.c
 *
 * $Id: orbit_utils.c,v 1.8 2005/02/12 15:52:33 lloydwood Exp $
 */
#include <stdio.h>
#include <math.h>

#include "constants.h"
#include "globals.h"
#include "orbit_utils.h"
#include "stats_utils.h"

/*
 * converts perifocal coordinates y into geocentric equatorial Cartesian coords x
 * using the orbital elements: i, Omega, omega.
 */
void
perifocal_to_geocentric(CartesianCoordinates * x, const CartesianCoordinates * y,
			const double i, const double Omega, const double omega)
{
  double ci, si, cO, sO, co, so, tx, ty, tz;

  tx = y->x;
  ty = y->y;
  tz = y->z;

  ci = cos(i);
  si = sin(i);
  cO = cos(Omega);
  sO = sin(Omega);
  co = cos(omega);
  so = sin(omega);

  x->x =
    (cO * co - sO * so * ci) * tx + (-cO * so - sO * co * ci) * ty +
    sO * si * tz;
  x->y =
    (sO * co + cO * so * ci) * tx + (-sO * so + cO * co * ci) * ty -
    cO * si * tz;
  x->z = so * si * tx + co * si * ty + ci * tz;
}

/*
 * oe_time_J0_to_geocentric
 *
 * Converts orbital elements at time 0 to Cartesian coordinates
 * in the geocentric equatorial system at time t.
 *
 * This assumes the central body is a perfect sphere.
 * previously commented. Unused.
 */

void
oe_time_J0_to_geocentric(CartesianCoordinates * px,
			 const double t, const OrbitalElements * poe,
			 const CentralBody * pcb)
{
  CartesianCoordinates y;
  double nu, cosnu, r, a, e;

  a = poe->a;
  e = poe->e;

  nu = time_to_true_anomaly(t, pcb, a, e, poe->T);
  cosnu = cos(nu);
  r = a * (1 - e * e) / (1 + e * cosnu);
  y.x = r * cosnu;
  y.y = r * sin(nu);
  y.z = 0.0;

  perifocal_to_geocentric(px, &y, poe->i, poe->Omega, poe->omega);
}


/*
 * oe_to_geocentric
 *
 * Returns the position of a satellite from a set
 * of classical orbital elements
 *
 */
void
oe_to_geocentric(CartesianCoordinates * px, const OrbitalElements * poe,
		 const CentralBody * pcb)
{
  CartesianCoordinates y;
  double nu, cosnu, r, a, e;

  a = poe->a;
  e = poe->e;

  nu = time_to_true_anomaly(0.0, pcb, a, e, poe->T);
  cosnu = cos(nu);
  r = a * (1 - e * e) / (1 + e * cosnu);
  y.x = r * cosnu;
  y.y = r * sin(nu);
  y.z = 0.0;

  perifocal_to_geocentric(px, &y, poe->i, poe->Omega, poe->omega);
}

/*
 * oe_time_J0_to_oe
 *
 * Given orbital elements for the satellite at time 0,
 * returns orbital elements for the satellite at time t.
 *
 * This uses the Keplerian orbits formulation (spherical central body).
 *
 * (Since we use the orbital element T (time of periapsis passage)
 * flowing forward in time results in decreasing T.)
 */
void
oe_time_J0_to_oe(OrbitalElements * final, const OrbitalElements * initial,
		 const double t, const CentralBody * pcb)
{
  double d_T = -1.0;

  /* copy initial orbital elements to final orbital elements */
  *final = *initial;

  /* adjust the time until periapsis */
  final->T += t * d_T;
}

/*
 * oe_time_J2_to_oe
 *
 * Given orbital elements for the satellite at time 0,
 * returns orbital elements for the satellite at time t.
 *
 * This uses the J2 oblateness term in the model of the central body.
 *
 * See Szebehely, Adventures in Celestial Mechanics, Chap. 11
 * and U of Texas, Austin, Applied Orbital Mechanics course notes (Lundberg),
 * 3-22.
 *
 */
void
oe_time_J2_to_oe(OrbitalElements * final, const OrbitalElements * initial,
		 const double t, const CentralBody * pcb)
{
  double a = final->a = initial->a;
  double e = final->e = initial->e;
  double i = final->i = initial->i;

  double R_a_sqr = pcb->radius * pcb->radius / a / a;
  double one_minus_e_sqr = 1 - e * 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(i);
  double cosi2 = cosi * cosi;
  double d_Omega = -2.0 * factor * n * cosi;
  double d_omega = factor * n * (5.0 * cosi2 - 1.0);
  double d_T =
    -(1.0 + factor * sqrt(one_minus_e_sqr) * (3.0 * cosi2 - 1.0));

  /* adjust the longitude of ascending node */
  final->Omega = initial->Omega + t * d_Omega;
  /* adjust the argument of periapsis */
  final->omega = initial->omega + t * d_omega;
  /* adjust the time until periapsis */
  final->T = initial->T + t * d_T;
}

/*
 * oe_time_to_oe
 *
 * Given orbital elements for the satellite at time 0,
 * returns orbital elements for the satellite at time t.
 *
 * Uses the J2 model right now!
 */
void
oe_time_to_oe(OrbitalElements * final, const OrbitalElements * initial,
	      const double t, const CentralBody * pcb)
{
  /* oe_time_J0_to_oe(final, initial, t, pcb); */
  oe_time_J2_to_oe(final, initial, t, pcb);
}

/*
 * oe_time_to_geocentric
 *
 * Converts orbital elements at time 0 to spherical coordinates
 * in the geocentric equatorial system at time t.
 *
 * Uses the model that oe_time_to_oe uses.
 */
void
oe_time_to_geocentric(CartesianCoordinates * px, const double t,
		      const OrbitalElements * poe, const CentralBody * pcb)
{
  OrbitalElements oe;

  oe_time_to_oe(&oe, poe, t, pcb);
  oe_to_geocentric(px, &oe, pcb);
}

/*
 * oe_time_to_geocentric_spherical
 *
 * Converts orbital elements at time 0 to spherical coordinates
 * in the geocentric equatorial system at time t.
 *
 * Uses the whatever model oe_time_to_geocentric uses.
 */
void
oe_time_to_geocentric_spherical(SphericalCoordinates * px, const double t,
				const OrbitalElements * poe,
				const CentralBody * pcb)
{
  CartesianCoordinates y;

  oe_time_to_geocentric(&y, t, poe, pcb);
  cartesian_to_spherical(px, &y);
}

/*
 * spherical_to_transform
 *
 * creates a transformation matrix to move a satellite centered at the
 * origin to the specifed point in spherical coordinates.  The vector
 * (0,0,-1) is mapped so that it points to the origin.  There is still
 * one degree of freedom in this specification!
 *
 */
void
spherical_to_transform(double m[4][4], const SphericalCoordinates * px,
		       const double scale)
{
  double theta, phi, ct, st, cp, sp, spct, spst, r0;

  const double zero = 0.0;

  theta = px->theta;
  ct = cos(theta);
  st = sin(theta);

  phi = px->phi;
  cp = cos(phi);
  sp = sin(phi);

  m[0][0] = cp * ct;
  m[0][1] = cp * st;
  m[0][2] = -sp;
  m[0][3] = zero;

  m[1][0] = -st;
  m[1][1] = ct;
  m[1][2] = zero;
  m[1][3] = zero;

  spct = sp * ct;
  spst = sp * st;

  m[2][0] = spct;
  m[2][1] = spst;
  m[2][2] = cp;
  m[2][3] = zero;

  r0 = px->r / scale;

  m[3][0] = r0 * spct;
  m[3][1] = r0 * spst;
  m[3][2] = r0 * cp;
  m[3][3] = 1.0;
}

/*
 * spherical_to_cartesian
 *
 * convert from spherical (r, theta, phi) to cartesian (x,y,z) coordinates.
 */
void
spherical_to_cartesian(CartesianCoordinates * u, const SphericalCoordinates * v)
{
  double r, theta, phi, sinphi;

  r = v->r;
  theta = v->theta;
  phi = v->phi;
  sinphi = sin(phi);

  u->x = r * cos(theta) * sinphi;
  u->y = r * sin(theta) * sinphi;
  u->z = r * cos(phi);
}

/*
 * converts geocentric cartesian coordinates to spherical coordinates
 * x = (r,theta,phi), where theta in (-Pi, Pi], phi in [0, Pi].
 */
void
cartesian_to_spherical(SphericalCoordinates * px, const CartesianCoordinates * py)
{
  double rsqr, x, y, z, x_r, x2_y2;

  x = py->x;
  y = py->y;
  x2_y2 = x * x + y * y;

  if (y < 0.0) {
    px->theta = -acos(x / sqrt(x2_y2));
  } else {
    px->theta = acos(x / sqrt(x2_y2));
  }

  z = py->z;
  rsqr = x2_y2 + z * z;
  px->r = x_r = sqrt(rsqr);
  px->phi = acos(z / x_r);

}

/*
 * lat_lon_to_spherical