Dogcows Code - chaz/yoink/blob - src/moof/cml/mathlib/quaternion_rotation.h

   1 /* -*- C++ -*- ------------------------------------------------------------
   2
   3 Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/
   4
   5 The Configurable Math Library (CML) is distributed under the terms of the
   6 Boost Software License, v1.0 (see cml/LICENSE for details).
   7
   8  *-----------------------------------------------------------------------*/
   9 /** @file
  10  *  @brief
  11  */
  12
  13 #ifndef quaternion_rotation_h
  14 #define quaternion_rotation_h
  15
  16 #include <cml/mathlib/checking.h>
  17
  18 /* Functions related to quaternion rotations.
  19  *
  20  * Note: A number of these functions simply wrap calls to the corresponding
  21  * matrix functions. Some of them (the 'aim-at' and 'align' functions in
  22  * particular) might be considered a bit superfluous, since the resulting
  23  * quaternion will most likely be converted to a matrix at some point anyway.
  24  * However, they're included here for completeness, and for convenience in
  25  * cases where a quaternion is being used as the primary rotation
  26  * representation.
  27 */
  28
  29 namespace cml {
  30
  31 //////////////////////////////////////////////////////////////////////////////
  32 // Rotation about world axes
  33 //////////////////////////////////////////////////////////////////////////////
  34
  35 /** Build a quaternion representing a rotation about the given world axis */
  36 template < class E, class A, class O, class C > void
  37 quaternion_rotation_world_axis(quaternion<E,A,O,C>& q, size_t axis, E angle)
  38 {
  39     typedef quaternion<E,A,O,C> quaternion_type;
  40     typedef typename quaternion_type::value_type value_type;
  41     typedef typename quaternion_type::order_type order_type;
  42
  43     /* Checking */
  44     detail::CheckIndex3(axis);
  45
  46     q.identity();
  47
  48     const size_t W = order_type::W;
  49     const size_t I = order_type::X + axis;
  50
  51     angle *= value_type(.5);
  52     q[I] = std::sin(angle);
  53     q[W] = std::cos(angle);
  54 }
  55
  56 /** Build a quaternion representing a rotation about the world x axis */
  57 template < class E, class A, class O, class C > void
  58 quaternion_rotation_world_x(quaternion<E,A,O,C>& q, E angle) {
  59     quaternion_rotation_world_axis(q,0,angle);
  60 }
  61
  62 /** Build a quaternion representing a rotation about the world y axis */
  63 template < class E, class A, class O, class C > void
  64 quaternion_rotation_world_y(quaternion<E,A,O,C>& q, E angle) {
  65     quaternion_rotation_world_axis(q,1,angle);
  66 }
  67
  68 /** Build a quaternion representing a rotation about the world z axis */
  69 template < class E, class A, class O, class C > void
  70 quaternion_rotation_world_z(quaternion<E,A,O,C>& q, E angle) {
  71     quaternion_rotation_world_axis(q,2,angle);
  72 }
  73
  74 //////////////////////////////////////////////////////////////////////////////
  75 // Rotation from an axis-angle pair
  76 //////////////////////////////////////////////////////////////////////////////
  77
  78 /** Build a quaternion from an axis-angle pair */
  79 template < class E, class A, class O, class C, class VecT > void
  80 quaternion_rotation_axis_angle(
  81     quaternion<E,A,O,C>& q, const VecT& axis, E angle)
  82 {
  83     typedef quaternion<E,A,O,C> quaternion_type;
  84     typedef typename quaternion_type::value_type value_type;
  85     typedef typename quaternion_type::order_type order_type;
  86
  87     /* Checking */
  88     detail::CheckVec3(axis);
  89
  90     enum {
  91         W = order_type::W,
  92         X = order_type::X,
  93         Y = order_type::Y,
  94         Z = order_type::Z
  95     };
  96
  97     angle *= value_type(.5);
  98
  99     /* @todo: If and when we have a set() function that takes a vector and a
 100      * scalar, this can be written as:
 101      *
 102      * q.set(std::cos(angle), axis * std::sin(angle));
 103      *
 104      * In which case the enum will also not be necessary.
 105      */
 106
 107     q[W] = std::cos(angle);
 108     value_type s = std::sin(angle);
 109     q[X] = axis[0] * s;
 110     q[Y] = axis[1] * s;
 111     q[Z] = axis[2] * s;
 112 }
 113
 114 //////////////////////////////////////////////////////////////////////////////
 115 // Rotation from a matrix
 116 //////////////////////////////////////////////////////////////////////////////
 117
 118 /** Build a quaternion from a rotation matrix */
 119 template < class E, class A, class O, class C, class MatT > void
 120 quaternion_rotation_matrix(quaternion<E,A,O,C>& q, const MatT& m)
 121 {
 122     typedef quaternion<E,A,O,C> quaternion_type;
 123     typedef typename quaternion_type::value_type value_type;
 124     typedef typename quaternion_type::order_type order_type;
 125
 126     /* Checking */
 127     detail::CheckMatLinear3D(m);
 128
 129     enum {
 130         W = order_type::W,
 131         X = order_type::X,
 132         Y = order_type::Y,
 133         Z = order_type::Z
 134     };
 135
 136     value_type tr = trace_3x3(m);
 137     if (tr >= value_type(0)) {
 138         q[W] = std::sqrt(tr + value_type(1)) * value_type(.5);
 139         value_type s = value_type(.25) / q[W];
 140         q[X] = (m.basis_element(1,2) - m.basis_element(2,1)) * s;
 141         q[Y] = (m.basis_element(2,0) - m.basis_element(0,2)) * s;
 142         q[Z] = (m.basis_element(0,1) - m.basis_element(1,0)) * s;
 143     } else {
 144         size_t largest_diagonal_element =
 145             index_of_max(
 146                 m.basis_element(0,0),
 147                 m.basis_element(1,1),
 148                 m.basis_element(2,2)
 149             );
 150         size_t i, j, k;
 151         cyclic_permutation(largest_diagonal_element, i, j, k);
 152         const size_t I = X + i;
 153         const size_t J = X + j;
 154         const size_t K = X + k;
 155         q[I] =
 156             std::sqrt(
 157                 m.basis_element(i,i) -
 158                 m.basis_element(j,j) -
 159                 m.basis_element(k,k) +
 160                 value_type(1)
 161             ) * value_type(.5);
 162         value_type s = value_type(.25) / q[I];
 163         q[J] = (m.basis_element(i,j) + m.basis_element(j,i)) * s;
 164         q[K] = (m.basis_element(i,k) + m.basis_element(k,i)) * s;
 165         q[W] = (m.basis_element(j,k) - m.basis_element(k,j)) * s;
 166     }
 167 }
 168
 169 //////////////////////////////////////////////////////////////////////////////
 170 // Rotation from Euler angles
 171 //////////////////////////////////////////////////////////////////////////////
 172
 173 /** Build a quaternion from an Euler-angle triple */
 174 template < class E, class A, class O, class C > void
 175 quaternion_rotation_euler(
 176     quaternion<E,A,O,C>& q, E angle_0, E angle_1, E angle_2,
 177     EulerOrder order)
 178 {
 179     typedef quaternion<E,A,O,C> quaternion_type;
 180     typedef typename quaternion_type::value_type value_type;
 181     typedef typename quaternion_type::order_type order_type;
 182
 183     size_t i, j, k;
 184     bool odd, repeat;
 185     detail::unpack_euler_order(order, i, j, k, odd, repeat);
 186
 187     const size_t W = order_type::W;
 188     const size_t I = order_type::X + i;
 189     const size_t J = order_type::X + j;
 190     const size_t K = order_type::X + k;
 191
 192     if (odd) {
 193         angle_1 = -angle_1;
 194     }
 195
 196     angle_0 *= value_type(.5);
 197     angle_1 *= value_type(.5);
 198     angle_2 *= value_type(.5);
 199
 200     value_type s0 = std::sin(angle_0);
 201     value_type c0 = std::cos(angle_0);
 202     value_type s1 = std::sin(angle_1);
 203     value_type c1 = std::cos(angle_1);
 204     value_type s2 = std::sin(angle_2);
 205     value_type c2 = std::cos(angle_2);
 206
 207     value_type s0s2 = s0 * s2;
 208     value_type s0c2 = s0 * c2;
 209     value_type c0s2 = c0 * s2;
 210     value_type c0c2 = c0 * c2;
 211
 212     if (repeat) {
 213         q[I] = c1 * (c0s2 + s0c2);
 214         q[J] = s1 * (c0c2 + s0s2);
 215         q[K] = s1 * (c0s2 - s0c2);
 216         q[W] = c1 * (c0c2 - s0s2);
 217     } else {
 218         q[I] = c1 * s0c2 - s1 * c0s2;
 219         q[J] = c1 * s0s2 + s1 * c0c2;
 220         q[K] = c1 * c0s2 - s1 * s0c2;
 221         q[W] = c1 * c0c2 + s1 * s0s2;
 222     }
 223     if (odd) {
 224         q[J] = -q[J];
 225     }
 226 }
 227
 228 //////////////////////////////////////////////////////////////////////////////
 229 // Rotation to align with a vector, multiple vectors, or the view plane
 230 //////////////////////////////////////////////////////////////////////////////
 231
 232 /** See vector_ortho.h for details */
 233 template < typename E,class A,class O,class C,class VecT_1,class VecT_2 > void
 234 quaternion_rotation_align(
 235     quaternion<E,A,O,C>& q,
 236     const VecT_1& align,
 237     const VecT_2& reference,
 238     bool normalize = true,
 239     AxisOrder order = axis_order_zyx)
 240 {
 241     typedef matrix< E,fixed<3,3>,row_basis,row_major > matrix_type;
 242
 243     matrix_type m;
 244     matrix_rotation_align(m,align,reference,normalize,order);
 245     quaternion_rotation_matrix(q,m);
 246 }
 247
 248 /** See vector_ortho.h for details */
 249 template < typename E, class A, class O, class C, class VecT > void
 250 quaternion_rotation_align(quaternion<E,A,O,C>& q, const VecT& align,
 251     bool normalize = true, AxisOrder order = axis_order_zyx)
 252 {
 253     typedef matrix< E,fixed<3,3>,row_basis,row_major > matrix_type;
 254
 255     matrix_type m;
 256     matrix_rotation_align(m,align,normalize,order);
 257     quaternion_rotation_matrix(q,m);
 258 }
 259
 260 /** See vector_ortho.h for details */
 261 template < typename E,class A,class O,class C,class VecT_1,class VecT_2 > void
 262 quaternion_rotation_align_axial(quaternion<E,A,O,C>& q, const VecT_1& align,
 263     const VecT_2& axis, bool normalize = true,
 264     AxisOrder order = axis_order_zyx)
 265 {
 266     typedef matrix< E,fixed<3,3>,row_basis,row_major > matrix_type;
 267
 268     matrix_type m;
 269     matrix_rotation_align_axial(m,align,axis,normalize,order);
 270     quaternion_rotation_matrix(q,m);
 271 }
 272
 273 /** See vector_ortho.h for details */
 274 template < typename E, class A, class O, class C, class MatT > void
 275 quaternion_rotation_align_viewplane(
 276     quaternion<E,A,O,C>& q,
 277     const MatT& view_matrix,
 278     Handedness handedness,
 279     AxisOrder order = axis_order_zyx)
 280 {
 281     typedef matrix< E,fixed<3,3>,row_basis,row_major > matrix_type;
 282
 283     matrix_type m;
 284     matrix_rotation_align_viewplane(m,view_matrix,handedness,order);
 285     quaternion_rotation_matrix(q,m);
 286 }
 287
 288 /** See vector_ortho.h for details */
 289 template < typename E, class A, class O, class C, class MatT > void
 290 quaternion_rotation_align_viewplane_LH(
 291     quaternion<E,A,O,C>& q,
 292     const MatT& view_matrix,
 293     AxisOrder order = axis_order_zyx)
 294 {
 295     typedef matrix< E,fixed<3,3>,row_basis,row_major > matrix_type;
 296
 297     matrix_type m;
 298     matrix_rotation_align_viewplane_LH(m,view_matrix,order);
 299     quaternion_rotation_matrix(q,m);
 300 }
 301
 302 /** See vector_ortho.h for details */
 303 template < typename E, class A, class O, class C, class MatT > void
 304 quaternion_rotation_align_viewplane_RH(
 305     quaternion<E,A,O,C>& q,
 306     const MatT& view_matrix,
 307     AxisOrder order = axis_order_zyx)
 308 {
 309     typedef matrix< E,fixed<3,3>,row_basis,row_major > matrix_type;
 310
 311     matrix_type m;
 312     matrix_rotation_align_viewplane_RH(m,view_matrix,order);
 313     quaternion_rotation_matrix(q,m);
 314 }
 315
 316 //////////////////////////////////////////////////////////////////////////////
 317 // Rotation to aim at a target
 318 //////////////////////////////////////////////////////////////////////////////
 319
 320 /** See vector_ortho.h for details */
 321 template < typename E, class A, class O, class C,
 322     class VecT_1, class VecT_2, class VecT_3 > void
 323 quaternion_rotation_aim_at(
 324     quaternion<E,A,O,C>& q,
 325     const VecT_1& pos,
 326     const VecT_2& target,
 327     const VecT_3& reference,
 328     AxisOrder order = axis_order_zyx)
 329 {
 330     typedef matrix< E,fixed<3,3>,row_basis,row_major > matrix_type;
 331
 332     matrix_type m;
 333     matrix_rotation_aim_at(m,pos,target,reference,order);
 334     quaternion_rotation_matrix(q,m);
 335 }
 336
 337 /** See vector_ortho.h for details */
 338 template < typename E, class A, class O, class C,
 339     class VecT_1, class VecT_2 > void
 340 quaternion_rotation_aim_at(
 341     quaternion<E,A,O,C>& q,
 342     const VecT_1& pos,
 343     const VecT_2& target,
 344     AxisOrder order = axis_order_zyx)
 345 {
 346     typedef matrix< E,fixed<3,3>,row_basis,row_major > matrix_type;
 347
 348     matrix_type m;
 349     matrix_rotation_aim_at(m,pos,target,order);
 350     quaternion_rotation_matrix(q,m);
 351 }
 352
 353 /** See vector_ortho.h for details */
 354 template < typename E, class A, class O, class C,
 355     class VecT_1, class VecT_2, class VecT_3 > void
 356 quaternion_rotation_aim_at_axial(
 357     quaternion<E,A,O,C>& q,
 358     const VecT_1& pos,
 359     const VecT_2& target,
 360     const VecT_3& axis,
 361     AxisOrder order = axis_order_zyx)
 362 {
 363     typedef matrix< E,fixed<3,3>,row_basis,row_major > matrix_type;
 364
 365     matrix_type m;
 366     matrix_rotation_aim_at_axial(m,pos,target,axis,order);
 367     quaternion_rotation_matrix(q,m);
 368 }
 369
 370 //////////////////////////////////////////////////////////////////////////////
 371 // Relative rotation about world axes
 372 //////////////////////////////////////////////////////////////////////////////
 373
 374 /* Rotate a quaternion about the given world axis */
 375 template < class E, class A, class O, class C > void
 376 quaternion_rotate_about_world_axis(quaternion<E,A,O,C>& q,size_t axis,E angle)
 377 {
 378     typedef quaternion<E,A,O,C> quaternion_type;
 379     typedef typename quaternion_type::value_type value_type;
 380     typedef typename quaternion_type::order_type order_type;
 381
 382     /* Checking */
 383     detail::CheckIndex3(axis);
 384
 385     size_t i, j, k;
 386     cyclic_permutation(axis, i, j, k);
 387
 388     const size_t W = order_type::W;
 389     const size_t I = order_type::X + i;
 390     const size_t J = order_type::X + j;
 391     const size_t K = order_type::X + k;
 392
 393     angle *= value_type(.5);
 394     value_type s = value_type(std::sin(angle));
 395     value_type c = value_type(std::cos(angle));
 396
 397     quaternion_type result;
 398     result[I] = c * q[I] + s * q[W];
 399     result[J] = c * q[J] - s * q[K];
 400     result[K] = c * q[K] + s * q[J];
 401     result[W] = c * q[W] - s * q[I];
 402     q = result;
 403 }
 404
 405 /* Rotate a quaternion about the world x axis */
 406 template < class E, class A, class O, class C > void
 407 quaternion_rotate_about_world_x(quaternion<E,A,O,C>& q, E angle) {
 408     quaternion_rotate_about_world_axis(q,0,angle);
 409 }
 410
 411 /* Rotate a quaternion about the world y axis */
 412 template < class E, class A, class O, class C > void
 413 quaternion_rotate_about_world_y(quaternion<E,A,O,C>& q, E angle) {
 414     quaternion_rotate_about_world_axis(q,1,angle);
 415 }
 416
 417 /* Rotate a quaternion about the world z axis */
 418 template < class E, class A, class O, class C > void
 419 quaternion_rotate_about_world_z(quaternion<E,A,O,C>& q, E angle) {
 420     quaternion_rotate_about_world_axis(q,2,angle);
 421 }
 422
 423 //////////////////////////////////////////////////////////////////////////////
 424 // Relative rotation about local axes
 425 //////////////////////////////////////////////////////////////////////////////
 426
 427 /* Rotate a quaternion about the given local axis */
 428 template < class E, class A, class O, class C > void
 429 quaternion_rotate_about_local_axis(quaternion<E,A,O,C>& q,size_t axis,E angle)
 430 {
 431     typedef quaternion<E,A,O,C> quaternion_type;
 432     typedef typename quaternion_type::value_type value_type;
 433     typedef typename quaternion_type::order_type order_type;
 434
 435     /* Checking */
 436     detail::CheckIndex3(axis);
 437
 438     size_t i, j, k;
 439     cyclic_permutation(axis, i, j, k);
 440
 441     const size_t W = order_type::W;
 442     const size_t I = order_type::X + i;
 443     const size_t J = order_type::X + j;
 444     const size_t K = order_type::X + k;
 445
 446     angle *= value_type(.5);
 447     value_type s = value_type(std::sin(angle));
 448     value_type c = value_type(std::cos(angle));
 449
 450     quaternion_type result;
 451     result[I] = c * q[I] + s * q[W];
 452     result[J] = c * q[J] + s * q[K];
 453     result[K] = c * q[K] - s * q[J];
 454     result[W] = c * q[W] - s * q[I];
 455     q = result;
 456 }
 457
 458 /* Rotate a quaternion about its local x axis */
 459 template < class E, class A, class O, class C > void
 460 quaternion_rotate_about_local_x(quaternion<E,A,O,C>& q, E angle) {
 461     quaternion_rotate_about_local_axis(q,0,angle);
 462 }
 463
 464 /* Rotate a quaternion about its local y axis */
 465 template < class E, class A, class O, class C > void
 466 quaternion_rotate_about_local_y(quaternion<E,A,O,C>& q, E angle) {
 467     quaternion_rotate_about_local_axis(q,1,angle);
 468 }
 469
 470 /* Rotate a quaternion about its local z axis */
 471 template < class E, class A, class O, class C > void
 472 quaternion_rotate_about_local_z(quaternion<E,A,O,C>& q, E angle) {
 473     quaternion_rotate_about_local_axis(q,2,angle);
 474 }
 475
 476 //////////////////////////////////////////////////////////////////////////////
 477 // Rotation from vector to vector
 478 //////////////////////////////////////////////////////////////////////////////
 479
 480 /* http://www.martinb.com/maths/algebra/vectors/angleBetween/index.htm. */
 481
 482 /** Build a quaternion to rotate from one vector to another */
 483 template < class E,class A,class O,class C,class VecT_1,class VecT_2 > void
 484 quaternion_rotation_vec_to_vec(
 485     quaternion<E,A,O,C>& q,
 486     const VecT_1& v1,
 487     const VecT_2& v2,
 488     bool unit_length_vectors = false)
 489 {
 490     typedef quaternion<E,A,O,C> quaternion_type;
 491     typedef typename quaternion_type::value_type value_type;
 492     typedef vector< value_type, fixed<3> > vector_type;
 493
 494     /* Checking handled by cross() */
 495
 496     /* @todo: If at some point quaternion<> has a set() function that takes a
 497      * vector and a scalar, this can then be written as:
 498      *
 499      * if (...) {
 500      *     q.set(value_type(1)+dot(v1,v2), cross(v1,v2));
 501      * } else {
 502      *     q.set(std::sqrt(...)+dot(v1,v2), cross(v1,v2));
 503      * }
 504      */
 505
 506     vector_type c = cross(v1,v2);
 507     if (unit_length_vectors) {
 508         q = quaternion_type(value_type(1) + dot(v1,v2), c.data());
 509     } else {
 510         q = quaternion_type(
 511             std::sqrt(v1.length_squared() * v2.length_squared()) + dot(v1,v2),
 512             c/*.data()*/
 513         );
 514     }
 515     q.normalize();
 516 }
 517
 518 //////////////////////////////////////////////////////////////////////////////
 519 // Scale the angle of a rotation matrix
 520 //////////////////////////////////////////////////////////////////////////////
 521
 522 template < typename E, class A, class O, class C > void
 523 quaternion_scale_angle(quaternion<E,A,O,C>& q, E t,
 524     E tolerance = epsilon<E>::placeholder())
 525 {
 526     typedef vector< E,fixed<3> > vector_type;
 527     typedef typename vector_type::value_type value_type;
 528
 529     vector_type axis;
 530     value_type angle;
 531     quaternion_to_axis_angle(q, axis, angle, tolerance);
 532     quaternion_rotation_axis_angle(q, axis, angle * t);
 533 }
 534
 535 //////////////////////////////////////////////////////////////////////////////
 536 // Support functions for uniform handling of pos- and neg-cross quaternions
 537 //////////////////////////////////////////////////////////////////////////////
 538
 539 namespace detail {
 540
 541 /** Concatenate two quaternions in the order q1->q2 */
 542 template < class QuatT_1, class QuatT_2 >
 543 typename et::QuaternionPromote2<QuatT_1,QuatT_2>::temporary_type
 544 quaternion_rotation_difference(
 545     const QuatT_1& q1, const QuatT_2& q2, positive_cross)
 546 {
 547     return q2 * conjugate(q1);
 548 }
 549
 550 /** Concatenate two quaternions in the order q1->q2 */
 551 template < class QuatT_1, class QuatT_2 >
 552 typename et::QuaternionPromote2<QuatT_1,QuatT_2>::temporary_type
 553 quaternion_rotation_difference(
 554     const QuatT_1& q1, const QuatT_2& q2, negative_cross)
 555 {
 556     return conjugate(q1) * q2;
 557 }
 558
 559 } // namespace detail
 560
 561 //////////////////////////////////////////////////////////////////////////////
 562 // Quaternions rotation difference
 563 //////////////////////////////////////////////////////////////////////////////
 564
 565 /** Return the rotational 'difference' between two quaternions */
 566 template < class QuatT_1, class QuatT_2 >
 567 typename et::QuaternionPromote2<QuatT_1,QuatT_2>::temporary_type
 568 quaternion_rotation_difference(const QuatT_1& q1, const QuatT_2& q2) {
 569     return detail::quaternion_rotation_difference(
 570         q1, q2, typename QuatT_1::cross_type());
 571 }
 572
 573 //////////////////////////////////////////////////////////////////////////////
 574 // Conversions
 575 //////////////////////////////////////////////////////////////////////////////
 576
 577 /** Convert a quaternion to an axis-angle pair */
 578 template < class QuatT, typename E, class A > void
 579 quaternion_to_axis_angle(
 580     const QuatT& q,
 581     vector<E,A>& axis,
 582     E& angle,
 583     E tolerance = epsilon<E>::placeholder())
 584 {
 585     typedef QuatT quaternion_type;
 586     typedef typename quaternion_type::value_type value_type;
 587     typedef typename quaternion_type::order_type order_type;
 588
 589     /* Checking */
 590     detail::CheckQuat(q);
 591
 592     axis = q.imaginary();
 593     value_type l = length(axis);
 594     if (l > tolerance) {
 595         axis /= l;
 596         angle = value_type(2) * std::atan2(l,q.real());
 597     } else {
 598         axis.zero();
 599         angle = value_type(0);
 600     }
 601 }
 602
 603 /** Convert a quaternion to an Euler-angle triple
 604  *
 605  * Note: I've implemented direct quaternion-to-Euler conversion, but as far as
 606  * I can tell it more or less reduces to converting the quaternion to a matrix
 607  * as you go. The direct method is a little more efficient in that it doesn't
 608  * require a temporary and only the necessary matrix elements need be
 609  * computed. However, the implementation is complex and there's considerable
 610  * opportunity for error, so from a development and debugging standpoint I
 611  * think it's better to just perform the conversion via matrix_to_euler(),
 612  * which is already known to be correct.
 613 */
 614
 615 template < class QuatT, typename Real > void
 616 quaternion_to_euler(
 617     const QuatT& q,
 618     Real& angle_0,
 619     Real& angle_1,
 620     Real& angle_2,
 621     EulerOrder order,
 622     Real tolerance = epsilon<Real>::placeholder())
 623 {
 624     typedef QuatT quaternion_type;
 625     typedef typename quaternion_type::value_type value_type;
 626     typedef matrix< value_type,fixed<3,3>,row_basis,row_major > matrix_type;
 627
 628     matrix_type m;
 629     matrix_rotation_quaternion(m, q);
 630     matrix_to_euler(m, angle_0, angle_1, angle_2, order, tolerance);
 631 }
 632
 633 } // namespace cml
 634
 635 #endif