Dogcows Code - chaz/yoink/blob - src/Moof/cml/mathlib/quaternion_basis.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_basis_h
  14 #define quaternion_basis_h
  15
  16 #include <cml/mathlib/checking.h>
  17
  18 /* Functions for getting the basis vectors of a quaternion rotation. */
  19
  20 namespace cml {
  21
  22 /** Get the i'th basis vector of a quaternion rotation */
  23 template < class QuatT > vector< typename QuatT::value_type, fixed<3> >
  24 quaternion_get_basis_vector(const QuatT& q, size_t i)
  25 {
  26     typedef QuatT quaternion_type;
  27     typedef typename quaternion_type::value_type value_type;
  28     typedef typename quaternion_type::order_type order_type;
  29     typedef vector< value_type, fixed<3> > vector_type;
  30
  31     /* Checking */
  32     detail::CheckQuat(q);
  33     detail::CheckIndex3(i);
  34
  35     size_t j, k;
  36     cyclic_permutation(i, i, j, k);
  37
  38     /* @todo: Clean this up. */
  39     const size_t W = order_type::W;
  40     const size_t I = order_type::X + i;
  41     const size_t J = order_type::X + j;
  42     const size_t K = order_type::X + k;
  43
  44     value_type j2 = q[J] + q[J];
  45     value_type k2 = q[K] + q[K];
  46
  47     /* @todo: use set_permuted() for the following when available. */
  48
  49     vector_type result;
  50     result[i] = value_type(1) - q[J] * j2 - q[K] * k2;
  51     result[j] = q[I] * j2 + q[W] * k2;
  52     result[k] = q[I] * k2 - q[W] * j2;
  53     return result;
  54 }
  55
  56 /** Get the x basis vector of a quaternion rotation */
  57 template < class QuatT > vector< typename QuatT::value_type, fixed<3> >
  58 quaternion_get_x_basis_vector(const QuatT& q) {
  59     return quaternion_get_basis_vector(q,0);
  60 }
  61
  62 /** Get the y basis vector of a quaternion rotation */
  63 template < class QuatT > vector< typename QuatT::value_type, fixed<3> >
  64 quaternion_get_y_basis_vector(const QuatT& q) {
  65     return quaternion_get_basis_vector(q,1);
  66 }
  67
  68 /** Get the z basis vector of a quaternion rotation */
  69 template < class QuatT > vector< typename QuatT::value_type, fixed<3> >
  70 quaternion_get_z_basis_vector(const QuatT& q) {
  71     return quaternion_get_basis_vector(q,2);
  72 }
  73
  74 /** Get the basis vectors of a quaternion rotation */
  75 template < class QuatT, typename E, class A > void
  76 quaternion_get_basis_vectors(
  77     const QuatT& q,
  78     vector<E,A>& x,
  79     vector<E,A>& y,
  80     vector<E,A>& z)
  81 {
  82     x = quaternion_get_x_basis_vector(q);
  83     y = quaternion_get_y_basis_vector(q);
  84     z = quaternion_get_z_basis_vector(q);
  85 }
  86
  87 } // namespace cml
  88
  89 #endif