1 /* 2 3 -Procedure m2q_c ( Matrix to quaternion ) 4 5 -Abstract 6 7 Find a unit quaternion corresponding to a specified rotation 8 matrix. 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 "SpiceZmc.h" 50 #undef m2q_c 51 52 m2q_c(ConstSpiceDouble r[3][3],SpiceDouble q[4])53 void m2q_c ( ConstSpiceDouble r[3][3], 54 SpiceDouble q[4] ) 55 /* 56 57 -Brief_I/O 58 59 Variable I/O Description 60 -------- --- -------------------------------------------------- 61 r I A rotation matrix. 62 q O A unit quaternion representing `r'. 63 64 -Detailed_Input 65 66 r is a rotation matrix. 67 68 -Detailed_Output 69 70 q is a unit-length SPICE-style quaternion representing 71 `r'. See the discussion of quaternion styles in 72 Particulars below. 73 74 `q' is a 4-dimensional vector. If `r' rotates vectors in 75 the counterclockwise sense by an angle of `theta' radians 76 about a unit vector `a', where 77 78 0 < theta < pi 79 - - 80 81 then letting h = theta/2, 82 83 q = ( cos(h), sin(h)a , sin(h)a , sin(h)a ). 84 1 2 3 85 86 The restriction that `theta' must be in the range [0, pi] 87 determines the output quaternion `q' uniquely 88 except when theta = pi; in this special case, both of 89 the quaternions 90 91 q = ( 0, a , a , a ) 92 1 2 3 93 and 94 95 q = ( 0, -a , -a , -a ) 96 1 2 3 97 98 are possible outputs. 99 100 -Parameters 101 102 None. 103 104 -Exceptions 105 106 1) If `r' is not a rotation matrix, the error SPICE(NOTAROTATION) 107 is signaled. 108 109 -Files 110 111 None. 112 113 -Particulars 114 115 A unit quaternion is a 4-dimensional vector for which the sum of 116 the squares of the components is 1. Unit quaternions can be used 117 to represent rotations in the following way: given a rotation 118 angle `theta', where 119 120 0 < theta < pi 121 - - 122 123 and a unit vector `a', we can represent the transformation that 124 rotates vectors in the counterclockwise sense by theta radians about 125 `a' using the quaternion `q', where 126 127 q = ( cos(theta/2), sin(theta/2)a , sin(theta/2)a , sin(theta/2)a ) 128 1 2 3 129 130 As mentioned in Detailed Output, our restriction on the range of 131 `theta' determines `q' uniquely, except when theta = pi. 132 133 The CSPICE routine q2m_c is an one-sided inverse of this routine: 134 given any rotation matrix `r', the calls 135 136 m2q_c ( r, q ); 137 q2m_c ( q, r ); 138 139 leave `r' unchanged, except for round-off error. However, the 140 calls 141 142 q2m_c ( q, r ); 143 m2q_c ( r, q ); 144 145 might preserve `q' or convert `q' to -q. 146 147 148 Quaternion Styles 149 ----------------- 150 151 There are different "styles" of quaternions used in 152 science and engineering applications. Quaternion styles 153 are characterized by 154 155 - The order of quaternion elements 156 157 - The quaternion multiplication formula 158 159 - The convention for associating quaternions 160 with rotation matrices 161 162 Two of the commonly used styles are 163 164 - "SPICE" 165 166 > Invented by Sir William Rowan Hamilton 167 > Frequently used in mathematics and physics textbooks 168 169 - "Engineering" 170 171 > Widely used in aerospace engineering applications 172 173 174 CSPICE function interfaces ALWAYS use SPICE quaternions. 175 Quaternions of any other style must be converted to SPICE 176 quaternions before they are passed to CSPICE functions. 177 178 179 Relationship between SPICE and Engineering Quaternions 180 ------------------------------------------------------ 181 182 Let M be a rotation matrix such that for any vector V, 183 184 M*V 185 186 is the result of rotating V by theta radians in the 187 counterclockwise direction about unit rotation axis vector A. 188 Then the SPICE quaternions representing M are 189 190 (+/-) ( cos(theta/2), 191 sin(theta/2) A(1), 192 sin(theta/2) A(2), 193 sin(theta/2) A(3) ) 194 195 while the engineering quaternions representing M are 196 197 (+/-) ( -sin(theta/2) A(1), 198 -sin(theta/2) A(2), 199 -sin(theta/2) A(3), 200 cos(theta/2) ) 201 202 For both styles of quaternions, if a quaternion q represents 203 a rotation matrix M, then -q represents M as well. 204 205 Given an engineering quaternion 206 207 QENG = ( q0, q1, q2, q3 ) 208 209 the equivalent SPICE quaternion is 210 211 QSPICE = ( q3, -q0, -q1, -q2 ) 212 213 214 Associating SPICE Quaternions with Rotation Matrices 215 ---------------------------------------------------- 216 217 Let FROM and TO be two right-handed reference frames, for 218 example, an inertial frame and a spacecraft-fixed frame. Let the 219 symbols 220 221 V , V 222 FROM TO 223 224 denote, respectively, an arbitrary vector expressed relative to 225 the FROM and TO frames. Let M denote the transformation matrix 226 that transforms vectors from frame FROM to frame TO; then 227 228 V = M * V 229 TO FROM 230 231 where the expression on the right hand side represents left 232 multiplication of the vector by the matrix. 233 234 Then if the unit-length SPICE quaternion q represents M, where 235 236 q = (q0, q1, q2, q3) 237 238 the elements of M are derived from the elements of q as follows: 239 240 +- -+ 241 | 2 2 | 242 | 1 - 2*( q2 + q3 ) 2*(q1*q2 - q0*q3) 2*(q1*q3 + q0*q2) | 243 | | 244 | | 245 | 2 2 | 246 M = | 2*(q1*q2 + q0*q3) 1 - 2*( q1 + q3 ) 2*(q2*q3 - q0*q1) | 247 | | 248 | | 249 | 2 2 | 250 | 2*(q1*q3 - q0*q2) 2*(q2*q3 + q0*q1) 1 - 2*( q1 + q2 ) | 251 | | 252 +- -+ 253 254 Note that substituting the elements of -q for those of q in the 255 right hand side leaves each element of M unchanged; this shows 256 that if a quaternion q represents a matrix M, then so does the 257 quaternion -q. 258 259 To map the rotation matrix M to a unit quaternion, we start by 260 decomposing the rotation matrix as a sum of symmetric 261 and skew-symmetric parts: 262 263 2 264 M = [ I + (1-cos(theta)) OMEGA ] + [ sin(theta) OMEGA ] 265 266 symmetric skew-symmetric 267 268 269 OMEGA is a skew-symmetric matrix of the form 270 271 +- -+ 272 | 0 -n3 n2 | 273 | | 274 OMEGA = | n3 0 -n1 | 275 | | 276 | -n2 n1 0 | 277 +- -+ 278 279 The vector N of matrix entries (n1, n2, n3) is the rotation axis 280 of M and theta is M's rotation angle. Note that N and theta 281 are not unique. 282 283 Let 284 285 C = cos(theta/2) 286 S = sin(theta/2) 287 288 Then the unit quaternions Q corresponding to M are 289 290 Q = +/- ( C, S*n1, S*n2, S*n3 ) 291 292 The mappings between quaternions and the corresponding rotations 293 are carried out by the CSPICE routines 294 295 q2m_c {quaternion to matrix} 296 m2q_c {matrix to quaternion} 297 298 m2q_c always returns a quaternion with scalar part greater than 299 or equal to zero. 300 301 302 SPICE Quaternion Multiplication Formula 303 --------------------------------------- 304 305 Given a SPICE quaternion 306 307 Q = ( q0, q1, q2, q3 ) 308 309 corresponding to rotation axis A and angle theta as above, we can 310 represent Q using "scalar + vector" notation as follows: 311 312 s = q0 = cos(theta/2) 313 314 v = ( q1, q2, q3 ) = sin(theta/2) * A 315 316 Q = s + v 317 318 Let Q1 and Q2 be SPICE quaternions with respective scalar 319 and vector parts s1, s2 and v1, v2: 320 321 Q1 = s1 + v1 322 Q2 = s2 + v2 323 324 We represent the dot product of v1 and v2 by 325 326 <v1, v2> 327 328 and the cross product of v1 and v2 by 329 330 v1 x v2 331 332 Then the SPICE quaternion product is 333 334 Q1*Q2 = s1*s2 - <v1,v2> + s1*v2 + s2*v1 + (v1 x v2) 335 336 If Q1 and Q2 represent the rotation matrices M1 and M2 337 respectively, then the quaternion product 338 339 Q1*Q2 340 341 represents the matrix product 342 343 M1*M2 344 345 346 -Examples 347 348 1) A case amenable to checking by hand calculation: 349 350 To convert the rotation matrix 351 352 +- -+ 353 | 0 1 0 | 354 | | 355 r = | -1 0 0 | 356 | | 357 | 0 0 1 | 358 +- -+ 359 360 also represented as 361 362 [ pi/2 ] 363 3 364 365 to a quaternion, we can use the code fragment 366 367 rotate_c ( halfpi_c(), 3, r ); 368 m2q_c ( r, q ); 369 370 m2q_c will return `q' as 371 372 ( sqrt(2)/2, 0, 0, -sqrt(2)/2 ) 373 374 Why? Well, `r' is a reference frame transformation that 375 rotates vectors by -pi/2 radians about the axis vector 376 377 a = ( 0, 0, 1 ) 378 379 Equivalently, `r' rotates vectors by pi/2 radians in 380 the counterclockwise sense about the axis vector 381 382 -a = ( 0, 0, -1 ) 383 384 so our definition of `q', 385 386 h = theta/2 387 388 q = ( cos(h), sin(h)a , sin(h)a , sin(h)a ) 389 1 2 3 390 391 implies that in this case, 392 393 q = ( cos(pi/4), 0, 0, -sin(pi/4) ) 394 395 = ( sqrt(2)/2, 0, 0, -sqrt(2)/2 ) 396 397 398 2) Finding a quaternion that represents a rotation specified by 399 a set of Euler angles: 400 401 Suppose our original rotation `r' is the product 402 403 [ tau ] [ pi/2 - delta ] [ alpha ] . 404 3 2 3 405 406 The code fragment 407 408 eul2m_c ( tau, halfpi_c() - delta, alpha, 409 3, 2, 3, r ); 410 411 m2q_c ( r, q ); 412 413 yields a quaternion `q' that represents `r'. 414 415 -Restrictions 416 417 None. 418 419 -Literature_References 420 421 NAIF document 179.0, "Rotations and their Habits", by 422 W. L. Taber. 423 424 -Author_and_Institution 425 426 N.J. Bachman (JPL) 427 E.D. Wright (JPL) 428 429 -Version 430 431 -CSPICE Version 1.1.1, 27-FEB-2008 (NJB) 432 433 Updated header; added information about SPICE 434 quaternion conventions. Made minor edits throughout 435 header. 436 437 -CSPICE Version 1.1.0, 21-OCT-1998 (NJB) 438 439 Made input matrix const. 440 441 -CSPICE Version 1.0.1, 13-FEB-1998 (EDW) 442 443 Minor corrections to header. 444 445 -CSPICE Version 1.0.0, 08-FEB-1998 (NJB) 446 447 Based on SPICELIB Version 1.0.1, 10-MAR-1992 (WLT) 448 449 -Index_Entries 450 451 matrix to quaternion 452 453 -& 454 */ 455 456 { /* Begin m2q_c */ 457 458 /* 459 Local variables 460 */ 461 SpiceDouble loc_r[3][3]; 462 463 464 /* 465 Participate in error tracing. 466 */ 467 chkin_c ( "m2q_c" ); 468 469 470 /* 471 Transpose the input matrix to put it in column-major order. 472 */ 473 xpose_c ( r, loc_r ); 474 475 476 /* 477 Call the f2c'd version of m2q: 478 */ 479 m2q_ ( (doublereal *) loc_r, 480 (doublereal *) q ); 481 482 483 chkout_c ( "m2q_c" ); 484 485 486 } /* End m2q_c */ 487