Dogcows Code - chaz/yoink/blob - src/cml/quaternion/quaternion_mul.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 Multiplication of two quaternions, p*q.
  11  *
  12  * This uses the expression tree, since the result is closed-form and can be
  13  * computed by index.
  14  */
  15
  16 #ifndef quaternion_mul_h
  17 #define quaternion_mul_h
  18
  19 #include <cml/mathlib/checking.h>
  20 #include <cml/quaternion/quaternion_promotions.h>
  21
  22 namespace cml {
  23 namespace detail {
  24
  25 template < class CrossType, class Real > struct SumOp;
  26
  27 template < class Real > struct SumOp< positive_cross, Real > {
  28     Real operator()(Real a, Real b) const {
  29         return a + b;
  30     }
  31 };
  32
  33 template < class Real > struct SumOp< negative_cross, Real > {
  34     Real operator()(Real a, Real b) const {
  35         return a - b;
  36     }
  37 };
  38
  39 template < class Quat1_T, class Quat2_T >
  40 typename et::QuaternionPromote<
  41     typename Quat1_T::temporary_type, typename Quat2_T::temporary_type
  42 >::temporary_type
  43 QuaternionMult(const Quat1_T& q1, const Quat2_T& q2)
  44 {
  45     detail::CheckQuat(q1);
  46     detail::CheckQuat(q2);
  47
  48     typedef typename et::QuaternionPromote<
  49         typename Quat1_T::temporary_type, typename Quat2_T::temporary_type
  50     >::temporary_type temporary_type;
  51
  52     typedef typename temporary_type::value_type value_type;
  53     typedef typename temporary_type::order_type order_type;
  54     typedef typename temporary_type::cross_type cross_type;
  55
  56     typedef detail::SumOp<cross_type, value_type> sum_op;
  57
  58     enum {
  59         W = order_type::W,
  60         X = order_type::X,
  61         Y = order_type::Y,
  62         Z = order_type::Z
  63     };
  64
  65     temporary_type result;
  66
  67     /* s1*s2-dot(v1,v2): */
  68     result[W] =
  69         q1[W]*q2[W] - q1[X]*q2[X] - q1[Y]*q2[Y] - q1[Z]*q2[Z];
  70
  71     /* (s1*v2 + s2*v1 + v1^v2) i: */
  72     result[X] =
  73         sum_op()(q1[W]*q2[X] + q2[W]*q1[X], q1[Y]*q2[Z] - q1[Z]*q2[Y]);
  74
  75     /* (s1*v2 + s2*v1 + v1^v2) j: */
  76     result[Y] =
  77         sum_op()(q1[W]*q2[Y] + q2[W]*q1[Y], q1[Z]*q2[X] - q1[X]*q2[Z]);
  78
  79     /* (s1*v2 + s2*v1 + v1^v2) k: */
  80     result[Z] =
  81         sum_op()(q1[W]*q2[Z] + q2[W]*q1[Z], q1[X]*q2[Y] - q1[Y]*q2[X]);
  82
  83     return result;
  84 }
  85
  86 } // namespace detail
  87
  88 /** Declare mul taking two quaternion operands. */
  89 template<typename E1, class AT1, typename E2, class AT2, class OT, class CT>
  90 inline typename et::QuaternionPromote<
  91     typename quaternion<E1,AT1,OT,CT>::temporary_type,
  92     typename quaternion<E2,AT2,OT,CT>::temporary_type
  93 >::temporary_type operator*(
  94     const quaternion<E1,AT1,OT,CT>& left,
  95     const quaternion<E2,AT2,OT,CT>& right)
  96 {
  97     return detail::QuaternionMult(left, right);
  98 }
  99
 100 /** Declare mul taking a quaternion and a et::QuaternionXpr. */
 101 template<typename E, class AT, class OT, class CT, class XprT>
 102 inline typename et::QuaternionPromote<
 103     typename quaternion<E,AT,OT,CT>::temporary_type,
 104     typename XprT::temporary_type
 105 >::temporary_type operator*(
 106     const quaternion<E,AT,OT,CT>& left,
 107     QUATXPR_ARG_TYPE right)
 108 {
 109     return detail::QuaternionMult(left, right);
 110 }
 111
 112 /** Declare mul taking an et::QuaternionXpr and a quaternion. */
 113 template<class XprT, typename E, class AT, class OT, class CT>
 114 inline typename et::QuaternionPromote<
 115     typename XprT::temporary_type,
 116     typename quaternion<E,AT,OT,CT>::temporary_type
 117 >::temporary_type operator*(
 118     QUATXPR_ARG_TYPE left,
 119     const quaternion<E,AT,OT,CT>& right)
 120 {
 121     return detail::QuaternionMult(left, right);
 122 }
 123
 124 /** Declare mul taking two et::QuaternionXpr operands. */
 125 template<class XprT1, class XprT2>
 126 inline typename et::QuaternionPromote<
 127     typename XprT1::temporary_type, typename XprT2::temporary_type
 128 >::temporary_type operator*(
 129     QUATXPR_ARG_TYPE_N(1) left,
 130     QUATXPR_ARG_TYPE_N(2) right)
 131 {
 132     return detail::QuaternionMult(left, right);
 133 }
 134
 135 } // namespace cml
 136
 137 #endif
 138
 139 // -------------------------------------------------------------------------
 140 // vim:ft=cpp