/*******************************************************************************
*$ Component_name:
*	Kep_Locate
*$ Abstract:
*	Returns the position and/or velocity of a planetary satellite at a
*	specified time, based on its orbital elements.
*$ Keywords:
*	KEPLER, ORBITAL_MOTION
*	C, PUBLIC
*$ Declarations:
*	void		Kep_Locate( orbit, time, location, velocity )
*	KEP_ORBIT	*orbit;		(typedef defined in "kepler.h")
*	double		time, location[3], velocity[3];
*$ Inputs:
*	*orbit		orbital element structure.
*	time		time (sec) at which to determine location.
*	location	pass a NULL pointer to suspend location calculation.
*	velocity	pass a NULL pointer to suspend velocity calculation.
*$ Outputs:
*	location[]	a vector consisting of the satellite's (x,y,z)
*			position coordinates at the selected time, in km.
*	velocity[]	a vector consisting of the satellite's (x,y,z)
*			velocity coordinates at the selected time, in km/sec.
*	For these coordinates, the origin is the center of the planet; the z
*	axis coincides with the planetary axis; the x axis points toward the
*	reference longitude within the equatorial plane.  Note that passing a
*	NULL pointer for either vector will prevent it from being calculated.
*
*$ Returns:
*	none
*$ Detailed_description:
*	This function returns the position and/or the velocity of a planetary
*	satellite at a specified time, based on its orbital elements as
*	previously specified by Kep_SetOrbit().  Note that the accuracy of this
*	result can be controlled using Kep_SetError().
*$ External_references:
*	Kep_Precess(), 	XKep_TrueAnom()
*$ Examples:
*	none
*$ Error_handling:
*	none
*$ Limitations:
*	It should be noted that the motion of a highly eccentric or inclined 
*	satellite about an oblate planet is an unsolved problem in orbital
*	kinematics.  This routine should be quite accurate when each of the
*	quantities (eccentricity, inclination, oblateness) is small, or when no
*	more than one of them is large.  If two or more are large, then results
*	could be somewhat unreliable.  The author welcomes suggestions about how
*	to make this routine more accurate. 
*
*	In its current form, the velocity returned by the function is an exact
*	equivalent to the time-derivative of the position.  However, tests
*	indicate that angular momentum is not conserved for highly eccentric
*	orbits about an oblate planet, even after allowing for nodal precession.
*$ Author_and_institution:
*	Mark R. Showalter
*	NASA/Ames Research Center
*$ Version_and_date:
*	1991 June 18
*$ Change_history:
*	none
*******************************************************************************/
#include <math.h>
#include "kepler.h"
#define NULL 0

void		Kep_Locate( orbit, time, location, velocity )
KEP_ORBIT	*orbit;
double		time, location[3], velocity[3];
{
double		peri, node, meanLon, meanAnom, theta, psi, delta;
double		cosTheta, sinTheta, cosDelta, sinDelta, cosNode, sinNode;
double		radius, r_over_a, v2, factor;
double		x_peri, y_peri, x_node, y_node, x_equator, y_equator, z_equator;
double		vx_peri, vy_peri, vx_node, vy_node, vx_equator, vy_equator,
		vz_equator;

/*******************************************************************************
* Precess the orbital elements
*******************************************************************************/

	Kep_Precess( orbit, time, &peri, &node, &meanLon );

/*******************************************************************************
* Calculate the true anomaly theta and the eccentric anomaly psi
*******************************************************************************/

	meanAnom = meanLon - peri;
	theta = XKep_TrueAnom( orbit->e, orbit->eccRatio, meanAnom, &psi );

/*******************************************************************************
* Solve for the location vector
*******************************************************************************/

/* Coordinates in the orbit plane, with the X-axis pointing to pericenter */
	r_over_a = 1. - orbit->e * cos(psi);
	radius = r_over_a * orbit->a;

	cosTheta = cos(theta);
	sinTheta = sin(theta);
	x_peri = radius * cosTheta;
	y_peri = radius * sinTheta;

/* Rotate about the orbital pole, so the X-axis points to the ascending node */
	delta = peri - node;
	cosDelta = cos(delta);
	sinDelta = sin(delta);
	x_node = x_peri * cosDelta - y_peri * sinDelta;
	y_node = x_peri * sinDelta + y_peri * cosDelta;

/* Rotate about the line of nodes into equatorial coordinates */
	x_equator = x_node;
	y_equator = y_node * orbit->cosI;
	z_equator = y_node * orbit->sinI;

/* Rotate about the planetary pole to the correct x-axis */
	cosNode = cos(node);
	sinNode = sin(node);

	if (location != NULL) {
		location[0] = x_equator * cosNode - y_equator * sinNode;
		location[1] = x_equator * sinNode + y_equator * cosNode;
		location[2] = z_equator;
	}

/*******************************************************************************
* If necessary, determine the velocity vector
*******************************************************************************/

if (velocity != NULL) {

/* Coordinates in the orbit plane, with the X-axis pointing to pericenter */
/* First, solve for velocity WRT pericenter */
	vx_peri = -sinTheta;
	vy_peri =  cosTheta + orbit->e;

	v2 = orbit->a2kappa2 * (2./r_over_a - 1.);
	factor = sqrt( v2 / (vx_peri*vx_peri + vy_peri*vy_peri) );
	vx_peri *= factor;
	vy_peri *= factor;

/* Add velocity of pericenter WRT node */
	vx_peri += -y_peri * orbit->dPNdT;
	vy_peri +=  x_peri * orbit->dPNdT;

/* Rotate about the orbital pole, so the X-axis points to the ascending node */
	vx_node = vx_peri * cosDelta - vy_peri * sinDelta;
	vy_node = vx_peri * sinDelta + vy_peri * cosDelta;

/* Rotate about the line of nodes into equatorial coordinates */
	vx_equator = vx_node;
	vy_equator = vy_node * orbit->cosI;
	vz_equator = vy_node * orbit->sinI;

/* Add velocity of ascending node */
	vx_equator += -y_equator * orbit->dNdT;
	vy_equator +=  x_equator * orbit->dNdT;

/* Rotate about the planetary pole to the correct x-axis */
	velocity[0] = vx_equator * cosNode - vy_equator * sinNode;
	velocity[1] = vx_equator * sinNode + vy_equator * cosNode;
	velocity[2] = vz_equator;
}

	return;
}

/******************************************************************************/