예제 #1
0
/* $Procedure DPSPCE ( Propagate a two line element set for deep space ) */
/* Subroutine */ int dpspce_(doublereal *time, doublereal *geophs, doublereal 
	*elems, doublereal *state)
{
    /* Initialized data */

    static logical doinit = TRUE_;
    static logical first = TRUE_;

    /* System generated locals */
    integer i__1, i__2;
    doublereal d__1, d__2;

    /* Builtin functions */
    integer s_rnge(char *, integer, char *, integer);
    double pow_dd(doublereal *, doublereal *), cos(doublereal), sqrt(
	    doublereal), sin(doublereal), d_mod(doublereal *, doublereal *), 
	    atan2(doublereal, doublereal);

    /* Local variables */
    static doublereal coef, eeta, aodp, delo, capu, uang, xmdf, xinc, xmam, 
	    aynl, elsq, temp;
    static logical cont;
    static doublereal rdot, cosu, sinu, coef1, t2cof, temp1, temp2, temp3, 
	    temp4, temp5, cos2u, temp6;
    extern /* Subroutine */ int zzdpinit_(doublereal *, doublereal *, 
	    doublereal *, doublereal *, doublereal *, doublereal *);
    static doublereal sin2u, a, e;
    static integer i__;
    static doublereal m[3], n[3], s, u[3], v[3], betal, scale, betao;
    extern /* Subroutine */ int chkin_(char *, ftnlen);
    static doublereal epoch, ecose, aycof, esine, a3ovk2, tempa, tempe, bstar,
	     cosio, xincl, etasq, rfdot, sinio, a1, rdotk, c1, c2, cosuk, c4, 
	    qoms24, sinuk, templ, x1m5th, x1mth2, x3thm1, x7thm1, psisq, 
	    xinck, xlcof, xmdot, xnode, xnodp;
    extern doublereal twopi_(void);
    static doublereal s4;
    extern /* Subroutine */ int vlcom_(doublereal *, doublereal *, doublereal 
	    *, doublereal *, doublereal *);
    static doublereal betao2, theta2, ae, xhdot1, ao, em, eo, qoms2t, pl, 
	    omgadf, rk, qo, uk, so;
    extern doublereal halfpi_(void);
    static doublereal xl, xn, omegao;
    extern /* Subroutine */ int latrec_(doublereal *, doublereal *, 
	    doublereal *, doublereal *);
    static doublereal perige, xnodcf, xnoddf, tsince, xnodek, omgdot, rfdotk, 
	    xnodeo;
    extern /* Subroutine */ int chkout_(char *, ftnlen);
    static doublereal ck2, lstelm[10], ck4, cosepw, sinepw, xkmper, xnodot, 
	    lstphs[8];
    extern logical return_(void);
    static doublereal pinvsq, xj2, xj3, xj4, eta, axn, xke, ayn, epw, tsi, 
	    xll, xmo, xno, tsq, xlt, del1;
    extern /* Subroutine */ int zzdpsec_(doublereal *, doublereal *, 
	    doublereal *, doublereal *, doublereal *, doublereal *, 
	    doublereal *, doublereal *, doublereal *);
    static doublereal pio2;
    extern /* Subroutine */ int zzdpper_(doublereal *, doublereal *, 
	    doublereal *, doublereal *, doublereal *, doublereal *);
    static doublereal pix2;

/* $ Abstract */

/*     This routine propagates NORAD two-line element data for */
/*     earth orbiting deep space vehicles (a vehicle with an */
/*     orbital period more than 225 minutes). */

/* $ 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 */
/*     TWO LINE ELEMENTS */
/*     DEEP SPACE PROPAGATOR */

/* $ Declarations */
/* $ Brief_I/O */

/*     VARIABLE  I/O  DESCRIPTION */
/*     --------  ---  -------------------------------------------------- */
/*     TIME       I   Time for state evaluation in seconds past ephemeris */
/*                    epoch J2000. */
/*     GEOPHS     I   The array of geophysical constants */
/*     ELEMS      I   Array of orbit elements */
/*     STATE      O   State vector at TIME */

/* $ Detailed_Input */

/*     TIME        is the epoch in seconds past ephemeris epoch J2000 */
/*                 to produced a state from the input elements. */

/*     GEOPHS      is a collection of 8 geophysical constants needed */
/*                 for computing a state.  The order of these */
/*                 constants must be: */

/*                 GEOPHS(1) = J2 gravitational harmonic for earth */
/*                 GEOPHS(2) = J3 gravitational harmonic for earth */
/*                 GEOPHS(3) = J4 gravitational harmonic for earth */

/*                 These first three constants are dimensionless. */

/*                 GEOPHS(4) = KE: Square root of the GM for earth where */
/*                             GM is expressed in earth radii cubed per */
/*                             minutes squared. */

/*                 GEOPHS(5) = QO: Low altitude bound for atmospheric */
/*                             model in km. */

/*                 GEOPHS(6) = SO: High altitude bound for atmospheric */
/*                             model in km. */


/*                 GEOPHS(7) = RE: Equatorial radius of the earth in km. */


/*                 GEOPHS(8) = AE: Distance units/earth radius */
/*                             (normally 1) */

/*                 Below are currently recommended values for these */
/*                 items: */

/*                   J2 =    1.082616D-3 */
/*                   J3 =   -2.53881D-6 */
/*                   J4 =   -1.65597D-6 */

/*                 The next item is the square root of GM for the */
/*                 earth given in units of earth-radii**1.5/Minute */

/*                   KE =    7.43669161D-2 */

/*                 The next two items define the top and */
/*                 bottom of the atmospheric drag model */
/*                 used by the type 10 ephemeris type. */
/*                 Don't adjust these unless you understand */
/*                 the full implications of such changes. */

/*                   QO =  120.0D0 */
/*                   SO =   78.0D0 */

/*                 The ER value is the equatorial radius in km */
/*                 of the earth as used by NORAD. */

/*                   ER = 6378.135D0 */

/*                 The value of AE is the number of */
/*                 distance units per earth radii used by */
/*                 the NORAD state propagation software. */
/*                 The value is 1 unless you've got */
/*                 a very good understanding of the NORAD */
/*                 routine SGP4 and the affect of changing */
/*                 this value.. */

/*                   AE =    1.0D0 */

/*     ELEMS       is an array containing two-line element data */
/*                 as prescribed below. The elements XNDD6O and BSTAR */
/*                 must have been scaled by the proper exponent stored */
/*                 in the two line elements set.  Moreover, the */
/*                 various items must be converted to the units shown */
/*                 here. */

/*                    ELEMS (  1 ) = XNDT2O in radians/minute**2 */
/*                    ELEMS (  2 ) = XNDD6O in radians/minute**3 */
/*                    ELEMS (  3 ) = BSTAR */
/*                    ELEMS (  4 ) = XINCL  in radians */
/*                    ELEMS (  5 ) = XNODEO in radians */
/*                    ELEMS (  6 ) = EO */
/*                    ELEMS (  7 ) = OMEGAO in radians */
/*                    ELEMS (  8 ) = XMO    in radians */
/*                    ELEMS (  9 ) = XNO    in radians/minute */
/*                    ELEMS ( 10 ) = EPOCH of the elements in seconds */
/*                                   past ephemeris epoch J2000. */

/* $ Detailed_Output */

/*     STATE       A 6 vector containing the X, Y, Z, Vx, Vy, Vz */
/*                 coordinates in the inertial frame (double */
/*                 precision). */

/* $ Parameters */

/*     None. */

/* $ Exceptions */

/*     Error free. */

/* $ Files */

/*     None. */

/* $ Particulars */

/*     This subroutine is an extensive rewrite of the SDP4 */
/*     routine as described in the Spacetrack 3 report.  All common */
/*     blocks were removed and all variables are explicitly defined. */

/*     The removal of common blocks causes the set of routines to */
/*     execute slower than the original version of SDP4.  However the */
/*     stability improves especially as concerns memory and */
/*     expanded internal documentation. */

/*     Trivial or redundant variables have been eliminated. */

/*       R         removed, occurrence replaced with RK */
/*       E6A       renamed TOL */
/*       THETA4    removed, relevant equation recast in Horner's form */
/*                 i.e. something like x^4 + x^2 -> x^2 ( x^2 + 1 ) */
/*       U         renamed UANG, U is now a euclidean 3 vector. */
/*       Ux,Uy,Uz  removed, replaced with 3-vector U */
/*       Vx,Vy,Vz  removed, replaced with 3-vector V */
/*       OMEGAQ    removed, usage replaced with OMEGAO */
/*       OMGDT     removed, same variable as OMGDOT, so all occurrences */
/*                 replaced with OMGDOT */
/*       SSL,SSG   replaced with the 5-vector SSX */
/*       SSH,SSE */
/*       SSI */

/*     Three functions present in the original Spacetrack report, ACTAN, */
/*     FMOD2P and THETAG, have been either replaced with an intrinsic */
/*     FORTRAN function (ACTAN -> DATAN2, FMOD2P -> DMOD) or recoded */
/*     using SPICELIB calls (THETAG). */

/*     The code at the end of this subroutine which calculates */
/*     orientation vectors, was replaced with a set of calls to */
/*     SPICELIB vector routines. */

/*     A direct comparison of output from the original Spacetrack 3 code */
/*     and these NAIF routines for the same elements and time parameters */
/*     will produce unacceptably different results. */

/* $ Examples */


/*   C---  Load the geophysical constants kernel and the leapsecond */
/*         kernel */
/*         CALL FURNSH( '/Users/ewright/lib/geophysical.ker' ) */
/*         CALL FURNSH( '/kernels/gen/lsk/naif0008.tls' ) */


/*   C---  Define a vehicle element array, TDRS 4 Geosynch */
/*         LINES( 1 ) = '1 19883U 89021B   97133.05943164 -.00000277  ' */
/*        .//           '00000-0  10000-3 0  3315' */
/*         LINES( 2 ) = '2 19883   0.5548  86.7278 0001786 312.2904 ' */
/*        .//           '172.2391  1.00269108202415' */


/*   C---  Identify the earliest first year for the elements */
/*         FRSTYR = 1988 */


/*   C---  Parse the elements to something SPICE can use */
/*         CALL GETELM ( FRSTYR, LINES, EPOCH, ELEMS ) */


/*   C---  Final time past epoch, 1400 mins (in seconds) */
/*         TF     = 1440.D0 * 60.D0 */

/*   C---  Step size for elements output 360 mins (in seconds) */
/*         DELT   = 360.D0  * 60.D0 */

/*   C---  Start time keyed off epoch */
/*         TIME   = EPOCH - 2.D0 * DELT */

/*         DO WHILE ( DABS(TIME - EPOCH) .LE. DABS(TF) ) */

/*            CALL DPSPCE ( TIME, GEOPHS, ELEMS, STATE ) */

/*            WRITE(*, FMT ='(7F17.8)' ) (TIME-EPOCH)/60.D0, */
/*        .                              (STATE(I),I=1,6) */

/*            TIME = TIME + DELT */

/*         END DO */


/* $ Restrictions */

/*     None. */

/* $ Literature_References */

/*     Hoots, Felix R., Ronald L. Roehrich (31 December 1988). "Models */
/*     for Propagation of NORAD Element Sets". United States Department */
/*     of Defense Spacetrack Report (3). */

/* $ Author_and_Institution */

/*     E.D. Wright      (JPL) */

/* $ Version */

/* -    SPICELIB Version 2.0.0, 23-JAN-2013 (EDW) */

/*        Corrected initialization block error. The ZZDPINIT call */
/*        causes a side-effect required for each DPSPCE call. */
/*        The ZZDPINIT call now occurs outside the initialization */
/*        block. Note from designer, side-effects are bad. */

/*        Added proper citation for Hoots paper. */

/* -    SPICELIB Version 1.2.2, 22-AUG-2006 (EDW) */

/*        Replaced references to LDPOOL with references */
/*        to FURNSH. */

/* -    SPICELIB Version 1.2.1, DEC-27-2000 (EDW) */

/*       Corrected error in header documentation. Horner's Rule */
/*       not Butcher's. */

/* -    SPICELIB Version 1.2.0, MAR-24-1999 (EDW) */

/*       Documentation expanded to include modifications made */
/*       to private routines.  Some english errors corrected. */

/*       Alphabetized variable declaration lists. */

/*       Temporary variable TEMP removed.  OMGDOT argument added to */
/*       ZZDPSEC call. */

/* -    SPICELIB Version 1.1.0, OCT-05-1998 (WLT) */

/*        Forced initialization section until we can figure out */
/*        why it doesn't work on SUNs. */

/* -    SPICELIB Version 1.0.1, MAR-11-1998 (EDW) */

/*       Corrected error in header describing GEOPHS array. */

/* -    SPICELIB Version 1.0.0, NOV-11-1998 (EDW) */

/* -& */
/* $ Index_Entries */

/*     NORAD two line elements deep space evaluator */

/* -& */

/*     Local variables */


/*     Define parameters for convergence tolerance and the value for 2/3, */
/*     0 and 1. */


/*     The geophysical Quantities */


/*     Elements */


/*     Other quantities */


/*     SPICELIB routines */


/*     Save everything. */


/*     Set initialization flags */


/*     Standard SPICE error handling. */

    if (return_()) {
	return 0;
    } else {
	chkin_("DPSPCE", (ftnlen)6);
    }

/*     If this is the very first time into this routine, set these */
/*     values. */

    if (first) {
	pix2 = twopi_();
	pio2 = halfpi_();
	first = FALSE_;
    }

/*     If initialization flag is FALSE, then this is not the first */
/*     call to this routine.  Check the stuff. */

    if (! doinit) {

/*        Check whether the current and last constants and elements */
/*        match.  If not, we need to reinitialize everything */
/*        since the propagation is dependent on the value of these */
/*        arrays. */

	for (i__ = 1; i__ <= 8; ++i__) {
	    if (lstphs[(i__1 = i__ - 1) < 8 && 0 <= i__1 ? i__1 : s_rnge(
		    "lstphs", i__1, "dpspce_", (ftnlen)547)] != geophs[(i__2 =
		     i__ - 1) < 8 && 0 <= i__2 ? i__2 : s_rnge("geophs", i__2,
		     "dpspce_", (ftnlen)547)]) {
		doinit = TRUE_;
	    }
	}
	for (i__ = 1; i__ <= 10; ++i__) {
	    if (lstelm[(i__1 = i__ - 1) < 10 && 0 <= i__1 ? i__1 : s_rnge(
		    "lstelm", i__1, "dpspce_", (ftnlen)556)] != elems[(i__2 = 
		    i__ - 1) < 10 && 0 <= i__2 ? i__2 : s_rnge("elems", i__2, 
		    "dpspce_", (ftnlen)556)]) {
		doinit = TRUE_;
	    }
	}
    }

/*     Initialization block.  Always called on the initial entry and */
/*     anytime the geophysical or elements array changes. */

    if (doinit) {
	doinit = FALSE_;

/*        Retrieve the geophysical constants from the GEOPHS array */

	xj2 = geophs[0];
	xj3 = geophs[1];
	xj4 = geophs[2];
	xke = geophs[3];
	qo = geophs[4];
	so = geophs[5];
	xkmper = geophs[6];
	ae = geophs[7];

/*        Save the geophysical constants for later comparison */

	for (i__ = 1; i__ <= 8; ++i__) {
	    lstphs[(i__1 = i__ - 1) < 8 && 0 <= i__1 ? i__1 : s_rnge("lstphs",
		     i__1, "dpspce_", (ftnlen)590)] = geophs[(i__2 = i__ - 1) 
		    < 8 && 0 <= i__2 ? i__2 : s_rnge("geophs", i__2, "dpspce_"
		    , (ftnlen)590)];
	}

/*        Unpack the elements array. */

	bstar = elems[2];
	xincl = elems[3];
	xnodeo = elems[4];
	eo = elems[5];
	omegao = elems[6];
	xmo = elems[7];
	xno = elems[8];
	epoch = elems[9];

/*        Save the elements for later comparison */

	for (i__ = 1; i__ <= 10; ++i__) {
	    lstelm[(i__1 = i__ - 1) < 10 && 0 <= i__1 ? i__1 : s_rnge("lstelm"
		    , i__1, "dpspce_", (ftnlen)610)] = elems[(i__2 = i__ - 1) 
		    < 10 && 0 <= i__2 ? i__2 : s_rnge("elems", i__2, "dpspce_"
		    , (ftnlen)610)];
	}

/*        Set common variables, the init flag and calculate the */
/*        WGS-72 physical and geopotential constants */

/*        CK2 =  0.5   * J2 * AE^2 */
/*        CK4 = -0.375 * J4 * AE^4 */

/*        These are values calculated only once and then saved for */
/*        future access. */

/* Computing 2nd power */
	d__1 = ae;
	ck2 = xj2 * .5 * (d__1 * d__1);
/* Computing 4th power */
	d__1 = ae, d__1 *= d__1;
	ck4 = xj4 * -.375 * (d__1 * d__1);
/* Computing 4th power */
	d__1 = (qo - so) * ae / xkmper, d__1 *= d__1;
	qoms2t = d__1 * d__1;
	s = ae * (so / xkmper + 1.);

/*        Recover original mean motion (XNODP) and semimajor axis (AODP) */
/*        from input elements */

	d__1 = xke / xno;
	a1 = pow_dd(&d__1, &c_b19);
	cosio = cos(xincl);
/* Computing 2nd power */
	d__1 = cosio;
	theta2 = d__1 * d__1;
	x3thm1 = theta2 * 3. - 1.;
/* Computing 2nd power */
	d__1 = eo;
	betao2 = 1. - d__1 * d__1;
	betao = sqrt(betao2);
/* Computing 2nd power */
	d__1 = a1;
	del1 = ck2 * 1.5 * x3thm1 / (d__1 * d__1 * betao * betao2);
	ao = a1 * (1. - del1 * (del1 * (del1 * 1.654320987654321 + 1.) + 
		.33333333333333331));
/* Computing 2nd power */
	d__1 = ao;
	delo = ck2 * 1.5 * x3thm1 / (d__1 * d__1 * betao * betao2);
	xnodp = xno / (delo + 1.);
	aodp = ao / (1. - delo);

/*        For perigee below 156 km, the values of S and QOMS2T are */
/*        altered */

	s4 = s;
	qoms24 = qoms2t;
	perige = (aodp * (1. - eo) - ae) * xkmper;
	if (perige < 156.) {
	    s4 = perige - 78.;
	    if (perige > 98.) {
/* Computing 4th power */
		d__1 = (120. - s4) * ae / xkmper, d__1 *= d__1;
		qoms24 = d__1 * d__1;
		s4 = s4 / xkmper + ae;
	    } else {
		s4 = 20.;
	    }
	}
/* Computing 2nd power */
	d__1 = aodp;
/* Computing 2nd power */
	d__2 = betao2;
	pinvsq = 1. / (d__1 * d__1 * (d__2 * d__2));
	tsi = 1. / (aodp - s4);
	eta = aodp * eo * tsi;
/* Computing 2nd power */
	d__1 = eta;
	etasq = d__1 * d__1;
	eeta = eo * eta;
	psisq = (d__1 = 1. - etasq, abs(d__1));
/* Computing 4th power */
	d__1 = tsi, d__1 *= d__1;
	coef = qoms24 * (d__1 * d__1);
	coef1 = coef / pow_dd(&psisq, &c_b20);
	c2 = coef1 * xnodp * (aodp * (etasq * 1.5 + 1. + eeta * (etasq + 4.)) 
		+ ck2 * .75 * tsi / psisq * x3thm1 * (etasq * 3. * (etasq + 
		8.) + 8.));
	c1 = bstar * c2;
	sinio = sin(xincl);
/* Computing 3rd power */
	d__1 = ae;
	a3ovk2 = -xj3 / ck2 * (d__1 * (d__1 * d__1));
	x1mth2 = 1. - theta2;
	c4 = xnodp * 2. * coef1 * aodp * betao2 * (eta * (etasq * .5 + 2.) + 
		eo * (etasq * 2. + .5) - ck2 * 2. * tsi / (aodp * psisq) * (
		x3thm1 * -3. * (1. - eeta * 2. + etasq * (1.5 - eeta * .5)) + 
		x1mth2 * .75 * (etasq * 2. - eeta * (etasq + 1.)) * cos(
		omegao * 2.)));
	temp1 = ck2 * 3. * pinvsq * xnodp;
	temp2 = temp1 * ck2 * pinvsq;
	temp3 = ck4 * 1.25 * pinvsq * pinvsq * xnodp;
	xmdot = xnodp + temp1 * .5 * betao * x3thm1 + temp2 * .0625 * betao * 
		(theta2 * (theta2 * 137. - 78.) + 13.);
	x1m5th = 1. - theta2 * 5.;
	omgdot = temp1 * -.5 * x1m5th + temp2 * .0625 * (theta2 * (theta2 * 
		395. - 114.) + 7.) + temp3 * (theta2 * (theta2 * 49. - 36.) + 
		3.);
	xhdot1 = -temp1 * cosio;
	xnodot = xhdot1 + (temp2 * .5 * (4. - theta2 * 19.) + temp3 * 2. * (
		3. - theta2 * 7.)) * cosio;
	xnodcf = betao2 * 3.5 * xhdot1 * c1;
	t2cof = c1 * 1.5;
	xlcof = a3ovk2 * .125 * sinio * (cosio * 5. + 3.) / (cosio + 1.);
	aycof = a3ovk2 * .25 * sinio;
	x7thm1 = theta2 * 7. - 1.;
    }
    zzdpinit_(&aodp, &xmdot, &omgdot, &xnodot, &xnodp, elems);

/*     Get the time since the EPOCH in minutes. */

    tsince = (*time - epoch) / 60.;

/*     Update for secular gravity and atmospheric drag */

    xmdf = xmo + xmdot * tsince;
    omgadf = omegao + omgdot * tsince;
    xnoddf = xnodeo + xnodot * tsince;
    tsq = tsince * tsince;
    xnode = xnoddf + xnodcf * tsq;
    tempa = 1. - c1 * tsince;
    tempe = bstar * c4 * tsince;
    templ = t2cof * tsq;
    xn = xnodp;

/*     Calculate the secular terms. */

    zzdpsec_(&xmdf, &omgadf, &xnode, &em, &xinc, &xn, &tsince, elems, &omgdot)
	    ;
    d__1 = xke / xn;
/* Computing 2nd power */
    d__2 = tempa;
    a = pow_dd(&d__1, &c_b19) * (d__2 * d__2);
    e = em - tempe;
    xmam = xmdf + xnodp * templ;

/*     Calculate the periodic terms. */

    zzdpper_(&tsince, &e, &xinc, &omgadf, &xnode, &xmam);
    xl = xmam + omgadf + xnode;
    xn = xke / pow_dd(&a, &c_b22);

/*      Long period periodics */

    axn = e * cos(omgadf);
/* Computing 2nd power */
    d__1 = e;
    temp = 1. / (a * (1. - d__1 * d__1));
    xll = temp * xlcof * axn;
    aynl = temp * aycof;
    xlt = xl + xll;
    ayn = e * sin(omgadf) + aynl;

/*     Solve Kepler's equation */

/*           U = EPW - AXN * SIN(EPW)  +  AYN * COS(EPW) */

/*     Where */

/*        AYN  = E * SIN(OMEGA)  +   AYNL */
/*        AXN  = E * COS(OMEGA) */

/*     And */

/*        AYNL =  -0.50D0 * SINIO * AE * J3 / (J2 * A * (1.0D0  -  E^2)) */


/*     Get the mod division of CAPU with 2 Pi */

    d__1 = xlt - xnode;
    capu = d_mod(&d__1, &pix2);
    if (capu < 0.) {
	capu += pix2;
    }

/*     Set initial states for the Kepler solution */

    epw = capu;
    cont = TRUE_;
    while(cont) {
	temp2 = epw;
	sinepw = sin(temp2);
	cosepw = cos(temp2);
	temp3 = axn * sinepw;
	temp4 = ayn * cosepw;
	temp5 = axn * cosepw;
	temp6 = ayn * sinepw;
	epw = (capu - temp4 + temp3 - temp2) / (1. - temp5 - temp6) + temp2;

/*        Test for convergence against the defined tolerance */

	if ((d__1 = epw - temp2, abs(d__1)) <= 1e-6) {
	    cont = FALSE_;
	}
    }

/*     Short period preliminary quantities */

    ecose = temp5 + temp6;
    esine = temp3 - temp4;
    elsq = axn * axn + ayn * ayn;
    temp = 1. - elsq;
    pl = a * temp;
    rk = a * (1. - ecose);
    temp1 = 1. / rk;
    rdot = xke * sqrt(a) * esine * temp1;
    rfdot = xke * sqrt(pl) * temp1;
    temp2 = a * temp1;
    betal = sqrt(temp);
    temp3 = 1. / (betal + 1.);
    cosu = temp2 * (cosepw - axn + ayn * esine * temp3);
    sinu = temp2 * (sinepw - ayn - axn * esine * temp3);

/*     Compute the angle from the x-axis of the point ( COSU, SINU ) */

    if (sinu != 0. || cosu != 0.) {
	uang = atan2(sinu, cosu);
	if (uang < 0.) {
	    uang += pix2;
	}
    } else {
	uang = 0.;
    }
    sin2u = sinu * 2. * cosu;
    cos2u = cosu * 2. * cosu - 1.;
    temp1 = ck2 * (1. / pl);
    temp2 = temp1 * (1. / pl);

/*     Update for short periodics */

    rk = rk * (1. - temp2 * 1.5 * betal * x3thm1) + temp1 * .5 * x1mth2 * 
	    cos2u;
    uk = uang - temp2 * .25 * x7thm1 * sin2u;
    xnodek = xnode + temp2 * 1.5 * cosio * sin2u;
    xinck = xinc + temp2 * 1.5 * cosio * sinio * cos2u;
    rdotk = rdot - xn * temp1 * x1mth2 * sin2u;
    rfdotk = rfdot + xn * temp1 * (x1mth2 * cos2u + x3thm1 * 1.5);

/*     Orientation vectors are calculated by */

/*     U = M sin(uk) + N cos(uk) */
/*     V = M cos(uk) - N sin(uk) */

/*     Where M and N are euclidean 3 vectors */

/*     M = (-sin(xnodek)cos(xinck), cos(xnodek)cos(xinck), sin(xinck) ) */
/*     N = (           cos(xnodek), sin(xnodek)          , 0          ) */

    sinuk = sin(uk);
    cosuk = cos(uk);

/*     Use LATREC to generate M and N.  M is a latitude to rectangle */
/*     conversion of a unit vector where PI/2 + XNODEK is the longitude */

    d__1 = pio2 + xnodek;
    latrec_(&c_b23, &d__1, &xinck, m);
    latrec_(&c_b23, &xnodek, &c_b25, n);

/*     Sum the components to obtain U and V */

    vlcom_(&sinuk, m, &cosuk, n, u);
    d__1 = -sinuk;
    vlcom_(&cosuk, m, &d__1, n, v);

/*     Determine the position and velocity then pack the STATE vector */
/*     with value scaled to KM and KPS. */

/*     R = RK    U +        0 V */
/*     V = RKDOT U + RK RFDOT V */

    scale = xkmper / ae;
    d__1 = rk * scale;
    vlcom_(&d__1, u, &c_b25, v, state);

/*     Now scale to KPS for the velocity component */

    scale /= 60.;
    d__1 = rdotk * scale;
    d__2 = rfdotk * scale;
    vlcom_(&d__1, u, &d__2, v, &state[3]);

/*     All done now.... */

    chkout_("DPSPCE", (ftnlen)6);
    return 0;
} /* dpspce_ */
예제 #2
0
파일: xfmsta.c 프로젝트: Dbelsa/coft
/* $Procedure      XFMSTA ( Transform state between coordinate systems) */
/* Subroutine */ int xfmsta_(doublereal *istate, char *icosys, char *ocosys, 
	char *body, doublereal *ostate, ftnlen icosys_len, ftnlen ocosys_len, 
	ftnlen body_len)
{
    /* Initialized data */

    static char cosys[40*6] = "RECTANGULAR                             " 
	    "CYLINDRICAL                             " "LATITUDINAL         "
	    "                    " "SPHERICAL                               " 
	    "GEODETIC                                " "PLANETOGRAPHIC      "
	    "                    ";
    static logical first = TRUE_;

    /* System generated locals */
    integer i__1, i__2;
    doublereal d__1, d__2;

    /* Builtin functions */
    double sqrt(doublereal);
    integer s_rnge(char *, integer, char *, integer);

    /* Local variables */
    extern /* Subroutine */ int zzbods2c_(integer *, char *, integer *, 
	    logical *, char *, integer *, logical *, ftnlen, ftnlen);
    doublereal ivel[3], ipos[3];
    extern /* Subroutine */ int vequ_(doublereal *, doublereal *);
    integer isys, osys;
    doublereal f;
    extern /* Subroutine */ int zzctruin_(integer *);
    integer i__, j;
    doublereal radii[3];
    extern /* Subroutine */ int chkin_(char *, ftnlen), errch_(char *, char *,
	     ftnlen, ftnlen), vpack_(doublereal *, doublereal *, doublereal *,
	     doublereal *);
    extern doublereal dpmax_(void);
    logical found;
    extern /* Subroutine */ int errdp_(char *, doublereal *, ftnlen), vequg_(
	    doublereal *, integer *, doublereal *);
    doublereal sqtmp;
    char isysu[40], osysu[40];
    static logical svfnd1;
    static integer svctr1[2];
    extern logical failed_(void);
    doublereal jacobi[9]	/* was [3][3] */;
    extern /* Subroutine */ int bodvcd_(integer *, char *, integer *, integer 
	    *, doublereal *, ftnlen), georec_(doublereal *, doublereal *, 
	    doublereal *, doublereal *, doublereal *, doublereal *), drdgeo_(
	    doublereal *, doublereal *, doublereal *, doublereal *, 
	    doublereal *, doublereal *), recgeo_(doublereal *, doublereal *, 
	    doublereal *, doublereal *, doublereal *, doublereal *), dgeodr_(
	    doublereal *, doublereal *, doublereal *, doublereal *, 
	    doublereal *, doublereal *);
    integer bodyid;
    extern integer isrchc_(char *, integer *, char *, ftnlen, ftnlen);
    static integer svbdid;
    extern /* Subroutine */ int latrec_(doublereal *, doublereal *, 
	    doublereal *, doublereal *), drdlat_(doublereal *, doublereal *, 
	    doublereal *, doublereal *), cylrec_(doublereal *, doublereal *, 
	    doublereal *, doublereal *), drdcyl_(doublereal *, doublereal *, 
	    doublereal *, doublereal *);
    doublereal toobig;
    extern /* Subroutine */ int sphrec_(doublereal *, doublereal *, 
	    doublereal *, doublereal *), drdsph_(doublereal *, doublereal *, 
	    doublereal *, doublereal *), pgrrec_(char *, doublereal *, 
	    doublereal *, doublereal *, doublereal *, doublereal *, 
	    doublereal *, ftnlen), drdpgr_(char *, doublereal *, doublereal *,
	     doublereal *, doublereal *, doublereal *, doublereal *, ftnlen), 
	    reccyl_(doublereal *, doublereal *, doublereal *, doublereal *), 
	    reclat_(doublereal *, doublereal *, doublereal *, doublereal *), 
	    sigerr_(char *, ftnlen), recsph_(doublereal *, doublereal *, 
	    doublereal *, doublereal *), chkout_(char *, ftnlen), recpgr_(
	    char *, doublereal *, doublereal *, doublereal *, doublereal *, 
	    doublereal *, doublereal *, ftnlen), dcyldr_(doublereal *, 
	    doublereal *, doublereal *, doublereal *), dlatdr_(doublereal *, 
	    doublereal *, doublereal *, doublereal *), ljucrs_(integer *, 
	    char *, char *, ftnlen, ftnlen), setmsg_(char *, ftnlen), dsphdr_(
	    doublereal *, doublereal *, doublereal *, doublereal *);
    static char svbody[36];
    extern /* Subroutine */ int dpgrdr_(char *, doublereal *, doublereal *, 
	    doublereal *, doublereal *, doublereal *, doublereal *, ftnlen);
    extern logical return_(void);
    integer dim;
    extern /* Subroutine */ int mxv_(doublereal *, doublereal *, doublereal *)
	    ;

/* $ Abstract */

/*     Transform a state between coordinate systems. */

/* $ 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 */

/*     CONVERSION */
/*     COORDINATE */
/*     EPHEMERIS */
/*     STATE */

/* $ Declarations */
/* $ Abstract */

/*     This include file defines the dimension of the counter */
/*     array used by various SPICE subsystems to uniquely identify */
/*     changes in their states. */

/* $ 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. */

/* $ Parameters */

/*     CTRSIZ      is the dimension of the counter array used by */
/*                 various SPICE subsystems to uniquely identify */
/*                 changes in their states. */

/* $ Author_and_Institution */

/*     B.V. Semenov    (JPL) */

/* $ Literature_References */

/*     None. */

/* $ Version */

/* -    SPICELIB Version 1.0.0, 29-JUL-2013 (BVS) */

/* -& */

/*     End of include file. */

/* $ Brief_I/O */

/*     VARIABLE  I/O  DESCRIPTION */
/*     --------  ---  ------------------------------------------------- */
/*     ISTATE     I   Input state. */
/*     ICOSYS     I   Current (input) coordinate system. */
/*     OCOSYS     I   Desired (output) coordinate system. */
/*     BODY       I   Name or NAIF ID of body with which */
/*                    coordinates are associated (if applicable). */
/*     OSTATE     O   Converted output state. */

/* $ Detailed_Input */

/*     ISTATE     is a state vector in the input (ICOSYS) coordinate */
/*                system representing position and velocity. */

/*                All angular measurements must be in radians. */

/*                Note: body radii values taken from the kernel */
/*                pool are used when converting to or from geodetic or */
/*                planetographic coordinates. It is the user's */
/*                responsibility to verify the distance inputs are in */
/*                the same units as the radii in the kernel pool, */
/*                typically kilometers. */

/*     ICOSYS     is the name of the coordinate system that the input */
/*                state vector (ISTATE) is currently in. */

/*                ICOSYS may be any of the following: */

/*                    'RECTANGULAR' */
/*                    'CYLINDRICAL' */
/*                    'LATITUDINAL' */
/*                    'SPHERICAL' */
/*                    'GEODETIC' */
/*                    'PLANETOGRAPHIC' */

/*                Leading spaces, trailing spaces, and letter case */
/*                are ignored. For example, ' cyLindRical  ' would be */
/*                accepted. */

/*     OCOSYS     is the name of the coordinate system that the state */
/*                should be converted to. */

/*                Please see the description of ICOSYS for details. */

/*     BODY       is the name or NAIF ID of the body associated with the */
/*                planetographic or geodetic coordinate system. */

/*                If neither of the coordinate system choices are */
/*                geodetic or planetographic, BODY may be an empty */
/*                string (' '). */

/*                Examples of accepted body names or IDs are: */
/*                         'Earth' */
/*                         '399' */

/*                Leading spaces, trailing spaces, and letter case are */
/*                ignored. */

/* $ Detailed_Output */

/*     OSTATE     is the state vector that has been converted to the */
/*                output coordinate system (OCOSYS). */

/* $ Parameters */

/*     None. */

/* $ Exceptions */

/*     1)  If either the input or output coordinate system is not */
/*         recognized, the error SPICE(COORDSYSNOTREC) is signaled. */

/*     2)  If the input body name cannot be converted to a NAIF ID */
/*         (applies to geodetic and planetographic coordinate */
/*         systems), the error 'SPICE(IDCODENOTFOUND)' is signaled. */

/*     3)  If the input state ISTATE is not valid, meaning the position */
/*         but not the velocity is along the z-axis, the error */
/*         'SPICE(INVALIDSTATE)' is signaled. */

/*         Note: If both the input position and velocity are along */
/*         the z-axis and the output coordinate system is not */
/*         rectangular, the velocity can still be calculated even */
/*         though the Jacobian is undefined. This case will not */
/*         signal an error. An example of the input position and */
/*         velocity along the z-axis is below. */

/*                       Term    Value */
/*                       -----   ------ */
/*                         x       0 */
/*                         y       0 */
/*                         z       z */
/*                       dx/dt     0 */
/*                       dy/dt     0 */
/*                       dz/dt   dz_dt */

/*     4)  If either the input or output coordinate system is */
/*         geodetic or planetographic and at least one of the body's */
/*         radii is less than or equal to zero, the error */
/*         SPICE(INVALIDRADIUS) will be signaled. */

/*     5)  If either the input or output coordinate system is */
/*         geodetic or planetographic and the difference of the */
/*         equatorial and polar radii divided by the equatorial radius */
/*         would produce numeric overflow, the error */
/*         'SPICE(INVALIDRADIUS)' will be signaled. */

/*     6)  If the product of the Jacobian and velocity components */
/*         may lead to numeric overflow, the error */
/*         'SPICE(NUMERICOVERFLOW)' is signaled. */

/* $ Files */

/*     SPK, PCK, CK, and FK kernels may be required. */

/*     If the input or output coordinate systems are either geodetic or */
/*     planetographic, a PCK providing the radii of the body */
/*     name BODY must be loaded via FURNSH. */

/*     Kernel data are normally loaded once per program run, NOT every */
/*     time this routine is called. */

/* $ Particulars */

/*     Input Order */
/*     ------------------------------------------- */

/*     The input and output states will be structured by the */
/*     following descriptions. */

/*     For rectangular coordinates, the state vector is the following */
/*     in which X, Y, and Z are the rectangular position components and */
/*     DX, DY, and DZ are the time derivatives of each position */
/*     component. */

/*             ISTATE = ( X, Y, Z, DX, DY, DZ ) */

/*     For cylindrical coordinates, the state vector is the following */
/*     in which R is the radius, LONG is the longitudes, Z is the */
/*     height, and DR, DLONG, and DZ are the time derivatives of each */
/*     position component. */

/*             ISTATE = ( R, LONG, Z, DR, DLONG, DZ ) */

/*     For latitudinal coordinates, the state vector is the following */
/*     in which R is the radius, LONG is the longitude, LAT is the */
/*     latitude, and DR, DLONG, and DLAT are the time derivatives of */
/*     each position component. */

/*             ISTATE = ( R, LONG, LAT, DR, DLONG, DLAT ) */

/*     For spherical coordinates, the state vector is the following in */
/*     which R is the radius, COLAT is the colatitude, LONG is the */
/*     longitude, and DR, DCOLAT, and DLONG are the time derivatives of */
/*     each position component. */

/*             ISTATE = ( R, COLAT, LONG, DR, DCOLAT, DLONG ) */

/*     For geodetic coordinates, the state vector is the following in */
/*     which LONG is the longitude, LAT is the latitude, ALT is the */
/*     altitude, and DLONG, DLAT, and DALT are the time derivatives of */
/*     each position component. */

/*             ISTATE = ( LONG, LAT, ALT, DLONG, DLAT, DALT ) */

/*     For planetographic coordinates, the state vector is the */
/*     following in which LONG is the longitude, LAT is the latitude, */
/*     ALT is the altitude, and DLONG, DLAT, and DALT are the time */
/*     derivatives of each position component. */

/*             ISTATE = ( LONG, LAT, ALT, DLONG, DLAT, DALT ) */


/*     Input Boundaries */
/*     ------------------------------------------- */

/*     There are intervals the input angles must fall within if */
/*     the input coordinate system is not rectangular. These */
/*     intervals are provided below. */

/*        Input variable    Input meaning   Input interval [rad] */
/*        --------------    -------------   ------------------------ */
/*            LONG           Longitude        0     <= LONG  <  2*pi */
/*            LAT            Latitude        -pi/2  <= LAT   <= pi/2 */
/*            COLAT          Colatitude       0     <= COLAT <= pi */


/* $ Examples */

/*     The numerical results shown for these examples may differ across */
/*     platforms. The results depend on the SPICE kernels used as */
/*     input, the compiler and supporting libraries, and the machine */
/*     specific arithmetic implementation. */

/*     1) Find the apparent state of Phoebe as seen by CASSINI in the */
/*        J2000 frame at 2004 Jun 11 19:32:00. Transform the state */
/*        from rectangular to latitudinal coordinates. For verification, */
/*        transform the state back from latitudinal to rectangular */
/*        coordinates. */

/*        Use the meta-kernel shown below to load the required SPICE */
/*        kernels. */

/*           KPL/MK */

/*           File name: xfmsta_ex1.tm */

/*           This meta-kernel is intended to support operation of SPICE */
/*           example programs. The kernels shown here should not be */
/*           assumed to contain adequate or correct versions of data */
/*           required by SPICE-based user applications. */

/*           In order for an application to use this meta-kernel, the */
/*           kernels referenced here must be present in the user's */
/*           current working directory. */

/*           The names and contents of the kernels referenced */
/*           by this meta-kernel are as follows: */

/*                  File name                     Contents */
/*                  ---------                     -------- */
/*                  cpck05Mar2004.tpc             Planet orientation and */
/*                                                radii */
/*                  naif0009.tls                  Leapseconds */
/*                  020514_SE_SAT105.bsp          Satellite ephemeris for */
/*                                                Saturn */
/*                  030201AP_SK_SM546_T45.bsp     CASSINI ephemeris */
/*                  981005_PLTEPH-DE405S.bsp      Planetary ephemeris */


/*           \begindata */

/*           KERNELS_TO_LOAD = ( 'naif0009.tls'  , */
/*                               '020514_SE_SAT105.bsp'  , */
/*                               '030201AP_SK_SM546_T45.bsp'  , */
/*                               '981005_PLTEPH-DE405S.bsp', */
/*                               'cpck05Mar2004.tpc'   ) */

/*           End of meta-kernel */

/*        Example code begins here. */

/*           PROGRAM  EX1_XFMSTA */
/*           IMPLICIT NONE */
/*     C */
/*     C     Local parameters */
/*     C */
/*     C     METAKR is the meta-kernel's filename. */
/*     C */
/*           CHARACTER*(*)         METAKR */
/*           PARAMETER           ( METAKR = 'xfmsta_ex1.tm' ) */

/*           CHARACTER*(*)         FORM */
/*           PARAMETER           ( FORM = '(F16.6, F16.6, F16.6)' ) */

/*     C */
/*     C     Local variables */
/*     C */
/*     C     STAREC is the state of Phoebe with respect to CASSINI in */
/*     C     rectangular coordinates. STALAT is the state rotated into */
/*     C     latitudinal coordinates. STREC2 is the state transformed */
/*     C     back into rectangular coordinates from latitudinal. */
/*     C */
/*           DOUBLE PRECISION      STAREC (6) */
/*           DOUBLE PRECISION      STALAT (6) */
/*           DOUBLE PRECISION      STREC2 (6) */

/*     C */
/*     C     ET is the ephemeris time (TDB) corresponding to the */
/*     C     observation. */
/*     C */
/*           DOUBLE PRECISION      ET */
/*           DOUBLE PRECISION      LT */

/*           INTEGER               I */

/*     C */
/*     C     The required kernels must be loaded. */
/*     C */
/*           CALL FURNSH ( METAKR ) */

/*     C */
/*     C     Calculate the state at 2004 Jun 11 19:32:00 UTC. */
/*     C */
/*           CALL STR2ET ( '2004-JUN-11-19:32:00', ET ) */

/*     C */
/*     C     Calculate the apparent state of Phoebe as seen by */
/*     C     CASSINI in the J2000 frame. */
/*     C */
/*           CALL SPKEZR ( 'PHOEBE',  ET, 'IAU_PHOEBE', 'LT+S', */
/*          .              'CASSINI', STAREC, LT ) */

/*     C */
/*     C     Transform the state from rectangular to latitudinal. */
/*     C     Notice that since neither the input nor output */
/*     C     coordinate frames are 'geodetic' or 'planetographic', */
/*     C     the input for the body name is a blank string. */
/*     C */
/*           CALL XFMSTA ( STAREC, 'RECTANGULAR', 'LATITUDINAL', ' ', */
/*          .              STALAT ) */

/*     C */
/*     C     Transform the state back to rectangular from latitudinal */
/*     C     for verification. This result should be very similar to */
/*     C     STAREC. */
/*     C */
/*           CALL XFMSTA ( STALAT, 'LATITUDINAL', 'RECTANGULAR',' ', */
/*          .              STREC2 ) */

/*     C */
/*     C     Report the results. */
/*     C */
/*           WRITE (*,*)    ' ' */
/*           WRITE (*,*)    'Phoebe as seen by CASSINI - rectangular' */
/*           WRITE (*,*)    '  Position [km]:' */
/*           WRITE (*,FORM) (STAREC(I), I = 1, 3) */
/*           WRITE (*,*)    '  Velocity [km/s]:' */
/*           WRITE (*,FORM) (STAREC(I), I = 4, 6) */
/*           WRITE (*,*)    ' ' */
/*           WRITE (*,*)    'Phoebe as seen by CASSINI - latitudinal' */
/*           WRITE (*,*)    '  Position [km, rad, rad]:' */
/*           WRITE (*,FORM) (STALAT(I), I = 1, 3) */
/*           WRITE (*,*)    '  Velocity [km/s, rad/s, rad/s]:' */
/*           WRITE (*,FORM) (STALAT(I), I = 4, 6) */
/*           WRITE (*,*)    ' ' */
/*           WRITE (*,*)    'Verification: ' */
/*           WRITE (*,*)    'Phoebe as seen by CASSINI - rectangular' */
/*           WRITE (*,*)    '  Position [km]:' */
/*           WRITE (*,FORM) (STREC2(I), I = 1, 3) */
/*           WRITE (*,*)    '  Velocity [km/s]:' */
/*           WRITE (*,FORM) (STREC2(I), I = 4, 6) */

/*           END */

/*        When this program was executed using gfortran on a PC Linux */
/*        64 bit environment, the output was: */

/*             Phoebe as seen by CASSINI - rectangular */
/*               Position [km]: */
/*                -1982.639762     -934.530471     -166.562595 */
/*               Velocity [km/s]: */
/*                    3.970832       -3.812496       -2.371663 */

/*             Phoebe as seen by CASSINI - latitudinal */
/*               Position [km, rad, rad]: */
/*                 2198.169858       -2.701121       -0.075846 */
/*               Velocity [km/s, rad/s, rad/s]: */
/*                   -1.780939        0.002346       -0.001144 */

/*             Verification: */
/*             Phoebe as seen by CASSINI - rectangular */
/*               Position [km]: */
/*                -1982.639762     -934.530471     -166.562595 */
/*               Velocity [km/s]: */
/*                    3.970832       -3.812496       -2.371663 */

/*     2) Transform a given state from cylindrical to planetographic */
/*        coordinates with respect to Earth. */

/*        Use the meta-kernel shown below to load the required SPICE */
/*        kernels. */

/*           KPL/MK */

/*           File name: xfmsta_ex2.tm */

/*           This meta-kernel is intended to support operation of SPICE */
/*           example programs. The kernels shown here should not be */
/*           assumed to contain adequate or correct versions of data */
/*           required by SPICE-based user applications. */

/*           In order for an application to use this meta-kernel, the */
/*           kernels referenced here must be present in the user's */
/*           current working directory. */

/*           The names and contents of the kernels referenced */
/*           by this meta-kernel are as follows: */

/*              File name                     Contents */
/*              ---------                     -------- */
/*              cpck05Mar2004.tpc             Planet orientation and */
/*                                            radii */

/*           \begindata */

/*              KERNELS_TO_LOAD = ( 'cpck05Mar2004.tpc' ) */

/*           \begintext */

/*           End of meta-kernel */


/*        Example code begins here. */

/*           PROGRAM  EX2_XFMSTA */
/*           IMPLICIT NONE */

/*     C */
/*     C     Local parameters */
/*     C */
/*     C     METAKR is the meta-kernel's filename. */
/*     C */
/*           CHARACTER*(*)         METAKR */
/*           PARAMETER           ( METAKR = 'xfmsta_ex2.tm' ) */

/*           CHARACTER*(*)         FORM */
/*           PARAMETER           ( FORM = '(F16.6, F16.6, F16.6)' ) */

/*     C */
/*     C     Local variables */
/*     C */
/*     C     STACYL is the state in cylindrical coordinates. */
/*     C */
/*           DOUBLE PRECISION      STACYL (6) */
/*     C */
/*     C     STAPLN is the state transformed into planetographic */
/*     C     coordinates. */
/*     C */
/*           DOUBLE PRECISION      STAPLN (6) */
/*     C */
/*     C     STCYL2 is the state transformed back into */
/*     C     cylindrical coordinates from planetographic. */
/*     C */
/*           DOUBLE PRECISION      STCYL2 (6) */

/*           INTEGER               I */

/*           DATA STACYL / 1.0D0, 0.5D0, 0.5D0, 0.2D0, 0.1D0, -0.2D0 / */
/*     C */
/*     C     The required kernels must be loaded. */
/*     C */
/*           CALL FURNSH ( METAKR ) */

/*     C */
/*     C     Transform the state from cylindrical to planetographic. */
/*     C     Note that since one of the coordinate systems is */
/*     C     planetographic, the body name must be input. */
/*     C */
/*           CALL XFMSTA ( STACYL, 'CYLINDRICAL', 'PLANETOGRAPHIC', */
/*          .              'EARTH', STAPLN ) */

/*     C */
/*     C     Transform the state back to cylindrical from */
/*     C     planetographic for verification. The result should be very */
/*     C     close to STACYL. */
/*     C */
/*           CALL XFMSTA ( STAPLN, 'PLANETOGRAPHIC', 'CYLINDRICAL', */
/*          .              'EARTH', STCYL2 ) */

/*     C */
/*     C     Report the results. */
/*     C */
/*           WRITE (*,*)    'Cylindrical state' */
/*           WRITE (*,*)    '  Position [km, rad, km]:' */
/*           WRITE (*,FORM) (STACYL(I), I = 1, 3) */
/*           WRITE (*,*)    '  Velocity [km/s, rad/s, km/s]:' */
/*           WRITE (*,FORM) (STACYL(I), I = 4, 6) */
/*           WRITE (*,*)    ' ' */
/*           WRITE (*,*) 'Planetographic state' */
/*           WRITE (*,*)    '  Position [rad, rad, km]:' */
/*           WRITE (*,FORM) (STAPLN(I), I = 1, 3) */
/*           WRITE (*,*)    '  Velocity [rad/s, rad/s, km/s]:' */
/*           WRITE (*,FORM) (STAPLN(I), I = 4, 6) */
/*           WRITE (*,*)    ' ' */
/*           WRITE (*,*)    'Verification:  Cylindrical state' */
/*           WRITE (*,*)    '  Position [km, rad, km]:' */
/*           WRITE (*,FORM) (STCYL2(I), I = 1, 3) */
/*           WRITE (*,*)    '  Velocity [km/s, rad/s, km/s]:' */
/*           WRITE (*,FORM) (STCYL2(I), I = 4, 6) */

/*           END */

/*        When this program was executed using gfortran on a PC Linux */
/*        64 bit environment, the output was: */

/*             Cylindrical state */
/*               Position [km, rad, km]: */
/*                    1.000000        0.500000        0.500000 */
/*               Velocity [km/s, rad/s, km/s]: */
/*                    0.200000        0.100000       -0.200000 */

/*             Planetographic state */
/*               Position [rad, rad, km]: */
/*                    0.500000        1.547727    -6356.238467 */
/*               Velocity [rad/s, rad/s, km/s]: */
/*                    0.100000       -0.004721       -0.195333 */

/*             Verification:  Cylindrical state */
/*               Position [km, rad, km]: */
/*                    1.000000        0.500000        0.500000 */
/*               Velocity [km/s, rad/s, km/s]: */
/*                    0.200000        0.100000       -0.200000 */

/* $ Restrictions */

/*     None. */

/* $ Literature_References */

/*     None. */

/* $ Author_and_Institution */

/*     S.C. Krening      (JPL) */
/*     B.V. Semenov      (JPL) */

/* $ Version */

/* -    SPICELIB Version 1.0.0  22-APR-2014 (SCK)(BVS) */

/* -& */
/* $ Index_Entries */

/*     state transformation between coordinate systems */
/*     convert state */

/* -& */

/*     SPICELIB functions */


/*     Local parameters */

/*     Potentially large numbers produced by transforming the */
/*     velocity using the Jacobian must not exceed DPMAX()/MARGIN: */


/*     The size of each coordinate system name must not exceed */
/*     CHSIZ characters. */


/*     NCOSYS is the number of coordinate systems supported by */
/*     this routine. */


/*     The following integer parameters represent the coordinate */
/*     systems supported by this routine. */


/*     Saved body name length. */


/*     Local variables */

/*     COSYS is the array of supported coordinate system names. */
/*     ISYSU and OSYSU are the input and output coordinate systems */
/*     from the user that are made insensitive to case or leading and */
/*     trailing spaces. */


/*     IPOS and IVEL are the input position and velocity translated */
/*     into rectangular. */


/*     For transformations including either geodetic or planetographic */
/*     coordinate systems, RADII is an array of the radii values */
/*     associated with the input body. These values will be loaded */
/*     from the kernel pool. */


/*     JACOBI is the Jacobian matrix that converts the velocity */
/*     coordinates between systems. */


/*     The flattening coefficient, F, is calculated when either */
/*     geodetic or planetographic coordinate systems are included */
/*     in the transformation. */


/*     SQTMP and TOOBIG are used to check for possible numeric */
/*     overflow situations. */


/*     BODYID and DIM are only used when the input or output coordinate */
/*     systems are geodetic or planetographic. The BODYID is the NAID ID */
/*     associated with the input body name. DIM is used while retrieving */
/*     the radii from the kernel pool. */


/*     ISYS and OSYS are the integer codes corresponding to the */
/*     input and output coordinate systems. I and J are iterators. */


/*     Saved name/ID item declarations. */


/*     Saved variables */


/*     Saved name/ID items. */


/*     Assign the names of the coordinate systems to a character */
/*     array in which each coordinate system name is located at */
/*     the index of the integer ID of the coordinate system. */


/*     Initial values. */


/*     There are three main sections of this routine: */

/*       1)  Error handling and initialization. */
/*       2)  Conversion of the input to rectangular coordinates. */
/*       3)  Conversion from rectangular to the output coordinates. */

/*     Error handling and initialization */
/*     ---------------------------------------------------------------- */

/*     Standard SPICE error handling. */

    if (return_()) {
	return 0;
    }
    chkin_("XFMSTA", (ftnlen)6);

/*     Initialization. */

    if (first) {

/*        Initialize counter. */

	zzctruin_(svctr1);
	first = FALSE_;
    }

/*     Remove initial and trailing spaces. */
/*     Convert the input coordinate systems to upper case. */

    ljucrs_(&c__0, icosys, isysu, icosys_len, (ftnlen)40);
    ljucrs_(&c__0, ocosys, osysu, ocosys_len, (ftnlen)40);

/*     Check to see if the input and output coordinate systems */
/*     provided by the user are acceptable. Store the integer */
/*     code of the input and output coordinate systems into */
/*     ISYS and OSYS. */

    isys = isrchc_(isysu, &c__6, cosys, (ftnlen)40, (ftnlen)40);
    osys = isrchc_(osysu, &c__6, cosys, (ftnlen)40, (ftnlen)40);

/*     If the coordinate systems are not acceptable, an error is */
/*     signaled. */

    if (isys == 0 || osys == 0) {
	if (isys == 0 && osys == 0) {

/*           Both the input and the output coordinate systems were not */
/*           recognized. */

	    setmsg_("Input coordinate system # and output coordinate system "
		    "# are not recognized.", (ftnlen)76);
	    errch_("#", icosys, (ftnlen)1, icosys_len);
	    errch_("#", ocosys, (ftnlen)1, ocosys_len);
	    sigerr_("SPICE(COORDSYSNOTREC)", (ftnlen)21);
	    chkout_("XFMSTA", (ftnlen)6);
	    return 0;
	} else if (isys == 0) {

/*           The input coordinate system was not recognized. */

	    setmsg_("Input coordinate system # was not recognized", (ftnlen)
		    44);
	    errch_("#", icosys, (ftnlen)1, icosys_len);
	    sigerr_("SPICE(COORDSYSNOTREC)", (ftnlen)21);
	    chkout_("XFMSTA", (ftnlen)6);
	    return 0;
	} else {

/*           The output coordinate system was not recognized. */

	    setmsg_("Output coordinate system # was not recognized", (ftnlen)
		    45);
	    errch_("#", ocosys, (ftnlen)1, ocosys_len);
	    sigerr_("SPICE(COORDSYSNOTREC)", (ftnlen)21);
	    chkout_("XFMSTA", (ftnlen)6);
	    return 0;
	}
    }

/*     If the input and output coordinate systems are equal, set the */
/*     output equal to the input since no conversion needs to take */
/*     place. */

    if (isys == osys) {
	vequg_(istate, &c__6, ostate);
	chkout_("XFMSTA", (ftnlen)6);
	return 0;
    }

/*     If converting to or from either geodetic or planetographic, the */
/*     NAIF ID must be found from the input body name BODY. If the */
/*     body name does not have a valid NAIF ID code, an error is */
/*     signaled. If the NAIF ID is valid, the radii of the body are */
/*     located and the flattening coefficient is calculated. */

    if (osys == 5 || osys == 6 || isys == 5 || isys == 6) {

/*        Find the NAIF ID code */

	zzbods2c_(svctr1, svbody, &svbdid, &svfnd1, body, &bodyid, &found, (
		ftnlen)36, body_len);

/*        If the body's name was found, find the body's radii and */
/*        compute flattening coefficient. Otherwise, signal an error. */

	if (found) {
	    bodvcd_(&bodyid, "RADII", &c__3, &dim, radii, (ftnlen)5);
	    if (failed_()) {
		chkout_("XFMSTA", (ftnlen)6);
		return 0;
	    }

/*           If either radius is less than or equal to zero, an error is */
/*           signaled. */

	    if (radii[2] <= 0. || radii[0] <= 0.) {
		setmsg_("At least one radii is less than or equal to zero. T"
			"he equatorial radius has a value of # and the polar "
			"radius has has a value of #.", (ftnlen)131);
		errdp_("#", radii, (ftnlen)1);
		errdp_("#", &radii[2], (ftnlen)1);
		sigerr_("SPICE(INVALIDRADIUS)", (ftnlen)20);
		chkout_("XFMSTA", (ftnlen)6);
		return 0;
	    }

/*           If the difference of the equatorial and polar radii */
/*           divided by the equatorial radius is greater than DPMAX, */
/*           a numeric overflow may occur, so an error is signaled. */

	    if (sqrt((d__1 = radii[0] - radii[2], abs(d__1))) / sqrt((abs(
		    radii[0]))) >= sqrt(dpmax_())) {
		setmsg_("The equatorial radius for # has a value of # and a "
			"polar radius of #. The flattening coefficient cannot"
			" be calculated due to numeric overflow.", (ftnlen)142)
			;
		errch_("#", body, (ftnlen)1, body_len);
		errdp_("#", radii, (ftnlen)1);
		errdp_("#", &radii[2], (ftnlen)1);
		sigerr_("SPICE(INVALIDRADIUS)", (ftnlen)20);
		chkout_("XFMSTA", (ftnlen)6);
		return 0;
	    }
	    f = (radii[0] - radii[2]) / radii[0];
	} else {
	    setmsg_("The input body name # does not have a valid NAIF ID cod"
		    "e.", (ftnlen)57);
	    errch_("#", body, (ftnlen)1, body_len);
	    sigerr_("SPICE(IDCODENOTFOUND)", (ftnlen)21);
	    chkout_("XFMSTA", (ftnlen)6);
	    return 0;
	}
    }

/*     Conversion of the input to rectangular coordinates */
/*     ---------------------------------------------------------------- */

/*     First, the position and velocity coordinates will be converted */
/*     into rectangular coordinates. If the input system is not */
/*     rectangular, then the velocity coordinates must be translated */
/*     into rectangular using the Jacobian. If the input system is */
/*     rectangular, then the input state must simply be saved into IPOS */
/*     and IVEL. */

/*     TOOBIG is used for preventing numerical overflow. The square */
/*     roots of values are used to safely check if overflow will occur. */

    toobig = sqrt(dpmax_() / 100.);
    if (isys != 1) {

/*        To rectangular... */

	if (isys == 2) {

/*                  ... from cylindrical */

	    cylrec_(istate, &istate[1], &istate[2], ipos);
	    drdcyl_(istate, &istate[1], &istate[2], jacobi);
	} else if (isys == 3) {

/*                  ... from latitudinal */

	    latrec_(istate, &istate[1], &istate[2], ipos);
	    drdlat_(istate, &istate[1], &istate[2], jacobi);
	} else if (isys == 4) {

/*                  ... from spherical */

	    sphrec_(istate, &istate[1], &istate[2], ipos);
	    drdsph_(istate, &istate[1], &istate[2], jacobi);
	} else if (isys == 5) {

/*                  ... from geodetic */

	    georec_(istate, &istate[1], &istate[2], radii, &f, ipos);
	    if (failed_()) {
		chkout_("XFMSTA", (ftnlen)6);
		return 0;
	    }
	    drdgeo_(istate, &istate[1], &istate[2], radii, &f, jacobi);
	} else if (isys == 6) {

/*                  ... from planetographic */

	    pgrrec_(body, istate, &istate[1], &istate[2], radii, &f, ipos, 
		    body_len);
	    if (failed_()) {
		chkout_("XFMSTA", (ftnlen)6);
		return 0;
	    }
	    drdpgr_(body, istate, &istate[1], &istate[2], radii, &f, jacobi, 
		    body_len);
	} else {
	    setmsg_("This error should never occur. This is an intermediate "
		    "step in which a non-rectangular input state should be tr"
		    "ansferred to rectangular.  The input coordinate system i"
		    "s not recognized, yet was not caught by an earlier check."
		    , (ftnlen)224);
	    sigerr_("SPICE(BUG1)", (ftnlen)11);
	    chkout_("XFMSTA", (ftnlen)6);
	    return 0;
	}

/*        Some DRD* routines are not error free. Be safe and check */
/*        FAILED to not use un-initialized JACOBI. */

	if (failed_()) {
	    chkout_("XFMSTA", (ftnlen)6);
	    return 0;
	}

/*        If the multiplication of the Jacobian and velocity can cause */
/*        overflow, signal an error. */

	for (i__ = 1; i__ <= 3; ++i__) {
	    for (j = 1; j <= 3; ++j) {
		sqtmp = sqrt((d__1 = jacobi[(i__1 = i__ + j * 3 - 4) < 9 && 0 
			<= i__1 ? i__1 : s_rnge("jacobi", i__1, "xfmsta_", (
			ftnlen)1054)], abs(d__1))) * sqrt((d__2 = istate[(
			i__2 = j + 2) < 6 && 0 <= i__2 ? i__2 : s_rnge("ista"
			"te", i__2, "xfmsta_", (ftnlen)1054)], abs(d__2)));
		if (sqtmp > toobig) {
		    setmsg_("The product of the Jacobian and velocity may ca"
			    "use numeric overflow.", (ftnlen)68);
		    sigerr_("SPICE(NUMERICOVERFLOW)", (ftnlen)22);
		    chkout_("XFMSTA", (ftnlen)6);
		    return 0;
		}
	    }
	}

/*        Transform the velocity into rectangular coordinates. */

	mxv_(jacobi, &istate[3], ivel);
    } else if (isys == 1) {

/*        If the input coordinate system is rectangular, the input */
/*        position does not need to be translated into rectangular. */

	vequ_(istate, ipos);
	vequ_(&istate[3], ivel);
    } else {
	setmsg_("This error should never occur. This is an ELSE statement. I"
		"f the input coordinate system is not rectangular, the IF sho"
		"uld be executed. If the input coordinate system is rectangul"
		"ar, the ELSE IF should be executed.", (ftnlen)214);
	sigerr_("SPICE(BUG2)", (ftnlen)11);
	chkout_("XFMSTA", (ftnlen)6);
	return 0;
    }

/*     Conversion from rectangular into the output coordinates */
/*     ---------------------------------------------------------------- */

/*     Convert to the output coordinate system. If the output */
/*     coordinate system is not rectangular, four calculations must */
/*     be made: */

/*       1)  Verify the position and velocity are not along the z-axis. */
/*           If the position and velocity are along the z-axis, the */
/*           velocity can still be converted even though the */
/*           Jacobian is not defined. If the position is along the */
/*           z-axis but the velocity is not, the velocity cannot be */
/*           converted to the output coordinate system. */

/*       2)  Calculate the Jacobian from rectangular to the output */
/*           coordinate system and verify the product of the Jacobian */
/*           and velocity will not cause numeric overflow. */

/*       3)  Transform the position to the output coordinate system. */

/*       4)  Transform the velocity to the output coordinates using */
/*           the Jacobian and the rectangular velocity IVEL. */

    if (osys != 1) {

/*        From rectangular for the case when the input position is along */
/*        the z-axis ... */

	if (abs(ipos[0]) + abs(ipos[1]) == 0.) {
	    if (abs(ivel[0]) + abs(ivel[1]) == 0.) {

/*              If the velocity is along the z-axis, then the velocity */
/*              can be computed in the output coordinate frame even */
/*              though the Jacobian is not defined. */

		if (osys == 2) {

/*                  ... to cylindrical */

		    vpack_(&c_b56, &c_b56, &ivel[2], &ostate[3]);
		    reccyl_(ipos, ostate, &ostate[1], &ostate[2]);
		} else if (osys == 3) {

/*                  ... to latitudinal */

		    vpack_(&ivel[2], &c_b56, &c_b56, &ostate[3]);
		    reclat_(ipos, ostate, &ostate[1], &ostate[2]);
		} else if (osys == 4) {

/*                  ... to spherical */

		    vpack_(&ivel[2], &c_b56, &c_b56, &ostate[3]);
		    recsph_(ipos, ostate, &ostate[1], &ostate[2]);
		} else if (osys == 5) {

/*                  ... to geodetic */

		    vpack_(&c_b56, &c_b56, &ivel[2], &ostate[3]);
		    recgeo_(ipos, radii, &f, ostate, &ostate[1], &ostate[2]);
		} else if (osys == 6) {

/*                  ... to planetographic */

		    vpack_(&c_b56, &c_b56, &ivel[2], &ostate[3]);
		    recpgr_(body, ipos, radii, &f, ostate, &ostate[1], &
			    ostate[2], body_len);
		} else {
		    setmsg_("This error should never occur. This is an inter"
			    "mediate step in which a position and velocity al"
			    "ong the z-axis are converted to a non-rectangula"
			    "r coordinate system from rectangular. The output"
			    " coordinate system is not recognized, yet was no"
			    "t caught by an earlier check.", (ftnlen)268);
		    sigerr_("SPICE(BUG3)", (ftnlen)11);
		    chkout_("XFMSTA", (ftnlen)6);
		    return 0;
		}

/*              The output state has been calculated for the special */
/*              case of the position and velocity existing along the */
/*              z-axis. */

		chkout_("XFMSTA", (ftnlen)6);
		return 0;
	    } else {

/*              The Jacobian is undefined and the velocity cannot be */
/*              converted since it is not along the z-axis. */
/*              Signal an error. */

		setmsg_("Invalid input state: z axis.", (ftnlen)28);
		sigerr_("SPICE(INVALIDSTATE)", (ftnlen)19);
		chkout_("XFMSTA", (ftnlen)6);
		return 0;
	    }
	}

/*        From rectangular for cases when the input position is not along */
/*        the z-axis ... */

	if (osys == 2) {

/*                  ... to cylindrical */

	    dcyldr_(ipos, &ipos[1], &ipos[2], jacobi);
	    reccyl_(ipos, ostate, &ostate[1], &ostate[2]);
	} else if (osys == 3) {

/*                  ... to latitudinal */

	    dlatdr_(ipos, &ipos[1], &ipos[2], jacobi);
	    reclat_(ipos, ostate, &ostate[1], &ostate[2]);
	} else if (osys == 4) {

/*                  ... to spherical */

	    dsphdr_(ipos, &ipos[1], &ipos[2], jacobi);
	    recsph_(ipos, ostate, &ostate[1], &ostate[2]);
	} else if (osys == 5) {

/*                  ... to geodetic */

	    dgeodr_(ipos, &ipos[1], &ipos[2], radii, &f, jacobi);
	    recgeo_(ipos, radii, &f, ostate, &ostate[1], &ostate[2]);
	} else if (osys == 6) {

/*                  ... to planetographic */

	    dpgrdr_(body, ipos, &ipos[1], &ipos[2], radii, &f, jacobi, 
		    body_len);
	    recpgr_(body, ipos, radii, &f, ostate, &ostate[1], &ostate[2], 
		    body_len);
	} else {
	    setmsg_("This error should never occur. This is an intermediate "
		    "step in which a state is converted to a non-rectangular "
		    "coordinate system from rectangular. The output coordinat"
		    "e system is not recognized, yet was not caught by an ear"
		    "lier check.", (ftnlen)234);
	    sigerr_("SPICE(BUG4)", (ftnlen)11);
	    chkout_("XFMSTA", (ftnlen)6);
	    return 0;
	}

/*        Many D*DR and REC* routines are not error free. Be safe and */
/*        check FAILED to not use un-initialized JACOBI. */

	if (failed_()) {
	    chkout_("XFMSTA", (ftnlen)6);
	    return 0;
	}

/*        If the multiplication of the Jacobian and velocity can cause */
/*        overflow, signal an error. */

	for (i__ = 1; i__ <= 3; ++i__) {
	    for (j = 1; j <= 3; ++j) {
		sqtmp = sqrt((d__1 = jacobi[(i__1 = i__ + j * 3 - 4) < 9 && 0 
			<= i__1 ? i__1 : s_rnge("jacobi", i__1, "xfmsta_", (
			ftnlen)1314)], abs(d__1))) * sqrt((d__2 = ivel[(i__2 =
			 j - 1) < 3 && 0 <= i__2 ? i__2 : s_rnge("ivel", i__2,
			 "xfmsta_", (ftnlen)1314)], abs(d__2)));
		if (sqtmp > toobig) {
		    setmsg_("The product of the Jacobian and velocity may ca"
			    "use numeric overflow.", (ftnlen)68);
		    sigerr_("SPICE(NUMERICOVERFLOW)", (ftnlen)22);
		    chkout_("XFMSTA", (ftnlen)6);
		    return 0;
		}
	    }
	}

/*        Calculate the velocity in the output coordinate system. */

	mxv_(jacobi, ivel, &ostate[3]);
    } else if (osys == 1) {

/*        If the output coordinate system is rectangular, the position */
/*        and velocity components of the output state are set equal to */
/*        the rectangular IPOS and IVEL, respectively, because the */
/*        components have already been converted to rectangular. */

	vequ_(ipos, ostate);
	vequ_(ivel, &ostate[3]);
    } else {
	setmsg_("This error should never occur. This is an ELSE statement. I"
		"f the output coordinate system is not rectangular, the IF sh"
		"ould be executed. If the output coordinate system is rectang"
		"ular, the ELSE IF should be executed.", (ftnlen)216);
	sigerr_("SPICE(BUG5)", (ftnlen)11);
	chkout_("XFMSTA", (ftnlen)6);
	return 0;
    }
    chkout_("XFMSTA", (ftnlen)6);
    return 0;
} /* xfmsta_ */
예제 #3
0
파일: zzedterm.c 프로젝트: haisamido/GMAT
/* $Procedure ZZEDTERM ( Ellipsoid terminator ) */
/* Subroutine */ int zzedterm_(char *type__, doublereal *a, doublereal *b, 
	doublereal *c__, doublereal *srcrad, doublereal *srcpos, integer *
	npts, doublereal *trmpts, ftnlen type_len)
{
    /* System generated locals */
    integer trmpts_dim2, i__1, i__2;
    doublereal d__1, d__2, d__3;

    /* Builtin functions */
    integer s_cmp(char *, char *, ftnlen, ftnlen);
    double asin(doublereal);
    integer s_rnge(char *, integer, char *, integer);
    double d_sign(doublereal *, doublereal *);

    /* Local variables */
    extern /* Subroutine */ int vadd_(doublereal *, doublereal *, doublereal *
	    );
    doublereal rmin, rmax;
    extern /* Subroutine */ int vscl_(doublereal *, doublereal *, doublereal *
	    );
    extern doublereal vdot_(doublereal *, doublereal *), vsep_(doublereal *, 
	    doublereal *);
    integer nitr;
    extern /* Subroutine */ int vsub_(doublereal *, doublereal *, doublereal *
	    ), vequ_(doublereal *, doublereal *);
    doublereal d__, e[3];
    integer i__;
    doublereal s, angle, v[3], x[3], delta, y[3], z__[3], inang;
    extern /* Subroutine */ int chkin_(char *, ftnlen), frame_(doublereal *, 
	    doublereal *, doublereal *);
    doublereal plane[4];
    extern /* Subroutine */ int ucase_(char *, char *, ftnlen, ftnlen), 
	    errch_(char *, char *, ftnlen, ftnlen), vpack_(doublereal *, 
	    doublereal *, doublereal *, doublereal *);
    doublereal theta;
    extern /* Subroutine */ int errdp_(char *, doublereal *, ftnlen);
    doublereal trans[9]	/* was [3][3] */, srcpt[3], vtemp[3];
    extern doublereal vnorm_(doublereal *), twopi_(void);
    extern /* Subroutine */ int ljust_(char *, char *, ftnlen, ftnlen), 
	    pl2nvc_(doublereal *, doublereal *, doublereal *);
    doublereal lambda;
    extern /* Subroutine */ int nvp2pl_(doublereal *, doublereal *, 
	    doublereal *);
    extern doublereal halfpi_(void);
    doublereal minang, minrad, maxang, maxrad;
    extern /* Subroutine */ int latrec_(doublereal *, doublereal *, 
	    doublereal *, doublereal *);
    doublereal angerr;
    logical umbral;
    extern doublereal touchd_(doublereal *);
    doublereal offset[3], prvdif;
    extern /* Subroutine */ int sigerr_(char *, ftnlen);
    doublereal outang, plcons, prvang;
    extern /* Subroutine */ int chkout_(char *, ftnlen), setmsg_(char *, 
	    ftnlen), errint_(char *, integer *, ftnlen);
    char loctyp[50];
    extern logical return_(void);
    extern /* Subroutine */ int vminus_(doublereal *, doublereal *);
    doublereal dir[3];
    extern /* Subroutine */ int mxv_(doublereal *, doublereal *, doublereal *)
	    ;
    doublereal vtx[3];

/* $ 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. */

/*     Compute a set of points on the umbral or penumbral terminator of */
/*     a specified ellipsoid, given a spherical light source. */

/* $ 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 */

/*     ELLIPSES */

/* $ Keywords */

/*     BODY */
/*     GEOMETRY */
/*     MATH */

/* $ Declarations */
/* $ Brief_I/O */

/*     Variable  I/O  Description */
/*     --------  ---  -------------------------------------------------- */
/*     TYPE       I   Terminator type. */
/*     A          I   Length of ellipsoid semi-axis lying on the x-axis. */
/*     B          I   Length of ellipsoid semi-axis lying on the y-axis. */
/*     C          I   Length of ellipsoid semi-axis lying on the z-axis. */
/*     SRCRAD     I   Radius of light source. */
/*     SRCPOS     I   Position of center of light source. */
/*     NPTS       I   Number of points in terminator point set. */
/*     TRMPTS     O   Terminator point set. */

/* $ Detailed_Input */

/*     TYPE           is a string indicating the type of terminator to */
/*                    compute:  umbral or penumbral.  The umbral */
/*                    terminator is the boundary of the portion of the */
/*                    ellipsoid surface in total shadow.  The penumbral */
/*                    terminator is the boundary of the portion of the */
/*                    surface that is completely illuminated.  Possible */
/*                    values of TYPE are */

/*                       'UMBRAL' */
/*                       'PENUMBRAL' */

/*                    Case and leading or trailing blanks in TYPE are */
/*                    not significant. */

/*     A, */
/*     B, */
/*     C              are the lengths of the semi-axes of a triaxial */
/*                    ellipsoid.  The ellipsoid is centered at the */
/*                    origin and oriented so that its axes lie on the */
/*                    x, y and z axes.  A, B, and C are the lengths of */
/*                    the semi-axes that point in the x, y, and z */
/*                    directions respectively. */

/*                    Length units associated with A, B, and C must */
/*                    match those associated with SRCRAD, SRCPOS, */
/*                    and the output TRMPTS. */

/*     SRCRAD         is the radius of the spherical light source. */

/*     SRCPOS         is the position of the center of the light source */
/*                    relative to the center of the ellipsoid. */

/*     NPTS           is the number of terminator points to compute. */


/* $ Detailed_Output */

/*     TRMPTS         is an array of points on the umbral or penumbral */
/*                    terminator of the ellipsoid, as specified by the */
/*                    input argument TYPE.  The Ith point is contained */
/*                    in the array elements */

/*                        TRMPTS(J,I),  J = 1, 2, 3 */

/*                    The terminator points are expressed in the */
/*                    body-fixed reference frame associated with the */
/*                    ellipsoid.  Units are those associated with */
/*                    the input axis lengths. */

/*                    Each terminator point is the point of tangency of */
/*                    a plane that is also tangent to the light source. */
/*                    These associated points of tangency on the light */
/*                    source have uniform distribution in longitude when */
/*                    expressed in a cylindrical coordinate system whose */
/*                    Z-axis is SRCPOS.  The magnitude of the separation */
/*                    in longitude between these tangency points on the */
/*                    light source is */

/*                       2*Pi / NPTS */

/*                    If the target is spherical, the terminator points */
/*                    also are uniformly distributed in longitude in the */
/*                    cylindrical system described above.  If the target */
/*                    is non-spherical, the longitude distribution of */
/*                    the points generally is not uniform. */

/* $ Parameters */

/*     None. */

/* $ Exceptions */

/*     1)  If the terminator type is not recognized, the error */
/*         SPICE(NOTSUPPORTED) is signaled. */

/*     2)  If the set size NPTS is not at least 1, the error */
/*         SPICE(INVALIDSIZE) is signaled. */

/*     3)  If any of the ellipsoid's semi-axis lengths is non-positive, */
/*         the error SPICE(INVALIDAXISLENGTH) is signaled. */

/*     4)  If the light source has non-positive radius, the error */
/*         SPICE(INVALIDRADIUS) is signaled. */

/*     5)  If the light source intersects the smallest sphere */
/*         centered at the origin and containing the ellipsoid, the */
/*         error SPICE(OBJECTSTOOCLOSE) is signaled. */

/* $ Files */

/*     None. */

/* $ Particulars */

/*     This routine models the boundaries of shadow regions on an */
/*     ellipsoid "illuminated" by a spherical light source.  Light rays */
/*     are assumed to travel along straight lines; refraction is not */
/*     modeled. */

/*     Points on the ellipsoid at which the entire cap of the light */
/*     source is visible are considered to be completely illuminated. */
/*     Points on the ellipsoid at which some portion (or all) of the cap */
/*     of the light source are blocked are considered to be in partial */
/*     (or total) shadow. */

/*     In this routine, we use the term "umbral terminator" to denote */
/*     the curve ususally called the "terminator":  this curve is the */
/*     boundary of the portion of the surface that lies in total shadow. */
/*     We use the term "penumbral terminator" to denote the boundary of */
/*     the completely illuminated portion of the surface. */

/*     In general, the terminator on an ellipsoid is a more complicated */
/*     curve than the limb (which is always an ellipse).  Aside from */
/*     various special cases, the terminator does not lie in a plane. */

/*     However, the condition for a point X on the ellipsoid to lie on */
/*     the terminator is simple:  a plane tangent to the ellipsoid at X */
/*     must also be tangent to the light source.  If this tangent plane */
/*     does not intersect the vector from the center of the ellipsoid to */
/*     the center of the light source, then X lies on the umbral */
/*     terminator; otherwise X lies on the penumbral terminator. */

/* $ Examples */

/*     See the SPICELIB routine EDTERM. */

/* $ Restrictions */

/*     This is a private SPICELIB routine.  User applications should not */
/*     call this routine. */

/* $ Literature_References */

/*     None. */

/* $ Author_and_Institution */

/*     N.J. Bachman    (JPL) */

/* $ Version */

/* -    SPICELIB Version 1.0.0, 03-FEB-2007 (NJB) */

/* -& */
/* $ Index_Entries */

/*     find terminator on ellipsoid */
/*     find umbral terminator on ellipsoid */
/*     find penumbral terminator on ellipsoid */

/* -& */

/*     SPICELIB functions */


/*     Local parameters */


/*     Local variables */


/*     Standard SPICELIB error handling. */

    /* Parameter adjustments */
    trmpts_dim2 = *npts;

    /* Function Body */
    if (return_()) {
	return 0;
    }
    chkin_("ZZEDTERM", (ftnlen)8);

/*     Check the terminator type. */

    ljust_(type__, loctyp, type_len, (ftnlen)50);
    ucase_(loctyp, loctyp, (ftnlen)50, (ftnlen)50);
    if (s_cmp(loctyp, "UMBRAL", (ftnlen)50, (ftnlen)6) == 0) {
	umbral = TRUE_;
    } else if (s_cmp(loctyp, "PENUMBRAL", (ftnlen)50, (ftnlen)9) == 0) {
	umbral = FALSE_;
    } else {
	setmsg_("Terminator type must be UMBRAL or PENUMBRAL but was actuall"
		"y #.", (ftnlen)63);
	errch_("#", type__, (ftnlen)1, type_len);
	sigerr_("SPICE(NOTSUPPORTED)", (ftnlen)19);
	chkout_("ZZEDTERM", (ftnlen)8);
	return 0;
    }

/*     Check the terminator set dimension. */

    if (*npts < 1) {
	setmsg_("Set must contain at least one point; NPTS  = #.", (ftnlen)47)
		;
	errint_("#", npts, (ftnlen)1);
	sigerr_("SPICE(INVALIDSIZE)", (ftnlen)18);
	chkout_("ZZEDTERM", (ftnlen)8);
	return 0;
    }

/*     The ellipsoid semi-axes must have positive length. */

    if (*a <= 0. || *b <= 0. || *c__ <= 0.) {
	setmsg_("Semi-axis lengths:  A = #, B = #, C = #. ", (ftnlen)41);
	errdp_("#", a, (ftnlen)1);
	errdp_("#", b, (ftnlen)1);
	errdp_("#", c__, (ftnlen)1);
	sigerr_("SPICE(INVALIDAXISLENGTH)", (ftnlen)24);
	chkout_("ZZEDTERM", (ftnlen)8);
	return 0;
    }

/*     Check the input light source radius. */

    if (*srcrad <= 0.) {
	setmsg_("Light source must have positive radius; actual radius was #."
		, (ftnlen)60);
	errdp_("#", srcrad, (ftnlen)1);
	sigerr_("SPICE(INVALIDRADIUS)", (ftnlen)20);
	chkout_("ZZEDTERM", (ftnlen)8);
	return 0;
    }

/*     The light source must not intersect the outer bounding */
/*     sphere of the ellipsoid. */

    d__ = vnorm_(srcpos);
/* Computing MAX */
    d__1 = max(*a,*b);
    rmax = max(d__1,*c__);
/* Computing MIN */
    d__1 = min(*a,*b);
    rmin = min(d__1,*c__);
    if (*srcrad + rmax >= d__) {

/*        The light source is too close. */

	setmsg_("Light source intersects outer bounding sphere of the ellips"
		"oid.  Light source radius = #; ellipsoid's longest axis = #;"
		" sum = #; distance between centers = #.", (ftnlen)158);
	errdp_("#", srcrad, (ftnlen)1);
	errdp_("#", &rmax, (ftnlen)1);
	d__1 = *srcrad + rmax;
	errdp_("#", &d__1, (ftnlen)1);
	errdp_("#", &d__, (ftnlen)1);
	sigerr_("SPICE(OBJECTSTOOCLOSE)", (ftnlen)22);
	chkout_("ZZEDTERM", (ftnlen)8);
	return 0;
    }

/*     Find bounds on the angular size of the target as seen */
/*     from the source. */

/* Computing MIN */
    d__1 = rmax / d__;
    minang = asin((min(d__1,1.)));
/* Computing MIN */
    d__1 = rmin / d__;
    maxang = asin((min(d__1,1.)));

/*     Let the inverse of the ellipsoid-light source vector be the */
/*     Z-axis of a frame we'll use to generate the terminator set. */

    vminus_(srcpos, z__);
    frame_(z__, x, y);

/*     Create the rotation matrix required to convert vectors */
/*     from the source-centered frame back to the target body-fixed */
/*     frame. */

    vequ_(x, trans);
    vequ_(y, &trans[3]);
    vequ_(z__, &trans[6]);

/*     Find the maximum and minimum target radii. */

/* Computing MAX */
    d__1 = max(*a,*b);
    maxrad = max(d__1,*c__);
/* Computing MIN */
    d__1 = min(*a,*b);
    minrad = min(d__1,*c__);
    if (umbral) {

/*        Compute the angular offsets from the axis of rays tangent to */
/*        both the source and the bounding spheres of the target, where */
/*        the tangency points lie in a half-plane bounded by the line */
/*        containing the origin and SRCPOS.  (We'll call this line */
/*        the "axis.") */

/*        OUTANG corresponds to the target's outer bounding sphere; */
/*        INANG to the inner bounding sphere. */

	outang = asin((*srcrad - maxrad) / d__);
	inang = asin((*srcrad - minrad) / d__);
    } else {

/*        Compute the angular offsets from the axis of rays tangent to */
/*        both the source and the bounding spheres of the target, where */
/*        the tangency points lie in opposite half-planes bounded by the */
/*        axis (compare the case above). */

/*        OUTANG corresponds to the target's outer bounding sphere; */
/*        INANG to the inner bounding sphere. */

	outang = asin((*srcrad + maxrad) / d__);
	inang = asin((*srcrad + minrad) / d__);
    }

/*     Compute the angular delta we'll use for generating */
/*     terminator points. */

    delta = twopi_() / *npts;

/*     Generate the terminator points. */

    i__1 = *npts;
    for (i__ = 1; i__ <= i__1; ++i__) {
	theta = (i__ - 1) * delta;

/*        Let SRCPT be the surface point on the source lying in */
/*        the X-Y plane of the frame produced by FRAME */
/*        and corresponding to the angle THETA. */

	latrec_(srcrad, &theta, &c_b30, srcpt);

/*        Now solve for the angle by which SRCPT must be rotated (toward */
/*        +Z in the umbral case, away from +Z in the penumbral case) */
/*        so that a plane tangent to the source at SRCPT is also tangent */
/*        to the target. The rotation is bracketed by OUTANG on the low */
/*        side and INANG on the high side in the umbral case; the */
/*        bracketing values are reversed in the penumbral case. */

	if (umbral) {
	    angle = outang;
	} else {
	    angle = inang;
	}
	prvdif = twopi_();
	prvang = angle + halfpi_();
	nitr = 0;
	for(;;) { /* while(complicated condition) */
	    d__2 = (d__1 = angle - prvang, abs(d__1));
	    if (!(nitr <= 10 && touchd_(&d__2) < prvdif))
	    	break;
	    ++nitr;
	    d__2 = (d__1 = angle - prvang, abs(d__1));
	    prvdif = touchd_(&d__2);
	    prvang = angle;

/*           Find the closest point on the ellipsoid to the plane */
/*           corresponding to "ANGLE". */

/*           The tangent point on the source is obtained by rotating */
/*           SRCPT by ANGLE towards +Z.  The plane's normal vector is */
/*           parallel to VTX in the source-centered frame. */

	    latrec_(srcrad, &theta, &angle, vtx);
	    vequ_(vtx, dir);

/*           VTX and DIR are expressed in the source-centered frame.  We */
/*           must translate VTX to the target frame and rotate both */
/*           vectors into that frame. */

	    mxv_(trans, vtx, vtemp);
	    vadd_(srcpos, vtemp, vtx);
	    mxv_(trans, dir, vtemp);
	    vequ_(vtemp, dir);

/*           Create the plane defined by VTX and DIR. */

	    nvp2pl_(dir, vtx, plane);

/*           Find the closest point on the ellipsoid to the plane. At */
/*           the point we seek, the outward normal on the ellipsoid is */
/*           parallel to the choice of plane normal that points away */
/*           from the origin.  We can always obtain this choice from */
/*           PL2NVC. */

	    pl2nvc_(plane, dir, &plcons);

/*           At the point */

/*               E = (x, y, z) */

/*           on the ellipsoid's surface, an outward normal */
/*           is */

/*               N = ( x/A**2, y/B**2, z/C**2 ) */

/*           which is also */

/*               lambda * ( DIR(1), DIR(2), DIR(3) ) */

/*           Equating components in the normal vectors yields */

/*               E = lambda * ( DIR(1)*A**2, DIR(2)*B**2, DIR(3)*C**2 ) */

/*           Taking the inner product with the point E itself and */
/*           applying the ellipsoid equation, we find */

/*               lambda * <DIR, E>  =  < N, E >  =  1 */

/*           The first term above is */

/*               lambda**2 * || ( A*DIR(1), B*DIR(2), C*DIR(3) ) ||**2 */

/*           So the positive root lambda is */

/*               1 / || ( A*DIR(1), B*DIR(2), C*DIR(3) ) || */

/*           Having lambda we can compute E. */

	    d__1 = *a * dir[0];
	    d__2 = *b * dir[1];
	    d__3 = *c__ * dir[2];
	    vpack_(&d__1, &d__2, &d__3, v);
	    lambda = 1. / vnorm_(v);
	    d__1 = *a * v[0];
	    d__2 = *b * v[1];
	    d__3 = *c__ * v[2];
	    vpack_(&d__1, &d__2, &d__3, e);
	    vscl_(&lambda, e, &trmpts[(i__2 = i__ * 3 - 3) < trmpts_dim2 * 3 
		    && 0 <= i__2 ? i__2 : s_rnge("trmpts", i__2, "zzedterm_", 
		    (ftnlen)586)]);

/*           Make a new estimate of the plane rotation required to touch */
/*           the target. */

	    vsub_(&trmpts[(i__2 = i__ * 3 - 3) < trmpts_dim2 * 3 && 0 <= i__2 
		    ? i__2 : s_rnge("trmpts", i__2, "zzedterm_", (ftnlen)592)]
		    , vtx, offset);

/*           Let ANGERR be an estimate of the magnitude of angular error */
/*           between the plane and the terminator. */

	    angerr = vsep_(dir, offset) - halfpi_();

/*           Let S indicate the sign of the altitude error:  where */
/*           S is positive, the plane is above E. */

	    d__1 = vdot_(e, dir);
	    s = d_sign(&c_b35, &d__1);
	    if (umbral) {

/*              If the plane is above the target, increase the */
/*              rotation angle; otherwise decrease the angle. */

		angle += s * angerr;
	    } else {

/*              This is the penumbral case; decreasing the angle */
/*              "lowers" the plane toward the target. */

		angle -= s * angerr;
	    }
	}
    }
    chkout_("ZZEDTERM", (ftnlen)8);
    return 0;
} /* zzedterm_ */