/* $Procedure CKE03 ( C-kernel, evaluate pointing record, data type 3 ) */
/* Subroutine */ int cke03_(logical *needav, doublereal *record, doublereal *
cmat, doublereal *av, doublereal *clkout)
{
/* System generated locals */
doublereal d__1;
/* Local variables */
doublereal frac, axis[3];
extern /* Subroutine */ int vequ_(doublereal *, doublereal *), mtxm_(
doublereal *, doublereal *, doublereal *), mxmt_(doublereal *,
doublereal *, doublereal *);
doublereal cmat1[9] /* was [3][3] */, cmat2[9] /* was [3][3] */, t,
angle, delta[9] /* was [3][3] */;
extern /* Subroutine */ int chkin_(char *, ftnlen), moved_(doublereal *,
integer *, doublereal *), vlcom_(doublereal *, doublereal *,
doublereal *, doublereal *, doublereal *);
doublereal q1[4], q2[4], t1, t2;
extern logical failed_(void);
extern /* Subroutine */ int raxisa_(doublereal *, doublereal *,
doublereal *), axisar_(doublereal *, doublereal *, doublereal *),
chkout_(char *, ftnlen);
doublereal av1[3], av2[3];
extern logical return_(void);
extern /* Subroutine */ int q2m_(doublereal *, doublereal *);
doublereal rot[9] /* was [3][3] */;
/* $ Abstract */
/* Evaluate a pointing record returned by CKR03 from a CK type 3 */
/* segment. Return the C-matrix and angular velocity vector associated */
/* with the time CLKOUT. */
/* $ 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 */
/* ROTATION */
/* $ Keywords */
/* POINTING */
/* $ Declarations */
/* $ Brief_I/O */
/* Variable I/O Description */
/* -------- --- -------------------------------------------------- */
/* NEEDAV I True if angular velocity is requested. */
/* RECORD I Data type 3 pointing record. */
/* CMAT O C-matrix. */
/* AV O Angular velocity vector. */
/* CLKOUT O SCLK associated with C-matrix. */
/* $ Detailed_Input */
/* NEEDAV is true if angular velocity is requested. */
/* RECORD is a set of double precision numbers returned by CKR03 */
/* that contain sufficient information from a type 3 CK */
/* segment to evaluate the C-matrix and the angular */
/* velocity vector at a particular time. Depending on */
/* the contents of RECORD, this routine will either */
/* interpolate between two pointing instances that */
/* bracket a request time, or it will simply return the */
/* pointing given by a single pointing instance. */
/* When pointing at the request time can be determined */
/* by linearly interpolating between the two pointing */
/* instances that bracket that time, the bracketing */
/* pointing instances are returned in RECORD as follows: */
/* RECORD( 1 ) = Left bracketing SCLK time. */
/* RECORD( 2 ) = lq0 \ */
/* RECORD( 3 ) = lq1 \ Left bracketing */
/* RECORD( 4 ) = lq2 / quaternion. */
/* RECORD( 5 ) = lq3 / */
/* RECORD( 6 ) = lav1 \ Left bracketing */
/* RECORD( 7 ) = lav2 | angular velocity */
/* RECORD( 8 ) = lav3 / ( optional ) */
/* RECORD( 9 ) = Right bracketing SCLK time. */
/* RECORD( 10 ) = rq0 \ */
/* RECORD( 11 ) = rq1 \ Right bracketing */
/* RECORD( 12 ) = rq2 / quaternion. */
/* RECORD( 13 ) = rq3 / */
/* RECORD( 14 ) = rav1 \ Right bracketing */
/* RECORD( 15 ) = rav2 | angular velocity */
/* RECORD( 16 ) = rav3 / ( optional ) */
/* RECORD( 17 ) = pointing request time */
/* The quantities lq0 - lq3 and rq0 - rq3 are the */
/* components of the quaternions that represent the */
/* C-matrices associated with the times that bracket */
/* the requested time. */
/* The quantities lav1, lav2, lav3 and rav1, rav2, rav3 */
/* are the components of the angular velocity vectors at */
/* the respective bracketing times. The components of the */
/* angular velocity vectors are specified relative to the */
/* inertial reference frame of the segment. */
/* When the routine is to simply return the pointing */
/* given by a particular pointing instance, then the */
/* values of that pointing instance are returned in both */
/* parts of RECORD ( i.e. RECORD(1-9) and RECORD(10-16) ). */
/* $ Detailed_Output */
/* CMAT is a rotation matrix that transforms the components */
/* of a vector expressed in the inertial frame given in */
/* the segment to components expressed in the instrument */
/* fixed frame at the returned time. */
/* Thus, if a vector v has components x, y, z in the */
/* inertial frame, then v has components x', y', z' in the */
/* instrument fixed frame where: */
/* [ x' ] [ ] [ x ] */
/* | y' | = | CMAT | | y | */
/* [ z' ] [ ] [ z ] */
/* If the x', y', z' components are known, use the */
/* transpose of the C-matrix to determine x, y, z as */
/* follows. */
/* [ x ] [ ]T [ x' ] */
/* | y | = | CMAT | | y' | */
/* [ z ] [ ] [ z' ] */
/* (Transpose of CMAT) */
/* AV is the angular velocity vector of the instrument fixed */
/* frame defined by CMAT. The angular velocity is */
/* returned only if NEEDAV is true. */
/* The direction of the angular velocity vector gives */
/* the right-handed axis about which the instrument fixed */
/* reference frame is rotating. The magnitude of AV is */
/* the magnitude of the instantaneous velocity of the */
/* rotation, in radians per second. */
/* The angular velocity vector is returned in component */
/* form */
/* AV = [ AV1 , AV2 , AV3 ] */
/* which is in terms of the inertial coordinate frame */
/* specified in the segment descriptor. */
/* CLKOUT is the encoded SCLK associated with the returned */
/* C-matrix and angular velocity vector. */
/* $ Parameters */
/* None. */
/* $ Exceptions */
/* 1) No explicit checking is done to determine whether RECORD is */
/* valid. However, routines in the call tree of this routine */
/* may signal errors if inputs are invalid or otherwise */
/* in appropriate. */
/* $ Files */
/* None. */
/* $ Particulars */
/* If the array RECORD contains pointing instances that bracket the */
/* request time then CKE03 will linearly interpolate between those */
/* two values to obtain pointing at the request time. If the */
/* pointing instances in RECORD are for the same time, then this */
/* routine will simply unpack the record and convert the quaternion */
/* to a C-matrix. */
/* The linear interpolation performed by this routine is defined */
/* as follows: */
/* 1) Let t be the time for which pointing is requested and */
/* let CMAT1 and CMAT2 be C-matrices associated with times */
/* t1 and t2 where: */
/* t1 < t2, and t1 <= t, and t <= t2. */
/* 2) Assume that the spacecraft frame rotates about a fixed */
/* axis at a constant angular rate from time t1 to time t2. */
/* The angle and rotation axis can be obtained from the */
/* rotation matrix ROT12 where: */
/* T T */
/* CMAT2 = ROT12 * CMAT1 */
/* or */
/* T */
/* ROT12 = CMAT2 * CMAT1 */
/* ROT12 ==> ( ANGLE, AXIS ) */
/* 3) To obtain pointing at time t, rotate the spacecraft frame */
/* about the vector AXIS from its orientation at time t1 by the */
/* angle THETA where: */
/* ( t - t1 ) */
/* THETA = ANGLE * ----------- */
/* ( t2 - t1 ) */
/* 4) Thus if ROT1t is the matrix that rotates vectors by the */
/* angle THETA about the vector AXIS, then the output C-matrix */
/* is given by: */
/* T T */
/* CMAT = ROT1t * CMAT1 */
/* T */
/* CMAT = CMAT1 * ROT1t */
/* 5) The angular velocity is treated independently of the */
/* C-matrix. If it is requested, then the AV at time t is */
/* the weighted average of the angular velocity vectors at */
/* the times t1 and t2: */
/* ( t - t1 ) */
/* W = ----------- */
/* ( t2 - t1 ) */
/* AV = ( 1 - W ) * AV1 + W * AV2 */
/* $ Examples */
/* The CKRnn routines are usually used in tandem with the CKEnn */
/* routines, which evaluate the record returned by CKRnn to give */
/* the pointing information and output time. */
/* The following code fragment searches through all of the segments */
/* in a file applicable to the Mars Observer spacecraft bus that */
/* are of data type 3, for a particular spacecraft clock time. */
/* It then evaluates the pointing for that epoch and prints the */
/* result. */
/* CHARACTER*(20) SCLKCH */
/* CHARACTER*(20) SCTIME */
/* CHARACTER*(40) IDENT */
/* INTEGER I */
/* INTEGER SC */
/* INTEGER INST */
/* INTEGER HANDLE */
/* INTEGER DTYPE */
/* INTEGER ICD ( 6 ) */
/* DOUBLE PRECISION SCLKDP */
/* DOUBLE PRECISION TOL */
/* DOUBLE PRECISION CLKOUT */
/* DOUBLE PRECISION DESCR ( 5 ) */
/* DOUBLE PRECISION DCD ( 2 ) */
/* DOUBLE PRECISION RECORD ( 17 ) */
/* DOUBLE PRECISION CMAT ( 3, 3 ) */
/* DOUBLE PRECISION AV ( 3 ) */
/* LOGICAL NEEDAV */
/* LOGICAL FND */
/* LOGICAL SFND */
/* SC = -94 */
/* INST = -94000 */
/* DTYPE = 3 */
/* NEEDAV = .FALSE. */
/* C */
/* C Load the MO SCLK kernel and the C-kernel. */
/* C */
/* CALL FURNSH ( 'MO_SCLK.TSC' ) */
/* CALL DAFOPR ( 'MO_CK.BC', HANDLE ) */
/* C */
/* C Get the spacecraft clock time. Then encode it for use */
/* C in the C-kernel. */
/* C */
/* WRITE (*,*) 'Enter spacecraft clock time string:' */
/* READ (*,FMT='(A)') SCLKCH */
/* CALL SCENCD ( SC, SCLKCH, SCLKDP ) */
/* C */
/* C Use a tolerance of 2 seconds ( half of the nominal */
/* C separation between MO pointing instances ). */
/* C */
/* CALL SCTIKS ( SC, '0000000002:000', TOL ) */
/* C */
/* C Search from the beginning of the CK file through all */
/* C of the segments. */
/* C */
/* CALL DAFBFS ( HANDLE ) */
/* CALL DAFFNA ( SFND ) */
/* FND = .FALSE. */
/* DO WHILE ( ( SFND ) .AND. ( .NOT. FND ) ) */
/* C */
/* C Get the segment identifier and descriptor. */
/* C */
/* CALL DAFGN ( IDENT ) */
/* CALL DAFGS ( DESCR ) */
/* C */
/* C Unpack the segment descriptor into its integer and */
/* C double precision components. */
/* C */
/* CALL DAFUS ( DESCR, 2, 6, DCD, ICD ) */
/* C */
/* C Determine if this segment should be processed. */
/* C */
/* IF ( ( INST .EQ. ICD( 1 ) ) .AND. */
/* . ( SCLKDP + TOL .GE. DCD( 1 ) ) .AND. */
/* . ( SCLKDP - TOL .LE. DCD( 2 ) ) .AND. */
/* . ( DTYPE .EQ. ICD( 3 ) ) ) THEN */
/* CALL CKR03 ( HANDLE, DESCR, SCLKDP, TOL, NEEDAV, */
/* . RECORD, FND ) */
/* IF ( FND ) THEN */
/* CALL CKE03 (NEEDAV,RECORD,CMAT,AV,CLKOUT) */
/* CALL SCDECD ( SC, CLKOUT, SCTIME ) */
/* WRITE (*,*) */
/* WRITE (*,*) 'Segment identifier: ', IDENT */
/* WRITE (*,*) */
/* WRITE (*,*) 'Pointing returned for time: ', */
/* . SCTIME */
/* WRITE (*,*) */
/* WRITE (*,*) 'C-matrix:' */
/* WRITE (*,*) */
/* WRITE (*,*) ( CMAT(1,I), I = 1, 3 ) */
/* WRITE (*,*) ( CMAT(2,I), I = 1, 3 ) */
/* WRITE (*,*) ( CMAT(3,I), I = 1, 3 ) */
/* WRITE (*,*) */
/* END IF */
/* END IF */
/* CALL DAFFNA ( SFND ) */
/* END DO */
/* $ Restrictions */
/* 1) No explicit checking is done on the input RECORD. */
/* $ Literature_References */
/* None. */
/* $ Author_and_Institution */
/* J.M. Lynch (JPL) */
/* F.S. Turner (JPL) */
/* $ Version */
/* - SPICELIB Version 2.0.1, 22-AUG-2006 (EDW) */
/* Replaced references to LDPOOL with references */
/* to FURNSH. */
/* - SPICELIB Version 2.0.0, 13-JUN-2002 (FST) */
/* This routine now participates in error handling properly. */
/* - SPICELIB Version 1.0.0, 25-NOV-1992 (JML) */
/* -& */
/* $ Index_Entries */
/* evaluate ck type_3 pointing data record */
/* -& */
/* $ Revisions */
/* - SPICELIB Version 2.0.0, 13-JUN-2002 (FST) */
/* Calls to CHKIN and CHKOUT in the standard SPICE error */
/* handling style were added. Versions prior to 2.0.0 */
/* were error free, however changes to RAXISA from error */
/* free to error signaling forced this update. */
/* Additionally, FAILED is now checked after the call to */
/* RAXISA. This prevents garbage from being placed into */
/* the output arguments. */
/* -& */
/* SPICELIB functions */
/* Local variables */
/* Standard SPICE error handling. */
if (return_()) {
return 0;
} else {
chkin_("CKE03", (ftnlen)5);
}
/* Unpack the record, for easier reading. */
t = record[16];
t1 = record[0];
t2 = record[8];
moved_(&record[1], &c__4, q1);
moved_(&record[5], &c__3, av1);
moved_(&record[9], &c__4, q2);
moved_(&record[13], &c__3, av2);
/* If T1 and T2 are the same then no interpolation or extrapolation */
/* is performed. Simply convert the quaternion to a C-matrix and */
/* return. */
if (t1 == t2) {
q2m_(q1, cmat);
*clkout = t1;
if (*needav) {
vequ_(av1, av);
}
chkout_("CKE03", (ftnlen)5);
return 0;
}
/* Interpolate between the two pointing instances to obtain pointing */
/* at the request time. */
/* Calculate what fraction of the interval the request time */
/* represents. */
frac = (t - t1) / (t2 - t1);
/* Convert the left and right quaternions to C-matrices. */
q2m_(q1, cmat1);
q2m_(q2, cmat2);
/* Find the matrix that rotates the spacecraft instrument frame from */
/* the orientation specified by CMAT1 to that specified by CMAT2. */
/* Then find the axis and angle of that rotation matrix. */
/* T T */
/* CMAT2 = ROT * CMAT1 */
/* T */
/* ROT = CMAT2 * CMAT1 */
mtxm_(cmat2, cmat1, rot);
raxisa_(rot, axis, &angle);
if (failed_()) {
chkout_("CKE03", (ftnlen)5);
return 0;
}
/* Calculate the matrix that rotates vectors about the vector AXIS */
/* by the angle ANGLE * FRAC. */
d__1 = angle * frac;
axisar_(axis, &d__1, delta);
/* The interpolated pointing at the request time is given by CMAT */
/* where: */
/* T T */
/* CMAT = DELTA * CMAT1 */
/* and */
/* T */
/* CMAT = CMAT1 * DELTA */
mxmt_(cmat1, delta, cmat);
/* Set CLKOUT equal to the time that pointing is being returned. */
*clkout = t;
/* If angular velocity is requested then take a weighted average */
/* of the angular velocities at the left and right endpoints. */
if (*needav) {
d__1 = 1. - frac;
vlcom_(&d__1, av1, &frac, av2, av);
}
chkout_("CKE03", (ftnlen)5);
return 0;
} /* cke03_ */
/* $Procedure M2EUL ( Matrix to Euler angles ) */
/* Subroutine */ int m2eul_(doublereal *r__, integer *axis3, integer *axis2,
integer *axis1, doublereal *angle3, doublereal *angle2, doublereal *
angle1)
{
/* Initialized data */
static integer next[3] = { 2,3,1 };
/* System generated locals */
integer i__1, i__2;
/* Builtin functions */
integer s_rnge(char *, integer, char *, integer);
double acos(doublereal), atan2(doublereal, doublereal), asin(doublereal);
/* Local variables */
doublereal sign;
extern /* Subroutine */ int vhat_(doublereal *, doublereal *), mtxm_(
doublereal *, doublereal *, doublereal *);
integer c__, i__;
logical degen;
extern /* Subroutine */ int chkin_(char *, ftnlen);
extern logical isrot_(doublereal *, doublereal *, doublereal *);
doublereal change[9] /* was [3][3] */;
extern /* Subroutine */ int cleard_(integer *, doublereal *), sigerr_(
char *, ftnlen), chkout_(char *, ftnlen);
doublereal tmpmat[9] /* was [3][3] */;
extern /* Subroutine */ int setmsg_(char *, ftnlen), errint_(char *,
integer *, ftnlen);
extern logical return_(void);
doublereal tmprot[9] /* was [3][3] */;
extern /* Subroutine */ int mxm_(doublereal *, doublereal *, doublereal *)
;
/* $ Abstract */
/* Factor a rotation matrix as a product of three rotations about */
/* specified coordinate axes. */
/* $ 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 */
/* ANGLE */
/* MATRIX */
/* ROTATION */
/* TRANSFORMATION */
/* $ Declarations */
/* $ Brief_I/O */
/* Variable I/O Description */
/* -------- --- -------------------------------------------------- */
/* R I A rotation matrix to be factored. */
/* AXIS3, */
/* AXIS2, */
/* AXIS1 I Numbers of third, second, and first rotation axes. */
/* ANGLE3, */
/* ANGLE2, */
/* ANGLE1 O Third, second, and first Euler angles, in radians. */
/* $ Detailed_Input */
/* R is a 3x3 rotation matrix that is to be factored as */
/* a product of three rotations about a specified */
/* coordinate axes. The angles of these rotations are */
/* called `Euler angles'. */
/* AXIS3, */
/* AXIS2, */
/* AXIS1 are the indices of the rotation axes of the */
/* `factor' rotations, whose product is R. R is */
/* factored as */
/* R = [ ANGLE3 ] [ ANGLE2 ] [ ANGLE1 ] . */
/* AXIS3 AXIS2 AXIS1 */
/* The axis numbers must belong to the set {1, 2, 3}. */
/* The second axis number MUST differ from the first */
/* and third axis numbers. */
/* See the $ Particulars section below for details */
/* concerning this notation. */
/* $ Detailed_Output */
/* ANGLE3, */
/* ANGLE2, */
/* ANGLE1 are the Euler angles corresponding to the matrix */
/* R and the axes specified by AXIS3, AXIS2, and */
/* AXIS1. These angles satisfy the equality */
/* R = [ ANGLE3 ] [ ANGLE2 ] [ ANGLE1 ] */
/* AXIS3 AXIS2 AXIS1 */
/* See the $ Particulars section below for details */
/* concerning this notation. */
/* The range of ANGLE3 and ANGLE1 is (-pi, pi]. */
/* The range of ANGLE2 depends on the exact set of */
/* axes used for the factorization. For */
/* factorizations in which the first and third axes */
/* are the same, */
/* R = [r] [s] [t] , */
/* a b a */
/* the range of ANGLE2 is [0, pi]. */
/* For factorizations in which the first and third */
/* axes are different, */
/* R = [r] [s] [t] , */
/* a b c */
/* the range of ANGLE2 is [-pi/2, pi/2]. */
/* For rotations such that ANGLE3 and ANGLE1 are not */
/* uniquely determined, ANGLE3 will always be set to */
/* zero; ANGLE1 is then uniquely determined. */
/* $ Parameters */
/* None. */
/* $ Exceptions */
/* 1) If any of AXIS3, AXIS2, or AXIS1 do not have values in */
/* { 1, 2, 3 }, */
/* then the error SPICE(BADAXISNUMBERS) is signaled. */
/* 2) An arbitrary rotation matrix cannot be expressed using */
/* a sequence of Euler angles unless the second rotation axis */
/* differs from the other two. If AXIS2 is equal to AXIS3 or */
/* AXIS1, then then error SPICE(BADAXISNUMBERS) is signaled. */
/* 3) If the input matrix R is not a rotation matrix, the error */
/* SPICE(NOTAROTATION) is signaled. */
/* 4) If ANGLE3 and ANGLE1 are not uniquely determined, ANGLE3 */
/* is set to zero, and ANGLE1 is determined. */
/* $ Files */
/* None. */
/* $ Particulars */
/* A word about notation: the symbol */
/* [ x ] */
/* i */
/* indicates a coordinate system rotation of x radians about the */
/* ith coordinate axis. To be specific, the symbol */
/* [ x ] */
/* 1 */
/* indicates a coordinate system rotation of x radians about the */
/* first, or x-, axis; the corresponding matrix is */
/* +- -+ */
/* | 1 0 0 | */
/* | | */
/* | 0 cos(x) sin(x) |. */
/* | | */
/* | 0 -sin(x) cos(x) | */
/* +- -+ */
/* Remember, this is a COORDINATE SYSTEM rotation by x radians; this */
/* matrix, when applied to a vector, rotates the vector by -x */
/* radians, not x radians. Applying the matrix to a vector yields */
/* the vector's representation relative to the rotated coordinate */
/* system. */
/* The analogous rotation about the second, or y-, axis is */
/* represented by */
/* [ x ] */
/* 2 */
/* which symbolizes the matrix */
/* +- -+ */
/* | cos(x) 0 -sin(x) | */
/* | | */
/* | 0 1 0 |, */
/* | | */
/* | sin(x) 0 cos(x) | */
/* +- -+ */
/* and the analogous rotation about the third, or z-, axis is */
/* represented by */
/* [ x ] */
/* 3 */
/* which symbolizes the matrix */
/* +- -+ */
/* | cos(x) sin(x) 0 | */
/* | | */
/* | -sin(x) cos(x) 0 |. */
/* | | */
/* | 0 0 1 | */
/* +- -+ */
/* The input matrix is assumed to be the product of three */
/* rotation matrices, each one of the form */
/* +- -+ */
/* | 1 0 0 | */
/* | | */
/* | 0 cos(r) sin(r) | (rotation of r radians about the */
/* | | x-axis), */
/* | 0 -sin(r) cos(r) | */
/* +- -+ */
/* +- -+ */
/* | cos(s) 0 -sin(s) | */
/* | | */
/* | 0 1 0 | (rotation of s radians about the */
/* | | y-axis), */
/* | sin(s) 0 cos(s) | */
/* +- -+ */
/* or */
/* +- -+ */
/* | cos(t) sin(t) 0 | */
/* | | */
/* | -sin(t) cos(t) 0 | (rotation of t radians about the */
/* | | z-axis), */
/* | 0 0 1 | */
/* +- -+ */
/* where the second rotation axis is not equal to the first or */
/* third. Any rotation matrix can be factored as a sequence of */
/* three such rotations, provided that this last criterion is met. */
/* This routine is related to the SPICELIB routine EUL2M, which */
/* produces a rotation matrix, given a sequence of Euler angles. */
/* This routine is a `right inverse' of EUL2M: the sequence of */
/* calls */
/* CALL M2EUL ( R, AXIS3, AXIS2, AXIS1, */
/* . ANGLE3, ANGLE2, ANGLE1 ) */
/* CALL EUL2M ( ANGLE3, ANGLE2, ANGLE1, */
/* . AXIS3, AXIS2, AXIS1, R ) */
/* preserves R, except for round-off error. */
/* On the other hand, the sequence of calls */
/* CALL EUL2M ( ANGLE3, ANGLE2, ANGLE1, */
/* . AXIS3, AXIS2, AXIS1, R ) */
/* CALL M2EUL ( R, AXIS3, AXIS2, AXIS1, */
/* . ANGLE3, ANGLE2, ANGLE1 ) */
/* preserve ANGLE3, ANGLE2, and ANGLE1 only if these angles start */
/* out in the ranges that M2EUL's outputs are restricted to. */
/* $ Examples */
/* 1) Conversion of instrument pointing from a matrix representation */
/* to Euler angles: */
/* Suppose we want to find camera pointing in alpha, delta, and */
/* kappa, given the inertial-to-camera coordinate transformation */
/* +- -+ */
/* | 0.49127379678135830 0.50872620321864170 0.70699908539882417 | */
/* | | */
/* | -0.50872620321864193 -0.49127379678135802 0.70699908539882428 | */
/* | | */
/* | 0.70699908539882406 -0.70699908539882439 0.01745240643728360 | */
/* +- -+ */
/* We want to find angles alpha, delta, kappa such that */
/* TICAM = [ kappa ] [ pi/2 - delta ] [ pi/2 + alpha ] . */
/* 3 1 3 */
/* We can use the following small program to do this computation: */
/* PROGRAM EX1 */
/* IMPLICIT NONE */
/* DOUBLE PRECISION DPR */
/* DOUBLE PRECISION HALFPI */
/* DOUBLE PRECISION TWOPI */
/* DOUBLE PRECISION ALPHA */
/* DOUBLE PRECISION ANG1 */
/* DOUBLE PRECISION ANG2 */
/* DOUBLE PRECISION DELTA */
/* DOUBLE PRECISION KAPPA */
/* DOUBLE PRECISION TICAM ( 3, 3 ) */
/* DATA TICAM / 0.49127379678135830D0, */
/* . -0.50872620321864193D0, */
/* . 0.70699908539882406D0, */
/* . 0.50872620321864170D0, */
/* . -0.49127379678135802D0, */
/* . -0.70699908539882439D0, */
/* . 0.70699908539882417D0, */
/* . 0.70699908539882428D0, */
/* . 0.01745240643728360D0 / */
/* CALL M2EUL ( TICAM, 3, 1, 3, KAPPA, ANG2, ANG1 ) */
/* DELTA = HALFPI() - ANG2 */
/* ALPHA = ANG1 - HALFPI() */
/* IF ( KAPPA .LT. 0.D0 ) THEN */
/* KAPPA = KAPPA + TWOPI() */
/* END IF */
/* IF ( ALPHA .LT. 0.D0 ) THEN */
/* ALPHA = ALPHA + TWOPI() */
/* END IF */
/* WRITE (*,'(1X,A,F24.14)') 'Alpha (deg): ', DPR() * ALPHA */
/* WRITE (*,'(1X,A,F24.14)') 'Delta (deg): ', DPR() * DELTA */
/* WRITE (*,'(1X,A,F24.14)') 'Kappa (deg): ', DPR() * KAPPA */
/* END */
/* The program's output should be something like */
/* Alpha (deg): 315.00000000000000 */
/* Delta (deg): 1.00000000000000 */
/* Kappa (deg): 45.00000000000000 */
/* possibly formatted a little differently, or degraded slightly */
/* by round-off. */
/* 2) Conversion of instrument pointing angles from a non-J2000, */
/* not necessarily inertial frame to J2000-relative RA, Dec, */
/* and Twist. */
/* Suppose that we have pointing for some instrument expressed as */
/* [ gamma ] [ beta ] [ alpha ] */
/* 3 2 3 */
/* with respect to some coordinate system S. For example, S */
/* could be a spacecraft-fixed system. */
/* We will suppose that the transformation from J2000 */
/* coordinates to system S coordinates is given by the rotation */
/* matrix J2S. */
/* The rows of J2S are the unit basis vectors of system S, given */
/* in J2000 coordinates. */
/* We want to express the pointing with respect to the J2000 */
/* system as the sequence of rotations */
/* [ kappa ] [ pi/2 - delta ] [ pi/2 + alpha ] . */
/* 3 1 3 */
/* First, we use subroutine EUL2M to form the transformation */
/* from system S to instrument coordinates S2INST. */
/* CALL EUL2M ( GAMMA, BETA, ALPHA, 3, 2, 3, S2INST ) */
/* Next, we form the transformation from J2000 to instrument */
/* coordinates J2INST. */
/* CALL MXM ( S2INST, J2S, J2INST ) */
/* Finally, we express J2INST using the desired Euler angles, as */
/* in the first example: */
/* CALL M2EUL ( J2INST, 3, 1, 3, TWIST, ANG2, ANG3 ) */
/* RA = ANG3 - HALFPI() */
/* DEC = HALFPI() - ANG2 */
/* If we wish to make sure that RA, DEC, and TWIST are in */
/* the ranges [0, 2pi), [-pi/2, pi/2], and [0, 2pi) */
/* respectively, we may add the code */
/* IF ( RA .LT. 0.D0 ) THEN */
/* RA = RA + TWOPI() */
/* END IF */
/* IF ( TWIST .LT. 0.D0 ) THEN */
/* TWIST = TWIST + TWOPI() */
/* END IF */
/* Note that DEC is already in the correct range, since ANG2 */
/* is in the range [0, pi] when the first and third input axes */
/* are equal. */
/* Now RA, DEC, and TWIST express the instrument pointing */
/* as RA, Dec, and Twist, relative to the J2000 system. */
/* A warning note: more than one definition of RA, Dec, and */
/* Twist is extant. Before using this example in an application, */
/* check that the definition given here is consistent with that */
/* used in your application. */
/* $ Restrictions */
/* None. */
/* $ Literature_References */
/* None. */
/* $ Author_and_Institution */
/* N.J. Bachman (JPL) */
/* $ Version */
/* - SPICELIB Version 1.2.1, 21-DEC-2006 (NJB) */
/* Error corrected in header example: input matrix */
/* previously did not match shown outputs. Offending */
/* example now includes complete program. */
/* - SPICELIB Version 1.2.0, 15-OCT-2005 (NJB) */
/* Updated to remove non-standard use of duplicate arguments */
/* in MXM and MTXM calls. A short error message cited in */
/* the Exceptions section of the header failed to match */
/* the actual short message used; this has been corrected. */
/* - SPICELIB Version 1.1.2, 13-OCT-2004 (NJB) */
/* Fixed header typo. */
/* - SPICELIB Version 1.1.1, 10-MAR-1992 (WLT) */
/* Comment section for permuted index source lines was added */
/* following the header. */
/* - SPICELIB Version 1.1.0, 02-NOV-1990 (NJB) */
/* Header upgraded to describe notation in more detail. Argument */
/* names were changed to describe the use of the arguments more */
/* accurately. No change in functionality was made; the operation */
/* of the routine is identical to that of the previous version. */
/* - SPICELIB Version 1.0.0, 03-SEP-1990 (NJB) */
/* -& */
/* $ Index_Entries */
/* matrix to euler angles */
/* -& */
/* $ Revisions */
/* - SPICELIB Version 1.2.0, 26-AUG-2005 (NJB) */
/* Updated to remove non-standard use of duplicate arguments */
/* in MXM and MTXM calls. A short error message cited in */
/* the Exceptions section of the header failed to match */
/* the actual short message used; this has been corrected. */
/* - SPICELIB Version 1.1.0, 02-NOV-1990 (NJB) */
/* Argument names were changed to describe the use of the */
/* arguments more accurately. The axis and angle numbers */
/* now decrease, rather than increase, from left to right. */
/* The current names reflect the order of operator application */
/* when the Euler angle rotations are applied to a vector: the */
/* rightmost matrix */
/* [ ANGLE1 ] */
/* AXIS1 */
/* is applied to the vector first, followed by */
/* [ ANGLE2 ] */
/* AXIS2 */
/* and then */
/* [ ANGLE3 ] */
/* AXIS3 */
/* Previously, the names reflected the order in which the Euler */
/* angle matrices appear on the page, from left to right. This */
/* naming convention was found to be a bit obtuse by a various */
/* users. */
/* No change in functionality was made; the operation of the */
/* routine is identical to that of the previous version. */
/* Also, the header was upgraded to describe the notation in more */
/* detail. The symbol */
/* [ x ] */
/* i */
/* is explained at mind-numbing length. An example was added */
/* that shows a specific set of inputs and the resulting output */
/* matrix. */
/* The angle sequence notation was changed to be consistent with */
/* Rotations required reading. */
/* 1-2-3 and a-b-c */
/* have been changed to */
/* 3-2-1 and c-b-a. */
/* Also, one `)' was changed to a `}'. */
/* The phrase `first and third' was changed to `first or third' */
/* in the $ Particulars section, where the criterion for the */
/* existence of an Euler angle factorization is stated. */
/* -& */
/* SPICELIB functions */
/* Local parameters */
/* NTOL and DETOL are used to determine whether R is a rotation */
/* matrix. */
/* NTOL is the tolerance for the norms of the columns of R. */
/* DTOL is the tolerance for the determinant of a matrix whose */
/* columns are the unitized columns of R. */
/* Local variables */
/* Saved variables */
/* Initial values */
/* Standard SPICE error handling. */
if (return_()) {
return 0;
} else {
chkin_("M2EUL", (ftnlen)5);
}
/* The first order of business is to screen out the goofy cases. */
/* Make sure the axis numbers are all right: They must belong to */
/* the set {1, 2, 3}... */
if (*axis3 < 1 || *axis3 > 3 || (*axis2 < 1 || *axis2 > 3) || (*axis1 < 1
|| *axis1 > 3)) {
setmsg_("Axis numbers are #, #, #. ", (ftnlen)28);
errint_("#", axis3, (ftnlen)1);
errint_("#", axis2, (ftnlen)1);
errint_("#", axis1, (ftnlen)1);
sigerr_("SPICE(BADAXISNUMBERS)", (ftnlen)21);
chkout_("M2EUL", (ftnlen)5);
return 0;
/* ...and the second axis number must differ from its neighbors. */
} else if (*axis3 == *axis2 || *axis1 == *axis2) {
setmsg_("Middle axis matches neighbor: # # #.", (ftnlen)36);
errint_("#", axis3, (ftnlen)1);
errint_("#", axis2, (ftnlen)1);
errint_("#", axis1, (ftnlen)1);
sigerr_("SPICE(BADAXISNUMBERS)", (ftnlen)21);
chkout_("M2EUL", (ftnlen)5);
return 0;
/* R must be a rotation matrix, or we may as well forget it. */
} else if (! isrot_(r__, &c_b15, &c_b15)) {
setmsg_("Input matrix is not a rotation.", (ftnlen)31);
sigerr_("SPICE(NOTAROTATION)", (ftnlen)19);
chkout_("M2EUL", (ftnlen)5);
return 0;
}
/* AXIS3, AXIS2, AXIS1 and R have passed their tests at this */
/* point. We take the liberty of working with TMPROT, a version */
/* of R that has unitized columns. */
for (i__ = 1; i__ <= 3; ++i__) {
vhat_(&r__[(i__1 = i__ * 3 - 3) < 9 && 0 <= i__1 ? i__1 : s_rnge(
"r", i__1, "m2eul_", (ftnlen)667)], &tmprot[(i__2 = i__ * 3 -
3) < 9 && 0 <= i__2 ? i__2 : s_rnge("tmprot", i__2, "m2eul_",
(ftnlen)667)]);
}
/* We now proceed to recover the promised Euler angles from */
/* TMPROT. */
/* The ideas behind our method are explained in excruciating */
/* detail in the ROTATION required reading, so we'll be terse. */
/* Nonetheless, a word of explanation is in order. */
/* The sequence of rotation axes used for the factorization */
/* belongs to one of two categories: a-b-a or c-b-a. We */
/* wish to handle each of these cases in one shot, rather than */
/* using different formulas for each sequence to recover the */
/* Euler angles. */
/* What we're going to do is use the Euler angle formula for the */
/* 3-1-3 factorization for all of the a-b-a sequences, and the */
/* formula for the 3-2-1 factorization for all of the c-b-a */
/* sequences. */
/* How can we get away with this? The Euler angle formulas for */
/* each factorization are different! */
/* Our trick is to apply a change-of-basis transformation to the */
/* input matrix R. For the a-b-a factorizations, we choose a new */
/* basis such that a rotation of ANGLE3 radians about the basis */
/* vector indexed by AXIS3 becomes a rotation of ANGLE3 radians */
/* about the third coordinate axis, and such that a rotation of */
/* ANGLE2 radians about the basis vector indexed by AXIS2 becomes */
/* a rotation of ANGLE2 radians about the first coordinate axis. */
/* So R can be factored as a 3-1-3 rotation relative to the new */
/* basis, and the Euler angles we obtain are the exact ones we */
/* require. */
/* The c-b-a factorizations can be handled in an analogous */
/* fashion. We transform R to a basis where the original axis */
/* sequence becomes a 3-2-1 sequence. In some cases, the angles */
/* we obtain will be the negatives of the angles we require. This */
/* will happen if and only if the ordered basis (here the e's are */
/* the standard basis vectors) */
/* { e e e } */
/* AXIS3 AXIS2 AXIS1 */
/* is not right-handed. An easy test for this condition is that */
/* AXIS2 is not the successor of AXIS3, where the ordering is */
/* cyclic. */
if (*axis3 == *axis1) {
/* The axis order is a-b-a. We're going to find a matrix CHANGE */
/* such that */
/* T */
/* CHANGE R CHANGE */
/* gives us R in the a useful basis, that is, a basis in which */
/* our original a-b-a rotation is a 3-1-3 rotation, but where the */
/* rotation angles are unchanged. To achieve this pleasant */
/* simplification, we set column 3 of CHANGE to to e(AXIS3), */
/* column 1 of CHANGE to e(AXIS2), and column 2 of CHANGE to */
/* (+/-) e(C), */
/* (C is the remaining index) depending on whether */
/* AXIS3-AXIS2-C is a right-handed sequence of axes: if it */
/* is, the sign is positive. (Here e(1), e(2), e(3) are the */
/* standard basis vectors.) */
/* Determine the sign of our third basis vector, so that we can */
/* ensure that our new basis is right-handed. The variable NEXT */
/* is just a little mapping that takes 1 to 2, 2 to 3, and 3 to */
/* 1. */
if (*axis2 == next[(i__1 = *axis3 - 1) < 3 && 0 <= i__1 ? i__1 :
s_rnge("next", i__1, "m2eul_", (ftnlen)746)]) {
sign = 1.;
} else {
sign = -1.;
}
/* Since the axis indices sum to 6, */
c__ = 6 - *axis3 - *axis2;
/* Set up the entries of CHANGE: */
cleard_(&c__9, change);
change[(i__1 = *axis3 + 5) < 9 && 0 <= i__1 ? i__1 : s_rnge("change",
i__1, "m2eul_", (ftnlen)762)] = 1.;
change[(i__1 = *axis2 - 1) < 9 && 0 <= i__1 ? i__1 : s_rnge("change",
i__1, "m2eul_", (ftnlen)763)] = 1.;
change[(i__1 = c__ + 2) < 9 && 0 <= i__1 ? i__1 : s_rnge("change",
i__1, "m2eul_", (ftnlen)764)] = sign * 1.;
/* Transform TMPROT. */
mxm_(tmprot, change, tmpmat);
mtxm_(change, tmpmat, tmprot);
/* Now we're ready to find the Euler angles, using a */
/* 3-1-3 factorization. In general, the matrix product */
/* [ a1 ] [ a2 ] [ a3 ] */
/* 3 1 3 */
/* has the form */
/* +- -+ */
/* | cos(a1)cos(a3) cos(a1)sin(a3) sin(a1)sin(a2) | */
/* | -sin(a1)cos(a2)sin(a3) +sin(a1)cos(a2)cos(a3) | */
/* | | */
/* | -sin(a1)cos(a3) -sin(a1)sin(a3) cos(a1)sin(a2) | */
/* | -cos(a1)cos(a2)sin(a3) +cos(a1)cos(a2)cos(a3) | */
/* | | */
/* | sin(a2)sin(a3) -sin(a2)cos(a3) cos(a2) | */
/* +- -+ */
/* but if a2 is 0 or pi, the product matrix reduces to */
/* +- -+ */
/* | cos(a1)cos(a3) cos(a1)sin(a3) 0 | */
/* | -sin(a1)cos(a2)sin(a3) +sin(a1)cos(a2)cos(a3) | */
/* | | */
/* | -sin(a1)cos(a3) -sin(a1)sin(a3) 0 | */
/* | -cos(a1)cos(a2)sin(a3) +cos(a1)cos(a2)cos(a3) | */
/* | | */
/* | 0 0 cos(a2) | */
/* +- -+ */
/* In this case, a1 and a3 are not uniquely determined. If we */
/* arbitrarily set a1 to zero, we arrive at the matrix */
/* +- -+ */
/* | cos(a3) sin(a3) 0 | */
/* | -cos(a2)sin(a3) cos(a2)cos(a3) 0 | */
/* | 0 0 cos(a2) | */
/* +- -+ */
/* We take care of this case first. We test three conditions */
/* that are mathematically equivalent, but may not be satisfied */
/* simultaneously because of round-off: */
degen = tmprot[6] == 0. && tmprot[7] == 0. || tmprot[2] == 0. &&
tmprot[5] == 0. || abs(tmprot[8]) == 1.;
/* In the following block of code, we make use of the fact that */
/* SIN ( ANGLE2 ) > 0 */
/* - */
/* in choosing the signs of the ATAN2 arguments correctly. Note */
/* that ATAN2(x,y) = -ATAN2(-x,-y). */
if (degen) {
*angle3 = 0.;
*angle2 = acos(tmprot[8]);
*angle1 = atan2(tmprot[3], tmprot[0]);
} else {
/* The normal case. */
*angle3 = atan2(tmprot[6], tmprot[7]);
*angle2 = acos(tmprot[8]);
*angle1 = atan2(tmprot[2], -tmprot[5]);
}
} else {
/* The axis order is c-b-a. We're going to find a matrix CHANGE */
/* such that */
/* T */
/* CHANGE R CHANGE */
/* gives us R in the a useful basis, that is, a basis in which */
/* our original c-b-a rotation is a 3-2-1 rotation, but where the */
/* rotation angles are unchanged, or at worst negated. To */
/* achieve this pleasant simplification, we set column 1 of */
/* CHANGE to to e(AXIS3), column 2 of CHANGE to e(AXIS2), and */
/* column 3 of CHANGE to */
/* (+/-) e(AXIS1), */
/* depending on whether AXIS3-AXIS2-AXIS1 is a right-handed */
/* sequence of axes: if it is, the sign is positive. (Here */
/* e(1), e(2), e(3) are the standard basis vectors.) */
/* We must be cautious here, because if AXIS3-AXIS2-AXIS1 is a */
/* right-handed sequence of axes, all of the rotation angles will */
/* be the same in our new basis, but if it's a left-handed */
/* sequence, the third angle will be negated. Let's get this */
/* straightened out right now. The variable NEXT is just a */
/* little mapping that takes 1 to 2, 2 to 3, and 3 to 1. */
if (*axis2 == next[(i__1 = *axis3 - 1) < 3 && 0 <= i__1 ? i__1 :
s_rnge("next", i__1, "m2eul_", (ftnlen)883)]) {
sign = 1.;
} else {
sign = -1.;
}
/* Set up the entries of CHANGE: */
cleard_(&c__9, change);
change[(i__1 = *axis3 - 1) < 9 && 0 <= i__1 ? i__1 : s_rnge("change",
i__1, "m2eul_", (ftnlen)894)] = 1.;
change[(i__1 = *axis2 + 2) < 9 && 0 <= i__1 ? i__1 : s_rnge("change",
i__1, "m2eul_", (ftnlen)895)] = 1.;
change[(i__1 = *axis1 + 5) < 9 && 0 <= i__1 ? i__1 : s_rnge("change",
i__1, "m2eul_", (ftnlen)896)] = sign * 1.;
/* Transform TMPROT. */
mxm_(tmprot, change, tmpmat);
mtxm_(change, tmpmat, tmprot);
/* Now we're ready to find the Euler angles, using a */
/* 3-2-1 factorization. In general, the matrix product */
/* [ a1 ] [ a2 ] [ a3 ] */
/* 1 2 3 */
/* has the form */
/* +- -+ */
/* | cos(a2)cos(a3) cos(a2)sin(a3) -sin(a2) | */
/* | | */
/* | -cos(a1)sin(a3) cos(a1)cos(a3) sin(a1)cos(a2) | */
/* | +sin(a1)sin(a2)cos(a3) +sin(a1)sin(a2)sin(a3) | */
/* | | */
/* | sin(a1)sin(a3) -sin(a1)cos(a3) cos(a1)cos(a2) | */
/* | +cos(a1)sin(a2)cos(a3) +cos(a1)sin(a2)sin(a3) | */
/* +- -+ */
/* but if a2 is -pi/2 or pi/2, the product matrix reduces to */
/* +- -+ */
/* | 0 0 -sin(a2) | */
/* | | */
/* | -cos(a1)sin(a3) cos(a1)cos(a3) 0 | */
/* | +sin(a1)sin(a2)cos(a3) +sin(a1)sin(a2)sin(a3) | */
/* | | */
/* | sin(a1)sin(a3) -sin(a1)cos(a3) 0 | */
/* | +cos(a1)sin(a2)cos(a3) +cos(a1)sin(a2)sin(a3) | */
/* +- -+ */
/* In this case, a1 and a3 are not uniquely determined. If we */
/* arbitrarily set a1 to zero, we arrive at the matrix */
/* +- -+ */
/* | 0 0 -sin(a2) | */
/* | -sin(a3) cos(a3) 0 |, */
/* | sin(a2)cos(a3) sin(a2)sin(a3) 0 | */
/* +- -+ */
/* We take care of this case first. We test three conditions */
/* that are mathematically equivalent, but may not be satisfied */
/* simultaneously because of round-off: */
degen = tmprot[0] == 0. && tmprot[3] == 0. || tmprot[7] == 0. &&
tmprot[8] == 0. || abs(tmprot[6]) == 1.;
/* In the following block of code, we make use of the fact that */
/* COS ( ANGLE2 ) > 0 */
/* - */
/* in choosing the signs of the ATAN2 arguments correctly. Note */
/* that ATAN2(x,y) = -ATAN2(-x,-y). */
if (degen) {
*angle3 = 0.;
*angle2 = asin(-tmprot[6]);
*angle1 = sign * atan2(-tmprot[1], tmprot[4]);
} else {
/* The normal case. */
*angle3 = atan2(tmprot[7], tmprot[8]);
*angle2 = asin(-tmprot[6]);
*angle1 = sign * atan2(tmprot[3], tmprot[0]);
}
}
chkout_("M2EUL", (ftnlen)5);
return 0;
} /* m2eul_ */
/* Subroutine */ int deri1_(doublereal *c__, integer *norbs, doublereal *
coord, integer *number, doublereal *work, doublereal *grad,
doublereal *f, integer *minear, doublereal *fd, doublereal *wmat,
doublereal *hmat, doublereal *fmat)
{
/* Initialized data */
static integer icalcn = 0;
/* System generated locals */
integer c_dim1, c_offset, work_dim1, work_offset, i__1, i__2, i__3, i__4,
i__5, i__6, i__7, i__8;
/* Builtin functions */
integer i_indx(char *, char *, ftnlen, ftnlen), s_wsle(cilist *), do_lio(
integer *, integer *, char *, ftnlen), e_wsle(void), s_wsfe(
cilist *), do_fio(integer *, char *, ftnlen), e_wsfe(void);
/* Local variables */
static integer i__, j, k, l, n1, n2, ll;
static doublereal gse;
extern doublereal dot_(doublereal *, doublereal *, integer *);
extern /* Subroutine */ int mxm_(doublereal *, integer *, doublereal *,
integer *, doublereal *, integer *);
static integer nend, nati, lcut, loop, natx;
static doublereal step;
static integer iprt;
extern /* Subroutine */ int mtxm_(doublereal *, integer *, doublereal *,
integer *, doublereal *, integer *), mecid_(doublereal *,
doublereal *, doublereal *, doublereal *), mecih_(doublereal *,
doublereal *, integer *, integer *);
static logical debug;
extern /* Subroutine */ int timer_(char *, ftnlen);
static integer ninit;
extern /* Subroutine */ int scopy_(integer *, doublereal *, integer *,
doublereal *, integer *), dfock2_(doublereal *, doublereal *,
doublereal *, doublereal *, integer *, integer *, integer *,
integer *, integer *), dijkl1_(doublereal *, integer *, integer *,
doublereal *, doublereal *, doublereal *, doublereal *);
static doublereal enucl2;
extern /* Subroutine */ int dhcore_(doublereal *, doublereal *,
doublereal *, doublereal *, integer *, integer *, doublereal *);
extern doublereal helect_(integer *, doublereal *, doublereal *,
doublereal *);
static integer linear;
extern /* Subroutine */ int supdot_(doublereal *, doublereal *,
doublereal *, integer *, integer *);
/* Fortran I/O blocks */
static cilist io___11 = { 0, 6, 0, 0, 0 };
static cilist io___13 = { 0, 6, 0, "(5F12.6)", 0 };
static cilist io___22 = { 0, 6, 0, 0, 0 };
static cilist io___23 = { 0, 0, 0, "(' * * * GRADIENT COMPONENT NUMBER',"
"I4)", 0 };
static cilist io___24 = { 0, 0, 0, "(' NON-RELAXED C.I-ACTIVE FOCK EIGEN"
"VALUES ', 'DERIVATIVES (E.V.)')", 0 };
static cilist io___25 = { 0, 0, 0, "(8F10.4)", 0 };
static cilist io___26 = { 0, 0, 0, "(' NON-RELAXED 2-ELECTRONS DERIVATIV"
"ES (E.V.)'/ ' I J K L d<I(1)J(1)|K(2)L(2)>')",
0 };
static cilist io___28 = { 0, 0, 0, "(4I5,F20.10)", 0 };
static cilist io___29 = { 0, 0, 0, "(' NON-RELAXED GRADIENT COMPONENT',F"
"10.4, ' KCAL/MOLE')", 0 };
/* COMDECK SIZES */
/* *********************************************************************** */
/* THIS FILE CONTAINS ALL THE ARRAY SIZES FOR USE IN MOPAC. */
/* THERE ARE ONLY 5 PARAMETERS THAT THE PROGRAMMER NEED SET: */
/* MAXHEV = MAXIMUM NUMBER OF HEAVY ATOMS (HEAVY: NON-HYDROGEN ATOMS) */
/* MAXLIT = MAXIMUM NUMBER OF HYDROGEN ATOMS. */
/* MAXTIM = DEFAULT TIME FOR A JOB. (SECONDS) */
/* MAXDMP = DEFAULT TIME FOR AUTOMATIC RESTART FILE GENERATION (SECS) */
/* ISYBYL = 1 IF MOPAC IS TO BE USED IN THE SYBYL PACKAGE, =0 OTHERWISE */
/* SEE ALSO NMECI, NPULAY AND MESP AT THE END OF THIS FILE */
/* *********************************************************************** */
/* THE FOLLOWING CODE DOES NOT NEED TO BE ALTERED BY THE PROGRAMMER */
/* *********************************************************************** */
/* ALL OTHER PARAMETERS ARE DERIVED FUNCTIONS OF THESE TWO PARAMETERS */
/* NAME DEFINITION */
/* NUMATM MAXIMUM NUMBER OF ATOMS ALLOWED. */
/* MAXORB MAXIMUM NUMBER OF ORBITALS ALLOWED. */
/* MAXPAR MAXIMUM NUMBER OF PARAMETERS FOR OPTIMISATION. */
/* N2ELEC MAXIMUM NUMBER OF TWO ELECTRON INTEGRALS ALLOWED. */
/* MPACK AREA OF LOWER HALF TRIANGLE OF DENSITY MATRIX. */
/* MORB2 SQUARE OF THE MAXIMUM NUMBER OF ORBITALS ALLOWED. */
/* MAXHES AREA OF HESSIAN MATRIX */
/* MAXALL LARGER THAN MAXORB OR MAXPAR. */
/* *********************************************************************** */
/* *********************************************************************** */
/* DECK MOPAC */
/* ******************************************************************** */
/* DERI1 COMPUTE THE NON-RELAXED DERIVATIVE OF THE NON-VARIATIONALLY */
/* OPTIMIZED WAVEFUNCTION ENERGY WITH RESPECT TO ONE CARTESIAN */
/* COORDINATE AT A TIME */
/* AND */
/* COMPUTE THE NON-RELAXED FOCK MATRIX DERIVATIVE IN M.O BASIS AS */
/* REQUIRED IN THE RELAXATION SECTION (ROUTINE 'DERI2'). */
/* INPUT */
/* C(NORBS,NORBS) : M.O. COEFFICIENTS. */
/* COORD : CARTESIAN COORDINATES ARRAY. */
/* NUMBER : LOCATION OF THE REQUIRED VARIABLE IN COORD. */
/* WORK : WORK ARRAY OF SIZE N*N. */
/* WMAT : WORK ARRAYS FOR d<PQ|RS> (2-CENTERS A.O) */
/* OUTPUT */
/* C,COORD,NUMBER : NOT MODIFIED. */
/* GRAD : DERIVATIVE OF THE HEAT OF FORMATION WITH RESPECT TO */
/* COORD(NUMBER), WITHOUT RELAXATION CORRECTION. */
/* F(MINEAR) : NON-RELAXED FOCK MATRIX DERIVATIVE WITH RESPECT TO */
/* COORD(NUMBER), EXPRESSED IN M.O BASIS, SCALED AND */
/* PACKED, OFF-DIAGONAL BLOCKS ONLY. */
/* FD : IDEM BUT UNSCALED, DIAGONAL BLOCKS, C.I-ACTIVE ONLY. */
/* *********************************************************************** */
/* Parameter adjustments */
work_dim1 = *norbs;
work_offset = 1 + work_dim1 * 1;
work -= work_offset;
c_dim1 = *norbs;
c_offset = 1 + c_dim1 * 1;
c__ -= c_offset;
--coord;
--f;
--fd;
--wmat;
--hmat;
--fmat;
/* Function Body */
if (icalcn != numcal_1.numcal) {
debug = i_indx(keywrd_1.keywrd, "DERI1", (ftnlen)241, (ftnlen)5) != 0;
iprt = 6;
linear = *norbs * (*norbs + 1) / 2;
icalcn = numcal_1.numcal;
}
if (debug) {
timer_("BEFORE DERI1", (ftnlen)12);
}
step = .001;
/* 2 POINTS FINITE DIFFERENCE TO GET THE INTEGRAL DERIVATIVES */
/* ---------------------------------------------------------- */
/* STORED IN HMAT AND WMAT, WITHOUT DIVIDING BY THE STEP SIZE. */
nati = (*number - 1) / 3 + 1;
natx = *number - (nati - 1) * 3;
dhcore_(&coord[1], &hmat[1], &wmat[1], &enucl2, &nati, &natx, &step);
/* HMAT HOLDS THE ONE-ELECTRON DERIVATIVES OF ATOM NATI FOR DIRECTION */
/* NATX W.R.T. ALL OTHER ATOMS */
/* WMAT HOLDS THE TWO-ELECTRON DERIVATIVES OF ATOM NATI FOR DIRECTION */
/* NATX W.R.T. ALL OTHER ATOMS */
step = .5 / step;
/* NON-RELAXED FOCK MATRIX DERIVATIVE IN A.O BASIS. */
/* ------------------------------------------------ */
/* STORED IN FMAT, DIVIDED BY STEP. */
scopy_(&linear, &hmat[1], &c__1, &fmat[1], &c__1);
dfock2_(&fmat[1], densty_1.p, densty_1.pa, &wmat[1], &molkst_1.numat,
molkst_1.nfirst, molkst_1.nmidle, molkst_1.nlast, &nati);
/* FMAT HOLDS THE ONE PLUS TWO - ELECTRON DERIVATIVES OF ATOM NATI FOR */
/* DIRECTION NATX W.R.T. ALL OTHER ATOMS */
/* DERIVATIVE OF THE SCF-ONLY ENERGY (I.E BEFORE C.I CORRECTION) */
*grad = (helect_(norbs, densty_1.p, &hmat[1], &fmat[1]) + enucl2) * step;
/* TAKE STEP INTO ACCOUNT IN FMAT */
i__1 = linear;
for (i__ = 1; i__ <= i__1; ++i__) {
/* L10: */
fmat[i__] *= step;
}
/* RIGHT-HAND SIDE SUPER-VECTOR F = C' FMAT C USED IN RELAXATION */
/* ----------------------------------------------------------- */
/* STORED IN NON-STANDARD PACKED FORM IN F(MINEAR) AND FD. */
/* THE SUPERVECTOR IS THE NON-RELAXED FOCK MATRIX DERIVATIVE IN */
/* M.O BASIS: F(IJ)= ( (C' * FOCK * C)(I,J) ) WITH I.GT.J . */
/* F IS SCALED AND PACKED IN SUPERVECTOR FORM WITH */
/* THE CONSECUTIVE FOLLOWING OFF-DIAGONAL BLOCKS: */
/* 1) OPEN-CLOSED I.E. F(IJ)=F(I,J) WITH I OPEN & J CLOSED */
/* AND I RUNNING FASTER THAN J, */
/* 2) VIRTUAL-CLOSED SAME RULE OF ORDERING, */
/* 3) VIRTUAL-OPEN SAME RULE OF ORDERING. */
/* FD IS PACKED OVER THE C.I-ACTIVE M.O WITH */
/* THE CONSECUTIVE DIAGONAL BLOCKS: */
/* 1) CLOSED-CLOSED IN CANONICAL ORDER, WITHOUT THE */
/* DIAGONAL ELEMENTS, */
/* 2) OPEN-OPEN SAME RULE OF ORDERING, */
/* 3) VIRTUAL-VIRTUAL SAME RULE OF ORDERING. */
/* PART 1 : WORK(N,N) = FMAT(N,N) * C(N,N) */
i__1 = *norbs;
for (i__ = 1; i__ <= i__1; ++i__) {
/* L20: */
supdot_(&work[i__ * work_dim1 + 1], &fmat[1], &c__[i__ * c_dim1 + 1],
norbs, &c__1);
}
/* PART 2 : F(IJ) = (C' * WORK)(I,J) ... OFF-DIAGONAL BLOCKS. */
l = 1;
if (cibits_1.nbo[1] != 0 && cibits_1.nbo[0] != 0) {
/* OPEN-CLOSED */
mtxm_(&c__[(cibits_1.nbo[0] + 1) * c_dim1 + 1], &cibits_1.nbo[1], &
work[work_offset], norbs, &f[l], cibits_1.nbo);
l += cibits_1.nbo[1] * cibits_1.nbo[0];
}
if (cibits_1.nbo[2] != 0 && cibits_1.nbo[0] != 0) {
/* VIRTUAL-CLOSED */
mtxm_(&c__[(molkst_1.nopen + 1) * c_dim1 + 1], &cibits_1.nbo[2], &
work[work_offset], norbs, &f[l], cibits_1.nbo);
l += cibits_1.nbo[2] * cibits_1.nbo[0];
}
if (cibits_1.nbo[2] != 0 && cibits_1.nbo[1] != 0) {
/* VIRTUAL-OPEN */
mtxm_(&c__[(molkst_1.nopen + 1) * c_dim1 + 1], &cibits_1.nbo[2], &
work[(cibits_1.nbo[0] + 1) * work_dim1 + 1], norbs, &f[l], &
cibits_1.nbo[1]);
}
/* SCALE F ACCORDING TO THE DIAGONAL METRIC TENSOR 'SCALAR '. */
i__1 = *minear;
for (i__ = 1; i__ <= i__1; ++i__) {
/* L30: */
f[i__] *= fokmat_1.scalar[i__ - 1];
}
if (debug) {
s_wsle(&io___11);
do_lio(&c__9, &c__1, " F IN DERI1", (ftnlen)11);
e_wsle();
j = min(20,*minear);
s_wsfe(&io___13);
i__1 = j;
for (i__ = 1; i__ <= i__1; ++i__) {
do_fio(&c__1, (char *)&f[i__], (ftnlen)sizeof(doublereal));
}
e_wsfe();
}
/* PART 3 : SUPER-VECTOR FD, C.I-ACTIVE DIAGONAL BLOCKS, UNSCALED. */
l = 1;
nend = 0;
for (loop = 1; loop <= 3; ++loop) {
ninit = nend + 1;
nend += cibits_1.nbo[loop - 1];
/* Computing MAX */
i__1 = ninit, i__2 = cibits_1.nelec + 1;
n1 = max(i__1,i__2);
/* Computing MIN */
i__1 = nend, i__2 = cibits_1.nelec + cibits_1.nmos;
n2 = min(i__1,i__2);
if (n2 < n1) {
goto L50;
}
i__1 = n2;
for (i__ = n1; i__ <= i__1; ++i__) {
if (i__ > ninit) {
i__2 = i__ - ninit;
mxm_(&c__[i__ * c_dim1 + 1], &c__1, &work[ninit * work_dim1 +
1], norbs, &fd[l], &i__2);
l = l + i__ - ninit;
}
/* L40: */
}
L50:
;
}
/* NON-RELAXED C.I CORRECTION TO THE ENERGY DERIVATIVE. */
/* ---------------------------------------------------- */
/* C.I-ACTIVE FOCK EIGENVALUES DERIVATIVES, STORED IN FD(CONTINUED). */
lcut = l;
i__1 = cibits_1.nelec + cibits_1.nmos;
for (i__ = cibits_1.nelec + 1; i__ <= i__1; ++i__) {
fd[l] = dot_(&c__[i__ * c_dim1 + 1], &work[i__ * work_dim1 + 1],
norbs);
/* L60: */
++l;
}
/* C.I-ACTIVE 2-ELECTRONS INTEGRALS DERIVATIVES. STORED IN XY. */
/* FMAT IS USED HERE AS SCRATCH SPACE */
dijkl1_(&c__[(cibits_1.nelec + 1) * c_dim1 + 1], norbs, &nati, &wmat[1], &
fmat[1], &hmat[1], &fmat[1]);
i__1 = cibits_1.nmos;
for (i__ = 1; i__ <= i__1; ++i__) {
i__2 = cibits_1.nmos;
for (j = 1; j <= i__2; ++j) {
i__3 = cibits_1.nmos;
for (k = 1; k <= i__3; ++k) {
i__4 = cibits_1.nmos;
for (l = 1; l <= i__4; ++l) {
/* L70: */
xyijkl_1.xy[i__ + (j + (k + (l << 3) << 3) << 3) - 585] *=
step;
}
}
}
}
/* BUILD THE C.I MATRIX DERIVATIVE, STORED IN WMAT. */
mecid_(&fd[lcut - cibits_1.nelec], &gse, vector_1.eigb, &work[work_offset]
);
if (debug) {
s_wsle(&io___22);
do_lio(&c__9, &c__1, " GSE:", (ftnlen)5);
do_lio(&c__5, &c__1, (char *)&gse, (ftnlen)sizeof(doublereal));
e_wsle();
/* # WRITE(6,*)' EIGB:',(EIGB(I),I=1,10) */
/* # WRITE(6,*)' WORK:',(WORK(I,1),I=1,10) */
}
mecih_(&work[work_offset], &wmat[1], &cibits_1.nmos, &cibits_1.lab);
/* NON-RELAXED C.I CONTRIBUTION TO THE ENERGY DERIVATIVE. */
supdot_(&work[work_offset], &wmat[1], civect_1.conf, &cibits_1.lab, &c__1)
;
*grad = (*grad + dot_(civect_1.conf, &work[work_offset], &cibits_1.lab)) *
23.061;
if (debug) {
io___23.ciunit = iprt;
s_wsfe(&io___23);
do_fio(&c__1, (char *)&(*number), (ftnlen)sizeof(integer));
e_wsfe();
io___24.ciunit = iprt;
s_wsfe(&io___24);
e_wsfe();
io___25.ciunit = iprt;
s_wsfe(&io___25);
i__4 = cibits_1.nmos;
for (i__ = 1; i__ <= i__4; ++i__) {
do_fio(&c__1, (char *)&fd[lcut - 1 + i__], (ftnlen)sizeof(
doublereal));
}
e_wsfe();
io___26.ciunit = iprt;
s_wsfe(&io___26);
e_wsfe();
i__4 = cibits_1.nmos;
for (i__ = 1; i__ <= i__4; ++i__) {
i__3 = i__;
for (j = 1; j <= i__3; ++j) {
i__2 = i__;
for (k = 1; k <= i__2; ++k) {
ll = k;
if (k == i__) {
ll = j;
}
i__1 = ll;
for (l = 1; l <= i__1; ++l) {
/* L80: */
io___28.ciunit = iprt;
s_wsfe(&io___28);
i__5 = cibits_1.nelec + i__;
do_fio(&c__1, (char *)&i__5, (ftnlen)sizeof(integer));
i__6 = cibits_1.nelec + j;
do_fio(&c__1, (char *)&i__6, (ftnlen)sizeof(integer));
i__7 = cibits_1.nelec + k;
do_fio(&c__1, (char *)&i__7, (ftnlen)sizeof(integer));
i__8 = cibits_1.nelec + l;
do_fio(&c__1, (char *)&i__8, (ftnlen)sizeof(integer));
do_fio(&c__1, (char *)&xyijkl_1.xy[i__ + (j + (k + (l
<< 3) << 3) << 3) - 585], (ftnlen)sizeof(
doublereal));
e_wsfe();
}
}
}
}
io___29.ciunit = iprt;
s_wsfe(&io___29);
do_fio(&c__1, (char *)&(*grad), (ftnlen)sizeof(doublereal));
e_wsfe();
timer_("AFTER DERI1", (ftnlen)11);
}
return 0;
} /* deri1_ */
