/* $Procedure STELAB ( Stellar Aberration ) */
/* Subroutine */ int stelab_(doublereal *pobj, doublereal *vobs, doublereal *
appobj)
{
/* Builtin functions */
double asin(doublereal);
/* Local variables */
extern /* Subroutine */ int vhat_(doublereal *, doublereal *);
doublereal vbyc[3];
extern /* Subroutine */ int vscl_(doublereal *, doublereal *, doublereal *
);
extern doublereal vdot_(doublereal *, doublereal *);
doublereal h__[3], u[3];
extern /* Subroutine */ int chkin_(char *, ftnlen), moved_(doublereal *,
integer *, doublereal *), errdp_(char *, doublereal *, ftnlen),
vcrss_(doublereal *, doublereal *, doublereal *);
extern doublereal vnorm_(doublereal *);
extern /* Subroutine */ int vrotv_(doublereal *, doublereal *, doublereal
*, doublereal *);
extern doublereal clight_(void);
doublereal onebyc, sinphi;
extern /* Subroutine */ int sigerr_(char *, ftnlen), chkout_(char *,
ftnlen), setmsg_(char *, ftnlen);
doublereal lensqr;
extern logical return_(void);
doublereal phi;
/* $ Abstract */
/* Correct the apparent position of an object for 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 */
/* None. */
/* $ Keywords */
/* EPHEMERIS */
/* $ Declarations */
/* $ Brief_I/O */
/* VARIABLE I/O DESCRIPTION */
/* -------- --- -------------------------------------------------- */
/* POBJ I Position of an object with respect to the */
/* observer. */
/* VOBS I Velocity of the observer with respect to the */
/* Solar System barycenter. */
/* APPOBJ O Apparent position of the object with respect to */
/* the observer, corrected for stellar aberration. */
/* $ Detailed_Input */
/* POBJ is the position (x, y, z, km) of an object with */
/* respect to the observer, possibly corrected for */
/* light time. */
/* VOBS is the velocity (dx/dt, dy/dt, dz/dt, km/sec) */
/* of the observer with respect to the Solar System */
/* barycenter. */
/* $ Detailed_Output */
/* APPOBJ is the apparent position of the object relative */
/* to the observer, corrected for stellar aberration. */
/* $ Parameters */
/* None. */
/* $ Exceptions */
/* 1) If the velocity of the observer is greater than or equal */
/* to the speed of light, the error SPICE(VALUEOUTOFRANGE) */
/* is signaled. */
/* $ Files */
/* None. */
/* $ Particulars */
/* Let r be the vector from the observer to the object, and v be */
/* - - */
/* the velocity of the observer with respect to the Solar System */
/* barycenter. Let w be the angle between them. The aberration */
/* angle phi is given by */
/* sin(phi) = v sin(w) / c */
/* Let h be the vector given by the cross product */
/* - */
/* h = r X v */
/* - - - */
/* Rotate r by phi radians about h to obtain the apparent position */
/* - - */
/* of the object. */
/* $ Examples */
/* In the following example, STELAB is used to correct the position */
/* of a target body for stellar aberration. */
/* (Previous subroutine calls have loaded the SPK file and */
/* the leapseconds kernel file.) */
/* C */
/* C Get the geometric state of the observer OBS relative to */
/* C the solar system barycenter. */
/* C */
/* CALL SPKSSB ( OBS, ET, 'J2000', SOBS ) */
/* C */
/* C Get the light-time corrected position TPOS of the target */
/* C body TARG as seen by the observer. Normally we would */
/* C call SPKPOS to obtain this vector, but we already have */
/* C the state of the observer relative to the solar system */
/* C barycenter, so we can avoid looking up that state twice */
/* C by calling SPKAPO. */
/* C */
/* CALL SPKAPO ( TARG, ET, 'J2000', SOBS, 'LT', TPOS, LT ) */
/* C */
/* C Apply the correction for stellar aberration to the */
/* C light-time corrected position of the target body. */
/* C The corrected position is returned in the argument */
/* C PCORR. */
/* C */
/* CALL STELAB ( TPOS, SOBS(4), PCORR ) */
/* Note that this example is somewhat contrived. The sequence */
/* of calls above could be replaced by a single call to SPKEZP, */
/* using the aberration correction flag 'LT+S'. */
/* For more information on aberration-corrected states or */
/* positions, see the headers of any of the routines */
/* SPKEZR */
/* SPKEZ */
/* SPKPOS */
/* SPKEZP */
/* $ Restrictions */
/* None. */
/* $ Literature_References */
/* 1) W.M. Owen, Jr., JPL IOM #314.8-524, "The Treatment of */
/* Aberration in Optical Navigation", 8 February 1985. */
/* $ Author_and_Institution */
/* N.J. Bachman (JPL) */
/* H.A. Neilan (JPL) */
/* W.L. Taber (JPL) */
/* I.M. Underwood (JPL) */
/* $ Version */
/* - SPICELIB Version 1.1.1, 8-JAN-2008 (NJB) */
/* The header example was updated to remove references */
/* to SPKAPP. */
/* - SPICELIB Version 1.1.0, 8-FEB-1999 (WLT) */
/* The example was corrected so that SOBS(4) is passed */
/* into STELAB instead of STARG(4). */
/* - SPICELIB Version 1.0.2, 10-MAR-1992 (WLT) */
/* Comment section for permuted index source lines was added */
/* following the header. */
/* - SPICELIB Version 1.0.1, 8-AUG-1990 (HAN) */
/* Examples section of the header was updated to replace */
/* calls to the GEF ephemeris readers by calls to the */
/* new SPK ephemeris reader. */
/* - SPICELIB Version 1.0.0, 31-JAN-1990 (IMU) (WLT) */
/* -& */
/* $ Index_Entries */
/* stellar aberration */
/* -& */
/* $ Revisions */
/* - Beta Version 2.1.0, 9-MAR-1989 (HAN) */
/* Declaration of the variable LIGHT was removed from the code. */
/* The variable was declared but never used. */
/* - Beta Version 2.0.0, 28-DEC-1988 (HAN) */
/* Error handling was added to check the velocity of the */
/* observer. If the velocity of the observer is greater */
/* than or equal to the speed of light, the error */
/* SPICE(VALUEOUTOFRANGE) is signalled. */
/* -& */
/* SPICELIB functions */
/* Local variables */
/* Standard SPICE error handling. */
if (return_()) {
return 0;
} else {
chkin_("STELAB", (ftnlen)6);
}
/* We are not going to compute the aberrated vector in exactly the */
/* way described in the particulars section. We can combine some */
/* steps and we take some precautions to prevent floating point */
/* overflows. */
/* Get a unit vector that points in the direction of the object */
/* ( u_obj ). */
vhat_(pobj, u);
/* Get the velocity vector scaled with respect to the speed of light */
/* ( v/c ). */
onebyc = 1. / clight_();
vscl_(&onebyc, vobs, vbyc);
/* If the square of the length of the velocity vector is greater than */
/* or equal to one, the speed of the observer is greater than or */
/* equal to the speed of light. The observer speed is definitely out */
/* of range. Signal an error and check out. */
lensqr = vdot_(vbyc, vbyc);
if (lensqr >= 1.) {
setmsg_("Velocity components of observer were: dx/dt = *, dy/dt = *"
", dz/dt = *.", (ftnlen)71);
errdp_("*", vobs, (ftnlen)1);
errdp_("*", &vobs[1], (ftnlen)1);
errdp_("*", &vobs[2], (ftnlen)1);
sigerr_("SPICE(VALUEOUTOFRANGE)", (ftnlen)22);
chkout_("STELAB", (ftnlen)6);
return 0;
}
/* Compute u_obj x (v/c) */
vcrss_(u, vbyc, h__);
/* If the magnitude of the vector H is zero, the observer is moving */
/* along the line of sight to the object, and no correction is */
/* required. Otherwise, rotate the position of the object by phi */
/* radians about H to obtain the apparent position. */
sinphi = vnorm_(h__);
if (sinphi != 0.) {
phi = asin(sinphi);
vrotv_(pobj, h__, &phi, appobj);
} else {
moved_(pobj, &c__3, appobj);
}
chkout_("STELAB", (ftnlen)6);
return 0;
} /* stelab_ */
/* $Procedure DVSEP ( Derivative of separation angle ) */
doublereal dvsep_(doublereal *s1, doublereal *s2)
{
/* System generated locals */
doublereal ret_val;
/* Local variables */
logical safe;
extern doublereal vdot_(doublereal *, doublereal *);
doublereal numr;
extern /* Subroutine */ int chkin_(char *, ftnlen);
doublereal denom;
extern /* Subroutine */ int dvhat_(doublereal *, doublereal *);
extern doublereal dpmax_(void);
extern /* Subroutine */ int vcrss_(doublereal *, doublereal *, doublereal
*);
extern doublereal vnorm_(doublereal *);
extern logical vzero_(doublereal *);
doublereal u1[6], u2[6];
extern /* Subroutine */ int sigerr_(char *, ftnlen), chkout_(char *,
ftnlen), setmsg_(char *, ftnlen);
doublereal pcross[3];
extern logical return_(void);
/* $ Abstract */
/* Calculate the time derivative of the separation angle between */
/* two input states, S1 and S2. */
/* $ 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 */
/* GEOMETRY */
/* DERIVATIVES */
/* $ Declarations */
/* $ Brief_I/O */
/* VARIABLE I/O DESCRIPTION */
/* -------- --- -------------------------------------------------- */
/* S1 I State vector of the first body. */
/* S2 I State vector of the second body. */
/* $ Detailed_Input */
/* S1 the state vector of the first target body as seen from */
/* the observer. */
/* S2 the state vector of the second target body as seen from */
/* the observer. */
/* An implicit assumption exists that both states lie in the same */
/* reference frame with the same observer for the same epoch. If this */
/* is not the case, the numerical result has no meaning. */
/* $ Detailed_Output */
/* The function returns the double precision value of the time */
/* derivative of the angular separation between S1 and S2. */
/* $ Parameters */
/* None. */
/* $ Exceptions */
/* 1) The error SPICE(NUMERICOVERFLOW) signals if the inputs S1, S2 */
/* define states with an angular separation rate ~ DPMAX(). */
/* 2) If called in RETURN mode, the return has value 0. */
/* 3) Linear dependent position components of S1 and S1 constitutes */
/* a non-error exception. The function returns 0 for this case. */
/* $ Files */
/* None. */
/* $ Particulars */
/* In this discussion, the notation */
/* < V1, V2 > */
/* indicates the dot product of vectors V1 and V2. The notation */
/* V1 x V2 */
/* indicates the cross product of vectors V1 and V2. */
/* To start out, note that we need consider only unit vectors, */
/* since the angular separation of any two non-zero vectors */
/* equals the angular separation of the corresponding unit vectors. */
/* Call these vectors U1 and U2; let their velocities be V1 and V2. */
/* For unit vectors having angular separation */
/* THETA */
/* the identity */
/* || U1 x U1 || = ||U1|| * ||U2|| * sin(THETA) (1) */
/* reduces to */
/* || U1 x U2 || = sin(THETA) (2) */
/* and the identity */
/* | < U1, U2 > | = || U1 || * || U2 || * cos(THETA) (3) */
/* reduces to */
/* | < U1, U2 > | = cos(THETA) (4) */
/* Since THETA is an angular separation, THETA is in the range */
/* 0 : Pi */
/* Then letting s be +1 if cos(THETA) > 0 and -1 if cos(THETA) < 0, */
/* we have for any value of THETA other than 0 or Pi */
/* 2 1/2 */
/* cos(THETA) = s * ( 1 - sin (THETA) ) (5) */
/* or */
/* 2 1/2 */
/* < U1, U2 > = s * ( 1 - sin (THETA) ) (6) */
/* At this point, for any value of THETA other than 0 or Pi, */
/* we can differentiate both sides with respect to time (T) */
/* to obtain */
/* 2 -1/2 */
/* < U1, V2 > + < V1, U2 > = s * (1/2)(1 - sin (THETA)) */
/* * (-2) sin(THETA)*cos(THETA) */
/* * d(THETA)/dT (7a) */
/* Using equation (5), and noting that s = 1/s, we can cancel */
/* the cosine terms on the right hand side */
/* -1 */
/* < U1, V2 > + < V1, U2 > = (1/2)(cos(THETA)) */
/* * (-2) sin(THETA)*cos(THETA) */
/* * d(THETA)/dT (7b) */
/* With (7b) reducing to */
/* < U1, V2 > + < V1, U2 > = - sin(THETA) * d(THETA)/dT (8) */
/* Using equation (2) and switching sides, we obtain */
/* || U1 x U2 || * d(THETA)/dT = - < U1, V2 > - < V1, U2 > (9) */
/* or, provided U1 and U2 are linearly independent, */
/* d(THETA)/dT = ( - < U1, V2 > - < V1, U2 > ) / ||U1 x U2|| (10) */
/* Note for times when U1 and U2 have angular separation 0 or Pi */
/* radians, the derivative of angular separation with respect to */
/* time doesn't exist. (Consider the graph of angular separation */
/* with respect to time; typically the graph is roughly v-shaped at */
/* the singular points.) */
/* $ Examples */
/* PROGRAM DVSEP_T */
/* IMPLICIT NONE */
/* DOUBLE PRECISION ET */
/* DOUBLE PRECISION LT */
/* DOUBLE PRECISION DSEPT */
/* DOUBLE PRECISION STATEE (6) */
/* DOUBLE PRECISION STATEM (6) */
/* INTEGER STRLEN */
/* PARAMETER ( STRLEN = 64 ) */
/* CHARACTER*(STRLEN) BEGSTR */
/* DOUBLE PRECISION DVSEP */
/* C */
/* C Load kernels. */
/* C */
/* CALL FURNSH ('standard.tm') */
/* C */
/* C An arbitrary time. */
/* C */
/* BEGSTR = 'JAN 1 2009' */
/* CALL STR2ET( BEGSTR, ET ) */
/* C */
/* C Calculate the state vectors sun to Moon, sun to earth at ET. */
/* C */
/* C */
/* CALL SPKEZR ( 'EARTH', ET, 'J2000', 'NONE', 'SUN', */
/* . STATEE, LT) */
/* CALL SPKEZR ( 'MOON', ET, 'J2000', 'NONE', 'SUN', */
/* . STATEM, LT) */
/* C */
/* C Calculate the time derivative of the angular separation of */
/* C the earth and Moon as seen from the sun at ET. */
/* C */
/* DSEPT = DVSEP( STATEE, STATEM ) */
/* WRITE(*,*) 'Time derivative of angular separation: ', DSEPT */
/* END */
/* The program compiled on OS X with g77 outputs (radians/sec): */
/* Time derivative of angular separation: 3.81211936E-09 */
/* $ Restrictions */
/* None. */
/* $ Literature_References */
/* None. */
/* $ Author_and_Institution */
/* E.D. Wright (JPL) */
/* N.J. Bachman (JPL) */
/* $ Version */
/* - SPICELIB Version 1.0.1, 15-MAR-2010 (EDW) */
/* Trivial header format clean-up. */
/* - SPICELIB Version 1.0.1, 31-MAR-2009 (EDW) */
/* -& */
/* $ Index_Entries */
/* time derivative of angular separation */
/* -& */
/* SPICELIB functions */
/* Local variables */
if (return_()) {
ret_val = 0.;
return ret_val;
}
chkin_("DVSEP", (ftnlen)5);
/* Compute the unit vectors and corresponding time derivatives */
/* for the input state vectors. */
dvhat_(s1, u1);
dvhat_(s2, u2);
/* Calculate the cross product vector of U1 and U2. As both vectors */
/* have magnitude one, the magnitude of the cross product equals */
/* sin(THETA), with THETA the angle between S1 and S2. */
vcrss_(u1, u2, pcross);
/* Now calculate the time derivate of the angular separation between */
/* S1 and S2. */
/* The routine needs to guard against both division by zero */
/* and numeric overflow. Before carrying out the division */
/* indicated by equation (10), the routine should verify that */
/* || U1 x U2 || > fudge factor * | numerator | / DPMAX() */
/* A fudge factor of 10.D0 should suffice. */
/* Note that the inequality is strict. */
/* Handle the parallel and anti-parallel cases. */
if (vzero_(pcross)) {
ret_val = 0.;
chkout_("DVSEP", (ftnlen)5);
return ret_val;
}
/* Now check for possible overflow. */
numr = vdot_(u1, &u2[3]) + vdot_(&u1[3], u2);
denom = vnorm_(pcross);
safe = denom > abs(numr) * 10. / dpmax_();
if (! safe) {
ret_val = 0.;
setmsg_("Numerical overflow event.", (ftnlen)25);
sigerr_("SPICE(NUMERICOVERFLOW)", (ftnlen)22);
chkout_("DVSEP", (ftnlen)5);
return ret_val;
}
ret_val = -numr / denom;
chkout_("DVSEP", (ftnlen)5);
return ret_val;
} /* dvsep_ */
/* $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 PCKE03 ( PCK, evaluate data record from type 3 segment ) */
/* Subroutine */ int pcke03_(doublereal *et, doublereal *record, doublereal *
rotmat)
{
/* System generated locals */
integer i__1, i__2;
/* Builtin functions */
integer s_rnge(char *, integer, char *, integer);
/* Local variables */
extern /* Subroutine */ int eul2m_(doublereal *, doublereal *, doublereal
*, integer *, integer *, integer *, doublereal *);
integer i__, j;
extern /* Subroutine */ int chkin_(char *, ftnlen), vcrss_(doublereal *,
doublereal *, doublereal *);
integer degree;
extern /* Subroutine */ int chbval_(doublereal *, integer *, doublereal *,
doublereal *, doublereal *);
integer ncoeff;
extern doublereal halfpi_(void);
integer cofloc;
doublereal eulang[6];
extern /* Subroutine */ int chkout_(char *, ftnlen);
doublereal drotdt[9] /* was [3][3] */;
extern logical return_(void);
doublereal mav[3];
extern doublereal rpd_(void);
doublereal rot[9] /* was [3][3] */;
/* $ Abstract */
/* Evaluate a single PCK data record from a segment of type 03 */
/* (Variable width Chebyshev Polynomials for RA, DEC, and W) to */
/* obtain a state transformation matrix. */
/* $ 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 SOFTsWARE 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 */
/* PCK */
/* $ Keywords */
/* PCK */
/* $ Declarations */
/* $ Brief_I/O */
/* Variable I/O Description */
/* -------- --- -------------------------------------------------- */
/* ET I Target epoch state transformation. */
/* RECORD I Data record valid for epoch ET. */
/* ROTMAT O State transformation matrix at epoch ET. */
/* $ Detailed_Input */
/* ET is a target epoch, at which a state transformation */
/* matrix is to be calculated. */
/* RECORD is a data record which, when evaluated at epoch ET, */
/* will give RA, DEC, and W and angular velocity */
/* for a body. The RA, DEC and W are relative to */
/* some inertial frame. The angular velocity is */
/* expressed relative to the body fixed coordinate frame. */
/* $ Detailed_Output */
/* ROTMAT is the state transformation matrix at epoch ET. */
/* $ Parameters */
/* None. */
/* $ Exceptions */
/* None. */
/* $ Files */
/* None. */
/* $ Particulars */
/* The exact format and structure of type 03 PCK segments are */
/* described in the PCK Required Reading file. */
/* A type 03 segment contains six sets of Chebyshev coefficients, */
/* one set each for RA, DEC, and W and one set each for the */
/* components of the angular velocity of the body. The coefficients */
/* for RA, DEC, and W are relative to some inertial reference */
/* frame. The coefficients for the components of angular velocity */
/* are relative to the body fixed frame and must be transformed */
/* via the position transformation corresponding to RA, DEC and W. */
/* PCKE03 calls the routine CHBVAL to evalute each polynomial, */
/* to obtain a complete set of values. These values are then */
/* used to determine a state transformation matrix that will */
/* rotate an inertially referenced state into the bodyfixed */
/* coordinate system. */
/* $ Examples */
/* The PCKEnn routines are almost always used in conjunction with */
/* the corresponding PCKRnn routines, which read the records from */
/* binary PCK files. */
/* The data returned by the PCKRnn routine is in its rawest form, */
/* taken directly from the segment. As such, it will be meaningless */
/* to a user unless he/she understands the structure of the data type */
/* completely. Given that understanding, however, the PCKRnn */
/* routines might be used to examine raw segment data before */
/* evaluating it with the PCKEnn routines. */
/* C */
/* C Get a segment applicable to a specified body and epoch. */
/* C */
/* CALL PCKSFS ( BODY, ET, HANDLE, DESCR, IDENT, FOUND ) */
/* C */
/* C Look at parts of the descriptor. */
/* C */
/* CALL DAFUS ( DESCR, 2, 6, DCD, ICD ) */
/* TYPE = ICD( 3 ) */
/* IF ( TYPE .EQ. 03 ) THEN */
/* CALL PCKR03 ( HANDLE, DESCR, ET, RECORD ) */
/* . */
/* . Look at the RECORD data. */
/* . */
/* CALL PCKE03 ( ET, RECORD, ROTMAT ) */
/* . */
/* . Apply the rotation and check out the state. */
/* . */
/* END IF */
/* $ Restrictions */
/* None. */
/* $ Literature_References */
/* None. */
/* $ Author_and_Institution */
/* K.R. Gehringer (JPL) */
/* W.L. Taber (JPL) */
/* $ Version */
/* - SPICELIB Version 3.0.1, 03-JAN-2014 (EDW) */
/* Minor edits to Procedure; clean trailing whitespace. */
/* Removed unneeded Revisions section. */
/* - SPICELIB Version 3.0.0, 6-OCT-1995 (WLT) */
/* Brian Carcich at Cornell discovered that the Euler */
/* angles were being re-arranged unnecessarily. As a */
/* result the state transformation matrix computed was */
/* not the one we expected. (The re-arrangement was */
/* a left-over from implementation 1.0.0. This problem */
/* has now been corrected. */
/* - SPICELIB Version 2.0.0, 28-JUL-1995 (WLT) */
/* Version 1.0.0 was written under the assumption that */
/* RA, DEC, W and dRA/dt, dDEC/dt and dW/dt were supplied */
/* in the input RECORD. This version repairs the */
/* previous misinterpretation. */
/* - SPICELIB Version 1.0.0, 14-MAR-1995 (KRG) */
/* -& */
/* $ Index_Entries */
/* evaluate type_03 pck segment */
/* -& */
/* SPICELIB Functions */
/* Local variables */
/* Standard SPICE error handling. */
if (return_()) {
return 0;
} else {
chkin_("PCKE03", (ftnlen)6);
}
/* The first number in the record is the number of Chebyshev */
/* Polynomial coefficients used to represent each component of the */
/* state vector. Following it are two numbers that will be used */
/* later, then the six sets of coefficients. */
ncoeff = (integer) record[0];
/* The degree of each polynomial is one less than the number of */
/* coefficients. */
degree = ncoeff - 1;
/* Call CHBVAL once for each quantity to obtain RA, DEC, and W values */
/* as well as values for the angular velocity. */
/* Note that we stick the angular velocity in the components 4 thru 6 */
/* of the array EULANG even though they are not derivatives of */
/* components 1 thru 3. It's just simpler to do it this way. */
/* Editorial Comment: */
/* Unlike every other SPICE routine, the units for the type 03 */
/* PCK segment are degrees. This inconsistency exists solely */
/* to support the NEAR project and the intransigence of one of the */
/* participants of that project. */
/* It's a bad design and we know it. */
/* ---W.L. Taber */
for (i__ = 1; i__ <= 6; ++i__) {
/* The coefficients for each variable are located contiguously, */
/* following the first three words in the record. */
cofloc = ncoeff * (i__ - 1) + 4;
/* CHBVAL needs as input the coefficients, the degree of the */
/* polynomial, the epoch, and also two variable transformation */
/* parameters, which are located, in our case, in the second and */
/* third slots of the record. */
chbval_(&record[cofloc - 1], °ree, &record[1], et, &eulang[(i__1 =
i__ - 1) < 6 && 0 <= i__1 ? i__1 : s_rnge("eulang", i__1,
"pcke03_", (ftnlen)262)]);
/* Convert to radians. */
eulang[(i__1 = i__ - 1) < 6 && 0 <= i__1 ? i__1 : s_rnge("eulang",
i__1, "pcke03_", (ftnlen)267)] = rpd_() * eulang[(i__2 = i__
- 1) < 6 && 0 <= i__2 ? i__2 : s_rnge("eulang", i__2, "pcke0"
"3_", (ftnlen)267)];
}
/* EULANG(1) is RA make it PHI */
/* EULANG(2) is DEC make it DELTA */
/* EULANG(3) is W */
eulang[0] = halfpi_() + eulang[0];
eulang[1] = halfpi_() - eulang[1];
/* Before we obtain the state transformation matrix, we need to */
/* compute the rotation components of the transformation.. */
/* The rotation we want to perform is: */
/* [W] [DELTA] [PHI] */
/* 3 1 3 */
/* The array of Euler angles is now: */
/* EULANG(1) = PHI */
/* EULANG(2) = DELTA */
/* EULANG(3) = W */
/* EULANG(4) = AV_1 (bodyfixed) */
/* EULANG(5) = AV_2 (bodyfixed) */
/* EULANG(6) = AV_3 (bodyfixed) */
/* Compute the rotation associated with the Euler angles. */
eul2m_(&eulang[2], &eulang[1], eulang, &c__3, &c__1, &c__3, rot);
/* This rotation transforms positions relative to the inertial */
/* frame to positions relative to the bodyfixed frame. */
/* We next need to get dROT/dt. */
/* For this discussion let P be the bodyfixed coordinates of */
/* a point that is fixed with respect to the bodyfixed frame. */
/* The velocity of P with respect to the inertial frame is */
/* given by */
/* t t */
/* V = ROT ( AV ) x ROT ( P ) */
/* t */
/* dROT */
/* = ---- ( P ) */
/* dt */
/* But */
/* t t t */
/* ROT ( AV ) x ROT ( P ) = ROT ( AV x P ) */
/* Let OMEGA be the cross product matrix corresponding to AV. */
/* Then */
/* t t */
/* ROT ( AV x P ) = ROT * OMEGA * P */
/* where * denotes matrix multiplication. */
/* From these observations it follows that */
/* t */
/* t dROT */
/* ROT * OMEGA * P = ---- * P */
/* dt */
/* Consequently, it follows that */
/* dROT t */
/* ---- = OMEGA * ROT */
/* dt */
/* = -OMEGA * ROT */
/* We compute dROT/dt now. Note that we can get the columns */
/* of -OMEGA*ROT by computing the cross products -AV x COL */
/* for each column COL of ROT. */
mav[0] = -eulang[3];
mav[1] = -eulang[4];
mav[2] = -eulang[5];
vcrss_(mav, rot, drotdt);
vcrss_(mav, &rot[3], &drotdt[3]);
vcrss_(mav, &rot[6], &drotdt[6]);
/* Now we simply fill in the blanks. */
for (i__ = 1; i__ <= 3; ++i__) {
for (j = 1; j <= 3; ++j) {
rotmat[(i__1 = i__ + j * 6 - 7) < 36 && 0 <= i__1 ? i__1 : s_rnge(
"rotmat", i__1, "pcke03_", (ftnlen)362)] = rot[(i__2 =
i__ + j * 3 - 4) < 9 && 0 <= i__2 ? i__2 : s_rnge("rot",
i__2, "pcke03_", (ftnlen)362)];
rotmat[(i__1 = i__ + 3 + j * 6 - 7) < 36 && 0 <= i__1 ? i__1 :
s_rnge("rotmat", i__1, "pcke03_", (ftnlen)363)] = drotdt[(
i__2 = i__ + j * 3 - 4) < 9 && 0 <= i__2 ? i__2 : s_rnge(
"drotdt", i__2, "pcke03_", (ftnlen)363)];
rotmat[(i__1 = i__ + (j + 3) * 6 - 7) < 36 && 0 <= i__1 ? i__1 :
s_rnge("rotmat", i__1, "pcke03_", (ftnlen)364)] = 0.;
rotmat[(i__1 = i__ + 3 + (j + 3) * 6 - 7) < 36 && 0 <= i__1 ?
i__1 : s_rnge("rotmat", i__1, "pcke03_", (ftnlen)365)] =
rot[(i__2 = i__ + j * 3 - 4) < 9 && 0 <= i__2 ? i__2 :
s_rnge("rot", i__2, "pcke03_", (ftnlen)365)];
}
}
chkout_("PCKE03", (ftnlen)6);
return 0;
} /* pcke03_ */
/* $Procedure VROTV ( Vector rotation about an axis ) */
/* Subroutine */ int vrotv_(doublereal *v, doublereal *axis, doublereal *
theta, doublereal *r__)
{
/* Builtin functions */
double cos(doublereal), sin(doublereal);
/* Local variables */
extern /* Subroutine */ int vadd_(doublereal *, doublereal *, doublereal *
), vhat_(doublereal *, doublereal *), vsub_(doublereal *,
doublereal *, doublereal *);
doublereal c__, p[3], s, x[3];
extern /* Subroutine */ int moved_(doublereal *, integer *, doublereal *),
vlcom_(doublereal *, doublereal *, doublereal *, doublereal *,
doublereal *), vproj_(doublereal *, doublereal *, doublereal *);
extern doublereal vnorm_(doublereal *);
extern /* Subroutine */ int vcrss_(doublereal *, doublereal *, doublereal
*);
doublereal v1[3], v2[3], rplane[3];
/* $ Abstract */
/* Rotate a vector about a specified axis vector by a specified */
/* angle and return the rotated vector. */
/* $ 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 */
/* ROTATION, VECTOR */
/* $ Declarations */
/* $ Brief_I/O */
/* VARIABLE I/O DESCRIPTION */
/* -------- --- -------------------------------------------------- */
/* V I Vector to be rotated. */
/* AXIS I Axis of the rotation. */
/* THETA I Angle of rotation (radians). */
/* R O Result of rotating V about AXIS by THETA. */
/* $ Detailed_Input */
/* V is a 3-dimensional vector to be rotated. */
/* AXIS is the axis about which the rotation is to be */
/* performed. */
/* THETA is the angle through which V is to be rotated about */
/* AXIS. */
/* $ Detailed_Output */
/* R is the result of rotating V about AXIS by THETA. */
/* If AXIS is the zero vector, R = V. */
/* $ Parameters */
/* None. */
/* $ Exceptions */
/* Error free. */
/* 1) If the input axis is the zero vector R will be returned */
/* as V. */
/* $ Files */
/* None. */
/* $ Particulars */
/* This routine computes the result of rotating (in a right handed */
/* sense) the vector V about the axis represented by AXIS through */
/* an angle of THETA radians. */
/* If W is a unit vector parallel to AXIS, then R is given by: */
/* R = V + ( 1 - cos(THETA) ) Wx(WxV) + sin(THETA) (WxV) */
/* where "x" above denotes the vector cross product. */
/* $ Examples */
/* If AXIS = ( 0, 0, 1 ) and THETA = PI/2 then the following results */
/* for R will be obtained */
/* V R */
/* ------------- ---------------- */
/* ( 1, 2, 3 ) ( -2, 1, 3 ) */
/* ( 1, 0, 0 ) ( 0, 1, 0 ) */
/* ( 0, 1, 0 ) ( -1, 0, 0 ) */
/* If AXIS = ( 0, 1, 0 ) and THETA = PI/2 then the following results */
/* for R will be obtained */
/* V R */
/* ------------- ---------------- */
/* ( 1, 2, 3 ) ( 3, 2, -1 ) */
/* ( 1, 0, 0 ) ( 0, 0, -1 ) */
/* ( 0, 1, 0 ) ( 0, 1, 0 ) */
/* If AXIS = ( 1, 1, 1 ) and THETA = PI/2 then the following results */
/* for R will be obtained */
/* V R */
/* ----------------------------- ----------------------------- */
/* ( 1.0, 2.0, 3.0 ) ( 2.577.., 0.845.., 2.577.. ) */
/* ( 2.577.., 0.845.., 2.577.. ) ( 3.0 2.0, 1.0 ) */
/* ( 3.0 2.0, 1.0 ) ( 1.422.., 3.154.., 1.422.. ) */
/* ( 1.422.., 3.154.., 1.422.. ) ( 1.0 2.0, 3.0 ) */
/* $ Restrictions */
/* None. */
/* $ Author_and_Institution */
/* N.J. Bachman (JPL) */
/* H.A. Neilan (JPL) */
/* W.L. Taber (JPL) */
/* $ Literature_References */
/* None. */
/* $ Version */
/* - SPICELIB Version 1.0.2, 5-FEB-2003 (NJB) */
/* Header examples were corrected. Exceptions section */
/* filled in. Miscellaneous header corrections were made. */
/* - SPICELIB Version 1.0.1, 10-MAR-1992 (WLT) */
/* Comment section for permuted index source lines was added */
/* following the header. */
/* - SPICELIB Version 1.0.0, 31-JAN-1990 (WLT) */
/* -& */
/* $ Index_Entries */
/* vector rotation about an axis */
/* -& */
/* $ Revisions */
/* - Beta Version 1.1.0, 17-FEB-1989 (HAN) (NJB) */
/* Contents of the Exceptions section was changed */
/* to "error free" to reflect the decision that the */
/* module will never participate in error handling. */
/* Also, the declarations of the unused variable I and the */
/* unused function VDOT were removed. */
/* -& */
/* SPICELIB functions */
/* Local Variables */
/* Just in case the user tries to rotate about the zero vector - */
/* check, and if so return the input vector */
if (vnorm_(axis) == 0.) {
moved_(v, &c__3, r__);
return 0;
}
/* Compute the unit vector that lies in the direction of the */
/* AXIS. Call it X. */
vhat_(axis, x);
/* Compute the projection of V onto AXIS. Call it P. */
vproj_(v, x, p);
/* Compute the component of V orthogonal to the AXIS. Call it V1. */
vsub_(v, p, v1);
/* Rotate V1 by 90 degrees about the AXIS and call the result V2. */
vcrss_(x, v1, v2);
/* Compute COS(THETA)*V1 + SIN(THETA)*V2. This is V1 rotated about */
/* the AXIS in the plane normal to the axis, call the result RPLANE */
c__ = cos(*theta);
s = sin(*theta);
vlcom_(&c__, v1, &s, v2, rplane);
/* Add the rotated component in the normal plane to AXIS to the */
/* projection of V onto AXIS (P) to obtain R. */
vadd_(rplane, p, r__);
return 0;
} /* vrotv_ */
/* $Procedure ZZHULLAX ( Pyramidal FOV convex hull to FOV axis ) */
/* Subroutine */ int zzhullax_(char *inst, integer *n, doublereal *bounds,
doublereal *axis, ftnlen inst_len)
{
/* System generated locals */
integer bounds_dim2, i__1, i__2;
doublereal d__1;
/* Builtin functions */
integer s_rnge(char *, integer, char *, integer);
/* Local variables */
extern /* Subroutine */ int vhat_(doublereal *, doublereal *);
doublereal xvec[3], yvec[3], zvec[3];
integer xidx;
extern doublereal vsep_(doublereal *, doublereal *);
integer next;
logical pass1;
integer i__, m;
doublereal r__, v[3], delta;
extern /* Subroutine */ int chkin_(char *, ftnlen), errch_(char *, char *,
ftnlen, ftnlen);
logical found;
extern /* Subroutine */ int errdp_(char *, doublereal *, ftnlen), vlcom_(
doublereal *, doublereal *, doublereal *, doublereal *,
doublereal *);
integer minix, maxix;
doublereal trans[9] /* was [3][3] */;
extern /* Subroutine */ int ucrss_(doublereal *, doublereal *, doublereal
*), vcrss_(doublereal *, doublereal *, doublereal *);
extern logical vzero_(doublereal *);
extern /* Subroutine */ int vrotv_(doublereal *, doublereal *, doublereal
*, doublereal *);
doublereal cp[3];
extern doublereal pi_(void);
logical ok;
extern doublereal halfpi_(void);
extern /* Subroutine */ int reclat_(doublereal *, doublereal *,
doublereal *, doublereal *), sigerr_(char *, ftnlen);
doublereal minlon;
extern /* Subroutine */ int chkout_(char *, ftnlen);
doublereal maxlon;
extern /* Subroutine */ int vhatip_(doublereal *), vsclip_(doublereal *,
doublereal *), setmsg_(char *, ftnlen), errint_(char *, integer *,
ftnlen);
extern logical return_(void);
doublereal lat, sep, lon;
extern /* Subroutine */ int mxv_(doublereal *, doublereal *, doublereal *)
;
doublereal ray1[3], ray2[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. */
/* Identify a face of the convex hull of an instrument's */
/* polygonal FOV, and use this face to generate an axis of the */
/* FOV. */
/* $ 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 */
/* CK */
/* FRAMES */
/* GF */
/* IK */
/* KERNEL */
/* $ Keywords */
/* FOV */
/* GEOMETRY */
/* INSTRUMENT */
/* $ Declarations */
/* $ Brief_I/O */
/* VARIABLE I/O DESCRIPTION */
/* -------- --- -------------------------------------------------- */
/* MARGIN P Minimum complement of FOV cone angle. */
/* INST I Instrument name. */
/* N I Number of FOV boundary vectors. */
/* BOUNDS I FOV boundary vectors. */
/* AXIS O Instrument FOV axis vector. */
/* $ Detailed_Input */
/* INST is the name of an instrument with which the field of */
/* view (FOV) of interest is associated. This name is */
/* used only to generate long error messages. */
/* N is the number of boundary vectors in the array */
/* BOUNDS. */
/* BOUNDS is an array of N vectors emanating from a common */
/* vertex and defining the edges of a pyramidal region in */
/* three-dimensional space: this the region within the */
/* FOV of the instrument designated by INST. The Ith */
/* vector of BOUNDS resides in elements (1:3,I) of this */
/* array. */
/* The vectors contained in BOUNDS are called the */
/* "boundary vectors" of the FOV. */
/* The boundary vectors must satisfy the constraints: */
/* 1) The boundary vectors must be contained within */
/* a right circular cone of angular radius less */
/* than than (pi/2) - MARGIN radians; in other */
/* words, there must be a vector A such that all */
/* boundary vectors have angular separation from */
/* A of less than (pi/2)-MARGIN radians. */
/* 2) There must be a pair of vectors U, V in BOUNDS */
/* such that all other boundary vectors lie in */
/* the same half space bounded by the plane */
/* containing U and V. Furthermore, all other */
/* boundary vectors must have orthogonal */
/* projections onto a plane normal to this plane */
/* such that the projections have angular */
/* separation of at least 2*MARGIN radians from */
/* the plane spanned by U and V. */
/* Given the first constraint above, there is plane PL */
/* such that each of the set of rays extending the */
/* boundary vectors intersects PL. (In fact, there is an */
/* infinite set of such planes.) The boundary vectors */
/* must be ordered so that the set of line segments */
/* connecting the intercept on PL of the ray extending */
/* the Ith vector to that of the (I+1)st, with the Nth */
/* intercept connected to the first, form a polygon (the */
/* "FOV polygon") constituting the intersection of the */
/* FOV pyramid with PL. This polygon may wrap in either */
/* the positive or negative sense about a ray emanating */
/* from the FOV vertex and passing through the plane */
/* region bounded by the FOV polygon. */
/* The FOV polygon need not be convex; it may be */
/* self-intersecting as well. */
/* No pair of consecutive vectors in BOUNDS may be */
/* linearly dependent. */
/* The boundary vectors need not have unit length. */
/* $ Detailed_Output */
/* AXIS is a unit vector normal to a plane containing the */
/* FOV polygon. All boundary vectors have angular */
/* separation from AXIS of not more than */
/* ( pi/2 ) - MARGIN */
/* radians. */
/* This routine signals an error if it cannot find */
/* a satisfactory value of AXIS. */
/* $ Parameters */
/* MARGIN is a small positive number used to constrain the */
/* orientation of the boundary vectors. See the two */
/* constraints described in the Detailed_Input section */
/* above for specifics. */
/* $ Exceptions */
/* 1) In the input vector count N is not at least 3, the error */
/* SPICE(INVALIDCOUNT) is signaled. */
/* 2) If any pair of consecutive boundary vectors has cross */
/* product zero, the error SPICE(DEGENERATECASE) is signaled. */
/* For this test, the first vector is considered the successor */
/* of the Nth. */
/* 3) If this routine can't find a face of the convex hull of */
/* the set of boundary vectors such that this face satisfies */
/* constraint (2) of the Detailed_Input section above, the */
/* error SPICE(FACENOTFOUND) is signaled. */
/* 4) If any boundary vectors have longitude too close to 0 */
/* or too close to pi radians in the face frame (see discussion */
/* of the search algorithm's steps 3 and 4 in Particulars */
/* below), the respective errors SPICE(NOTSUPPORTED) or */
/* SPICE(FOVTOOWIDE) are signaled. */
/* 5) If any boundary vectors have angular separation of more than */
/* (pi/2)-MARGIN radians from the candidate FOV axis, the */
/* error SPICE(FOVTOOWIDE) is signaled. */
/* $ Files */
/* The boundary vectors input to this routine are typically */
/* obtained from an IK file. */
/* $ Particulars */
/* Normally implementation is not discussed in SPICE headers, but we */
/* make an exception here because this routine's implementation and */
/* specification are deeply intertwined. */
/* This routine produces an "axis" for a polygonal FOV using the */
/* following approach: */
/* 1) Test pairs of consecutive FOV boundary vectors to see */
/* whether there's a pair such that the plane region bounded */
/* by these vectors is */
/* a) part of the convex hull of the set of boundary vectors */
/* b) such that all other boundary vectors have angular */
/* separation of at least MARGIN from the plane */
/* containing these vectors */
/* This search has O(N**2) run time dependency on N. */
/* If this test produces a candidate face of the convex hull, */
/* proceed to step 3. */
/* 2) If step (1) fails, repeat the search for a candidate */
/* convex hull face, but this time search over every pair of */
/* distinct boundary vectors. */
/* This search has O(N**3) run time dependency on N. */
/* If this search fails, signal an error. */
/* 3) Produce a set of basis vectors for a reference frame, */
/* which we'll call the "face frame," using as the +X axis */
/* the angle bisector of the vectors bounding the candidate */
/* face, the +Y axis the inward normal vector to this face, */
/* and the +Z axis completing a right-handed basis. */
/* 4) Transform each boundary vector, other than the two vectors */
/* defining the selected convex hull face, to the face frame */
/* and compute the vector's longitude in that frame. Find the */
/* maximum and minimum longitudes of the vectors in the face */
/* frame. */
/* If any vector's longitude is less than 2*MARGIN or greater */
/* than pi - 2*MARGIN radians, signal an error. */
/* 5) Let DELTA be the difference between pi and the maximum */
/* longitude found in step (4). Rotate the +Y axis (which */
/* points in the inward normal direction relative to the */
/* selected face) by -DELTA/2 radians about the +Z axis of */
/* the face frame. This rotation aligns the +Y axis with the */
/* central longitude of the set of boundary vectors. The */
/* resulting vector is our candidate FOV axis. */
/* 6) Check the angular separation of the candidate FOV axis */
/* against each boundary vector. If any vector has angular */
/* separation of more than (pi/2)-MARGIN radians from the */
/* axis, signal an error. */
/* Note that there are reasonable FOVs that cannot be handled by the */
/* algorithm described here. For example, any FOV whose cross */
/* section is a regular convex polygon can be made unusable by */
/* adding boundary vectors aligned with the angle bisectors of each */
/* face of the pyramid defined by the FOV's boundary vectors. The */
/* resulting set of boundary vectors has no face in its convex hull */
/* such that all other boundary vectors have positive angular */
/* separation from that face. */
/* Because of this limitation, this algorithm should be used only */
/* after a simple FOV axis-finding approach, such as using as the */
/* FOV axis the average of the boundary vectors, has been tried */
/* unsuccessfully. */
/* Note that it's easy to construct FOVs where the average of the */
/* boundary vectors doesn't yield a viable axis: a FOV of angular */
/* width nearly equal to pi radians, with a sufficiently large */
/* number of boundary vectors on one side and few boundary vectors */
/* on the other, is one such example. This routine can find an */
/* axis for many such intractable FOVs---that's why this routine */
/* should be called after the simple approach fails. */
/* $ Examples */
/* See SPICELIB private routine ZZFOVAXI. */
/* $ Restrictions */
/* 1) This is a SPICE private routine. User applications should not */
/* call this routine. */
/* 2) There are "reasonable" polygonal FOVs that cannot be handled */
/* by this routine. See the discussion in Particulars above. */
/* $ Literature_References */
/* None. */
/* $ Author_and_Institution */
/* N.J. Bachman (JPL) */
/* $ Version */
/* - SPICELIB 1.0.0, 05-MAR-2009 (NJB) */
/* -& */
/* $ Index_Entries */
/* Create axis vector for polygonal FOV */
/* -& */
/* SPICELIB functions */
/* Local parameters */
/* Local variables */
/* Parameter adjustments */
bounds_dim2 = *n;
/* Function Body */
if (return_()) {
return 0;
}
chkin_("ZZHULLAX", (ftnlen)8);
/* Nothing found yet. */
found = FALSE_;
xidx = 0;
/* We must have at least 3 boundary vectors. */
if (*n < 3) {
setmsg_("Polygonal FOV requires at least 3 boundary vectors but numb"
"er supplied for # was #.", (ftnlen)83);
errch_("#", inst, (ftnlen)1, inst_len);
errint_("#", n, (ftnlen)1);
sigerr_("SPICE(INVALIDCOUNT)", (ftnlen)19);
chkout_("ZZHULLAX", (ftnlen)8);
return 0;
}
/* Find an exterior face of the pyramid defined by the */
/* input boundary vectors. Since most polygonal FOVs will have */
/* an exterior face bounded by two consecutive rays, we'll */
/* try pairs of consecutive rays first. If this fails, we'll */
/* try each pair of rays. */
i__ = 1;
while(i__ <= *n && ! found) {
/* Set the index of the next ray. When we get to the */
/* last boundary vector, the next ray is the first. */
if (i__ == *n) {
next = 1;
} else {
next = i__ + 1;
}
/* Find the cross product of the first ray with the */
/* second. Depending on the ordering of the boundary */
/* vectors, this could be an inward or outward normal, */
/* in the case the current face is exterior. */
vcrss_(&bounds[(i__1 = i__ * 3 - 3) < bounds_dim2 * 3 && 0 <= i__1 ?
i__1 : s_rnge("bounds", i__1, "zzhullax_", (ftnlen)408)], &
bounds[(i__2 = next * 3 - 3) < bounds_dim2 * 3 && 0 <= i__2 ?
i__2 : s_rnge("bounds", i__2, "zzhullax_", (ftnlen)408)], cp);
/* We insist on consecutive boundary vectors being */
/* linearly independent. */
if (vzero_(cp)) {
setmsg_("Polygonal FOV must have linearly independent consecutiv"
"e boundary but vectors at indices # and # have cross pro"
"duct equal to the zero vector. Instrument is #.", (ftnlen)
158);
errint_("#", &i__, (ftnlen)1);
errint_("#", &next, (ftnlen)1);
errch_("#", inst, (ftnlen)1, inst_len);
sigerr_("SPICE(DEGENERATECASE)", (ftnlen)21);
chkout_("ZZHULLAX", (ftnlen)8);
return 0;
}
/* See whether the other boundary vectors have angular */
/* separation of at least MARGIN from the plane containing */
/* the current face. */
pass1 = TRUE_;
ok = TRUE_;
m = 1;
while(m <= *n && ok) {
/* Find the angular separation of CP and the Mth vector if the */
/* latter is not an edge of the current face. */
if (m != i__ && m != next) {
sep = vsep_(cp, &bounds[(i__1 = m * 3 - 3) < bounds_dim2 * 3
&& 0 <= i__1 ? i__1 : s_rnge("bounds", i__1, "zzhull"
"ax_", (ftnlen)446)]);
if (pass1) {
/* Adjust CP if necessary so that it points */
/* toward the interior of the pyramid. */
if (sep > halfpi_()) {
/* Invert the cross product vector and adjust SEP */
/* accordingly. Within this "M" loop, all other */
/* angular separations will be computed using the new */
/* value of CP. */
vsclip_(&c_b20, cp);
sep = pi_() - sep;
}
pass1 = FALSE_;
}
ok = sep < halfpi_() - 1e-12;
}
if (ok) {
/* Consider the next boundary vector. */
++m;
}
}
/* We've tested each boundary vector against the current face, or */
/* else the loop terminated early because a vector with */
/* insufficient angular separation from the plane containing the */
/* face was found. */
if (ok) {
/* The current face is exterior. It's bounded by rays I and */
/* NEXT. */
xidx = i__;
found = TRUE_;
} else {
/* Look at the next face of the pyramid. */
++i__;
}
}
/* If we didn't find an exterior face, we'll have to look at each */
/* face bounded by a pair of rays, even if those rays are not */
/* adjacent. (This can be a very slow process is N is large.) */
if (! found) {
i__ = 1;
while(i__ <= *n && ! found) {
/* Consider all ray pairs (I,NEXT) where NEXT > I. */
next = i__ + 1;
while(next <= *n && ! found) {
/* Find the cross product of the first ray with the second. */
/* If the current face is exterior, CP could be an inward */
/* or outward normal, depending on the ordering of the */
/* boundary vectors. */
vcrss_(&bounds[(i__1 = i__ * 3 - 3) < bounds_dim2 * 3 && 0 <=
i__1 ? i__1 : s_rnge("bounds", i__1, "zzhullax_", (
ftnlen)530)], &bounds[(i__2 = next * 3 - 3) <
bounds_dim2 * 3 && 0 <= i__2 ? i__2 : s_rnge("bounds",
i__2, "zzhullax_", (ftnlen)530)], cp);
/* It's allowable for non-consecutive boundary vectors to */
/* be linearly dependent, but if we have such a pair, */
/* it doesn't define an exterior face. */
if (! vzero_(cp)) {
/* The rays having direction vectors indexed I and NEXT */
/* define a semi-infinite sector of a plane that might */
/* be of interest. */
/* Check whether all of the boundary vectors that are */
/* not edges of the current face have angular separation */
/* of at least MARGIN from the plane containing the */
/* current face. */
pass1 = TRUE_;
ok = TRUE_;
m = 1;
while(m <= *n && ok) {
/* Find the angular separation of CP and the Mth */
/* vector if the latter is not an edge of the current */
/* face. */
if (m != i__ && m != next) {
sep = vsep_(cp, &bounds[(i__1 = m * 3 - 3) <
bounds_dim2 * 3 && 0 <= i__1 ? i__1 :
s_rnge("bounds", i__1, "zzhullax_", (
ftnlen)560)]);
if (pass1) {
/* Adjust CP if necessary so that it points */
/* toward the interior of the pyramid. */
if (sep > halfpi_()) {
/* Invert the cross product vector and */
/* adjust SEP accordingly. Within this "M" */
/* loop, all other angular separations will */
/* be computed using the new value of CP. */
vsclip_(&c_b20, cp);
sep = pi_() - sep;
}
pass1 = FALSE_;
}
ok = sep < halfpi_() - 1e-12;
}
if (ok) {
/* Consider the next boundary vector. */
++m;
}
}
/* We've tested each boundary vector against the current */
/* face, or else the loop terminated early because a */
/* vector with insufficient angular separation from the */
/* plane containing the face was found. */
if (ok) {
/* The current face is exterior. It's bounded by rays */
/* I and NEXT. */
xidx = i__;
found = TRUE_;
}
/* End of angular separation test block. */
}
/* End of non-zero cross product block. */
if (! found) {
/* Look at the face bounded by the rays */
/* at indices I and NEXT+1. */
++next;
}
}
/* End of NEXT loop. */
if (! found) {
/* Look at the face bounded by the pairs of rays */
/* including the ray at index I+1. */
++i__;
}
}
/* End of I loop. */
}
/* End of search for exterior face using each pair of rays. */
/* If we still haven't found an exterior face, we can't continue. */
if (! found) {
setmsg_("Unable to find face of convex hull of FOV of instrument #.",
(ftnlen)58);
errch_("#", inst, (ftnlen)1, inst_len);
sigerr_("SPICE(FACENOTFOUND)", (ftnlen)19);
chkout_("ZZHULLAX", (ftnlen)8);
return 0;
}
/* Arrival at this point means that the rays at indices */
/* XIDX and NEXT define a plane such that all boundary */
/* vectors lie in a half-space bounded by that plane. */
/* We're now going to define a set of orthonormal basis vectors: */
/* +X points along the angle bisector of the bounding vectors */
/* of the exterior face. */
/* +Y points along CP. */
/* +Z is the cross product of +X and +Y. */
/* We'll call the reference frame having these basis vectors */
/* the "face frame." */
vhat_(&bounds[(i__1 = i__ * 3 - 3) < bounds_dim2 * 3 && 0 <= i__1 ? i__1 :
s_rnge("bounds", i__1, "zzhullax_", (ftnlen)683)], ray1);
vhat_(&bounds[(i__1 = next * 3 - 3) < bounds_dim2 * 3 && 0 <= i__1 ? i__1
: s_rnge("bounds", i__1, "zzhullax_", (ftnlen)684)], ray2);
vlcom_(&c_b36, ray1, &c_b36, ray2, xvec);
vhatip_(xvec);
vhat_(cp, yvec);
ucrss_(xvec, yvec, zvec);
/* Create a transformation matrix to map the input boundary */
/* vectors into the face frame. */
for (i__ = 1; i__ <= 3; ++i__) {
trans[(i__1 = i__ * 3 - 3) < 9 && 0 <= i__1 ? i__1 : s_rnge("trans",
i__1, "zzhullax_", (ftnlen)698)] = xvec[(i__2 = i__ - 1) < 3
&& 0 <= i__2 ? i__2 : s_rnge("xvec", i__2, "zzhullax_", (
ftnlen)698)];
trans[(i__1 = i__ * 3 - 2) < 9 && 0 <= i__1 ? i__1 : s_rnge("trans",
i__1, "zzhullax_", (ftnlen)699)] = yvec[(i__2 = i__ - 1) < 3
&& 0 <= i__2 ? i__2 : s_rnge("yvec", i__2, "zzhullax_", (
ftnlen)699)];
trans[(i__1 = i__ * 3 - 1) < 9 && 0 <= i__1 ? i__1 : s_rnge("trans",
i__1, "zzhullax_", (ftnlen)700)] = zvec[(i__2 = i__ - 1) < 3
&& 0 <= i__2 ? i__2 : s_rnge("zvec", i__2, "zzhullax_", (
ftnlen)700)];
}
/* Now we're going to compute the longitude of each boundary in the */
/* face frame. The vectors with indices XIDX and NEXT are excluded. */
/* We expect all longitudes to be between MARGIN and pi - MARGIN. */
minlon = pi_();
maxlon = 0.;
minix = 1;
maxix = 1;
i__1 = *n;
for (i__ = 1; i__ <= i__1; ++i__) {
if (i__ != xidx && i__ != next) {
/* The current vector is not a boundary of our edge, */
/* so find its longitude. */
mxv_(trans, &bounds[(i__2 = i__ * 3 - 3) < bounds_dim2 * 3 && 0 <=
i__2 ? i__2 : s_rnge("bounds", i__2, "zzhullax_", (
ftnlen)720)], v);
reclat_(v, &r__, &lon, &lat);
/* Update the longitude bounds. */
if (lon < minlon) {
minix = i__;
minlon = lon;
}
if (lon > maxlon) {
maxix = i__;
maxlon = lon;
}
}
}
/* If the longitude bounds are not as expected, don't try */
/* to continue. */
if (minlon < 2e-12) {
setmsg_("Minimum boundary vector longitude in exterior face frame is"
" # radians. Minimum occurs at index #. This FOV does not con"
"form to the requirements of this routine. Instrument is #.", (
ftnlen)177);
errdp_("#", &minlon, (ftnlen)1);
errint_("#", &minix, (ftnlen)1);
errch_("#", inst, (ftnlen)1, inst_len);
sigerr_("SPICE(NOTSUPPORTED)", (ftnlen)19);
chkout_("ZZHULLAX", (ftnlen)8);
return 0;
} else if (maxlon > pi_() - 2e-12) {
setmsg_("Maximum boundary vector longitude in exterior face frame is"
" # radians. Maximum occurs at index #. This FOV does not con"
"form to the requirements of this routine. Instrument is #.", (
ftnlen)177);
errdp_("#", &maxlon, (ftnlen)1);
errint_("#", &maxix, (ftnlen)1);
errch_("#", inst, (ftnlen)1, inst_len);
sigerr_("SPICE(FOVTOOWIDE)", (ftnlen)17);
chkout_("ZZHULLAX", (ftnlen)8);
return 0;
}
/* Let delta represent the amount we can rotate the exterior */
/* face clockwise about +Z without contacting another boundary */
/* vector. */
delta = pi_() - maxlon;
/* Rotate +Y by -DELTA/2 about +Z. The result is our candidate */
/* FOV axis. Make the axis vector unit length. */
d__1 = -delta / 2;
vrotv_(yvec, zvec, &d__1, axis);
vhatip_(axis);
/* If we have a viable result, ALL boundary vectors have */
/* angular separation less than HALFPI-MARGIN from AXIS. */
i__1 = *n;
for (i__ = 1; i__ <= i__1; ++i__) {
sep = vsep_(&bounds[(i__2 = i__ * 3 - 3) < bounds_dim2 * 3 && 0 <=
i__2 ? i__2 : s_rnge("bounds", i__2, "zzhullax_", (ftnlen)794)
], axis);
if (sep > halfpi_() - 1e-12) {
setmsg_("Boundary vector at index # has angular separation of # "
"radians from candidate FOV axis. This FOV does not confo"
"rm to the requirements of this routine. Instrument is #.",
(ftnlen)167);
errint_("#", &i__, (ftnlen)1);
errdp_("#", &sep, (ftnlen)1);
errch_("#", inst, (ftnlen)1, inst_len);
sigerr_("SPICE(FOVTOOWIDE)", (ftnlen)17);
chkout_("ZZHULLAX", (ftnlen)8);
return 0;
}
}
chkout_("ZZHULLAX", (ftnlen)8);
return 0;
} /* zzhullax_ */
/* $Procedure SPKE15 ( Evaluate a type 15 SPK data record) */
/* Subroutine */ int spke15_(doublereal *et, doublereal *recin, doublereal *
state)
{
/* System generated locals */
doublereal d__1;
/* Builtin functions */
double sqrt(doublereal), d_mod(doublereal *, doublereal *), d_sign(
doublereal *, doublereal *);
/* Local variables */
doublereal near__, dmdt;
extern /* Subroutine */ int vscl_(doublereal *, doublereal *, doublereal *
);
extern doublereal vdot_(doublereal *, doublereal *), vsep_(doublereal *,
doublereal *);
extern /* Subroutine */ int vequ_(doublereal *, doublereal *);
integer j2flg;
doublereal p, angle, dnode, z__;
extern /* Subroutine */ int chkin_(char *, ftnlen);
doublereal epoch, speed, dperi, theta, manom;
extern /* Subroutine */ int moved_(doublereal *, integer *, doublereal *),
errdp_(char *, doublereal *, ftnlen), vcrss_(doublereal *,
doublereal *, doublereal *);
extern doublereal twopi_(void);
extern logical vzero_(doublereal *);
extern /* Subroutine */ int vrotv_(doublereal *, doublereal *, doublereal
*, doublereal *);
doublereal oneme2, state0[6];
extern /* Subroutine */ int prop2b_(doublereal *, doublereal *,
doublereal *, doublereal *);
doublereal pa[3], gm, ta, dt;
extern doublereal pi_(void);
doublereal tp[3], pv[3], cosinc;
extern /* Subroutine */ int sigerr_(char *, ftnlen), vhatip_(doublereal *)
, chkout_(char *, ftnlen), vsclip_(doublereal *, doublereal *),
setmsg_(char *, ftnlen);
doublereal tmpsta[6], oj2;
extern logical return_(void);
doublereal ecc;
extern doublereal dpr_(void);
doublereal dot, rpl, k2pi;
/* $ Abstract */
/* Evaluates a single SPK data record from a segment of type 15 */
/* (Precessing Conic Propagation). */
/* $ 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 */
/* -------- --- -------------------------------------------------- */
/* ET I Target epoch. */
/* RECIN I Data record. */
/* STATE O State (position and velocity). */
/* $ Detailed_Input */
/* ET is a target epoch, specified as ephemeris seconds past */
/* J2000, at which a state vector is to be computed. */
/* RECIN is a data record which, when evaluated at epoch ET, */
/* will give the state (position and velocity) of some */
/* body, relative to some center, in some inertial */
/* reference frame. */
/* The structure of RECIN is: */
/* RECIN(1) epoch of periapsis */
/* in ephemeris seconds past J2000. */
/* RECIN(2)-RECIN(4) unit trajectory pole vector */
/* RECIN(5)-RECIN(7) unit periapsis vector */
/* RECIN(8) semi-latus rectum---p in the */
/* equation: */
/* r = p/(1 + ECC*COS(Nu)) */
/* RECIN(9) eccentricity */
/* RECIN(10) J2 processing flag describing */
/* what J2 corrections are to be */
/* applied when the orbit is */
/* propagated. */
/* All J2 corrections are applied */
/* if this flag has a value that */
/* is not 1,2 or 3. */
/* If the value of the flag is 3 */
/* no corrections are done. */
/* If the value of the flag is 1 */
/* no corrections are computed for */
/* the precession of the line */
/* of apsides. However, regression */
/* of the line of nodes is */
/* performed. */
/* If the value of the flag is 2 */
/* no corrections are done for */
/* the regression of the line of */
/* nodes. However, precession of the */
/* line of apsides is performed. */
/* Note that J2 effects are computed */
/* only if the orbit is elliptic and */
/* does not intersect the central */
/* body. */
/* RECIN(11)-RECIN(13) unit central body pole vector */
/* RECIN(14) central body GM */
/* RECIN(15) central body J2 */
/* RECIN(16) central body radius */
/* Units are radians, km, seconds */
/* $ Detailed_Output */
/* STATE is the state produced by evaluating RECIN at ET. */
/* Units are km and km/sec. */
/* $ Parameters */
/* None. */
/* $ Files */
/* None. */
/* $ Exceptions */
/* 1) If the eccentricity is less than zero, the error */
/* 'SPICE(BADECCENTRICITY)' will be signalled. */
/* 2) If the semi-latus rectum is non-positive, the error */
/* 'SPICE(BADLATUSRECTUM)' is signalled. */
/* 3) If the pole vector, trajectory pole vector or periapsis vector */
/* has zero length, the error 'SPICE(BADVECTOR)' is signalled. */
/* 4) If the trajectory pole vector and the periapsis vector are */
/* not orthogonal, the error 'SPICE(BADINITSTATE)' is */
/* signalled. The test for orthogonality is very crude. The */
/* routine simply checks that the absolute value of the dot */
/* product of the unit vectors parallel to the trajectory pole */
/* and periapse vectors is less than 0.00001. This check is */
/* intended to catch blunders, not to enforce orthogonality to */
/* double precision tolerance. */
/* 5) If the mass of the central body is non-positive, the error */
/* 'SPICE(NONPOSITIVEMASS)' is signalled. */
/* 6) If the radius of the central body is negative, the error */
/* 'SPICE(BADRADIUS)' is signalled. */
/* $ Particulars */
/* This algorithm applies J2 corrections for precessing the */
/* node and argument of periapse for an object orbiting an */
/* oblate spheroid. */
/* Note the effects of J2 are incorporated only for elliptic */
/* orbits that do not intersect the central body. */
/* While the derivation of the effect of the various harmonics */
/* of gravitational field are beyond the scope of this header */
/* the effect of the J2 term of the gravity model are as follows */
/* The line of node precesses. Over one orbit average rate of */
/* precession, DNode/dNu, is given by */
/* 3 J2 */
/* dNode/dNu = - ----------------- DCOS( inc ) */
/* 2 (P/RPL)**2 */
/* (Since this is always less than zero for oblate spheroids, this */
/* should be called regression of nodes.) */
/* The line of apsides precesses. The average rate of precession */
/* DPeri/dNu is given by */
/* 3 J2 */
/* dPeri/dNu = ----------------- ( 5*DCOS ( inc ) - 1 ) */
/* 2 (P/RPL)**2 */
/* Details of these formulae are given in the Battin's book (see */
/* literature references below). */
/* It is assumed that this routine is used in conjunction with */
/* the routine SPKR15 as shown here: */
/* CALL SPKR15 ( HANDLE, DESCR, ET, RECIN ) */
/* CALL SPKE15 ( ET, RECIN, STATE ) */
/* where it is known in advance that the HANDLE, DESCR pair points */
/* to a type 15 data segment. */
/* $ Examples */
/* The SPKEnn routines are almost always used in conjunction with */
/* the corresponding SPKRnn routines, which read the records from */
/* SPK files. */
/* The data returned by the SPKRnn routine is in its rawest form, */
/* taken directly from the segment. As such, it will be meaningless */
/* to a user unless he/she understands the structure of the data type */
/* completely. Given that understanding, however, the SPKRnn */
/* routines might be used to examine raw segment data before */
/* evaluating it with the SPKEnn routines. */
/* C */
/* C Get a segment applicable to a specified body and epoch. */
/* C */
/* CALL SPKSFS ( BODY, ET, HANDLE, DESCR, IDENT, FOUND ) */
/* C */
/* C Look at parts of the descriptor. */
/* C */
/* CALL DAFUS ( DESCR, 2, 6, DCD, ICD ) */
/* CENTER = ICD( 2 ) */
/* REF = ICD( 3 ) */
/* TYPE = ICD( 4 ) */
/* IF ( TYPE .EQ. 15 ) THEN */
/* CALL SPKR15 ( HANDLE, DESCR, ET, RECORD ) */
/* . */
/* . Look at the RECORD data. */
/* . */
/* CALL SPKE15 ( ET, RECORD, STATE ) */
/* . */
/* . Check out the evaluated state. */
/* . */
/* END IF */
/* $ Restrictions */
/* None. */
/* $ Author_and_Institution */
/* K.R. Gehringer (JPL) */
/* S. Schlaifer (JPL) */
/* W.L. Taber (JPL) */
/* $ Literature_References */
/* [1] `Fundamentals of Celestial Mechanics', Second Edition 1989 */
/* by J.M.A. Danby; Willman-Bell, Inc., P.O. Box 35025 */
/* Richmond Virginia; pp 345-347. */
/* [2] `Astronautical Guidance', by Richard H. Battin. 1964 */
/* McGraw-Hill Book Company, San Francisco. pp 199 */
/* $ Version */
/* - SPICELIB Version 1.2.0, 02-SEP-2005 (NJB) */
/* Updated to remove non-standard use of duplicate arguments */
/* in VHAT, VROTV, and VSCL calls. */
/* - SPICELIB Version 1.1.0, 29-FEB-1996 (KRG) */
/* The declaration for the SPICELIB function PI is now */
/* preceded by an EXTERNAL statement declaring PI to be an */
/* external function. This removes a conflict with any */
/* compilers that have a PI intrinsic function. */
/* - SPICELIB Version 1.0.0, 15-NOV-1994 (WLT) (SS) */
/* -& */
/* $ Index_Entries */
/* evaluate type_15 spk segment */
/* -& */
/* $ Revisions */
/* - SPICELIB Version 1.2.0, 02-SEP-2005 (NJB) */
/* Updated to remove non-standard use of duplicate arguments */
/* in VHAT, VROTV, and VSCL calls. */
/* - SPICELIB Version 1.1.0, 29-FEB-1996 (KRG) */
/* The declaration for the SPICELIB function PI is now */
/* preceded by an EXTERNAL statement declaring PI to be an */
/* external function. This removes a conflict with any */
/* compilers that have a PI intrinsic function. */
/* - SPICELIB Version 1.0.0, 15-NOV-1994 (WLT) (SS) */
/* -& */
/* SPICELIB Functions */
/* Local Variables */
/* Standard SPICE error handling. */
if (return_()) {
return 0;
}
chkin_("SPKE15", (ftnlen)6);
/* Fetch the various entities from the input record, first the epoch. */
epoch = recin[0];
/* The trajectory pole vector. */
vequ_(&recin[1], tp);
/* The periapsis vector. */
vequ_(&recin[4], pa);
/* Semi-latus rectum ( P in the P/(1 + ECC*COS(Nu) ), */
/* and eccentricity. */
p = recin[7];
ecc = recin[8];
/* J2 processing flag. */
j2flg = (integer) recin[9];
/* Central body pole vector. */
vequ_(&recin[10], pv);
/* The central mass, J2 and radius of the central body. */
gm = recin[13];
oj2 = recin[14];
rpl = recin[15];
/* Check all the inputs here for obvious failures. Yes, perhaps */
/* this is overkill. However, there is a lot more computation */
/* going on in this routine so that the small amount of overhead */
/* here should not be significant. */
if (p <= 0.) {
setmsg_("The semi-latus rectum supplied to the SPK type 15 evaluator"
" was non-positive. This value must be positive. The value s"
"upplied was #.", (ftnlen)133);
errdp_("#", &p, (ftnlen)1);
sigerr_("SPICE(BADLATUSRECTUM)", (ftnlen)21);
chkout_("SPKE15", (ftnlen)6);
return 0;
} else if (ecc < 0.) {
setmsg_("The eccentricity supplied for a type 15 segment is negative"
". It must be non-negative. The value supplied to the type 1"
"5 evaluator was #. ", (ftnlen)138);
errdp_("#", &ecc, (ftnlen)1);
sigerr_("SPICE(BADECCENTRICITY)", (ftnlen)22);
chkout_("SPKE15", (ftnlen)6);
return 0;
} else if (gm <= 0.) {
setmsg_("The mass supplied for the central body of a type 15 segment"
" was non-positive. Masses must be positive. The value suppl"
"ied was #. ", (ftnlen)130);
errdp_("#", &gm, (ftnlen)1);
sigerr_("SPICE(NONPOSITIVEMASS)", (ftnlen)22);
chkout_("SPKE15", (ftnlen)6);
return 0;
} else if (vzero_(tp)) {
setmsg_("The trajectory pole vector supplied to SPKE15 had length ze"
"ro. The most likely cause of this problem is a corrupted SPK"
" (ephemeris) file. ", (ftnlen)138);
sigerr_("SPICE(BADVECTOR)", (ftnlen)16);
chkout_("SPKE15", (ftnlen)6);
return 0;
} else if (vzero_(pa)) {
setmsg_("The periapse vector supplied to SPKE15 had length zero. The"
" most likely cause of this problem is a corrupted SPK (ephem"
"eris) file. ", (ftnlen)131);
sigerr_("SPICE(BADVECTOR)", (ftnlen)16);
chkout_("SPKE15", (ftnlen)6);
return 0;
} else if (vzero_(pv)) {
setmsg_("The central pole vector supplied to SPKE15 had length zero."
" The most likely cause of this problem is a corrupted SPK (e"
"phemeris) file. ", (ftnlen)135);
sigerr_("SPICE(BADVECTOR)", (ftnlen)16);
chkout_("SPKE15", (ftnlen)6);
return 0;
} else if (rpl < 0.) {
setmsg_("The central body radius was negative. It must be zero or po"
"sitive. The value supplied was #. ", (ftnlen)94);
errdp_("#", &rpl, (ftnlen)1);
sigerr_("SPICE(BADRADIUS)", (ftnlen)16);
chkout_("SPKE15", (ftnlen)6);
return 0;
}
/* Convert TP, PV and PA to unit vectors. */
/* (It won't hurt to polish them up a bit here if they are already */
/* unit vectors.) */
vhatip_(pa);
vhatip_(tp);
vhatip_(pv);
/* One final check. Make sure the pole and periapse vectors are */
/* orthogonal. (We will use a very crude check but this should */
/* rule out any obvious errors.) */
dot = vdot_(pa, tp);
if (abs(dot) > 1e-5) {
angle = vsep_(pa, tp) * dpr_();
setmsg_("The periapsis and trajectory pole vectors are not orthogona"
"l. The anglebetween them is # degrees. ", (ftnlen)98);
errdp_("#", &angle, (ftnlen)1);
sigerr_("SPICE(BADINITSTATE)", (ftnlen)19);
chkout_("SPKE15", (ftnlen)6);
return 0;
}
/* Compute the distance and speed at periapse. */
near__ = p / (ecc + 1.);
speed = sqrt(gm / p) * (ecc + 1.);
/* Next get the position at periapse ... */
vscl_(&near__, pa, state0);
/* ... and the velocity at periapsis. */
vcrss_(tp, pa, &state0[3]);
vsclip_(&speed, &state0[3]);
/* Determine the elapsed time from periapse to the requested */
/* epoch and propagate the state at periapsis to the epoch of */
/* interest. */
/* Note that we are making use of the following fact. */
/* If R is a rotation, then the states obtained by */
/* the following blocks of code are mathematically the */
/* same. (In reality they may differ slightly due to */
/* roundoff.) */
/* Code block 1. */
/* CALL MXV ( R, STATE0, STATE0 ) */
/* CALL MXV ( R, STATE0(4), STATE0(4) ) */
/* CALL PROP2B( GM, STATE0, DT, STATE ) */
/* Code block 2. */
/* CALL PROP2B( GM, STATE0, DT, STATE ) */
/* CALL MXV ( R, STATE, STATE ) */
/* CALL MXV ( R, STATE(4), STATE(4) ) */
/* This allows us to first compute the propagation of our initial */
/* state and then if needed perform the precession of the line */
/* of nodes and apsides by simply precessing the resulting state. */
dt = *et - epoch;
prop2b_(&gm, state0, &dt, state);
/* If called for, handle precession needed due to the J2 term. Note */
/* that the motion of the lines of nodes and apsides is formulated */
/* in terms of the true anomaly. This means we need the accumulated */
/* true anomaly in order to properly transform the state. */
if (j2flg != 3 && oj2 != 0. && ecc < 1. && near__ > rpl) {
/* First compute the change in mean anomaly since periapsis. */
/* Computing 2nd power */
d__1 = ecc;
oneme2 = 1. - d__1 * d__1;
dmdt = oneme2 / p * sqrt(gm * oneme2 / p);
manom = dmdt * dt;
/* Next compute the angle THETA such that THETA is between */
/* -pi and pi and such than MANOM = THETA + K*2*pi for */
/* some integer K. */
d__1 = twopi_();
theta = d_mod(&manom, &d__1);
if (abs(theta) > pi_()) {
d__1 = twopi_();
theta -= d_sign(&d__1, &theta);
}
k2pi = manom - theta;
/* We can get the accumulated true anomaly from the propagated */
/* state theta and the accumulated mean anomaly prior to this */
/* orbit. */
ta = vsep_(pa, state);
ta = d_sign(&ta, &theta);
ta += k2pi;
/* Determine how far the line of nodes and periapsis have moved. */
cosinc = vdot_(pv, tp);
/* Computing 2nd power */
d__1 = rpl / p;
z__ = ta * 1.5 * oj2 * (d__1 * d__1);
dnode = -z__ * cosinc;
/* Computing 2nd power */
d__1 = cosinc;
dperi = z__ * (d__1 * d__1 * 2.5 - .5);
/* Precess the periapsis by rotating the state vector about the */
/* trajectory pole */
if (j2flg != 1) {
vrotv_(state, tp, &dperi, tmpsta);
vrotv_(&state[3], tp, &dperi, &tmpsta[3]);
moved_(tmpsta, &c__6, state);
}
/* Regress the line of nodes by rotating the state */
/* about the pole of the central body. */
if (j2flg != 2) {
vrotv_(state, pv, &dnode, tmpsta);
vrotv_(&state[3], pv, &dnode, &tmpsta[3]);
moved_(tmpsta, &c__6, state);
}
/* We could perform the rotations above in the other order, */
/* but we would also have to rotate the pole before precessing */
/* the line of apsides. */
}
/* That's all folks. Check out and return. */
chkout_("SPKE15", (ftnlen)6);
return 0;
} /* spke15_ */
/* $Procedure ZZFOVAXI ( Generate an axis vector for polygonal FOV ) */
/* Subroutine */ int zzfovaxi_(char *inst, integer *n, doublereal *bounds,
doublereal *axis, ftnlen inst_len)
{
/* System generated locals */
integer bounds_dim2, i__1, i__2, i__3;
doublereal d__1;
/* Builtin functions */
integer s_rnge(char *, integer, char *, integer);
/* Local variables */
extern /* Subroutine */ int vadd_(doublereal *, doublereal *, doublereal *
);
doublereal uvec[3];
extern /* Subroutine */ int vhat_(doublereal *, doublereal *);
extern doublereal vsep_(doublereal *, doublereal *);
integer next;
extern /* Subroutine */ int vequ_(doublereal *, doublereal *), zzhullax_(
char *, integer *, doublereal *, doublereal *, ftnlen);
integer i__;
doublereal v[3];
extern /* Subroutine */ int chkin_(char *, ftnlen), errch_(char *, char *,
ftnlen, ftnlen);
doublereal limit;
extern /* Subroutine */ int vcrss_(doublereal *, doublereal *, doublereal
*);
extern logical vzero_(doublereal *);
doublereal cp[3];
extern logical failed_(void);
logical ok;
extern /* Subroutine */ int cleard_(integer *, doublereal *);
extern doublereal halfpi_(void);
extern /* Subroutine */ int sigerr_(char *, ftnlen), vhatip_(doublereal *)
, chkout_(char *, ftnlen), vsclip_(doublereal *, doublereal *),
setmsg_(char *, ftnlen), errint_(char *, integer *, ftnlen);
extern logical return_(void);
doublereal sep;
/* $ 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. */
/* Generate an axis of an instrument's polygonal FOV such that all */
/* of the FOV's boundary vectors have angular separation of strictly */
/* less than pi/2 radians from this axis. */
/* $ 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 */
/* CK */
/* FRAMES */
/* GF */
/* IK */
/* KERNEL */
/* $ Keywords */
/* FOV */
/* GEOMETRY */
/* INSTRUMENT */
/* $ Declarations */
/* $ Brief_I/O */
/* VARIABLE I/O DESCRIPTION */
/* -------- --- -------------------------------------------------- */
/* MARGIN P Minimum complement of FOV cone angle. */
/* INST I Instrument name. */
/* N I Number of FOV boundary vectors. */
/* BOUNDS I FOV boundary vectors. */
/* AXIS O Instrument FOV axis vector. */
/* $ Detailed_Input */
/* INST is the name of an instrument with which the field of */
/* view (FOV) of interest is associated. This name is */
/* used only to generate long error messages. */
/* N is the number of boundary vectors in the array */
/* BOUNDS. */
/* BOUNDS is an array of N vectors emanating from a common */
/* vertex and defining the edges of a pyramidal region in */
/* three-dimensional space: this the region within the */
/* FOV of the instrument designated by INST. The Ith */
/* vector of BOUNDS resides in elements (1:3,I) of this */
/* array. */
/* The vectors contained in BOUNDS are called the */
/* "boundary vectors" of the FOV. */
/* The boundary vectors must satisfy the constraints: */
/* 1) The boundary vectors must be contained within */
/* a right circular cone of angular radius less */
/* than than (pi/2) - MARGIN radians; in other */
/* words, there must be a vector A such that all */
/* boundary vectors have angular separation from */
/* A of less than (pi/2)-MARGIN radians. */
/* 2) There must be a pair of vectors U, V in BOUNDS */
/* such that all other boundary vectors lie in */
/* the same half space bounded by the plane */
/* containing U and V. Furthermore, all other */
/* boundary vectors must have orthogonal */
/* projections onto a plane normal to this plane */
/* such that the projections have angular */
/* separation of at least 2*MARGIN radians from */
/* the plane spanned by U and V. */
/* Given the first constraint above, there is plane PL */
/* such that each of the set of rays extending the */
/* boundary vectors intersects PL. (In fact, there is an */
/* infinite set of such planes.) The boundary vectors */
/* must be ordered so that the set of line segments */
/* connecting the intercept on PL of the ray extending */
/* the Ith vector to that of the (I+1)st, with the Nth */
/* intercept connected to the first, form a polygon (the */
/* "FOV polygon") constituting the intersection of the */
/* FOV pyramid with PL. This polygon may wrap in either */
/* the positive or negative sense about a ray emanating */
/* from the FOV vertex and passing through the plane */
/* region bounded by the FOV polygon. */
/* The FOV polygon need not be convex; it may be */
/* self-intersecting as well. */
/* No pair of consecutive vectors in BOUNDS may be */
/* linearly dependent. */
/* The boundary vectors need not have unit length. */
/* $ Detailed_Output */
/* AXIS is a unit vector normal to a plane containing the */
/* FOV polygon. All boundary vectors have angular */
/* separation from AXIS of not more than */
/* ( pi/2 ) - MARGIN */
/* radians. */
/* This routine signals an error if it cannot find */
/* a satisfactory value of AXIS. */
/* $ Parameters */
/* MARGIN is a small positive number used to constrain the */
/* orientation of the boundary vectors. See the two */
/* constraints described in the Detailed_Input section */
/* above for specifics. */
/* $ Exceptions */
/* 1) In the input vector count N is not at least 3, the error */
/* SPICE(INVALIDCOUNT) is signaled. */
/* 2) If any pair of consecutive boundary vectors has cross */
/* product zero, the error SPICE(DEGENERATECASE) is signaled. */
/* For this test, the first vector is considered the successor */
/* of the Nth. */
/* 3) If this routine can't find a face of the convex hull of */
/* the set of boundary vectors such that this face satisfies */
/* constraint (2) of the Detailed_Input section above, the */
/* error SPICE(FACENOTFOUND) is signaled. */
/* 4) If any boundary vectors have longitude too close to 0 */
/* or too close to pi radians in the face frame (see discussion */
/* of the search algorithm's steps 3 and 4 in Particulars */
/* below), the respective errors SPICE(NOTSUPPORTED) or */
/* SPICE(FOVTOOWIDE) are signaled. */
/* 5) If any boundary vectors have angular separation of more than */
/* (pi/2)-MARGIN radians from the candidate FOV axis, the */
/* error SPICE(FOVTOOWIDE) is signaled. */
/* $ Files */
/* The boundary vectors input to this routine are typically */
/* obtained from an IK file. */
/* $ Particulars */
/* Normally implementation is not discussed in SPICE headers, but we */
/* make an exception here because this routine's implementation and */
/* specification are deeply intertwined. */
/* This routine first computes the average of the unitized input */
/* boundary vectors; if this vector satisfies the angular separation */
/* constraint (1) in Detailed_Input, a unit length copy of this */
/* vector is returned as the FOV axis. */
/* If the procedure above fails, an algorithm based on selection */
/* of a suitable face of the boundary vector's convex hull is tried. */
/* See the routine ZZHULLAX for details. */
/* If the second approach fails, an error is signaled. */
/* Note that it's easy to construct FOVs where the average of the */
/* boundary vectors doesn't yield a viable axis: a FOV of angular */
/* width nearly equal to pi radians, with a sufficiently large */
/* number of boundary vectors on one side and few boundary vectors */
/* on the other, is one such example. This routine can find an */
/* axis for many such intractable FOVs---that's why ZZHULLAX */
/* is called after the simple approach fails. */
/* $ Examples */
/* See SPICELIB private routine ZZGFFVIN. */
/* $ Restrictions */
/* 1) This is a SPICE private routine. User applications should not */
/* call this routine. */
/* 2) There may "reasonable" polygonal FOVs that cannot be handled */
/* by this routine. See the discussions in Detailed_Input, */
/* Exceptions, and Particulars above for restrictions on the */
/* input set of FOV boundary vectors. */
/* $ Literature_References */
/* None. */
/* $ Author_and_Institution */
/* N.J. Bachman (JPL) */
/* $ Version */
/* - SPICELIB Version 1.0.0, 05-MAR-2009 (NJB) */
/* -& */
/* SPICELIB functions */
/* Local parameters */
/* Local variables */
/* Parameter adjustments */
bounds_dim2 = *n;
/* Function Body */
if (return_()) {
return 0;
}
chkin_("ZZFOVAXI", (ftnlen)8);
/* We must have at least 3 boundary vectors. */
if (*n < 3) {
setmsg_("Polygonal FOV requires at least 3 boundary vectors but numb"
"er supplied for # was #.", (ftnlen)83);
errch_("#", inst, (ftnlen)1, inst_len);
errint_("#", n, (ftnlen)1);
sigerr_("SPICE(INVALIDCOUNT)", (ftnlen)19);
chkout_("ZZFOVAXI", (ftnlen)8);
return 0;
}
/* Check for linearly dependent consecutive boundary vectors. */
i__1 = *n;
for (i__ = 1; i__ <= i__1; ++i__) {
/* Set the index of the next ray. When we get to the */
/* last boundary vector, the next ray is the first. */
if (i__ == *n) {
next = 1;
} else {
next = i__ + 1;
}
/* Find the cross product of the first ray with the */
/* second. Depending on the ordering of the boundary */
/* vectors, this could be an inward or outward normal, */
/* in the case the current face is is exterior. */
vcrss_(&bounds[(i__2 = i__ * 3 - 3) < bounds_dim2 * 3 && 0 <= i__2 ?
i__2 : s_rnge("bounds", i__2, "zzfovaxi_", (ftnlen)313)], &
bounds[(i__3 = next * 3 - 3) < bounds_dim2 * 3 && 0 <= i__3 ?
i__3 : s_rnge("bounds", i__3, "zzfovaxi_", (ftnlen)313)], cp);
/* We insist on consecutive boundary vectors being */
/* linearly independent. */
if (vzero_(cp)) {
setmsg_("Polygonal FOV must have linearly independent consecutiv"
"e boundary but vectors at indices # and # have cross pro"
"duct equal to the zero vector. Instrument is #.", (ftnlen)
158);
errint_("#", &i__, (ftnlen)1);
errint_("#", &next, (ftnlen)1);
errch_("#", inst, (ftnlen)1, inst_len);
sigerr_("SPICE(DEGENERATECASE)", (ftnlen)21);
chkout_("ZZFOVAXI", (ftnlen)8);
return 0;
}
}
/* First try the average of the FOV unit boundary vectors as */
/* a candidate axis. In many cases, this simple approach */
/* does the trick. */
cleard_(&c__3, axis);
i__1 = *n;
for (i__ = 1; i__ <= i__1; ++i__) {
vhat_(&bounds[(i__2 = i__ * 3 - 3) < bounds_dim2 * 3 && 0 <= i__2 ?
i__2 : s_rnge("bounds", i__2, "zzfovaxi_", (ftnlen)346)],
uvec);
vadd_(uvec, axis, v);
vequ_(v, axis);
}
d__1 = 1. / *n;
vsclip_(&d__1, axis);
/* If each boundary vector has sufficiently small */
/* angular separation from AXIS, we're done. */
limit = halfpi_() - 1e-12;
ok = TRUE_;
i__ = 1;
while(i__ <= *n && ok) {
sep = vsep_(&bounds[(i__1 = i__ * 3 - 3) < bounds_dim2 * 3 && 0 <=
i__1 ? i__1 : s_rnge("bounds", i__1, "zzfovaxi_", (ftnlen)365)
], axis);
if (sep > limit) {
ok = FALSE_;
} else {
++i__;
}
}
if (! ok) {
/* See whether we can find an axis using a */
/* method based on finding a face of the convex */
/* hull of the FOV. ZZHULLAX signals an error */
/* if it doesn't succeed. */
zzhullax_(inst, n, bounds, axis, inst_len);
if (failed_()) {
chkout_("ZZFOVAXI", (ftnlen)8);
return 0;
}
}
/* At this point AXIS is valid. Make the axis vector unit length. */
vhatip_(axis);
chkout_("ZZFOVAXI", (ftnlen)8);
return 0;
} /* zzfovaxi_ */
