Dogcows Code - chaz/yoink/blob - src/cml/quaternion/quaternion.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  *  @todo Return a VectorXpr adaptor from the imaginary() method of
  13  *  quaternion and the expression node types.
  14  *
  15  *  @todo swap multiplication order based upon template param
  16  *
  17  *  @todo change element order based upon template param
  18  */
  19
  20 #ifndef quaternion_h
  21 #define quaternion_h
  22
  23 #include <cml/mathlib/epsilon.h>
  24 #include <cml/quaternion/quaternion_expr.h>
  25 #include <cml/quaternion/quaternion_dot.h>
  26 #include <cml/util.h>
  27
  28 /* This is used below to create a more meaningful compile-time error when
  29  * the quaternion class is not created with a fixed-size 4-vector:
  30  */
  31 struct quaternion_requires_fixed_size_array_type_error;
  32
  33 namespace cml {
  34
  35 /** A configurable quaternion type.
  36  *
  37  * @note Quaternions with two different orders cannot be used in the same
  38  * expression.
  39  */
  40 template<
  41     typename Element,
  42     class ArrayType,
  43     class Order,
  44     class Cross
  45 >
  46 class quaternion
  47 {
  48     /* The ArrayType must be fixed<> or external<>: */
  49     CML_STATIC_REQUIRE_M(
  50             (same_type< ArrayType, fixed<> >::is_true
  51              || same_type< ArrayType, external<> >::is_true),
  52             quaternion_requires_fixed_size_array_type_error);
  53
  54   public:
  55
  56     /* Shorthand for the array type generator: */
  57     typedef ArrayType storage_type;
  58     typedef typename ArrayType::template rebind<4>::other generator_type;
  59
  60     /* Vector representing the quaternion.  Use the rebinding template to
  61      * set the vector size:
  62      */
  63     typedef vector<Element, generator_type> vector_type;
  64
  65     /* Vector temporary type: */
  66     typedef typename vector_type::temporary_type vector_temporary;
  67
  68     /* Quaternion order: */
  69     typedef Order order_type;
  70
  71     /* Quaternion multiplication order: */
  72     typedef Cross cross_type;
  73
  74     /* Scalar type representing the scalar part: */
  75     typedef typename vector_type::value_type value_type;
  76     typedef typename vector_type::reference reference;
  77     typedef typename vector_type::const_reference const_reference;
  78     /* XXX Need to verify that this is a true scalar type. */
  79
  80     /* The quaternion type: */
  81     typedef quaternion<Element,storage_type,order_type,cross_type>
  82         quaternion_type;
  83
  84     /* For integration into the expression template code: */
  85     typedef quaternion_type expr_type;
  86
  87     /* For integration into the expression template code: */
  88     typedef quaternion<
  89         Element, typename vector_temporary::storage_type,
  90         order_type, cross_type> temporary_type;
  91
  92     /* For integration into the expression templates code: */
  93     typedef quaternion_type& expr_reference;
  94     typedef const quaternion_type& expr_const_reference;
  95
  96     /* For matching by storage type: */
  97     typedef typename vector_type::memory_tag memory_tag;
  98
  99     /* For matching by size type: */
 100     typedef typename vector_type::size_tag size_tag;
 101
 102     /* Get the imaginary part type: */
 103     typedef typename vector_temporary::subvector_type imaginary_type;
 104
 105     /* For matching by result-type: */
 106     typedef cml::et::quaternion_result_tag result_tag;
 107
 108     /* For matching by assignability: */
 109     typedef cml::et::assignable_tag assignable_tag;
 110
 111
 112   public:
 113
 114     /** Record result size as an enum. */
 115     enum { array_size = 4 };
 116
 117     /** Localize the ordering as an enum. */
 118     enum {
 119         W = order_type::W,
 120         X = order_type::X,
 121         Y = order_type::Y,
 122         Z = order_type::Z
 123     };
 124
 125
 126   public:
 127
 128     /** Return the scalar part. */
 129     value_type real() const { return m_q[W]; }
 130
 131     /** Return the imaginary vector. */
 132     imaginary_type imaginary() const {
 133         /*
 134         imaginary_type v;
 135         v[0] = m_q[X]; v[1] = m_q[Y]; v[2] = m_q[Z];
 136         return v;
 137         */
 138         return imaginary_type(m_q[X], m_q[Y], m_q[Z]);
 139     }
 140
 141     /** Return the vector representing the quaternion. */
 142     const vector_type& as_vector() const {
 143         return m_q;
 144     }
 145
 146     /** Return the Cayley norm of the quaternion. */
 147     value_type norm() const {
 148         return length_squared();
 149     }
 150
 151     /** Return square of the quaternion length. */
 152     value_type length_squared() const {
 153         return cml::dot(*this,*this);
 154     }
 155
 156     /** Return the quaternion length. */
 157     value_type length() const {
 158         return std::sqrt(length_squared());
 159     }
 160
 161     /** Normalize this quaternion (divide by its length).
 162      *
 163      * @todo Make this return a QuaternionXpr.
 164      */
 165     quaternion_type& normalize() {
 166         return (*this /= length());
 167     }
 168
 169     /** Set this quaternion to the conjugate. */
 170     quaternion_type& conjugate() {
 171         return (*this) = cml::conjugate(*this);
 172     }
 173
 174     /** Set this quaternion to the inverse. */
 175     quaternion_type& inverse() {
 176         return (*this) = cml::inverse(*this);
 177     }
 178
 179     /** Set this quaternion to the multiplicative identity. */
 180     quaternion_type& identity() {
 181         m_q[W] = value_type(1);
 182         m_q[X] = value_type(0);
 183         m_q[Y] = value_type(0);
 184         m_q[Z] = value_type(0);
 185         return *this;
 186     }
 187
 188     /** Return the log of this quaternion. */
 189     temporary_type log(
 190         value_type tolerance = epsilon<value_type>::placeholder()) const
 191     {
 192         value_type a = acos_safe(real());
 193         value_type s = std::sin(a);
 194
 195         if (s > tolerance) {
 196             return temporary_type(value_type(0), imaginary() * (a / s));
 197         } else {
 198             return temporary_type(value_type(0), imaginary());
 199         }
 200     }
 201
 202     /**
 203      * Return the result of the exponential function as applied to
 204      * this quaternion.
 205      */
 206     temporary_type exp(
 207         value_type tolerance = epsilon<value_type>::placeholder()) const
 208     {
 209         imaginary_type v = imaginary();
 210         value_type a = cml::length(v);
 211
 212         if (a > tolerance) {
 213             return temporary_type(std::cos(a), v * (std::sin(a) / a));
 214         } else {
 215             return temporary_type(std::cos(a), v);
 216         }
 217     }
 218
 219
 220     /** Const access to the quaternion as a vector. */
 221     const_reference operator[](size_t i) const { return m_q[i]; }
 222
 223     /** Mutable access to the quaternion as a vector. */
 224     reference operator[](size_t i) { return m_q[i]; }
 225
 226     /** Fill quaternion with random elements.
 227      *
 228      * @warning This does not generate uniformly random rotations.
 229      */
 230     void random(value_type min, value_type max) {
 231         for (size_t i = 0; i < 4; ++i) {
 232             m_q[i] = random_real(min,max);
 233         }
 234     }
 235
 236   public:
 237
 238     /** Default initializer.
 239      *
 240      * @note The default constructor cannot be used with an external<>
 241      * array type.
 242      */
 243     quaternion() {}
 244
 245     /** Initializer for an external<> vector type. */
 246     quaternion(Element* const array) : m_q(array) {}
 247
 248     /** Copy construct from the same type of quaternion. */
 249     quaternion(const quaternion_type& q) : m_q(q.m_q) {}
 250
 251     /** Construct from a quaternion having a different array type. */
 252     template<typename E, class AT> quaternion(
 253             const quaternion<E,AT,order_type,cross_type>& q)
 254         : m_q(q.as_vector()) {}
 255
 256     /** Copy construct from a QuaternionXpr. */
 257     template<typename XprT> quaternion(QUATXPR_ARG_TYPE e) {
 258         typedef typename XprT::order_type arg_order;
 259         m_q[W] = e[arg_order::W];
 260         m_q[X] = e[arg_order::X];
 261         m_q[Y] = e[arg_order::Y];
 262         m_q[Z] = e[arg_order::Z];
 263     }
 264
 265
 266
 267     /** Initialize from a 4-vector.
 268      *
 269      * If Order is scalar_first, then v[0] is the real part.  Otherwise,
 270      * v[3] is the real part.
 271      */
 272     quaternion(const vector_type& v) : m_q(v) {}
 273
 274     /** Initialize from an array of scalars.
 275      *
 276      * If Order is scalar_first, then v[0] is the real part.  Otherwise,
 277      * v[3] is the real part.
 278      *
 279      * @note The target vector must have CML_VEC_COPY_FROM_ARRAY
 280      * implemented, so this cannot be used with external<> vectors.
 281      */
 282     quaternion(const value_type v[4]) : m_q(v) {}
 283
 284     /** Initialize from 4 scalars.
 285      *
 286      * If Order is scalar_first, then a is the real part, and (b,c,d) is
 287      * the imaginary part.  Otherwise, (a,b,c) is the imaginary part, and d
 288      * is the real part.
 289      */
 290     quaternion(
 291             const value_type& a, const value_type& b,
 292             const value_type& c, const value_type& d)
 293     {
 294       /* Call the overloaded assignment function: */
 295       assign(a, b, c, d, Order());
 296     }
 297
 298     /** Initialize both the real and imaginary parts.
 299      *
 300      * The imaginary part is given by a 3-vector.  Although the imaginary
 301      * part is specified first, the proper coefficient order (vector or
 302      * scalar first) is maintained.
 303      */
 304     quaternion(const value_type& s, const imaginary_type& v) {
 305         m_q[W] = s; m_q[X] = v[0]; m_q[Y] = v[1]; m_q[Z] = v[2];
 306     }
 307
 308     /** Initialize both the real and imaginary parts.
 309      *
 310      * The imaginary part is given by a 3-vector.  Although the imaginary
 311      * part is specified second, the proper coefficient order (vector or
 312      * scalar first) is maintained.
 313      */
 314     quaternion(const imaginary_type& v, const value_type& s) {
 315         m_q[W] = s; m_q[X] = v[0]; m_q[Y] = v[1]; m_q[Z] = v[2];
 316     }
 317
 318     /** Initialize both the real and imaginary parts.
 319      *
 320      * The imaginary part is given by an array of scalars.  Although the
 321      * imaginary part is specified first, the proper coefficient order
 322      * (vector or scalar first) is maintained.
 323      */
 324     quaternion(const value_type v[3], const value_type& s) {
 325         m_q[W] = s; m_q[X] = v[0]; m_q[Y] = v[1]; m_q[Z] = v[2];
 326     }
 327
 328     /** Initialize both the real and imaginary parts.
 329      *
 330      * The imaginary part is given by an array of scalars.  Although the
 331      * imaginary part is specified second, the proper coefficient order
 332      * (vector or scalar first) is maintained.
 333      */
 334     quaternion(const value_type& s, const value_type v[3]) {
 335         m_q[W] = s; m_q[X] = v[0]; m_q[Y] = v[1]; m_q[Z] = v[2];
 336     }
 337
 338
 339
 340     /** Initialize from a VectorXpr. */
 341     template<typename XprT>
 342         quaternion(VECXPR_ARG_TYPE e) : m_q(e) {}
 343
 344     /** Initialize both the real and imaginary parts.
 345      *
 346      * The imaginary part is initialized with a VectorXpr.
 347      */
 348     template<typename XprT>
 349         quaternion(const value_type& s, VECXPR_ARG_TYPE e) {
 350             m_q[W] = s; m_q[X] = e[0]; m_q[Y] = e[1]; m_q[Z] = e[2];
 351         }
 352
 353     // @todo: Are we missing:
 354
 355     // quaternion(VECXPR_ARG_TYPE e, const value_type& s) {}
 356
 357     // Or is that covered elsewhere?
 358
 359     /** In-place op from a quaternion.
 360      *
 361      * This assumes that _op_ is defined for both the quaternion's vector
 362      * type and its scalar type.
 363      */
 364 #define CML_QUAT_ASSIGN_FROM_QUAT(_op_)                                 \
 365     template<typename E, class AT> const quaternion_type&               \
 366     operator _op_ (const quaternion<E,AT,order_type,cross_type>& q) {   \
 367         m_q[W] _op_ q[W];                                               \
 368         m_q[X] _op_ q[X];                                               \
 369         m_q[Y] _op_ q[Y];                                               \
 370         m_q[Z] _op_ q[Z];                                               \
 371         return *this;                                                   \
 372     }
 373
 374     /** In-place op from a QuaternionXpr.
 375      *
 376      * This assumes that _op_ is defined for the quaternion's scalar type.
 377      */
 378 #define CML_QUAT_ASSIGN_FROM_QUATXPR(_op_)                              \
 379     template<typename XprT> quaternion_type&                            \
 380     operator _op_ (QUATXPR_ARG_TYPE e) {                                \
 381         typedef typename XprT::order_type arg_order;                    \
 382         m_q[W] _op_ e[arg_order::W];                                    \
 383         m_q[X] _op_ e[arg_order::X];                                    \
 384         m_q[Y] _op_ e[arg_order::Y];                                    \
 385         m_q[Z] _op_ e[arg_order::Z];                                    \
 386         return *this;                                                   \
 387     }
 388
 389     /** In-place op from a scalar type.
 390      *
 391      * This assumes that _op_ is defined for the quaternion's scalar type.
 392      */
 393 #define CML_QUAT_ASSIGN_FROM_SCALAR(_op_,_op_name_)               \
 394     quaternion_type& operator _op_ (const value_type& s) {        \
 395         typedef _op_name_ <value_type,value_type> OpT;            \
 396         OpT().apply(m_q[W],s);                                    \
 397         OpT().apply(m_q[X],s);                                    \
 398         OpT().apply(m_q[Y],s);                                    \
 399         OpT().apply(m_q[Z],s);                                    \
 400         return *this;                                             \
 401     }
 402
 403     CML_QUAT_ASSIGN_FROM_QUAT(=)
 404     CML_QUAT_ASSIGN_FROM_QUAT(+=)
 405     CML_QUAT_ASSIGN_FROM_QUAT(-=)
 406
 407     CML_QUAT_ASSIGN_FROM_QUATXPR(=)
 408     CML_QUAT_ASSIGN_FROM_QUATXPR(+=)
 409     CML_QUAT_ASSIGN_FROM_QUATXPR(-=)
 410
 411     CML_QUAT_ASSIGN_FROM_SCALAR(*=, cml::et::OpMulAssign)
 412     CML_QUAT_ASSIGN_FROM_SCALAR(/=, cml::et::OpDivAssign)
 413
 414 #undef CML_QUAT_ASSIGN_FROM_QUAT
 415 #undef CML_QUAT_ASSIGN_FROM_QUATXPR
 416 #undef CML_QUAT_ASSIGN_FROM_SCALAR
 417
 418     /** Accumulated multiplication with a quaternion.
 419      *
 420      * Compute p = p * q for two quaternions p and q.
 421      *
 422      * @internal Using operator* here is okay, as long as cml/quaternion.h
 423      * is included before using this method (the only supported case for
 424      * end-user code). This is because modern compilers won't instantiate a
 425      * method in a template class until it is used, and including the main
 426      * header ensures all definitions are available before any possible use
 427      * of this method.
 428      */
 429     quaternion_type& operator*=(const quaternion_type& q) {
 430         return (*this = *this * q);
 431     }
 432
 433     /** Accumulated multiplication with a quaternion expression.
 434      *
 435      * Compute p = p * e for a quaternion p and a quaternion expression e.
 436      *
 437      * @internal Using operator* here is okay, as long as cml/quaternion.h
 438      * is included before using this method (the only supported case for
 439      * end-user code). This is because modern compilers won't instantiate a
 440      * method in a template class until it is used, and including the main
 441      * header ensures all definitions are available before any possible use
 442      * of this method.
 443      */
 444     template<typename XprT> quaternion_type& operator*=(QUATXPR_ARG_TYPE e) {
 445         return (*this = *this * e);
 446     }
 447
 448     /** Return access to the data as a raw pointer. */
 449     typename vector_type::pointer data() { return m_q.data(); }
 450
 451     /** Return access to the data as a const raw pointer. */
 452     const typename vector_type::pointer data() const { return m_q.data(); }
 453
 454
 455     /* NOTE: Quaternion division no longer supported, but I'm leaving the
 456        code here for reference (Jesse) */
 457
 458     #if 0
 459     /** Accumulated division with a quaternion.
 460      *
 461      * Compute p = p * inverse(q).
 462      *
 463      * @note Because quaternion multiplication is non-commutative, division
 464      * is ambiguous.  This method assumes a multiplication order consistent
 465      * with the notational order; i.e. p = q / r means p = q*inverse(r).
 466      *
 467      * @internal Using operator* and cml::inverse here is okay, as long as
 468      * cml/quaternion.h is included before using this method (the only
 469      * supported case for end-user code). This is because modern compilers
 470      * won't instantiate a method in a template class until it is used, and
 471      * including the main header ensures all definitions are available
 472      * before any possible use of this method.
 473      */
 474     quaternion_type& operator/=(const quaternion_type& q) {
 475         return (*this = *this * cml::inverse(q));
 476     }
 477
 478     /** Accumulated division with a quaternion expression.
 479      *
 480      * Compute p = p * inverse(q).
 481      *
 482      * @note Because quaternion multiplication is non-commutative, division
 483      * is ambiguous.  This method assumes a multiplication order consistent
 484      * with the notational order; i.e. p = q / r means p = q*inverse(r).
 485      *
 486      * @internal Using operator* and cml::inverse here is okay, as long as
 487      * cml/quaternion.h is included before using this method (the only
 488      * supported case for end-user code). This is because modern compilers
 489      * won't instantiate a method in a template class until it is used, and
 490      * including the main header ensures all definitions are available
 491      * before any possible use of this method.
 492      */
 493     template<typename XprT> quaternion_type& operator/=(QUATXPR_ARG_TYPE e) {
 494         return (*this = *this * cml::inverse(e));
 495     }
 496     #endif
 497
 498
 499   protected:
 500
 501     /** Overloaded function to assign the quaternion from 4 scalars. */
 502     void assign(const value_type& a, const value_type& b,
 503         const value_type& c, const value_type& d, scalar_first)
 504     {
 505       m_q[W] = a; m_q[X] = b; m_q[Y] = c; m_q[Z] = d;
 506     }
 507
 508     /** Overloaded function to assign the quaternion from 4 scalars. */
 509     void assign(const value_type& a, const value_type& b,
 510         const value_type& c, const value_type& d, vector_first)
 511     {
 512       m_q[X] = a; m_q[Y] = b; m_q[Z] = c; m_q[W] = d;
 513     }
 514
 515
 516   protected:
 517
 518     vector_type                 m_q;
 519 };
 520
 521 } // namespace cml
 522
 523 #endif
 524
 525 // -------------------------------------------------------------------------
 526 // vim:ft=cpp