1 /* 2 3 -Procedure q2m_c ( Quaternion to matrix ) 4 5 -Abstract 6 7 Find the rotation matrix corresponding to a specified unit 8 quaternion. 9 10 -Disclaimer 11 12 THIS SOFTWARE AND ANY RELATED MATERIALS WERE CREATED BY THE 13 CALIFORNIA INSTITUTE OF TECHNOLOGY (CALTECH) UNDER A U.S. 14 GOVERNMENT CONTRACT WITH THE NATIONAL AERONAUTICS AND SPACE 15 ADMINISTRATION (NASA). THE SOFTWARE IS TECHNOLOGY AND SOFTWARE 16 PUBLICLY AVAILABLE UNDER U.S. EXPORT LAWS AND IS PROVIDED "AS-IS" 17 TO THE RECIPIENT WITHOUT WARRANTY OF ANY KIND, INCLUDING ANY 18 WARRANTIES OF PERFORMANCE OR MERCHANTABILITY OR FITNESS FOR A 19 PARTICULAR USE OR PURPOSE (AS SET FORTH IN UNITED STATES UCC 20 SECTIONS 2312-2313) OR FOR ANY PURPOSE WHATSOEVER, FOR THE 21 SOFTWARE AND RELATED MATERIALS, HOWEVER USED. 22 23 IN NO EVENT SHALL CALTECH, ITS JET PROPULSION LABORATORY, OR NASA 24 BE LIABLE FOR ANY DAMAGES AND/OR COSTS, INCLUDING, BUT NOT 25 LIMITED TO, INCIDENTAL OR CONSEQUENTIAL DAMAGES OF ANY KIND, 26 INCLUDING ECONOMIC DAMAGE OR INJURY TO PROPERTY AND LOST PROFITS, 27 REGARDLESS OF WHETHER CALTECH, JPL, OR NASA BE ADVISED, HAVE 28 REASON TO KNOW, OR, IN FACT, SHALL KNOW OF THE POSSIBILITY. 29 30 RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF 31 THE SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY 32 CALTECH AND NASA FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE 33 ACTIONS OF RECIPIENT IN THE USE OF THE SOFTWARE. 34 35 -Required_Reading 36 37 ROTATION 38 39 -Keywords 40 41 MATH 42 MATRIX 43 ROTATION 44 45 */ 46 47 #include "SpiceUsr.h" 48 #include "SpiceZfc.h" 49 #include "SpiceZim.h" 50 #undef q2m_c 51 52 q2m_c(ConstSpiceDouble q[4],SpiceDouble r[3][3])53 void q2m_c ( ConstSpiceDouble q[4], 54 SpiceDouble r[3][3] ) 55 /* 56 57 -Brief_I/O 58 59 Variable I/O Description 60 -------- --- -------------------------------------------------- 61 q I A unit quaternion. 62 r O A rotation matrix corresponding to `q'. 63 64 -Detailed_Input 65 66 q is a unit-length SPICE-style quaternion representing 67 a rotation. `q' has the property that 68 69 || q || = 1 70 71 See the discussion of quaternion styles in 72 Particulars below. 73 74 -Detailed_Output 75 76 r is a 3 by 3 rotation matrix representing the same 77 rotation as does `q'. See the discussion titled 78 "Associating SPICE Quaternions with Rotation 79 Matrices" in Particulars below. 80 81 -Parameters 82 83 None. 84 85 -Exceptions 86 87 Error free. 88 89 1) If `q' is not a unit quaternion, the output matrix `r' is 90 unlikely to be a rotation matrix. 91 92 -Files 93 94 None. 95 96 -Particulars 97 98 If a 4-dimensional vector `q' satisfies the equality 99 100 || q || = 1 101 102 or equivalently 103 104 2 2 2 2 105 q(0) + q(1) + q(2) + q(3) = 1, 106 107 then we can always find a unit vector `q' and a scalar `theta' such 108 that 109 110 q = 111 112 ( cos(theta/2), sin(theta/2)a(1), sin(theta/2)a(2), sin(theta/2)a(3) ) 113 114 We can interpret `a' and `theta' as the axis and rotation angle of a 115 rotation in 3-space. If we restrict `theta' to the range [0, pi], 116 then `theta' and `a' are uniquely determined, except if theta = pi. 117 In this special case, `a' and -a are both valid rotation axes. 118 119 Every rotation is represented by a unique orthogonal matrix; this 120 routine returns that unique rotation matrix corresponding to `q'. 121 122 The CSPICE routine m2q_c is a one-sided inverse of this routine: 123 given any rotation matrix `r', the calls 124 125 m2q_c ( r, q ) 126 q2m_c ( q, r ) 127 128 leave `r' unchanged, except for round-off error. However, the 129 calls 130 131 q2m_c ( q, r ) 132 m2q_c ( r, q ) 133 134 might preserve `q' or convert `q' to -q. 135 136 137 Quaternion Styles 138 ----------------- 139 140 There are different "styles" of quaternions used in 141 science and engineering applications. Quaternion styles 142 are characterized by 143 144 - The order of quaternion elements 145 146 - The quaternion multiplication formula 147 148 - The convention for associating quaternions 149 with rotation matrices 150 151 Two of the commonly used styles are 152 153 - "SPICE" 154 155 > Invented by Sir William Rowan Hamilton 156 > Frequently used in mathematics and physics textbooks 157 158 - "Engineering" 159 160 > Widely used in aerospace engineering applications 161 162 163 CSPICE function interfaces ALWAYS use SPICE quaternions. 164 Quaternions of any other style must be converted to SPICE 165 quaternions before they are passed to CSPICE functions. 166 167 168 Relationship between SPICE and Engineering Quaternions 169 ------------------------------------------------------ 170 171 Let M be a rotation matrix such that for any vector V, 172 173 M*V 174 175 is the result of rotating V by theta radians in the 176 counterclockwise direction about unit rotation axis vector A. 177 Then the SPICE quaternions representing M are 178 179 (+/-) ( cos(theta/2), 180 sin(theta/2) A(1), 181 sin(theta/2) A(2), 182 sin(theta/2) A(3) ) 183 184 while the engineering quaternions representing M are 185 186 (+/-) ( -sin(theta/2) A(1), 187 -sin(theta/2) A(2), 188 -sin(theta/2) A(3), 189 cos(theta/2) ) 190 191 For both styles of quaternions, if a quaternion q represents 192 a rotation matrix M, then -q represents M as well. 193 194 Given an engineering quaternion 195 196 QENG = ( q0, q1, q2, q3 ) 197 198 the equivalent SPICE quaternion is 199 200 QSPICE = ( q3, -q0, -q1, -q2 ) 201 202 203 Associating SPICE Quaternions with Rotation Matrices 204 ---------------------------------------------------- 205 206 Let FROM and TO be two right-handed reference frames, for 207 example, an inertial frame and a spacecraft-fixed frame. Let the 208 symbols 209 210 V , V 211 FROM TO 212 213 denote, respectively, an arbitrary vector expressed relative to 214 the FROM and TO frames. Let M denote the transformation matrix 215 that transforms vectors from frame FROM to frame TO; then 216 217 V = M * V 218 TO FROM 219 220 where the expression on the right hand side represents left 221 multiplication of the vector by the matrix. 222 223 Then if the unit-length SPICE quaternion q represents M, where 224 225 q = (q0, q1, q2, q3) 226 227 the elements of M are derived from the elements of q as follows: 228 229 +- -+ 230 | 2 2 | 231 | 1 - 2*( q2 + q3 ) 2*(q1*q2 - q0*q3) 2*(q1*q3 + q0*q2) | 232 | | 233 | | 234 | 2 2 | 235 M = | 2*(q1*q2 + q0*q3) 1 - 2*( q1 + q3 ) 2*(q2*q3 - q0*q1) | 236 | | 237 | | 238 | 2 2 | 239 | 2*(q1*q3 - q0*q2) 2*(q2*q3 + q0*q1) 1 - 2*( q1 + q2 ) | 240 | | 241 +- -+ 242 243 Note that substituting the elements of -q for those of q in the 244 right hand side leaves each element of M unchanged; this shows 245 that if a quaternion q represents a matrix M, then so does the 246 quaternion -q. 247 248 To map the rotation matrix M to a unit quaternion, we start by 249 decomposing the rotation matrix as a sum of symmetric 250 and skew-symmetric parts: 251 252 2 253 M = [ I + (1-cos(theta)) OMEGA ] + [ sin(theta) OMEGA ] 254 255 symmetric skew-symmetric 256 257 258 OMEGA is a skew-symmetric matrix of the form 259 260 +- -+ 261 | 0 -n3 n2 | 262 | | 263 OMEGA = | n3 0 -n1 | 264 | | 265 | -n2 n1 0 | 266 +- -+ 267 268 The vector N of matrix entries (n1, n2, n3) is the rotation axis 269 of M and theta is M's rotation angle. Note that N and theta 270 are not unique. 271 272 Let 273 274 C = cos(theta/2) 275 S = sin(theta/2) 276 277 Then the unit quaternions Q corresponding to M are 278 279 Q = +/- ( C, S*n1, S*n2, S*n3 ) 280 281 The mappings between quaternions and the corresponding rotations 282 are carried out by the CSPICE routines 283 284 q2m_c {quaternion to matrix} 285 m2q_c {matrix to quaternion} 286 287 m2q_c always returns a quaternion with scalar part greater than 288 or equal to zero. 289 290 291 SPICE Quaternion Multiplication Formula 292 --------------------------------------- 293 294 Given a SPICE quaternion 295 296 Q = ( q0, q1, q2, q3 ) 297 298 corresponding to rotation axis A and angle theta as above, we can 299 represent Q using "scalar + vector" notation as follows: 300 301 s = q0 = cos(theta/2) 302 303 v = ( q1, q2, q3 ) = sin(theta/2) * A 304 305 Q = s + v 306 307 Let Q1 and Q2 be SPICE quaternions with respective scalar 308 and vector parts s1, s2 and v1, v2: 309 310 Q1 = s1 + v1 311 Q2 = s2 + v2 312 313 We represent the dot product of v1 and v2 by 314 315 <v1, v2> 316 317 and the cross product of v1 and v2 by 318 319 v1 x v2 320 321 Then the SPICE quaternion product is 322 323 Q1*Q2 = s1*s2 - <v1,v2> + s1*v2 + s2*v1 + (v1 x v2) 324 325 If Q1 and Q2 represent the rotation matrices M1 and M2 326 respectively, then the quaternion product 327 328 Q1*Q2 329 330 represents the matrix product 331 332 M1*M2 333 334 335 -Examples 336 337 338 1) A case amenable to checking by hand calculation: 339 340 To convert the rotation matrix 341 342 +- -+ 343 | 0 1 0 | 344 | | 345 r = | -1 0 0 | 346 | | 347 | 0 0 1 | 348 +- -+ 349 350 also represented as 351 352 [ pi/2 ] 353 3 354 355 to a quaternion, we can use the code fragment 356 357 rotate_c ( halfpi_c(), 3, r ); 358 m2q_c ( r, q ); 359 360 m2q_c will return `q' as 361 362 ( sqrt(2)/2, 0, 0, -sqrt(2)/2 ) 363 364 Why? Well, `r' is a reference frame transformation that 365 rotates vectors by -pi/2 radians about the axis vector 366 367 a = ( 0, 0, 1 ) 368 369 Equivalently, `r' rotates vectors by pi/2 radians in 370 the counterclockwise sense about the axis vector 371 372 -a = ( 0, 0, -1 ) 373 374 so our definition of `q', 375 376 h = theta/2 377 378 q = ( cos(h), sin(h)a , sin(h)a , sin(h)a ) 379 1 2 3 380 381 implies that in this case, 382 383 q = ( cos(pi/4), 0, 0, -sin(pi/4) ) 384 385 = ( sqrt(2)/2, 0, 0, -sqrt(2)/2 ) 386 387 388 2) Finding a set of Euler angles that represent a rotation 389 specified by a quaternion: 390 391 Suppose our rotation `r' is represented by the quaternion 392 `q'. To find angles `tau', `alpha', `delta' such that 393 394 395 r = [ tau ] [ pi/2 - delta ] [ alpha ] 396 3 2 3 397 398 we can use the code fragment 399 400 401 q2m_c ( q, r ); 402 m2eul_c ( r, 3, 2, 3, tau, delta, alpha ); 403 404 delta = halfpi_c() - delta; 405 406 -Restrictions 407 408 None. 409 410 -Literature_References 411 412 [1] NAIF document 179.0, "Rotations and their Habits", by 413 W. L. Taber. 414 415 -Author_and_Institution 416 417 N.J. Bachman (JPL) 418 E.D. Wright (JPL) 419 420 -Version 421 422 -CSPICE Version 1.3.2, 27-FEB-2008 (NJB) 423 424 Updated header; added information about SPICE quaternion 425 conventions. Made miscellaneous edits throughout header. 426 427 -CSPICE Version 1.3.1, 06-FEB-2003 (EDW) 428 429 Corrected typo error in Examples section. 430 431 -CSPICE Version 1.3.0, 24-JUL-2001 (NJB) 432 433 Changed prototype: input q is now type (ConstSpiceDouble [4]). 434 Implemented interface macro for casting input q to const. 435 436 -CSPICE Version 1.2.0, 08-FEB-1998 (NJB) 437 438 Removed local variables used for temporary capture of outputs. 439 Removed tracing calls, since the underlying Fortran routine 440 is error-free. 441 442 -CSPICE Version 1.0.0, 25-OCT-1997 (NJB) 443 444 Based on SPICELIB Version 1.0.1, 10-MAR-1992 (WLT) 445 446 -Index_Entries 447 448 quaternion to matrix 449 450 -& 451 */ 452 453 { /* Begin q2m_c */ 454 455 456 /* 457 Call the f2c'd version of q2m: 458 */ 459 q2m_ ( (doublereal *) q, 460 (doublereal *) r ); 461 462 /* 463 Transpose the output matrix to put it in row-major order. 464 */ 465 xpose_c ( r, r ); 466 467 468 } /* End q2m_c */ 469