/* $Procedure ZZSTELAB ( Private --- stellar aberration correction ) */
/* Subroutine */ int zzstelab_(logical *xmit, doublereal *accobs, doublereal *
vobs, doublereal *starg, doublereal *scorr, doublereal *dscorr)
{
/* System generated locals */
integer i__1;
doublereal d__1, d__2;
/* Builtin functions */
double sqrt(doublereal);
integer s_rnge(char *, integer, char *, integer);
/* Local variables */
extern /* Subroutine */ int vadd_(doublereal *, doublereal *, doublereal *
);
doublereal dphi, rhat[3];
extern /* Subroutine */ int vhat_(doublereal *, doublereal *);
extern doublereal vdot_(doublereal *, doublereal *);
extern /* Subroutine */ int vequ_(doublereal *, doublereal *);
doublereal term1[3], term2[3], term3[3], c__, lcacc[3];
integer i__;
doublereal s;
extern /* Subroutine */ int chkin_(char *, ftnlen);
doublereal saoff[6] /* was [3][2] */, drhat[3];
extern /* Subroutine */ int dvhat_(doublereal *, doublereal *);
doublereal ptarg[3], evobs[3], srhat[6], vphat[3], vtarg[3];
extern /* Subroutine */ int vlcom_(doublereal *, doublereal *, doublereal
*, doublereal *, doublereal *), vperp_(doublereal *, doublereal *,
doublereal *);
extern doublereal vnorm_(doublereal *);
extern logical vzero_(doublereal *);
extern /* Subroutine */ int vlcom3_(doublereal *, doublereal *,
doublereal *, doublereal *, doublereal *, doublereal *,
doublereal *), cleard_(integer *, doublereal *);
doublereal vp[3];
extern doublereal clight_(void);
doublereal dptmag, ptgmag, eptarg[3], dvphat[3], lcvobs[3];
extern /* Subroutine */ int qderiv_(integer *, doublereal *, doublereal *,
doublereal *, doublereal *), sigerr_(char *, ftnlen), chkout_(
char *, ftnlen), setmsg_(char *, ftnlen);
doublereal svphat[6];
extern logical return_(void);
extern /* Subroutine */ int vminus_(doublereal *, doublereal *);
doublereal sgn, dvp[3], svp[6];
/* $ Abstract */
/* SPICE Private routine intended solely for the support of SPICE */
/* routines. Users should not call this routine directly due */
/* to the volatile nature of this routine. */
/* Return the state (position and velocity) of a target body */
/* relative to an observing body, optionally corrected for light */
/* time (planetary aberration) and stellar aberration. */
/* $ Disclaimer */
/* THIS SOFTWARE AND ANY RELATED MATERIALS WERE CREATED BY THE */
/* CALIFORNIA INSTITUTE OF TECHNOLOGY (CALTECH) UNDER A U.S. */
/* GOVERNMENT CONTRACT WITH THE NATIONAL AERONAUTICS AND SPACE */
/* ADMINISTRATION (NASA). THE SOFTWARE IS TECHNOLOGY AND SOFTWARE */
/* PUBLICLY AVAILABLE UNDER U.S. EXPORT LAWS AND IS PROVIDED "AS-IS" */
/* TO THE RECIPIENT WITHOUT WARRANTY OF ANY KIND, INCLUDING ANY */
/* WARRANTIES OF PERFORMANCE OR MERCHANTABILITY OR FITNESS FOR A */
/* PARTICULAR USE OR PURPOSE (AS SET FORTH IN UNITED STATES UCC */
/* SECTIONS 2312-2313) OR FOR ANY PURPOSE WHATSOEVER, FOR THE */
/* SOFTWARE AND RELATED MATERIALS, HOWEVER USED. */
/* IN NO EVENT SHALL CALTECH, ITS JET PROPULSION LABORATORY, OR NASA */
/* BE LIABLE FOR ANY DAMAGES AND/OR COSTS, INCLUDING, BUT NOT */
/* LIMITED TO, INCIDENTAL OR CONSEQUENTIAL DAMAGES OF ANY KIND, */
/* INCLUDING ECONOMIC DAMAGE OR INJURY TO PROPERTY AND LOST PROFITS, */
/* REGARDLESS OF WHETHER CALTECH, JPL, OR NASA BE ADVISED, HAVE */
/* REASON TO KNOW, OR, IN FACT, SHALL KNOW OF THE POSSIBILITY. */
/* RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF */
/* THE SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY */
/* CALTECH AND NASA FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE */
/* ACTIONS OF RECIPIENT IN THE USE OF THE SOFTWARE. */
/* $ Required_Reading */
/* SPK */
/* $ Keywords */
/* EPHEMERIS */
/* $ Declarations */
/* $ Brief_I/O */
/* Variable I/O Description */
/* -------- --- -------------------------------------------------- */
/* XMIT I Reception/transmission flag. */
/* ACCOBS I Observer acceleration relative to SSB. */
/* VOBS I Observer velocity relative to to SSB. */
/* STARG I State of target relative to observer. */
/* SCORR O Stellar aberration correction for position. */
/* DSCORR O Stellar aberration correction for velocity. */
/* $ Detailed_Input */
/* XMIT is a logical flag which is set to .TRUE. for the */
/* "transmission" case in which photons *depart* from */
/* the observer's location at an observation epoch ET */
/* and arrive at the target's location at the light-time */
/* corrected epoch ET+LT, where LT is the one-way light */
/* time between observer and target; XMIT is set to */
/* .FALSE. for "reception" case in which photons depart */
/* from the target's location at the light-time */
/* corrected epoch ET-LT and *arrive* at the observer's */
/* location at ET. */
/* Note that the observation epoch is not used in this */
/* routine. */
/* XMIT must be consistent with any light time */
/* corrections used for the input state STARG: if that */
/* state has been corrected for "reception" light time; */
/* XMIT must be .FALSE.; otherwise XMIT must be .TRUE. */
/* ACCOBS is the geometric acceleration of the observer */
/* relative to the solar system barycenter. Units are */
/* km/sec**2. ACCOBS must be expressed relative to */
/* an inertial reference frame. */
/* VOBS is the geometric velocity of the observer relative to */
/* the solar system barycenter. VOBS must be expressed */
/* relative to the same inertial reference frame as */
/* ACCOBS. Units are km/sec. */
/* STARG is the Cartesian state of the target relative to the */
/* observer. Normally STARG has been corrected for */
/* one-way light time, but this is not required. STARG */
/* must be expressed relative to the same inertial */
/* reference frame as ACCOBS. Components are */
/* (x, y, z, dx, dy, dz). Units are km and km/sec. */
/* $ Detailed_Output */
/* SCORR is the stellar aberration correction for the position */
/* component of STARG. Adding SCORR to this position */
/* vector produces the input observer-target position, */
/* corrected for stellar aberration. */
/* The reference frame of SCORR is the common frame */
/* relative to which the inputs ACCOBS, VOBS, and STARG */
/* are expressed. Units are km. */
/* DSCORR is the stellar aberration correction for the velocity */
/* component of STARG. Adding DSCORR to this velocity */
/* vector produces the input observer-target velocity, */
/* corrected for stellar aberration. */
/* The reference frame of DSCORR is the common frame */
/* relative to which the inputs ACCOBS, VOBS, and STARG */
/* are expressed. Units are km/s. */
/* $ Parameters */
/* None. */
/* $ Exceptions */
/* 1) If attempt to divide by zero occurs, the error */
/* SPICE(DIVIDEBYZERO) will be signaled. This case may occur */
/* due to uninitialized inputs. */
/* 2) Loss of precision will occur for geometric cases in which */
/* VOBS is nearly parallel to the position component of STARG. */
/* $ Files */
/* None. */
/* $ Particulars */
/* This routine computes a Newtonian estimate of the stellar */
/* aberration correction of an input state. Normally the input state */
/* has already been corrected for one-way light time. */
/* Since stellar aberration corrections are typically "small" */
/* relative to the magnitude of the input observer-target position */
/* and velocity, this routine avoids loss of precision by returning */
/* the corrections themselves rather than the corrected state */
/* vector. This allows the caller to manipulate (for example, */
/* interpolate) the corrections with greater accuracy. */
/* $ Examples */
/* See SPICELIB routine SPKACS. */
/* $ Restrictions */
/* None. */
/* $ Literature_References */
/* SPK Required Reading. */
/* $ Author_and_Institution */
/* N.J. Bachman (JPL) */
/* $ Version */
/* - SPICELIB Version 2.0.0, 15-APR-2014 (NJB) */
/* Added RETURN test and discovery check-in. */
/* Check for division by zero was added. This */
/* case might occur due to uninitialized inputs. */
/* - SPICELIB Version 1.0.1, 12-FEB-2009 (NJB) */
/* Minor updates were made to the inline documentation. */
/* - SPICELIB Version 1.0.0, 17-JAN-2008 (NJB) */
/* -& */
/* Note for the maintenance programmer */
/* =================================== */
/* The source code of the test utility T_ZZSTLABN must be */
/* kept in sync with the source code of this routine. That */
/* routine uses a value of SEPLIM that forces the numeric */
/* branch of the velocity computation to be taken in all */
/* cases. See the documentation of that routine for details. */
/* SPICELIB functions */
/* Local parameters */
/* Let PHI be the (non-negative) rotation angle of the stellar */
/* aberration correction; then SEPLIM is a limit on how close PHI */
/* may be to zero radians while stellar aberration velocity is */
/* computed analytically. When sin(PHI) is less than SEPLIM, the */
/* velocity must be computed numerically. */
/* Let TDELTA be the time interval, measured in seconds, */
/* used for numerical differentiation of the stellar */
/* aberration correction, when this is necessary. */
/* Local variables */
/* Use discovery check-in. */
if (return_()) {
return 0;
}
/* In the discussion below, the dot product of vectors X and Y */
/* is denoted by */
/* <X,Y> */
/* The speed of light is denoted by the lower case letter "c." BTW, */
/* variable names used here are case-sensitive: upper case "C" */
/* represents a different quantity which is unrelated to the speed */
/* of light. */
/* Variable names ending in "HAT" denote unit vectors. Variable */
/* names starting with "D" denote derivatives with respect to time. */
/* We'll compute the correction SCORR and its derivative with */
/* respect to time DSCORR for the reception case. In the */
/* transmission case, we perform the same computation with the */
/* negatives of the observer velocity and acceleration. */
/* In the code below, we'll store the position and velocity portions */
/* of the input observer-target state STARG in the variables PTARG */
/* and VTARG, respectively. */
/* Let VP be the component of VOBS orthogonal to PTARG. VP */
/* is defined as */
/* VOBS - < VOBS, RHAT > RHAT (1) */
/* where RHAT is the unit vector */
/* PTARG/||PTARG|| */
/* Then */
/* ||VP||/c (2) */
/* is the magnitude of */
/* s = sin( phi ) (3) */
/* where phi is the stellar aberration correction angle. We'll */
/* need the derivative with respect to time of (2). */
/* Differentiating (1) with respect to time yields the */
/* velocity DVP, where, letting */
/* DRHAT = d(RHAT) / dt */
/* VPHAT = VP / ||VP|| */
/* DVPMAG = d( ||VP|| ) / dt */
/* we have */
/* DVP = d(VP)/dt */
/* = ACCOBS - ( ( <VOBS,DRHAT> + <ACCOBS, RHAT> )*RHAT */
/* + <VOBS,RHAT> * DRHAT ) (4) */
/* and */
/* DVPMAG = < DVP, VPHAT > (5) */
/* Now we can find the derivative with respect to time of */
/* the stellar aberration angle phi: */
/* ds/dt = d(sin(phi))/dt = d(phi)/dt * cos(phi) (6) */
/* Using (2) and (5), we have for positive phi, */
/* ds/dt = (1/c)*DVPMAG = (1/c)*<DVP, VPHAT> (7) */
/* Then for positive phi */
/* d(phi)/dt = (1/cos(phi)) * (1/c) * <DVP, VPHAT> (8) */
/* Equation (8) is well-defined as along as VP is non-zero: */
/* if VP is the zero vector, VPHAT is undefined. We'll treat */
/* the singular and near-singular cases separately. */
/* The aberration correction itself is a rotation by angle phi */
/* from RHAT towards VP, so the corrected vector is */
/* ( sin(phi)*VPHAT + cos(phi)*RHAT ) * ||PTARG|| */
/* and we can express the offset of the corrected vector from */
/* PTARG, which is the output SCORR, as */
/* SCORR = */
/* ( sin(phi)*VPHAT + (cos(phi)-1)*RHAT ) * ||PTARG|| (9) */
/* Let DPTMAG be defined as */
/* DPTMAG = d ( ||PTARG|| ) / dt (10) */
/* Then the derivative with respect to time of SCORR is */
/* DSCORR = */
/* ( sin(phi)*DVPHAT */
/* + cos(phi)*d(phi)/dt * VPHAT */
/* + (cos(phi) - 1) * DRHAT */
/* + ( -sin(phi)*d(phi)/dt ) * RHAT ) * ||PTARG|| */
/* + ( sin(phi)*VPHAT + (cos(phi)-1)*RHAT ) * DPTMAG (11) */
/* Computations begin here: */
/* Split STARG into position and velocity components. Compute */
/* RHAT */
/* DRHAT */
/* VP */
/* DPTMAG */
if (*xmit) {
vminus_(vobs, lcvobs);
vminus_(accobs, lcacc);
} else {
vequ_(vobs, lcvobs);
vequ_(accobs, lcacc);
}
vequ_(starg, ptarg);
vequ_(&starg[3], vtarg);
dvhat_(starg, srhat);
vequ_(srhat, rhat);
vequ_(&srhat[3], drhat);
vperp_(lcvobs, rhat, vp);
dptmag = vdot_(vtarg, rhat);
/* Compute sin(phi) and cos(phi), which we'll call S and C */
/* respectively. Note that phi is always close to zero for */
/* realistic inputs (for which ||VOBS|| << CLIGHT), so the */
/* cosine term is positive. */
s = vnorm_(vp) / clight_();
/* Computing MAX */
d__1 = 0., d__2 = 1 - s * s;
c__ = sqrt((max(d__1,d__2)));
if (c__ == 0.) {
/* C will be used as a divisor later (in the computation */
/* of DPHI), so we'll put a stop to the problem here. */
chkin_("ZZSTELAB", (ftnlen)8);
setmsg_("Cosine of the aberration angle is 0; this cannot occur for "
"realistic observer velocities. This case can arise due to un"
"initialized inputs. This cosine value is used as a divisor i"
"n a later computation, so it must not be equal to zero.", (
ftnlen)234);
sigerr_("SPICE(DIVIDEBYZERO)", (ftnlen)19);
chkout_("ZZSTELAB", (ftnlen)8);
return 0;
}
/* Compute the unit vector VPHAT and the stellar */
/* aberration correction. We avoid relying on */
/* VHAT's exception handling for the zero vector. */
if (vzero_(vp)) {
cleard_(&c__3, vphat);
} else {
vhat_(vp, vphat);
}
/* Now we can use equation (9) to obtain the stellar */
/* aberration correction SCORR: */
/* SCORR = */
/* ( sin(phi)*VPHAT + (cos(phi)-1)*RHAT ) * ||PTARG|| */
ptgmag = vnorm_(ptarg);
d__1 = ptgmag * s;
d__2 = ptgmag * (c__ - 1.);
vlcom_(&d__1, vphat, &d__2, rhat, scorr);
/* Now we use S as an estimate of PHI to decide if we're */
/* going to differentiate the stellar aberration correction */
/* analytically or numerically. */
/* Note that S is non-negative by construction, so we don't */
/* need to use the absolute value of S here. */
if (s >= 1e-6) {
/* This is the analytic case. */
/* Compute DVP---the derivative of VP with respect to time. */
/* Recall equation (4): */
/* DVP = d(VP)/dt */
/* = ACCOBS - ( ( <VOBS,DRHAT> + <ACCOBS, RHAT> )*RHAT */
/* + <VOBS,RHAT> * DRHAT ) */
d__1 = -vdot_(lcvobs, drhat) - vdot_(lcacc, rhat);
d__2 = -vdot_(lcvobs, rhat);
vlcom3_(&c_b7, lcacc, &d__1, rhat, &d__2, drhat, dvp);
vhat_(vp, vphat);
/* Now we can compute DVPHAT, the derivative of VPHAT: */
vequ_(vp, svp);
vequ_(dvp, &svp[3]);
dvhat_(svp, svphat);
vequ_(&svphat[3], dvphat);
/* Compute the DPHI, the time derivative of PHI, using equation 8: */
/* d(phi)/dt = (1/cos(phi)) * (1/c) * <DVP, VPHAT> */
dphi = 1. / (c__ * clight_()) * vdot_(dvp, vphat);
/* At long last we've assembled all of the "ingredients" required */
/* to compute DSCORR: */
/* DSCORR = */
/* ( sin(phi)*DVPHAT */
/* + cos(phi)*d(phi)/dt * VPHAT */
/* + (cos(phi) - 1) * DRHAT */
/* + ( -sin(phi)*d(phi)/dt ) * RHAT ) * ||PTARG|| */
/* + ( sin(phi)*VPHAT + (cos(phi)-1)*RHAT ) * DPTMAG */
d__1 = c__ * dphi;
vlcom_(&s, dvphat, &d__1, vphat, term1);
d__1 = c__ - 1.;
d__2 = -s * dphi;
vlcom_(&d__1, drhat, &d__2, rhat, term2);
vadd_(term1, term2, term3);
d__1 = dptmag * s;
d__2 = dptmag * (c__ - 1.);
vlcom3_(&ptgmag, term3, &d__1, vphat, &d__2, rhat, dscorr);
} else {
/* This is the numeric case. We're going to differentiate */
/* the stellar aberration correction offset vector using */
/* a quadratic estimate. */
for (i__ = 1; i__ <= 2; ++i__) {
/* Set the sign of the time offset. */
if (i__ == 1) {
sgn = -1.;
} else {
sgn = 1.;
}
/* Estimate the observer's velocity relative to the */
/* solar system barycenter at the current epoch. We use */
/* the local copies of the input velocity and acceleration */
/* to make a linear estimate. */
d__1 = sgn * 1.;
vlcom_(&c_b7, lcvobs, &d__1, lcacc, evobs);
/* Estimate the observer-target vector. We use the */
/* observer-target state velocity to make a linear estimate. */
d__1 = sgn * 1.;
vlcom_(&c_b7, starg, &d__1, &starg[3], eptarg);
/* Let RHAT be the unit observer-target position. */
/* Compute the component of the observer's velocity */
/* that is perpendicular to the target position; call */
/* this vector VP. Also compute the unit vector in */
/* the direction of VP. */
vhat_(eptarg, rhat);
vperp_(evobs, rhat, vp);
if (vzero_(vp)) {
cleard_(&c__3, vphat);
} else {
vhat_(vp, vphat);
}
/* Compute the sine and cosine of the correction */
/* angle. */
s = vnorm_(vp) / clight_();
/* Computing MAX */
d__1 = 0., d__2 = 1 - s * s;
c__ = sqrt((max(d__1,d__2)));
/* Compute the vector offset of the correction. */
ptgmag = vnorm_(eptarg);
d__1 = ptgmag * s;
d__2 = ptgmag * (c__ - 1.);
vlcom_(&d__1, vphat, &d__2, rhat, &saoff[(i__1 = i__ * 3 - 3) < 6
&& 0 <= i__1 ? i__1 : s_rnge("saoff", i__1, "zzstelab_", (
ftnlen)597)]);
}
/* Now compute the derivative. */
qderiv_(&c__3, saoff, &saoff[3], &c_b7, dscorr);
}
/* At this point the correction offset SCORR and its derivative */
/* with respect to time DSCORR are both set. */
return 0;
} /* zzstelab_ */
/* $Procedure QXQ (Quaternion times quaternion) */
/* Subroutine */ int qxq_(doublereal *q1, doublereal *q2, doublereal *qout)
{
extern doublereal vdot_(doublereal *, doublereal *);
doublereal cross[3];
extern /* Subroutine */ int vcrss_(doublereal *, doublereal *, doublereal
*), vlcom3_(doublereal *, doublereal *, doublereal *, doublereal *
, doublereal *, doublereal *, doublereal *);
/* $ Abstract */
/* Multiply two quaternions. */
/* $ Disclaimer */
/* THIS SOFTWARE AND ANY RELATED MATERIALS WERE CREATED BY THE */
/* CALIFORNIA INSTITUTE OF TECHNOLOGY (CALTECH) UNDER A U.S. */
/* GOVERNMENT CONTRACT WITH THE NATIONAL AERONAUTICS AND SPACE */
/* ADMINISTRATION (NASA). THE SOFTWARE IS TECHNOLOGY AND SOFTWARE */
/* PUBLICLY AVAILABLE UNDER U.S. EXPORT LAWS AND IS PROVIDED "AS-IS" */
/* TO THE RECIPIENT WITHOUT WARRANTY OF ANY KIND, INCLUDING ANY */
/* WARRANTIES OF PERFORMANCE OR MERCHANTABILITY OR FITNESS FOR A */
/* PARTICULAR USE OR PURPOSE (AS SET FORTH IN UNITED STATES UCC */
/* SECTIONS 2312-2313) OR FOR ANY PURPOSE WHATSOEVER, FOR THE */
/* SOFTWARE AND RELATED MATERIALS, HOWEVER USED. */
/* IN NO EVENT SHALL CALTECH, ITS JET PROPULSION LABORATORY, OR NASA */
/* BE LIABLE FOR ANY DAMAGES AND/OR COSTS, INCLUDING, BUT NOT */
/* LIMITED TO, INCIDENTAL OR CONSEQUENTIAL DAMAGES OF ANY KIND, */
/* INCLUDING ECONOMIC DAMAGE OR INJURY TO PROPERTY AND LOST PROFITS, */
/* REGARDLESS OF WHETHER CALTECH, JPL, OR NASA BE ADVISED, HAVE */
/* REASON TO KNOW, OR, IN FACT, SHALL KNOW OF THE POSSIBILITY. */
/* RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF */
/* THE SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY */
/* CALTECH AND NASA FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE */
/* ACTIONS OF RECIPIENT IN THE USE OF THE SOFTWARE. */
/* $ Required_Reading */
/* ROTATION */
/* $ Keywords */
/* MATH */
/* POINTING */
/* ROTATION */
/* $ Declarations */
/* $ Brief_I/O */
/* VARIABLE I/O DESCRIPTION */
/* -------- --- -------------------------------------------------- */
/* Q1 I First SPICE quaternion factor. */
/* Q2 I Second SPICE quaternion factor. */
/* QOUT O Product of Q1 and Q2. */
/* $ Detailed_Input */
/* Q1 is a 4-vector representing a SPICE-style */
/* quaternion. See the discussion of quaternion */
/* styles in Particulars below. */
/* Note that multiple styles of quaternions */
/* are in use. This routine will not work properly */
/* if the input quaternions do not conform to */
/* the SPICE convention. See the Particulars */
/* section for details. */
/* Q2 is a second SPICE-style quaternion. */
/* $ Detailed_Output */
/* QOUT is 4-vector representing the quaternion product */
/* Q1 * Q2 */
/* Representing Q(i) as the sums of scalar (real) */
/* part s(i) and vector (imaginary) part v(i) */
/* respectively, */
/* Q1 = s1 + v1 */
/* Q2 = s2 + v2 */
/* QOUT has scalar part s3 defined by */
/* s3 = s1 * s2 - <v1, v2> */
/* and vector part v3 defined by */
/* v3 = s1 * v2 + s2 * v1 + v1 x v2 */
/* where the notation < , > denotes the inner */
/* product operator and x indicates the cross */
/* product operator. */
/* $ Parameters */
/* None. */
/* $ Exceptions */
/* Error free. */
/* $ Files */
/* None. */
/* $ Particulars */
/* Quaternion Styles */
/* ----------------- */
/* There are different "styles" of quaternions used in */
/* science and engineering applications. Quaternion styles */
/* are characterized by */
/* - The order of quaternion elements */
/* - The quaternion multiplication formula */
/* - The convention for associating quaternions */
/* with rotation matrices */
/* Two of the commonly used styles are */
/* - "SPICE" */
/* > Invented by Sir William Rowan Hamilton */
/* > Frequently used in mathematics and physics textbooks */
/* - "Engineering" */
/* > Widely used in aerospace engineering applications */
/* SPICELIB subroutine interfaces ALWAYS use SPICE quaternions. */
/* Quaternions of any other style must be converted to SPICE */
/* quaternions before they are passed to SPICELIB routines. */
/* Relationship between SPICE and Engineering Quaternions */
/* ------------------------------------------------------ */
/* Let M be a rotation matrix such that for any vector V, */
/* M*V */
/* is the result of rotating V by theta radians in the */
/* counterclockwise direction about unit rotation axis vector A. */
/* Then the SPICE quaternions representing M are */
/* (+/-) ( cos(theta/2), */
/* sin(theta/2) A(1), */
/* sin(theta/2) A(2), */
/* sin(theta/2) A(3) ) */
/* while the engineering quaternions representing M are */
/* (+/-) ( -sin(theta/2) A(1), */
/* -sin(theta/2) A(2), */
/* -sin(theta/2) A(3), */
/* cos(theta/2) ) */
/* For both styles of quaternions, if a quaternion q represents */
/* a rotation matrix M, then -q represents M as well. */
/* Given an engineering quaternion */
/* QENG = ( q0, q1, q2, q3 ) */
/* the equivalent SPICE quaternion is */
/* QSPICE = ( q3, -q0, -q1, -q2 ) */
/* Associating SPICE Quaternions with Rotation Matrices */
/* ---------------------------------------------------- */
/* Let FROM and TO be two right-handed reference frames, for */
/* example, an inertial frame and a spacecraft-fixed frame. Let the */
/* symbols */
/* V , V */
/* FROM TO */
/* denote, respectively, an arbitrary vector expressed relative to */
/* the FROM and TO frames. Let M denote the transformation matrix */
/* that transforms vectors from frame FROM to frame TO; then */
/* V = M * V */
/* TO FROM */
/* where the expression on the right hand side represents left */
/* multiplication of the vector by the matrix. */
/* Then if the unit-length SPICE quaternion q represents M, where */
/* q = (q0, q1, q2, q3) */
/* the elements of M are derived from the elements of q as follows: */
/* +- -+ */
/* | 2 2 | */
/* | 1 - 2*( q2 + q3 ) 2*(q1*q2 - q0*q3) 2*(q1*q3 + q0*q2) | */
/* | | */
/* | | */
/* | 2 2 | */
/* M = | 2*(q1*q2 + q0*q3) 1 - 2*( q1 + q3 ) 2*(q2*q3 - q0*q1) | */
/* | | */
/* | | */
/* | 2 2 | */
/* | 2*(q1*q3 - q0*q2) 2*(q2*q3 + q0*q1) 1 - 2*( q1 + q2 ) | */
/* | | */
/* +- -+ */
/* Note that substituting the elements of -q for those of q in the */
/* right hand side leaves each element of M unchanged; this shows */
/* that if a quaternion q represents a matrix M, then so does the */
/* quaternion -q. */
/* To map the rotation matrix M to a unit quaternion, we start by */
/* decomposing the rotation matrix as a sum of symmetric */
/* and skew-symmetric parts: */
/* 2 */
/* M = [ I + (1-cos(theta)) OMEGA ] + [ sin(theta) OMEGA ] */
/* symmetric skew-symmetric */
/* OMEGA is a skew-symmetric matrix of the form */
/* +- -+ */
/* | 0 -n3 n2 | */
/* | | */
/* OMEGA = | n3 0 -n1 | */
/* | | */
/* | -n2 n1 0 | */
/* +- -+ */
/* The vector N of matrix entries (n1, n2, n3) is the rotation axis */
/* of M and theta is M's rotation angle. Note that N and theta */
/* are not unique. */
/* Let */
/* C = cos(theta/2) */
/* S = sin(theta/2) */
/* Then the unit quaternions Q corresponding to M are */
/* Q = +/- ( C, S*n1, S*n2, S*n3 ) */
/* The mappings between quaternions and the corresponding rotations */
/* are carried out by the SPICELIB routines */
/* Q2M {quaternion to matrix} */
/* M2Q {matrix to quaternion} */
/* M2Q always returns a quaternion with scalar part greater than */
/* or equal to zero. */
/* SPICE Quaternion Multiplication Formula */
/* --------------------------------------- */
/* Given a SPICE quaternion */
/* Q = ( q0, q1, q2, q3 ) */
/* corresponding to rotation axis A and angle theta as above, we can */
/* represent Q using "scalar + vector" notation as follows: */
/* s = q0 = cos(theta/2) */
/* v = ( q1, q2, q3 ) = sin(theta/2) * A */
/* Q = s + v */
/* Let Q1 and Q2 be SPICE quaternions with respective scalar */
/* and vector parts s1, s2 and v1, v2: */
/* Q1 = s1 + v1 */
/* Q2 = s2 + v2 */
/* We represent the dot product of v1 and v2 by */
/* <v1, v2> */
/* and the cross product of v1 and v2 by */
/* v1 x v2 */
/* Then the SPICE quaternion product is */
/* Q1*Q2 = s1*s2 - <v1,v2> + s1*v2 + s2*v1 + (v1 x v2) */
/* If Q1 and Q2 represent the rotation matrices M1 and M2 */
/* respectively, then the quaternion product */
/* Q1*Q2 */
/* represents the matrix product */
/* M1*M2 */
/* $ Examples */
/* 1) Let QID, QI, QJ, QK be the "basis" quaternions */
/* QID = ( 1, 0, 0, 0 ) */
/* QI = ( 0, 1, 0, 0 ) */
/* QJ = ( 0, 0, 1, 0 ) */
/* QK = ( 0, 0, 0, 1 ) */
/* respectively. Then the calls */
/* CALL QXQ ( QI, QJ, IXJ ) */
/* CALL QXQ ( QJ, QK, JXK ) */
/* CALL QXQ ( QK, QI, KXI ) */
/* produce the results */
/* IXJ = QK */
/* JXK = QI */
/* KXI = QJ */
/* All of the calls */
/* CALL QXQ ( QI, QI, QOUT ) */
/* CALL QXQ ( QJ, QJ, QOUT ) */
/* CALL QXQ ( QK, QK, QOUT ) */
/* produce the result */
/* QOUT = -QID */
/* For any quaternion Q, the calls */
/* CALL QXQ ( QID, Q, QOUT ) */
/* CALL QXQ ( Q, QID, QOUT ) */
/* produce the result */
/* QOUT = Q */
/* 2) Composition of rotations: let CMAT1 and CMAT2 be two */
/* C-matrices (which are rotation matrices). Then the */
/* following code fragment computes the product CMAT1 * CMAT2: */
/* C */
/* C Convert the C-matrices to quaternions. */
/* C */
/* CALL M2Q ( CMAT1, Q1 ) */
/* CALL M2Q ( CMAT2, Q2 ) */
/* C */
/* C Find the product. */
/* C */
/* CALL QXQ ( Q1, Q2, QOUT ) */
/* C */
/* C Convert the result to a C-matrix. */
/* C */
/* CALL Q2M ( QOUT, CMAT3 ) */
/* C */
/* C Multiply CMAT1 and CMAT2 directly. */
/* C */
/* CALL MXM ( CMAT1, CMAT2, CMAT4 ) */
/* C */
/* C Compare the results. The difference DIFF of */
/* C CMAT3 and CMAT4 should be close to the zero */
/* C matrix. */
/* C */
/* CALL VSUBG ( 9, CMAT3, CMAT4, DIFF ) */
/* $ Restrictions */
/* None. */
/* $ Literature_References */
/* None. */
/* $ Author_and_Institution */
/* N.J. Bachman (JPL) */
/* $ Version */
/* - SPICELIB Version 1.0.1, 26-FEB-2008 (NJB) */
/* Updated header; added information about SPICE */
/* quaternion conventions. */
/* - SPICELIB Version 1.0.0, 18-AUG-2002 (NJB) */
/* -& */
/* $ Index_Entries */
/* quaternion times quaternion */
/* multiply quaternion by quaternion */
/* -& */
/* SPICELIB functions */
/* Local variables */
/* Compute the scalar part of the product. */
qout[0] = q1[0] * q2[0] - vdot_(&q1[1], &q2[1]);
/* And now the vector part. The SPICELIB routine VLCOM3 computes */
/* a linear combination of three 3-vectors. */
vcrss_(&q1[1], &q2[1], cross);
vlcom3_(q1, &q2[1], q2, &q1[1], &c_b2, cross, &qout[1]);
return 0;
} /* qxq_ */
/* $Procedure EQNCPV (Equinoctial Elements to position and velocity) */
/* Subroutine */ int eqncpv_(doublereal *et, doublereal *epoch, doublereal *
eqel, doublereal *rapol, doublereal *decpol, doublereal *state)
{
/* Initialized data */
static logical first = TRUE_;
/* System generated locals */
doublereal d__1;
/* Builtin functions */
double sqrt(doublereal), sin(doublereal), cos(doublereal), d_mod(
doublereal *, doublereal *);
/* Local variables */
doublereal nfac, node, mldt, temp[3], a, b, h__, k, l, eecan, p, q, r__;
extern /* Subroutine */ int chkin_(char *, ftnlen);
doublereal dlpdt, prate;
extern /* Subroutine */ int errdp_(char *, doublereal *, ftnlen);
doublereal xhold[6];
extern /* Subroutine */ int vlcom_(doublereal *, doublereal *, doublereal
*, doublereal *, doublereal *);
doublereal trans[9] /* was [3][3] */;
extern doublereal twopi_(void);
doublereal x1, y1;
extern /* Subroutine */ int vlcom3_(doublereal *, doublereal *,
doublereal *, doublereal *, doublereal *, doublereal *,
doublereal *);
doublereal ca, cd, cf, di, cn, ra, sa, rb, sd, dt, sf, ml, dx, dy, vf[3],
vg[3], sn, nodedt;
extern doublereal kepleq_(doublereal *, doublereal *, doublereal *);
extern /* Subroutine */ int sigerr_(char *, ftnlen), chkout_(char *,
ftnlen), setmsg_(char *, ftnlen);
static doublereal pi2;
doublereal dx1, dy1;
extern logical return_(void);
doublereal ecc, can, dlp, san;
extern /* Subroutine */ int mxv_(doublereal *, doublereal *, doublereal *)
;
/* $ Abstract */
/* Compute the state (position and velocity of an object whose */
/* trajectory is described via equinoctial elements relative to some */
/* fixed plane (usually the equatorial plane of some planet). */
/* $ Disclaimer */
/* THIS SOFTWARE AND ANY RELATED MATERIALS WERE CREATED BY THE */
/* CALIFORNIA INSTITUTE OF TECHNOLOGY (CALTECH) UNDER A U.S. */
/* GOVERNMENT CONTRACT WITH THE NATIONAL AERONAUTICS AND SPACE */
/* ADMINISTRATION (NASA). THE SOFTWARE IS TECHNOLOGY AND SOFTWARE */
/* PUBLICLY AVAILABLE UNDER U.S. EXPORT LAWS AND IS PROVIDED "AS-IS" */
/* TO THE RECIPIENT WITHOUT WARRANTY OF ANY KIND, INCLUDING ANY */
/* WARRANTIES OF PERFORMANCE OR MERCHANTABILITY OR FITNESS FOR A */
/* PARTICULAR USE OR PURPOSE (AS SET FORTH IN UNITED STATES UCC */
/* SECTIONS 2312-2313) OR FOR ANY PURPOSE WHATSOEVER, FOR THE */
/* SOFTWARE AND RELATED MATERIALS, HOWEVER USED. */
/* IN NO EVENT SHALL CALTECH, ITS JET PROPULSION LABORATORY, OR NASA */
/* BE LIABLE FOR ANY DAMAGES AND/OR COSTS, INCLUDING, BUT NOT */
/* LIMITED TO, INCIDENTAL OR CONSEQUENTIAL DAMAGES OF ANY KIND, */
/* INCLUDING ECONOMIC DAMAGE OR INJURY TO PROPERTY AND LOST PROFITS, */
/* REGARDLESS OF WHETHER CALTECH, JPL, OR NASA BE ADVISED, HAVE */
/* REASON TO KNOW, OR, IN FACT, SHALL KNOW OF THE POSSIBILITY. */
/* RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF */
/* THE SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY */
/* CALTECH AND NASA FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE */
/* ACTIONS OF RECIPIENT IN THE USE OF THE SOFTWARE. */
/* $ Required_Reading */
/* None. */
/* $ Keywords */
/* EPHEMERIS */
/* $ Declarations */
/* $ Brief_I/O */
/* VARIABLE I/O DESCRIPTION */
/* -------- --- -------------------------------------------------- */
/* ET I Epoch in seconds past J2000 to find state */
/* EPOCH I Epoch of elements in seconds past J2000 */
/* EQEL I Array of equinoctial elements */
/* RAPOL I Right Ascension of the pole of the reference plane */
/* DECPOL I Declination of the pole of the reference plane */
/* STATE O State of the object described by EQEL. */
/* $ Detailed_Input */
/* ET is the epoch (ephemeris time) at which the state */
/* of the target body is to be computed. ET is measured */
/* in seconds past the J2000 epoch. */
/* EPOCH is the epoch of the equinoctial elements in seconds */
/* past the J2000 epoch. */
/* EQEL is an array of 9 double precision numbers that */
/* are the equinoctial elements for some orbit expressed */
/* relative to the equatorial frame of the central body. */
/* (The z-axis of the equatorial frame is the direction */
/* of the pole of the central body relative to some */
/* inertial frame. The x-axis is given by the cross */
/* product of the Z-axis of the inertial frame */
/* with the direction of the pole of the central body. */
/* The Y-axis completes a right handed frame. */
/* (If the z-axis of the equatorial frame is aligned */
/* with the z-axis of the inertial frame, then the */
/* x-axis of the equatorial frame will be located at */
/* 90 degrees + RAPOL in the inertial frame.) */
/* The specific arrangement of the elements is spelled */
/* out below. The following terms are used in the */
/* discussion of elements of EQEL */
/* INC --- inclination of the orbit */
/* ARGP --- argument of periapse */
/* NODE --- longitude of the ascending node */
/* E --- eccentricity of the orbit */
/* EQEL(1) is the semi-major axis (A) of the orbit in km. */
/* EQEL(2) is the value of H at the specified epoch. */
/* ( E*SIN(ARGP+NODE) ). */
/* EQEL(3) is the value of K at the specified epoch */
/* ( E*COS(ARGP+NODE) ). */
/* EQEL(4) is the mean longitude (MEAN0+ARGP+NODE)at */
/* the epoch of the elements measured in radians. */
/* EQEL(5) is the value of P (TAN(INC/2)*SIN(NODE))at */
/* the specified epoch. */
/* EQEL(6) is the value of Q (TAN(INC/2)*COS(NODE))at */
/* the specified epoch. */
/* EQEL(7) is the rate of the longitude of periapse */
/* (dARGP/dt + dNODE/dt ) at the epoch of */
/* the elements. This rate is assumed to hold */
/* for all time. The rate is measured in */
/* radians per second. */
/* EQEL(8) is the derivative of the mean longitude */
/* ( dM/dt + dARGP/dt + dNODE/dt ). This */
/* rate is assumed to be constant and is */
/* measured in radians/second. */
/* EQEL(9) is the rate of the longitude of the ascending */
/* node ( dNODE/dt). This rate is measured */
/* in radians per second. */
/* RAPOL Right Ascension of the pole of the reference plane */
/* with respect to some inertial frame (measured in */
/* radians). */
/* DECPOL Declination of the pole of the reference plane */
/* with respect to some inertial frame (measured in */
/* radians). */
/* $ Detailed_Output */
/* STATE State of the object described by EQEL relative to the */
/* inertial frame used to define RAPOL and DECPOL. Units */
/* are in km and km/sec. */
/* $ Parameters */
/* None. */
/* $ Exceptions */
/* 1) If the eccentricity corresponding to the input elements is */
/* greater than 0.9, the error SPICE(ECCOUTOFRANGE) is signalled. */
/* 2) If the semi-major axis of the elements is non-positive, the */
/* error SPICE(BADSEMIAXIS) is signalled. */
/* $ Files */
/* None. */
/* $ Particulars */
/* This routine evaluates the input equinoctial elements for */
/* the specified epoch and return the corresponding state. */
/* This routine was adapted from a routine provided by */
/* Bob Jacobson of the Planetary Dynamics Group of */
/* the Navigation and Flight Mechanics Section at JPL. */
/* $ Examples */
/* Suppose you have classical elements and rates of */
/* change of the ascending node and argument of periapse */
/* for some satellite of the earth. */
/* By transforming the classical elements */
/* this routine can be used to compute the state of the */
/* object at an arbitrary epoch. The code below illustrates */
/* how you might do this. */
/* The table below illustrates the meanings of the various */
/* variables used in the discussion below. */
/* Variable Meaning */
/* -------- ---------------------------------- */
/* A Semi-major axis in km */
/* ECC Eccentricity of orbit */
/* INC Inclination of orbit */
/* NODE Longitude of the ascending node at epoch */
/* OMEGA Argument of periapse at epoch */
/* M Mean anomaly at epoch */
/* DMDT Mean anomaly rate in radians/second */
/* DNODE Rate of change of longitude of ascending node */
/* in radians/second */
/* DARGP Rate of change of argument of periapse in */
/* radians/second */
/* EPOCH is the epoch of the elements in seconds past */
/* the J2000 epoch. */
/* EQEL(1) = A */
/* EQEL(2) = ECC * DSIN ( OMEGA + NODE ) */
/* EQEL(3) = ECC * DCOS ( OMEGA + NODE ) */
/* EQEL(4) = M + OMEGA + NODE */
/* EQEL(5) = TAN(INC/2.0D0) * DSIN(NODE) */
/* EQEL(6) = TAN(INC/2.0D0) * DCOS(NODE) */
/* EQEL(7) = DARGP */
/* EQEL(8) = DARGP + DMDT + DNODE */
/* EQEL(9) = DNODE */
/* We shall compute the state of the satellite in the */
/* pole and equator reference system. */
/* RAPOL = -HALFPI() */
/* DECPOL = HALFPI() */
/* Now compute the state at the desired epoch ET. */
/* CALL EQNCPV ( ET, EPOCH, EQEL, RAPOL, DECPOL, STATE ) */
/* $ Restrictions */
/* The equinoctial elements used by this routine are taken */
/* from "Tangent" formulation of equinoctial elements */
/* p = Tan(inclination/2) * Sin(R.A. of ascending node) */
/* q = Tan(inclination/2) * Cos(R.A. of ascending node) */
/* Other formulations use Sine instead of Tangent. We shall */
/* call these the "Sine" formulations. */
/* p = Sin(inclination/2) * Sin(R.A. of ascending node) */
/* q = Sin(inclination/2) * Cos(R.A. of ascending node) */
/* If you have equinoctial elements from this alternative */
/* formulation you should replace p and q by the */
/* expressions below. */
/* P = P / DSQRT ( 1.0D0 - P*P - Q*Q ) */
/* Q = Q / DSQRT ( 1.0D0 - P*P - Q*Q ) */
/* This will convert the Sine formulation to the Tangent formulation. */
/* $ Literature_References */
/* JPL Engineering Memorandum 314-513 "Optical Navigation Program */
/* Mathematical Models" by William M. Owen, Jr. and Robin M Vaughan */
/* August 9, 1991 */
/* $ Author_and_Institution */
/* W.L. Taber (JPL) */
/* R.A. Jacobson (JPL) */
/* B.V. Semenov (JPL) */
/* $ Version */
/* - SPICELIB Version 1.0.2, 18-MAY-2010 (BVS) */
/* Removed "C$" marker from text in the header. */
/* - SPICELIB Version 1.0.1, 31-JAN-2008 (BVS) */
/* Removed non-standard header section heading */
/* 'Declarations_of_external_functions'. */
/* - SPICELIB Version 1.0.0, 8-JAN-1997 (WLT) */
/* -& */
/* $ Index_Entries */
/* Compute a state from equinoctial elements */
/* -& */
/* SPICELIB Functions. */
/* LOCAL VARIABLES */
/* Constants computed on first pass */
/* Standard SPICE exception handling code. */
if (return_()) {
return 0;
}
chkin_("EQNCPV", (ftnlen)6);
/* The first time through this routine we fetch the various */
/* constants we need for this routine. */
if (first) {
first = FALSE_;
pi2 = twopi_();
}
/* Take care of the various errors that can arise with the */
/* input elements. */
if (eqel[0] <= 0.) {
setmsg_("The semi-major axis supplied to EQNCPV was non-positive. Th"
"e value is required to be positive by this routine. The valu"
"e supplied was #. ", (ftnlen)137);
errdp_("#", eqel, (ftnlen)1);
sigerr_("SPICE(BADSEMIAXIS)", (ftnlen)18);
chkout_("EQNCPV", (ftnlen)6);
return 0;
}
ecc = sqrt(eqel[1] * eqel[1] + eqel[2] * eqel[2]);
if (ecc > .9) {
setmsg_("The routine EQNCPV can reliably evaluate states from equino"
"ctial elements if the eccentricity of the orbit associated w"
"ith the elements is less than 0.9. The eccentricity associa"
"ted with the elements supplies is #. The values of H and K "
"are: # and # respectively. ", (ftnlen)266);
errdp_("#", &ecc, (ftnlen)1);
errdp_("#", &eqel[1], (ftnlen)1);
errdp_("#", &eqel[2], (ftnlen)1);
sigerr_("SPICE(ECCOUTOFRANGE)", (ftnlen)20);
chkout_("EQNCPV", (ftnlen)6);
return 0;
}
/* Form the transformation from planetary equator to the inertial */
/* reference frame. */
sa = sin(*rapol);
ca = cos(*rapol);
sd = sin(*decpol);
cd = cos(*decpol);
trans[0] = -sa;
trans[3] = -ca * sd;
trans[6] = ca * cd;
trans[1] = ca;
trans[4] = -sa * sd;
trans[7] = sa * cd;
trans[2] = 0.;
trans[5] = cd;
trans[8] = sd;
/* Compute the offset of the input epoch (ET) from the */
/* epoch of the elements. */
dt = *et - *epoch;
/* Obtain the elements, rates, and other parameters. First get */
/* the semi-major axis. */
a = eqel[0];
/* Recall that H and K at the epoch of the elements are in */
/* EQEL(2) and EQEL(3) respectively. */
/* H_0 = E*Sin(ARGP_0 + NODE_0 ) */
/* K_0 = E*Cos(ARGP_0 + NODE_0 ) */
/* The values of H and K at the epoch of interest is */
/* H_dt = E*Sin(ARGP_0 + NODE_0 + dt*d(ARGP+NODE)/dt ) */
/* K_dt = E*Cos(ARGP_0 + NODE_0 + dt*d(ARGP+NODE)/dt ) */
/* But using the identities Sin(A+B) = Sin(A)Cos(B) + Sin(B)Cos(A) */
/* Cos(A+B) = Cos(A)Cos(B) - Sin(A)Sin(B) */
/* We can re-write the expression for H_dt and K_dt as */
/* H_dt = E*Sin(ARGP_0 + NODE_0 )Cos(dt*d(ARGP+NODE)/dt ) */
/* + E*Cos(ARGP_0 + NODE_0 )Sin(dt*d(ARGP+NODE)/dt ) */
/* = H_0 * Cos(dt*d(ARGP+NODE)/dt ) */
/* + K_0 * Sin(dt*d(ARGP+NODE)/dt ) */
/* and */
/* K_dt = E*Cos(ARGP_0 + NODE_0)Cos(dt*d(ARGP+NODE)/dt) */
/* - E*Sin(ARGP_0 + NODE_0)Sin(dt*d(ARGP+NODE)/dt) */
/* = K_0 * Cos(dt*d(ARGP+NODE)/dt) */
/* - H_0 * Sin(dt*d(ARGP+NODE)/dt) */
/* Thus we can easily compute H and K at the current epoch. */
/* Recall that the derivative of the longitude of periapse is */
/* in entry 7 of EQEL. */
dlpdt = eqel[6];
dlp = dt * dlpdt;
can = cos(dlp);
san = sin(dlp);
h__ = eqel[1] * can + eqel[2] * san;
k = eqel[2] * can - eqel[1] * san;
/* The mean longitude at epoch is in the 4th element of EQEL. */
l = eqel[3];
/* The values for P and Q at epoch are stored in entries 5 and 6 */
/* of the array EQEL. Recall that */
/* P_0 = TAN(INC/2)*SIN(NODE_0) */
/* Q_0 = TAN(INC/2)*COS(NODE_0) */
/* We need P and Q offset from the initial epoch by DT. */
/* P = TAN(INC/2)*SIN(NODE_0 + dt*dNODE/dt) */
/* Q = TAN(INC/2)*COS(NODE_0 + dt*dNODE/dt) */
/* Applying the same identities as we did before we have */
/* P = P_0 * Cos( dt*dNODE/dt ) + Q_0 * Sin( dt*dNODE/dt ) */
/* Q = Q_0 * Cos( dt*dNODE/dt ) - P_0 * Sin( dt*dNODE/dt ) */
nodedt = eqel[8];
node = dt * nodedt;
cn = cos(node);
sn = sin(node);
p = eqel[4] * cn + eqel[5] * sn;
q = eqel[5] * cn - eqel[4] * sn;
mldt = eqel[7];
/* We compute the rate of change of the argument of periapse */
/* by taking the difference between the rate of the longitude */
/* of periapse and the rate of the node. */
prate = dlpdt - nodedt;
/* Form Broucke's beta parameter */
b = sqrt(1. - h__ * h__ - k * k);
b = 1. / (b + 1.);
/* Construct the coordinate axes */
di = 1. / (p * p + 1. + q * q);
vf[0] = (1. - p * p + q * q) * di;
vf[1] = p * 2. * q * di;
vf[2] = p * -2. * di;
vg[0] = p * 2. * q * di;
vg[1] = (p * p + 1. - q * q) * di;
vg[2] = q * 2. * di;
/* Compute the mean longitude */
d__1 = mldt * dt;
ml = l + d_mod(&d__1, &pi2);
/* Obtain the eccentric longitude from Kepler's equation */
eecan = kepleq_(&ml, &h__, &k);
/* Trigonometric functions of the eccentric longitude */
sf = sin(eecan);
cf = cos(eecan);
/* Position in the orbit plane */
/* Computing 2nd power */
d__1 = h__;
x1 = a * ((1. - b * (d__1 * d__1)) * cf + (h__ * k * b * sf - k));
/* Computing 2nd power */
d__1 = k;
y1 = a * ((1. - b * (d__1 * d__1)) * sf + (h__ * k * b * cf - h__));
/* Radial distance and functions of the radial distance */
rb = h__ * sf + k * cf;
r__ = a * (1. - rb);
ra = mldt * a * a / r__;
/* Velocity in the orbit plane */
dx1 = ra * (-sf + h__ * b * rb);
dy1 = ra * (cf - k * b * rb);
/* Correction factor for periapsis rate */
nfac = 1. - dlpdt / mldt;
/* Include precession in velocity */
dx = nfac * dx1 - prate * y1;
dy = nfac * dy1 + prate * x1;
/* Form the planetary mean equator position vector */
vlcom_(&x1, vf, &y1, vg, xhold);
/* Form the planetary mean equator velocity vector */
temp[0] = -nodedt * xhold[1];
temp[1] = nodedt * xhold[0];
temp[2] = 0.;
vlcom3_(&c_b13, temp, &dx, vf, &dy, vg, &xhold[3]);
/* Transform to an inertial state vector */
mxv_(trans, xhold, state);
mxv_(trans, &xhold[3], &state[3]);
chkout_("EQNCPV", (ftnlen)6);
return 0;
} /* eqncpv_ */
