/src/m2q.c
C | 632 lines | 74 code | 209 blank | 349 comment | 11 complexity | d659e81eabdea158fd545d9e1d2bd32f MD5 | raw file
- /* m2q.f -- translated by f2c (version 19980913).
- You must link the resulting object file with the libraries:
- -lf2c -lm (in that order)
- */
- #include "f2c.h"
- /* Table of constant values */
- static doublereal c_b2 = .1;
- /* $Procedure M2Q ( Matrix to quaternion ) */
- /* Subroutine */ int m2q_(doublereal *r__, doublereal *q)
- {
- /* Builtin functions */
- double sqrt(doublereal);
- /* Local variables */
- doublereal c__, s[3];
- extern /* Subroutine */ int chkin_(char *, ftnlen);
- doublereal trace, l2;
- extern logical isrot_(doublereal *, doublereal *, doublereal *);
- doublereal mtrace, factor, cc4;
- extern /* Subroutine */ int sigerr_(char *, ftnlen), chkout_(char *,
- ftnlen);
- doublereal polish;
- extern /* Subroutine */ int setmsg_(char *, ftnlen);
- doublereal s114, s224, s334;
- /* $ Abstract */
- /* Find a unit quaternion corresponding to a specified rotation */
- /* 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 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 */
- /* MATRIX */
- /* ROTATION */
- /* $ Declarations */
- /* $ Brief_I/O */
- /* Variable I/O Description */
- /* -------- --- -------------------------------------------------- */
- /* R I A rotation matrix. */
- /* Q O A unit quaternion representing R. */
- /* $ Detailed_Input */
- /* R is a rotation matrix. */
- /* $ Detailed_Output */
- /* Q is a unit-length SPICE-style quaternion */
- /* representing R. See the discussion of quaternion */
- /* styles in Particulars below. */
- /* Q is a 4-dimensional vector. If R rotates vectors */
- /* in the counterclockwise sense by an angle of theta */
- /* radians about a unit vector A, where */
- /* 0 < theta < pi */
- /* - - */
- /* then letting h = theta/2, */
- /* Q = ( cos(h), sin(h)A , sin(h)A , sin(h)A ). */
- /* 1 2 3 */
- /* The restriction that theta must be in the range */
- /* [0, pi] determines the output quaternion Q */
- /* uniquely except when theta = pi; in this special */
- /* case, both of the quaternions */
- /* Q = ( 0, A , A , A ) */
- /* 1 2 3 */
- /* and */
- /* Q = ( 0, -A , -A , -A ) */
- /* 1 2 3 */
- /* are possible outputs. */
- /* $ Parameters */
- /* None. */
- /* $ Exceptions */
- /* 1) If R is not a rotation matrix, the error SPICE(NOTAROTATION) */
- /* is signaled. */
- /* $ Files */
- /* None. */
- /* $ Particulars */
- /* A unit quaternion is a 4-dimensional vector for which the sum of */
- /* the squares of the components is 1. Unit quaternions can be used */
- /* to represent rotations in the following way: given a rotation */
- /* angle theta, where */
- /* 0 < theta < pi */
- /* - - */
- /* and a unit vector A, we can represent the transformation that */
- /* rotates vectors in the counterclockwise sense by theta radians */
- /* about A using the quaternion Q, where */
- /* Q = */
- /* ( cos(theta/2), sin(theta/2)a , sin(theta/2)a , sin(theta/2)a ) */
- /* 1 2 3 */
- /* As mentioned in Detailed Output, our restriction on the range of */
- /* theta determines Q uniquely, except when theta = pi. */
- /* The SPICELIB routine Q2M is an one-sided inverse of this routine: */
- /* given any rotation matrix R, the calls */
- /* CALL M2Q ( R, Q ) */
- /* CALL Q2M ( Q, R ) */
- /* leave R unchanged, except for round-off error. However, the */
- /* calls */
- /* CALL Q2M ( Q, R ) */
- /* CALL M2Q ( R, Q ) */
- /* might preserve Q or convert Q to -Q. */
- /* 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) A case amenable to checking by hand calculation: */
- /* To convert the rotation matrix */
- /* +- -+ */
- /* | 0 1 0 | */
- /* | | */
- /* R = | -1 0 0 | */
- /* | | */
- /* | 0 0 1 | */
- /* +- -+ */
- /* also represented as */
- /* [ pi/2 ] */
- /* 3 */
- /* to a quaternion, we can use the code fragment */
- /* CALL ROTATE ( HALFPI(), 3, R ) */
- /* CALL M2Q ( R, Q ) */
- /* M2Q will return Q as */
- /* ( sqrt(2)/2, 0, 0, -sqrt(2)/2 ) */
- /* Why? Well, R is a reference frame transformation that */
- /* rotates vectors by -pi/2 radians about the axis vector */
- /* A = ( 0, 0, 1 ) */
- /* Equivalently, R rotates vectors by pi/2 radians in */
- /* the counterclockwise sense about the axis vector */
- /* -A = ( 0, 0, -1 ) */
- /* so our definition of Q, */
- /* h = theta/2 */
- /* Q = ( cos(h), sin(h)A , sin(h)A , sin(h)A ) */
- /* 1 2 3 */
- /* implies that in this case, */
- /* Q = ( cos(pi/4), 0, 0, -sin(pi/4) ) */
- /* = ( sqrt(2)/2, 0, 0, -sqrt(2)/2 ) */
- /* 2) Finding a quaternion that represents a rotation specified by */
- /* a set of Euler angles: */
- /* Suppose our original rotation R is the product */
- /* [ TAU ] [ pi/2 - DELTA ] [ ALPHA ] */
- /* 3 2 3 */
- /* The code fragment */
- /* CALL EUL2M ( TAU, HALFPI() - DELTA, ALPHA, */
- /* . 3, 2, 3, R ) */
- /* CALL M2Q ( R, Q ) */
- /* yields a quaternion Q that represents R. */
- /* $ Restrictions */
- /* None. */
- /* $ Literature_References */
- /* None. */
- /* $ Author_and_Institution */
- /* N.J. Bachman (JPL) */
- /* W.L. Taber (JPL) */
- /* $ Version */
- /* - SPICELIB Version 2.0.1, 27-FEB-2008 (NJB) */
- /* Updated header; added information about SPICE */
- /* quaternion conventions. Made various minor edits */
- /* throughout header. */
- /* - SPICELIB Version 2.0.0, 17-SEP-1999 (WLT) */
- /* The routine was re-implemented to sharpen the numerical */
- /* stability of the routine and eliminate calls to SIN */
- /* and COS functions. */
- /* - 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, 30-AUG-1990 (NJB) */
- /* -& */
- /* $ Index_Entries */
- /* matrix to quaternion */
- /* -& */
- /* 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 */
- /* If R is not a rotation matrix, we can't proceed. */
- if (! isrot_(r__, &c_b2, &c_b2)) {
- chkin_("M2Q", (ftnlen)3);
- setmsg_("Input matrix was not a rotation.", (ftnlen)32);
- sigerr_("SPICE(NOTAROTATION)", (ftnlen)19);
- chkout_("M2Q", (ftnlen)3);
- return 0;
- }
- /* If our quaternion is C, S1, S2, S3 (the S's being the imaginary */
- /* part) and we let */
- /* CSi = C * Si */
- /* Sij = Si * Sj */
- /* then the rotation matrix corresponding to our quaternion is: */
- /* R(1,1) = 1.0D0 - 2*S22 - 2*S33 */
- /* R(2,1) = 2*S12 + 2*CS3 */
- /* R(3,1) = 2*S13 - 2*CS2 */
- /* R(1,2) = 2*S12 - 2*CS3 */
- /* R(2,2) = 1.0D0 - 2*S11 - 2*S33 */
- /* R(3,2) = 2*S23 + 2*CS1 */
- /* R(1,3) = 2*S13 + 2*CS2 */
- /* R(2,3) = 2*S23 - 2*CS1 */
- /* R(3,3) = 1.0D0 - 2*S11 - 2*S22 */
- /* From the above we can see that */
- /* TRACE = 3 - 4*(S11 + S22 + S33) */
- /* so that */
- /* 1.0D0 + TRACE = 4 - 4*(S11 + S22 + S33) */
- /* = 4*(CC + S11 + S22 + S33) */
- /* - 4*(S11 + S22 + S33) */
- /* = 4*CC */
- /* Thus up to sign */
- /* C = 0.5D0 * DSQRT( 1.0D0 + TRACE ) */
- /* But we also have */
- /* 1.0D0 + TRACE - 2.0D0*R(i,i) = 4.0D0 - 4.0D0(Sii + Sjj + Skk) */
- /* - 2.0D0 + 4.0D0(Sjj + Skk ) */
- /* = 2.0D0 - 4.0D0*Sii */
- /* So that */
- /* 1.0D0 - TRACE + 2.0D0*R(i,i) = 4.0D0*Sii */
- /* and so up to sign */
- /* Si = 0.5D0*DSQRT( 1.0D0 - TRACE + 2.0D0*R(i,i) ) */
- /* in addition to this observation, we note that all of the */
- /* product pairs can easily be computed */
- /* CS1 = (R(3,2) - R(2,3))/4.0D0 */
- /* CS2 = (R(1,3) - R(3,1))/4.0D0 */
- /* CS3 = (R(2,1) - R(1,2))/4.0D0 */
- /* S12 = (R(2,1) + R(1,2))/4.0D0 */
- /* S13 = (R(3,1) + R(1,3))/4.0D0 */
- /* S23 = (R(2,3) + R(3,2))/4.0D0 */
- /* But taking sums or differences of numbers that are nearly equal */
- /* or nearly opposite results in a loss of precision. As a result */
- /* we should take some care in which terms to select when computing */
- /* C, S1, S2, S3. However, by simply starting with one of the */
- /* large quantities cc, S11, S22, or S33 we can make sure that we */
- /* use the best of the 6 quantities above when computing the */
- /* remaining components of the quaternion. */
- trace = r__[0] + r__[4] + r__[8];
- mtrace = 1. - trace;
- cc4 = trace + 1.;
- s114 = mtrace + r__[0] * 2.;
- s224 = mtrace + r__[4] * 2.;
- s334 = mtrace + r__[8] * 2.;
- /* Note that if you simply add CC4 + S114 + S224 + S334 */
- /* you get four. Thus at least one of the 4 terms is greater than 1. */
- if (1. <= cc4) {
- c__ = sqrt(cc4 * .25);
- factor = 1. / (c__ * 4.);
- s[0] = (r__[5] - r__[7]) * factor;
- s[1] = (r__[6] - r__[2]) * factor;
- s[2] = (r__[1] - r__[3]) * factor;
- } else if (1. <= s114) {
- s[0] = sqrt(s114 * .25);
- factor = 1. / (s[0] * 4.);
- c__ = (r__[5] - r__[7]) * factor;
- s[1] = (r__[3] + r__[1]) * factor;
- s[2] = (r__[6] + r__[2]) * factor;
- } else if (1. <= s224) {
- s[1] = sqrt(s224 * .25);
- factor = 1. / (s[1] * 4.);
- c__ = (r__[6] - r__[2]) * factor;
- s[0] = (r__[3] + r__[1]) * factor;
- s[2] = (r__[7] + r__[5]) * factor;
- } else {
- s[2] = sqrt(s334 * .25);
- factor = 1. / (s[2] * 4.);
- c__ = (r__[1] - r__[3]) * factor;
- s[0] = (r__[6] + r__[2]) * factor;
- s[1] = (r__[7] + r__[5]) * factor;
- }
- /* If the magnitude of this quaternion is not one, we polish it */
- /* up a bit. */
- l2 = c__ * c__ + s[0] * s[0] + s[1] * s[1] + s[2] * s[2];
- if (l2 != 1.) {
- polish = 1. / sqrt(l2);
- c__ *= polish;
- s[0] *= polish;
- s[1] *= polish;
- s[2] *= polish;
- }
- if (c__ > 0.) {
- q[0] = c__;
- q[1] = s[0];
- q[2] = s[1];
- q[3] = s[2];
- } else {
- q[0] = -c__;
- q[1] = -s[0];
- q[2] = -s[1];
- q[3] = -s[2];
- }
- return 0;
- } /* m2q_ */