Dogcows Code - chaz/yoink/blob - src/cml/quaternion/inverse.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 Defines an operator for quaternion inverse.
  11  */
  12
  13 #ifndef quaternion_inverse_h
  14 #define quaternion_inverse_h
  15
  16 #include <cml/quaternion/quaternion_expr.h>
  17 #include <cml/quaternion/quaternion_functions.h>
  18
  19 namespace cml {
  20 namespace et {
  21
  22 /** An expression node for inverting a quaternion.
  23  *
  24  * This internally creates a ConjugateOp node to process the conjugate
  25  * of the given expression.  The values produced by the ConjugateOp are then
  26  * divided by the Cayley norm of the expression on the fly.
  27  */
  28 template<class ExprT>
  29 class QuaternionInverseOp
  30 {
  31   public:
  32
  33     typedef QuaternionInverseOp<ExprT> expr_type;
  34
  35     /* Record ary-ness of the expression: */
  36     typedef unary_expression expr_ary;
  37
  38     /* Copy the expression by value into higher-up expressions: */
  39     typedef expr_type expr_const_reference;
  40
  41     /* The subexpression is a ConjugateOp: */
  42     typedef et::ConjugateOp<ExprT> subexpression_type;
  43     typedef ExprTraits<subexpression_type> expr_traits;
  44
  45     /* Get traits for the ExprT: */
  46     typedef ExprTraits<ExprT> arg_traits;
  47     typedef typename arg_traits::const_reference arg_reference;
  48
  49     typedef typename subexpression_type::value_type value_type;
  50     typedef quaternion_result_tag result_tag;
  51     typedef typename subexpression_type::size_tag size_tag;
  52
  53     /* Reference type for the subexpression: */
  54     typedef typename expr_traits::const_reference expr_reference;
  55
  56     /* Get the result type (same as for subexpression): */
  57     typedef typename expr_traits::result_type result_type;
  58
  59     /* For matching by assignability: */
  60     typedef cml::et::not_assignable_tag assignable_tag;
  61
  62     /* Get the temporary type: */
  63     typedef typename result_type::temporary_type temporary_type;
  64
  65     /* Get the vector type: */
  66     typedef typename result_type::vector_type vector_type;
  67
  68     /* Get the imaginary part type: */
  69     typedef typename vector_type::subvector_type imaginary_type;
  70
  71     /* Record the order type: */
  72     typedef typename result_type::order_type order_type;
  73
  74
  75   public:
  76
  77     /** Record result size as an enum. */
  78     enum { array_size = ExprT::array_size };
  79
  80     /** Localize the ordering as an enum. */
  81     enum {
  82         W = order_type::W,
  83         X = order_type::X,
  84         Y = order_type::Y,
  85         Z = order_type::Z
  86     };
  87
  88
  89   public:
  90
  91     /** Return the real part of the expression. */
  92     value_type real() const {
  93         return m_expr.real()/m_norm;
  94     }
  95
  96     /** Return the vector part of the expression.
  97      *
  98      * @todo This could be returned as a VectorXpr also.
  99      */
 100     imaginary_type imaginary() const {
 101         return m_expr.imaginary()/m_norm;
 102     }
 103
 104     /** Return the Cayley norm of the expression. */
 105     value_type norm() const {
 106         return length_squared();
 107     }
 108
 109     /** Return square of the quaternion length. */
 110     value_type length_squared() const {
 111         return dot(
 112                 QuaternionXpr<expr_type>(*this),
 113                 QuaternionXpr<expr_type>(*this));
 114     }
 115
 116     /** Return the quaternion length. */
 117     value_type length() const {
 118         return std::sqrt(length_squared());
 119     }
 120
 121     /** Return the result as a normalized quaternion. */
 122     temporary_type normalize() const {
 123         temporary_type q(QuaternionXpr<expr_type>(*this));
 124         return q.normalize();
 125     }
 126
 127     /** Compute inverse result at index i.
 128      *
 129      * The inverse of a quaternion p is ~p/norm(p).
 130      */
 131     value_type operator[](size_t i) const {
 132         return m_expr[i]/m_norm;
 133     }
 134
 135
 136   public:
 137
 138     /** Return size of this expression (same as argument's size). */
 139     size_t size() const {
 140         return m_expr.size();
 141     }
 142
 143     /** Return reference to contained expression. */
 144     expr_reference expression() const { return m_expr; }
 145
 146
 147   public:
 148
 149     /** Construct from an input expression. */
 150     explicit QuaternionInverseOp(arg_reference arg)
 151         //: m_expr(arg), m_norm(cml::norm(arg)) {}
 152         : m_expr(arg), m_norm(arg.norm()) {}
 153
 154     /** Copy constructor. */
 155     QuaternionInverseOp(const expr_type& e)
 156         : m_expr(e.m_expr), m_norm(e.m_norm) {}
 157
 158
 159   protected:
 160
 161     subexpression_type m_expr;
 162     value_type m_norm;
 163
 164
 165   private:
 166
 167     /* Cannot be assigned to: */
 168     expr_type& operator=(const expr_type&);
 169 };
 170
 171 /** Expression traits class for QuaternionInverseOp<>. */
 172 template<class ExprT>
 173 struct ExprTraits< QuaternionInverseOp<ExprT> >
 174 {
 175     typedef QuaternionInverseOp<ExprT> expr_type;
 176     typedef ExprT arg_type;
 177
 178     typedef typename expr_type::value_type value_type;
 179     typedef typename expr_type::expr_const_reference const_reference;
 180     typedef typename expr_type::result_tag result_tag;
 181     typedef typename expr_type::size_tag size_tag;
 182     typedef typename expr_type::result_type result_type;
 183     typedef typename expr_type::assignable_tag assignable_tag;
 184     typedef expr_node_tag node_tag;
 185
 186     value_type get(const expr_type& v, size_t i) const { return v[i]; }
 187     size_t size(const expr_type& e) const { return e.size(); }
 188 };
 189
 190 } // namespace et
 191
 192 /** Inverse of a quaternion. */
 193 template<typename E, class AT, class OrderT, class CrossT> inline
 194 et::QuaternionXpr< et::QuaternionInverseOp< quaternion<E,AT,OrderT,CrossT> > >
 195 inverse(const quaternion<E,AT,OrderT,CrossT>& arg)
 196 {
 197     typedef et::QuaternionInverseOp< quaternion<E,AT,OrderT,CrossT> > ExprT;
 198     return et::QuaternionXpr<ExprT>(ExprT(arg));
 199 }
 200
 201 /** Inverse of a QuaternionXpr. */
 202 template<class XprT> inline
 203 et::QuaternionXpr< et::QuaternionInverseOp<XprT> >
 204 inverse(QUATXPR_ARG_TYPE arg)
 205 {
 206     typedef et::QuaternionInverseOp<XprT> ExprT;
 207     return et::QuaternionXpr<ExprT>(ExprT(arg.expression()));
 208 }
 209
 210 /* NOTE: Quaternion division no longer supported, but I'm leaving the
 211    code here for reference (Jesse) */
 212
 213 #if 0
 214 /** Declare div taking two quaternion operands. */
 215 template<typename E1, class AT1, typename E2, class AT2, class OT, class CT>
 216 inline typename et::QuaternionPromote<
 217     quaternion<E1,AT1,OT,CT>, quaternion<E2,AT2,OT,CT>
 218 >::temporary_type
 219 operator/(
 220         const quaternion<E1,AT1,OT,CT>& left,
 221         const quaternion<E2,AT2,OT,CT>& right)
 222 {
 223     return left*inverse(right);
 224 }
 225
 226 /** Declare div taking a quaternion and a et::QuaternionXpr. */
 227 template<typename E, class AT, class OT, class CT, class XprT>
 228 inline typename et::QuaternionPromote<
 229     quaternion<E,AT,OT,CT>, typename XprT::result_type
 230 >::temporary_type
 231 operator/(
 232         const quaternion<E,AT,OT,CT>& left,
 233         QUATXPR_ARG_TYPE right)
 234 {
 235     return left*inverse(right);
 236 }
 237
 238 /** Declare div taking an et::QuaternionXpr and a quaternion. */
 239 template<class XprT, typename E, class AT, class OT, class CT>
 240 inline typename et::QuaternionPromote<
 241     typename XprT::result_type, quaternion<E,AT,OT,CT>
 242 >::temporary_type
 243 operator/(
 244         QUATXPR_ARG_TYPE left,
 245         const quaternion<E,AT,OT,CT>& right)
 246 {
 247     return left*inverse(right);
 248 }
 249
 250 /** Declare div taking two et::QuaternionXpr operands. */
 251 template<class XprT1, class XprT2>
 252 inline typename et::QuaternionPromote<
 253     typename XprT1::result_type, typename XprT2::result_type
 254 >::temporary_type
 255 operator/(
 256         QUATXPR_ARG_TYPE_N(1) left,
 257         QUATXPR_ARG_TYPE_N(2) right)
 258 {
 259     return left*inverse(right);
 260 }
 261 #endif
 262
 263 } // namespace cml
 264
 265 #endif
 266
 267 // -------------------------------------------------------------------------
 268 // vim:ft=cpp