PageRenderTime 26ms CodeModel.GetById 17ms RepoModel.GetById 1ms app.codeStats 0ms

/src/qdq2av.c

https://github.com/mattbornski/spice
C | 722 lines | 18 code | 229 blank | 475 comment | 0 complexity | 6f45a749f5a1d043824f9f87c86ddddd MD5 | raw file
  1. /* qdq2av.f -- translated by f2c (version 19980913).
  2. You must link the resulting object file with the libraries:
  3. -lf2c -lm (in that order)
  4. */
  5. #include "f2c.h"
  6. /* Table of constant values */
  7. static integer c__4 = 4;
  8. static doublereal c_b3 = -2.;
  9. /* $Procedure QDQ2AV (Quaternion and quaternion derivative to a.v.) */
  10. /* Subroutine */ int qdq2av_(doublereal *q, doublereal *dq, doublereal *av)
  11. {
  12. doublereal qhat[4];
  13. extern /* Subroutine */ int vscl_(doublereal *, doublereal *, doublereal *
  14. ), vhatg_(doublereal *, integer *, doublereal *);
  15. doublereal qtemp[4], qstar[4];
  16. extern /* Subroutine */ int vminus_(doublereal *, doublereal *), qxq_(
  17. doublereal *, doublereal *, doublereal *);
  18. /* $ Abstract */
  19. /* Derive angular velocity from a unit quaternion and its derivative */
  20. /* with respect to time. */
  21. /* $ Disclaimer */
  22. /* THIS SOFTWARE AND ANY RELATED MATERIALS WERE CREATED BY THE */
  23. /* CALIFORNIA INSTITUTE OF TECHNOLOGY (CALTECH) UNDER A U.S. */
  24. /* GOVERNMENT CONTRACT WITH THE NATIONAL AERONAUTICS AND SPACE */
  25. /* ADMINISTRATION (NASA). THE SOFTWARE IS TECHNOLOGY AND SOFTWARE */
  26. /* PUBLICLY AVAILABLE UNDER U.S. EXPORT LAWS AND IS PROVIDED "AS-IS" */
  27. /* TO THE RECIPIENT WITHOUT WARRANTY OF ANY KIND, INCLUDING ANY */
  28. /* WARRANTIES OF PERFORMANCE OR MERCHANTABILITY OR FITNESS FOR A */
  29. /* PARTICULAR USE OR PURPOSE (AS SET FORTH IN UNITED STATES UCC */
  30. /* SECTIONS 2312-2313) OR FOR ANY PURPOSE WHATSOEVER, FOR THE */
  31. /* SOFTWARE AND RELATED MATERIALS, HOWEVER USED. */
  32. /* IN NO EVENT SHALL CALTECH, ITS JET PROPULSION LABORATORY, OR NASA */
  33. /* BE LIABLE FOR ANY DAMAGES AND/OR COSTS, INCLUDING, BUT NOT */
  34. /* LIMITED TO, INCIDENTAL OR CONSEQUENTIAL DAMAGES OF ANY KIND, */
  35. /* INCLUDING ECONOMIC DAMAGE OR INJURY TO PROPERTY AND LOST PROFITS, */
  36. /* REGARDLESS OF WHETHER CALTECH, JPL, OR NASA BE ADVISED, HAVE */
  37. /* REASON TO KNOW, OR, IN FACT, SHALL KNOW OF THE POSSIBILITY. */
  38. /* RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF */
  39. /* THE SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY */
  40. /* CALTECH AND NASA FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE */
  41. /* ACTIONS OF RECIPIENT IN THE USE OF THE SOFTWARE. */
  42. /* $ Required_Reading */
  43. /* ROTATION */
  44. /* $ Keywords */
  45. /* MATH */
  46. /* POINTING */
  47. /* ROTATION */
  48. /* $ Declarations */
  49. /* $ Brief_I/O */
  50. /* VARIABLE I/O DESCRIPTION */
  51. /* -------- --- -------------------------------------------------- */
  52. /* Q I Unit SPICE quaternion. */
  53. /* DQ I Derivative of Q with respect to time. */
  54. /* AV O Angular velocity defined by Q and DQ. */
  55. /* $ Detailed_Input */
  56. /* Q is a unit length 4-vector representing a */
  57. /* SPICE-style quaternion. See the discussion of */
  58. /* quaternion styles in Particulars below. */
  59. /* DQ is a 4-vector representing the derivative of */
  60. /* Q with respect to time. */
  61. /* $ Detailed_Output */
  62. /* AV is 3-vector representing the angular velocity */
  63. /* defined by Q and DQ, that is, the angular velocity */
  64. /* of the frame defined by the rotation matrix */
  65. /* associated with Q. This rotation matrix can be */
  66. /* obtained via the SPICELIB routine Q2M; see the */
  67. /* Particulars section for the explicit matrix */
  68. /* entries. */
  69. /* AV is the vector (imaginary) part of the */
  70. /* quaternion product */
  71. /* * */
  72. /* -2 * Q * DQ */
  73. /* This angular velocity is the same vector that */
  74. /* could be obtained (much less efficiently ) by */
  75. /* mapping Q and DQ to the corresponding C-matrix R */
  76. /* and its derivative DR, then calling the SPICELIB */
  77. /* routine XF2RAV. */
  78. /* AV has units of */
  79. /* radians / T */
  80. /* where */
  81. /* 1 / T */
  82. /* is the unit associated with DQ. */
  83. /* $ Parameters */
  84. /* None. */
  85. /* $ Exceptions */
  86. /* Error free. */
  87. /* 1) A unitized version of input quaternion is used in the */
  88. /* computation. No attempt is made to diagnose an invalid */
  89. /* input quaternion. */
  90. /* $ Files */
  91. /* None. */
  92. /* $ Particulars */
  93. /* Quaternion Styles */
  94. /* ----------------- */
  95. /* There are different "styles" of quaternions used in */
  96. /* science and engineering applications. Quaternion styles */
  97. /* are characterized by */
  98. /* - The order of quaternion elements */
  99. /* - The quaternion multiplication formula */
  100. /* - The convention for associating quaternions */
  101. /* with rotation matrices */
  102. /* Two of the commonly used styles are */
  103. /* - "SPICE" */
  104. /* > Invented by Sir William Rowan Hamilton */
  105. /* > Frequently used in mathematics and physics textbooks */
  106. /* - "Engineering" */
  107. /* > Widely used in aerospace engineering applications */
  108. /* SPICELIB subroutine interfaces ALWAYS use SPICE quaternions. */
  109. /* Quaternions of any other style must be converted to SPICE */
  110. /* quaternions before they are passed to SPICELIB routines. */
  111. /* Relationship between SPICE and Engineering Quaternions */
  112. /* ------------------------------------------------------ */
  113. /* Let M be a rotation matrix such that for any vector V, */
  114. /* M*V */
  115. /* is the result of rotating V by theta radians in the */
  116. /* counterclockwise direction about unit rotation axis vector A. */
  117. /* Then the SPICE quaternions representing M are */
  118. /* (+/-) ( cos(theta/2), */
  119. /* sin(theta/2) A(1), */
  120. /* sin(theta/2) A(2), */
  121. /* sin(theta/2) A(3) ) */
  122. /* while the engineering quaternions representing M are */
  123. /* (+/-) ( -sin(theta/2) A(1), */
  124. /* -sin(theta/2) A(2), */
  125. /* -sin(theta/2) A(3), */
  126. /* cos(theta/2) ) */
  127. /* For both styles of quaternions, if a quaternion q represents */
  128. /* a rotation matrix M, then -q represents M as well. */
  129. /* Given an engineering quaternion */
  130. /* QENG = ( q0, q1, q2, q3 ) */
  131. /* the equivalent SPICE quaternion is */
  132. /* QSPICE = ( q3, -q0, -q1, -q2 ) */
  133. /* Associating SPICE Quaternions with Rotation Matrices */
  134. /* ---------------------------------------------------- */
  135. /* Let FROM and TO be two right-handed reference frames, for */
  136. /* example, an inertial frame and a spacecraft-fixed frame. Let the */
  137. /* symbols */
  138. /* V , V */
  139. /* FROM TO */
  140. /* denote, respectively, an arbitrary vector expressed relative to */
  141. /* the FROM and TO frames. Let M denote the transformation matrix */
  142. /* that transforms vectors from frame FROM to frame TO; then */
  143. /* V = M * V */
  144. /* TO FROM */
  145. /* where the expression on the right hand side represents left */
  146. /* multiplication of the vector by the matrix. */
  147. /* Then if the unit-length SPICE quaternion q represents M, where */
  148. /* q = (q0, q1, q2, q3) */
  149. /* the elements of M are derived from the elements of q as follows: */
  150. /* +- -+ */
  151. /* | 2 2 | */
  152. /* | 1 - 2*( q2 + q3 ) 2*(q1*q2 - q0*q3) 2*(q1*q3 + q0*q2) | */
  153. /* | | */
  154. /* | | */
  155. /* | 2 2 | */
  156. /* M = | 2*(q1*q2 + q0*q3) 1 - 2*( q1 + q3 ) 2*(q2*q3 - q0*q1) | */
  157. /* | | */
  158. /* | | */
  159. /* | 2 2 | */
  160. /* | 2*(q1*q3 - q0*q2) 2*(q2*q3 + q0*q1) 1 - 2*( q1 + q2 ) | */
  161. /* | | */
  162. /* +- -+ */
  163. /* Note that substituting the elements of -q for those of q in the */
  164. /* right hand side leaves each element of M unchanged; this shows */
  165. /* that if a quaternion q represents a matrix M, then so does the */
  166. /* quaternion -q. */
  167. /* To map the rotation matrix M to a unit quaternion, we start by */
  168. /* decomposing the rotation matrix as a sum of symmetric */
  169. /* and skew-symmetric parts: */
  170. /* 2 */
  171. /* M = [ I + (1-cos(theta)) OMEGA ] + [ sin(theta) OMEGA ] */
  172. /* symmetric skew-symmetric */
  173. /* OMEGA is a skew-symmetric matrix of the form */
  174. /* +- -+ */
  175. /* | 0 -n3 n2 | */
  176. /* | | */
  177. /* OMEGA = | n3 0 -n1 | */
  178. /* | | */
  179. /* | -n2 n1 0 | */
  180. /* +- -+ */
  181. /* The vector N of matrix entries (n1, n2, n3) is the rotation axis */
  182. /* of M and theta is M's rotation angle. Note that N and theta */
  183. /* are not unique. */
  184. /* Let */
  185. /* C = cos(theta/2) */
  186. /* S = sin(theta/2) */
  187. /* Then the unit quaternions Q corresponding to M are */
  188. /* Q = +/- ( C, S*n1, S*n2, S*n3 ) */
  189. /* The mappings between quaternions and the corresponding rotations */
  190. /* are carried out by the SPICELIB routines */
  191. /* Q2M {quaternion to matrix} */
  192. /* M2Q {matrix to quaternion} */
  193. /* M2Q always returns a quaternion with scalar part greater than */
  194. /* or equal to zero. */
  195. /* SPICE Quaternion Multiplication Formula */
  196. /* --------------------------------------- */
  197. /* Given a SPICE quaternion */
  198. /* Q = ( q0, q1, q2, q3 ) */
  199. /* corresponding to rotation axis A and angle theta as above, we can */
  200. /* represent Q using "scalar + vector" notation as follows: */
  201. /* s = q0 = cos(theta/2) */
  202. /* v = ( q1, q2, q3 ) = sin(theta/2) * A */
  203. /* Q = s + v */
  204. /* Let Q1 and Q2 be SPICE quaternions with respective scalar */
  205. /* and vector parts s1, s2 and v1, v2: */
  206. /* Q1 = s1 + v1 */
  207. /* Q2 = s2 + v2 */
  208. /* We represent the dot product of v1 and v2 by */
  209. /* <v1, v2> */
  210. /* and the cross product of v1 and v2 by */
  211. /* v1 x v2 */
  212. /* Then the SPICE quaternion product is */
  213. /* Q1*Q2 = s1*s2 - <v1,v2> + s1*v2 + s2*v1 + (v1 x v2) */
  214. /* If Q1 and Q2 represent the rotation matrices M1 and M2 */
  215. /* respectively, then the quaternion product */
  216. /* Q1*Q2 */
  217. /* represents the matrix product */
  218. /* M1*M2 */
  219. /* About this routine */
  220. /* ================== */
  221. /* Given a time-dependent SPICE quaternion representing the */
  222. /* attitude of an object, we can obtain the object's angular */
  223. /* velocity AV in terms of the quaternion Q and its derivative */
  224. /* with respect to time DQ: */
  225. /* * */
  226. /* AV = Im ( -2 * Q * DQ ) (1) */
  227. /* That is, AV is the vector (imaginary) part of the product */
  228. /* on the right hand side (RHS) of equation (1). The scalar part */
  229. /* of the RHS is zero. */
  230. /* We'll now provide an explanation of formula (1). For any */
  231. /* time-dependent rotation, the associated angular velocity at a */
  232. /* given time is a function of the rotation and its derivative at */
  233. /* that time. This fact enables us to extend a proof for a limited */
  234. /* subset of rotations to *all* rotations: if we find a formula */
  235. /* that, for any rotation in our subset, gives us the angular */
  236. /* velocity as a function of the rotation and its derivative, then */
  237. /* that formula must be true for all rotations. */
  238. /* We start out by considering the set of rotation matrices */
  239. /* R(t) = M(t)C (2) */
  240. /* where C is a constant rotation matrix and M(t) represents a */
  241. /* matrix that "rotates" with constant, unit magnitude angular */
  242. /* velocity and that is equal to the identity matrix at t = 0. */
  243. /* For future reference, we'll consider C to represent a coordinate */
  244. /* transformation from frame F1 to frame F2. We'll call F1 the */
  245. /* "base frame" of C. We'll let AVF2 be the angular velocity of */
  246. /* M(t) relative to F2 and AVF1 be the same angular velocity */
  247. /* relative to F1. */
  248. /* Referring to the axis-and-angle decomposition of M(t) */
  249. /* 2 */
  250. /* M(t) = I + sin(t)OMEGA + (1-cos(t))OMEGA (3) */
  251. /* (see the Rotation Required Reading for a derivation) we */
  252. /* have */
  253. /* d(M(t))| */
  254. /* -------| = OMEGA (4) */
  255. /* dt |t=0 */
  256. /* Then the derivative of R(t) at t = 0 is given by */
  257. /* d(R(t))| */
  258. /* -------| = OMEGA * C (5) */
  259. /* dt |t=0 */
  260. /* The rotation axis A associated with OMEGA is defined by (6) */
  261. /* A(1) = - OMEGA(2,3) */
  262. /* A(2) = OMEGA(1,3) */
  263. /* A(3) = - OMEGA(1,2) */
  264. /* Since the coordinate system rotation M(t) rotates vectors about A */
  265. /* through angle t radians at time t, the angular velocity AVF2 of */
  266. /* M(t) is actually given by */
  267. /* AVF2 = - A (7) */
  268. /* This angular velocity is represented relative to the image */
  269. /* frame F2 associated with the coordinate transformation C. */
  270. /* Now, let's proceed to the angular velocity formula for */
  271. /* quaternions. */
  272. /* To avoid some verbiage, we'll freely use 3-vectors to represent */
  273. /* the corresponding pure imaginary quaternions. */
  274. /* Letting QR(t), QM(t), and QC be quaternions representing the */
  275. /* time-dependent matrices R(t), M(t) and C respectively, where */
  276. /* QM(t) is selected to be a differentiable function of t in a */
  277. /* neighborhood of t = 0, the quaternion representing R(t) is */
  278. /* QR(t) = QM(t) * QC (8) */
  279. /* Differentiating with respect to t, then evaluating derivatives */
  280. /* at t = 0, we have */
  281. /* d(QR(t))| d(QM(t))| */
  282. /* --------| = --------| * QC (9) */
  283. /* dt |t=0 dt |t=0 */
  284. /* Since QM(t) represents a rotation having axis A and rotation */
  285. /* angle t, then (according to the relationship between SPICE */
  286. /* quaternions and rotations set out in the Rotation Required */
  287. /* Reading), we see QM(t) must be the quaternion (represented as the */
  288. /* sum of scalar and vector parts): */
  289. /* cos(t/2) + sin(t/2) * A (10) */
  290. /* where A is the rotation axis corresponding to the matrix */
  291. /* OMEGA introduced in equation (3). By inspection */
  292. /* d(QM(t))| */
  293. /* --------| = 1/2 * A (11) */
  294. /* dt |t=0 */
  295. /* which is a quaternion with scalar part zero. This allows us to */
  296. /* rewrite the quaternion derivative */
  297. /* d(QR(t))| */
  298. /* --------| = 1/2 * A * QC (12) */
  299. /* dt |t=0 */
  300. /* or for short, */
  301. /* DQ = 1/2 * A * QC (13) */
  302. /* Since from (7) we know the angular velocity AVF2 of the frame */
  303. /* associated with QM(t) is the negative of the rotation axis */
  304. /* defined by (3), we have */
  305. /* DQ = - 1/2 * AVF2 * QC (14) */
  306. /* Since */
  307. /* AVF2 = C * AVF1 (15) */
  308. /* we can apply the quaternion transformation formula */
  309. /* (from the Rotation Required Reading) */
  310. /* * */
  311. /* AVF2 = QC * AVF1 * QC (16) */
  312. /* Now we re-write (15) as */
  313. /* * */
  314. /* DQ = - 1/2 * ( QC * AVF1 * QC ) * QC */
  315. /* = - 1/2 * QC * AVF1 (17) */
  316. /* Then the angular velocity vector AVF1 is given by */
  317. /* * */
  318. /* AVF1 = -2 * QC * DQ (18) */
  319. /* The relation (18) has now been demonstrated for quaternions */
  320. /* having constant, unit magnitude angular velocity. But since */
  321. /* all time-dependent quaternions having value QC and derivative */
  322. /* DQ at a given time t have the same angular velocity at time t, */
  323. /* that angular velocity must be AVF1. */
  324. /* $ Examples */
  325. /* The following test program creates a quaternion and quaternion */
  326. /* derivative from a known rotation matrix and angular velocity */
  327. /* vector. The angular velocity is recovered from the quaternion */
  328. /* and quaternion derivative by calling QDQ2AV and by an */
  329. /* alternate method; the results are displayed for comparison. */
  330. /* PROGRAM TQDQ2AV */
  331. /* IMPLICIT NONE */
  332. /* C */
  333. /* C Start with a known rotation and angular velocity. Find */
  334. /* C the quaternion and quaternion derivative. The latter is */
  335. /* C computed from */
  336. /* C */
  337. /* C * */
  338. /* C AV = -2 * Q * DQ */
  339. /* C */
  340. /* C DQ = -1/2 * Q * AV */
  341. /* C */
  342. /* C */
  343. /* C SPICELIB Functions */
  344. /* C */
  345. /* DOUBLE PRECISION RPD */
  346. /* C */
  347. /* C Local variables */
  348. /* C */
  349. /* DOUBLE PRECISION ANGLE ( 3 ) */
  350. /* DOUBLE PRECISION AV ( 3 ) */
  351. /* DOUBLE PRECISION AVX ( 3 ) */
  352. /* DOUBLE PRECISION DM ( 3, 3 ) */
  353. /* DOUBLE PRECISION DQ ( 0 : 3 ) */
  354. /* DOUBLE PRECISION EXPAV ( 3 ) */
  355. /* DOUBLE PRECISION M ( 3, 3 ) */
  356. /* DOUBLE PRECISION MOUT ( 3, 3 ) */
  357. /* DOUBLE PRECISION Q ( 0 : 3 ) */
  358. /* DOUBLE PRECISION QAV ( 0 : 3 ) */
  359. /* DOUBLE PRECISION XTRANS ( 6, 6 ) */
  360. /* INTEGER I */
  361. /* INTEGER J */
  362. /* C */
  363. /* C Pick some Euler angles and form a rotation matrix. */
  364. /* C */
  365. /* ANGLE(1) = -20.0 * RPD() */
  366. /* ANGLE(2) = 50.0 * RPD() */
  367. /* ANGLE(3) = -60.0 * RPD() */
  368. /* CALL EUL2M ( ANGLE(3), ANGLE(2), ANGLE(1), 3, 1, 3, M ) */
  369. /* CALL M2Q ( M, Q ) */
  370. /* C */
  371. /* C Choose an angular velocity vector. */
  372. /* C */
  373. /* EXPAV(1) = 1.0D0 */
  374. /* EXPAV(2) = 2.0D0 */
  375. /* EXPAV(3) = 3.0D0 */
  376. /* C */
  377. /* C Form the quaternion derivative. */
  378. /* C */
  379. /* QAV(0) = 0.D0 */
  380. /* CALL VEQU ( EXPAV, QAV(1) ) */
  381. /* CALL QXQ ( Q, QAV, DQ ) */
  382. /* CALL VSCLG ( -0.5D0, DQ, 4, DQ ) */
  383. /* C */
  384. /* C Recover angular velocity from Q and DQ using QDQ2AV. */
  385. /* C */
  386. /* CALL QDQ2AV ( Q, DQ, AV ) */
  387. /* C */
  388. /* C Now we'll obtain the angular velocity from Q and */
  389. /* C DQ by an alternate method. */
  390. /* C */
  391. /* C Convert Q back to a rotation matrix. */
  392. /* C */
  393. /* CALL Q2M ( Q, M ) */
  394. /* C */
  395. /* C Convert Q and DQ to a rotation derivative matrix. This */
  396. /* C somewhat messy procedure is based on differentiating the */
  397. /* C formula for deriving a rotation from a quaternion, then */
  398. /* C substituting components of Q and DQ into the derivative */
  399. /* C formula. */
  400. /* C */
  401. /* DM(1,1) = -4.D0 * ( Q(2)*DQ(2) + Q(3)*DQ(3) ) */
  402. /* DM(1,2) = 2.D0 * ( Q(1)*DQ(2) + Q(2)*DQ(1) */
  403. /* . - Q(0)*DQ(3) - Q(3)*DQ(0) ) */
  404. /* DM(1,3) = 2.D0 * ( Q(1)*DQ(3) + Q(3)*DQ(1) */
  405. /* . + Q(0)*DQ(2) + Q(2)*DQ(0) ) */
  406. /* DM(2,1) = 2.D0 * ( Q(1)*DQ(2) + Q(2)*DQ(1) */
  407. /* . + Q(0)*DQ(3) + Q(3)*DQ(0) ) */
  408. /* DM(2,2) = -4.D0 * ( Q(1)*DQ(1) + Q(3)*DQ(3) ) */
  409. /* DM(2,3) = 2.D0 * ( Q(2)*DQ(3) + Q(3)*DQ(2) */
  410. /* . - Q(0)*DQ(1) - Q(1)*DQ(0) ) */
  411. /* DM(3,1) = 2.D0 * ( Q(3)*DQ(1) + Q(1)*DQ(3) */
  412. /* . - Q(0)*DQ(2) - Q(2)*DQ(0) ) */
  413. /* DM(3,2) = 2.D0 * ( Q(2)*DQ(3) + Q(3)*DQ(2) */
  414. /* . + Q(0)*DQ(1) + Q(1)*DQ(0) ) */
  415. /* DM(3,3) = -4.D0 * ( Q(1)*DQ(1) + Q(2)*DQ(2) ) */
  416. /* C */
  417. /* C Form the state transformation matrix corresponding to M */
  418. /* C and DM. */
  419. /* CALL CLEARD ( 36, XTRANS ) */
  420. /* C */
  421. /* C Upper left block: */
  422. /* C */
  423. /* DO I = 1, 3 */
  424. /* DO J = 1, 3 */
  425. /* XTRANS(I,J) = M(I,J) */
  426. /* END DO */
  427. /* END DO */
  428. /* C */
  429. /* C Lower right block: */
  430. /* C */
  431. /* DO I = 1, 3 */
  432. /* DO J = 1, 3 */
  433. /* XTRANS(3+I,3+J) = M(I,J) */
  434. /* END DO */
  435. /* END DO */
  436. /* C */
  437. /* C Lower left block: */
  438. /* C */
  439. /* DO I = 1, 3 */
  440. /* DO J = 1, 3 */
  441. /* XTRANS(3+I,J) = DM(I,J) */
  442. /* END DO */
  443. /* END DO */
  444. /* C */
  445. /* C Now use XF2RAV to produce the expected angular velocity. */
  446. /* C */
  447. /* CALL XF2RAV ( XTRANS, MOUT, AVX ) */
  448. /* C */
  449. /* C The results should match to nearly full double */
  450. /* C precision. */
  451. /* C */
  452. /* WRITE(*,*) 'Original angular velocity: ', EXPAV */
  453. /* WRITE(*,*) 'QDQ2AV''s angular velocity: ', AV */
  454. /* WRITE(*,*) 'XF2RAV''s angular velocity: ', AVX */
  455. /* END */
  456. /* $ Restrictions */
  457. /* None. */
  458. /* $ Literature_References */
  459. /* None. */
  460. /* $ Author_and_Institution */
  461. /* N.J. Bachman (JPL) */
  462. /* $ Version */
  463. /* - SPICELIB Version 1.1.1, 26-FEB-2008 (NJB) */
  464. /* Updated header; added information about SPICE */
  465. /* quaternion conventions. */
  466. /* - SPICELIB Version 1.1.0, 31-AUG-2005 (NJB) */
  467. /* Updated to remove non-standard use of duplicate arguments */
  468. /* in VSCL call. */
  469. /* - SPICELIB Version 1.0.1, 24-FEB-2004 (NJB) */
  470. /* Made minor edits to the Particulars header section. */
  471. /* - SPICELIB Version 1.0.0, 26-AUG-2002 (NJB) */
  472. /* -& */
  473. /* $ Index_Entries */
  474. /* angular velocity from quaternion and derivative */
  475. /* -& */
  476. /* $ Revisions */
  477. /* - SPICELIB Version 1.1.0, 31-AUG-2005 (NJB) */
  478. /* Updated to remove non-standard use of duplicate arguments */
  479. /* in VSCL call. */
  480. /* -& */
  481. /* Local variables */
  482. /* Get a unitized copy of the input quaternion. */
  483. vhatg_(q, &c__4, qhat);
  484. /* Get the conjugate QSTAR of QHAT. */
  485. qstar[0] = qhat[0];
  486. vminus_(&qhat[1], &qstar[1]);
  487. /* Compute the angular velocity via the relationship */
  488. /* * */
  489. /* AV = -2 * Q * DQ */
  490. qxq_(qstar, dq, qtemp);
  491. vscl_(&c_b3, &qtemp[1], av);
  492. return 0;
  493. } /* qdq2av_ */