1 /* -*- C++ -*- ------------------------------------------------------------
3 Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/
5 The Configurable Math Library (CML) is distributed under the terms of the
6 Boost Software License, v1.0 (see cml/LICENSE for details).
8 *-----------------------------------------------------------------------*/
12 * @todo Return a VectorXpr adaptor from the imaginary() method of
13 * quaternion and the expression node types.
15 * @todo swap multiplication order based upon template param
17 * @todo change element order based upon template param
23 #include <cml/mathlib/epsilon.h>
24 #include <cml/quaternion/quaternion_expr.h>
25 #include <cml/quaternion/quaternion_dot.h>
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:
31 struct quaternion_requires_fixed_size_array_type_error
;
35 /** A configurable quaternion type.
37 * @note Quaternions with two different orders cannot be used in the same
48 /* The ArrayType must be fixed<> or external<>: */
50 (same_type
< ArrayType
, fixed
<> >::is_true
51 || same_type
< ArrayType
, external
<> >::is_true
),
52 quaternion_requires_fixed_size_array_type_error
);
56 /* Shorthand for the array type generator: */
57 typedef ArrayType storage_type
;
58 typedef typename
ArrayType::template rebind
<4>::other generator_type
;
60 /* Vector representing the quaternion. Use the rebinding template to
61 * set the vector size:
63 typedef vector
<Element
, generator_type
> vector_type
;
65 /* Vector temporary type: */
66 typedef typename
vector_type::temporary_type vector_temporary
;
68 /* Quaternion order: */
69 typedef Order order_type
;
71 /* Quaternion multiplication order: */
72 typedef Cross cross_type
;
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. */
80 /* The quaternion type: */
81 typedef quaternion
<Element
,storage_type
,order_type
,cross_type
>
84 /* For integration into the expression template code: */
85 typedef quaternion_type expr_type
;
87 /* For integration into the expression template code: */
89 Element
, typename
vector_temporary::storage_type
,
90 order_type
, cross_type
> temporary_type
;
92 /* For integration into the expression templates code: */
93 typedef quaternion_type
& expr_reference
;
94 typedef const quaternion_type
& expr_const_reference
;
96 /* For matching by storage type: */
97 typedef typename
vector_type::memory_tag memory_tag
;
99 /* For matching by size type: */
100 typedef typename
vector_type::size_tag size_tag
;
102 /* Get the imaginary part type: */
103 typedef typename
vector_temporary::subvector_type imaginary_type
;
105 /* For matching by result-type: */
106 typedef cml::et::quaternion_result_tag result_tag
;
108 /* For matching by assignability: */
109 typedef cml::et::assignable_tag assignable_tag
;
114 /** Record result size as an enum. */
115 enum { array_size
= 4 };
117 /** Localize the ordering as an enum. */
128 /** Return the scalar part. */
129 value_type
real() const { return m_q
[W
]; }
131 /** Return the imaginary vector. */
132 imaginary_type
imaginary() const {
135 v[0] = m_q[X]; v[1] = m_q[Y]; v[2] = m_q[Z];
138 return imaginary_type(m_q
[X
], m_q
[Y
], m_q
[Z
]);
141 /** Return the vector representing the quaternion. */
142 const vector_type
& as_vector() const {
146 /** Return the Cayley norm of the quaternion. */
147 value_type
norm() const {
148 return length_squared();
151 /** Return square of the quaternion length. */
152 value_type
length_squared() const {
153 return cml::dot(*this,*this);
156 /** Return the quaternion length. */
157 value_type
length() const {
158 return std::sqrt(length_squared());
161 /** Normalize this quaternion (divide by its length).
163 * @todo Make this return a QuaternionXpr.
165 quaternion_type
& normalize() {
166 return (*this /= length());
169 /** Set this quaternion to the conjugate. */
170 quaternion_type
& conjugate() {
171 return (*this) = cml::conjugate(*this);
174 /** Set this quaternion to the inverse. */
175 quaternion_type
& inverse() {
176 return (*this) = cml::inverse(*this);
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);
188 /** Return the log of this quaternion. */
190 value_type tolerance
= epsilon
<value_type
>::placeholder()) const
192 value_type a
= acos_safe(real());
193 value_type s
= std::sin(a
);
196 return temporary_type(value_type(0), imaginary() * (a
/ s
));
198 return temporary_type(value_type(0), imaginary());
203 * Return the result of the exponential function as applied to
207 value_type tolerance
= epsilon
<value_type
>::placeholder()) const
209 imaginary_type v
= imaginary();
210 value_type a
= cml::length(v
);
213 return temporary_type(std::cos(a
), v
* (std::sin(a
) / a
));
215 return temporary_type(std::cos(a
), v
);
220 /** Const access to the quaternion as a vector. */
221 const_reference
operator[](size_t i
) const { return m_q
[i
]; }
223 /** Mutable access to the quaternion as a vector. */
224 reference
operator[](size_t i
) { return m_q
[i
]; }
226 /** Fill quaternion with random elements.
228 * @warning This does not generate uniformly random rotations.
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
);
238 /** Default initializer.
240 * @note The default constructor cannot be used with an external<>
245 /** Initializer for an external<> vector type. */
246 quaternion(Element
* const array
) : m_q(array
) {}
248 /** Copy construct from the same type of quaternion. */
249 quaternion(const quaternion_type
& q
) : m_q(q
.m_q
) {}
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()) {}
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
];
267 /** Initialize from a 4-vector.
269 * If Order is scalar_first, then v[0] is the real part. Otherwise,
270 * v[3] is the real part.
272 quaternion(const vector_type
& v
) : m_q(v
) {}
274 /** Initialize from an array of scalars.
276 * If Order is scalar_first, then v[0] is the real part. Otherwise,
277 * v[3] is the real part.
279 * @note The target vector must have CML_VEC_COPY_FROM_ARRAY
280 * implemented, so this cannot be used with external<> vectors.
282 quaternion(const value_type v
[4]) : m_q(v
) {}
284 /** Initialize from 4 scalars.
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
291 const value_type
& a
, const value_type
& b
,
292 const value_type
& c
, const value_type
& d
)
294 /* Call the overloaded assignment function: */
295 assign(a
, b
, c
, d
, Order());
298 /** Initialize both the real and imaginary parts.
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.
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];
308 /** Initialize both the real and imaginary parts.
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.
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];
318 /** Initialize both the real and imaginary parts.
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.
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];
328 /** Initialize both the real and imaginary parts.
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.
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];
340 /** Initialize from a VectorXpr. */
341 template<typename XprT
>
342 quaternion(VECXPR_ARG_TYPE e
) : m_q(e
) {}
344 /** Initialize both the real and imaginary parts.
346 * The imaginary part is initialized with a VectorXpr.
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];
353 // @todo: Are we missing:
355 // quaternion(VECXPR_ARG_TYPE e, const value_type& s) {}
357 // Or is that covered elsewhere?
359 /** In-place op from a quaternion.
361 * This assumes that _op_ is defined for both the quaternion's vector
362 * type and its scalar type.
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) { \
374 /** In-place op from a QuaternionXpr.
376 * This assumes that _op_ is defined for the quaternion's scalar type.
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]; \
389 /** In-place op from a scalar type.
391 * This assumes that _op_ is defined for the quaternion's scalar type.
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); \
403 CML_QUAT_ASSIGN_FROM_QUAT(=)
404 CML_QUAT_ASSIGN_FROM_QUAT(+=)
405 CML_QUAT_ASSIGN_FROM_QUAT(-=)
407 CML_QUAT_ASSIGN_FROM_QUATXPR(=)
408 CML_QUAT_ASSIGN_FROM_QUATXPR(+=)
409 CML_QUAT_ASSIGN_FROM_QUATXPR(-=)
411 CML_QUAT_ASSIGN_FROM_SCALAR(*=, cml::et::OpMulAssign
)
412 CML_QUAT_ASSIGN_FROM_SCALAR(/=, cml::et::OpDivAssign
)
414 #undef CML_QUAT_ASSIGN_FROM_QUAT
415 #undef CML_QUAT_ASSIGN_FROM_QUATXPR
416 #undef CML_QUAT_ASSIGN_FROM_SCALAR
418 /** Accumulated multiplication with a quaternion.
420 * Compute p = p * q for two quaternions p and q.
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
429 quaternion_type
& operator*=(const quaternion_type
& q
) {
430 return (*this = *this * q
);
433 /** Accumulated multiplication with a quaternion expression.
435 * Compute p = p * e for a quaternion p and a quaternion expression e.
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
444 template<typename XprT
> quaternion_type
& operator*=(QUATXPR_ARG_TYPE e
) {
445 return (*this = *this * e
);
448 /* NOTE: Quaternion division no longer supported, but I'm leaving the
449 code here for reference (Jesse) */
452 /** Accumulated division with a quaternion.
454 * Compute p = p * inverse(q).
456 * @note Because quaternion multiplication is non-commutative, division
457 * is ambiguous. This method assumes a multiplication order consistent
458 * with the notational order; i.e. p = q / r means p = q*inverse(r).
460 * @internal Using operator* and cml::inverse here is okay, as long as
461 * cml/quaternion.h is included before using this method (the only
462 * supported case for end-user code). This is because modern compilers
463 * won't instantiate a method in a template class until it is used, and
464 * including the main header ensures all definitions are available
465 * before any possible use of this method.
467 quaternion_type
& operator/=(const quaternion_type
& q
) {
468 return (*this = *this * cml::inverse(q
));
471 /** Accumulated division with a quaternion expression.
473 * Compute p = p * inverse(q).
475 * @note Because quaternion multiplication is non-commutative, division
476 * is ambiguous. This method assumes a multiplication order consistent
477 * with the notational order; i.e. p = q / r means p = q*inverse(r).
479 * @internal Using operator* and cml::inverse here is okay, as long as
480 * cml/quaternion.h is included before using this method (the only
481 * supported case for end-user code). This is because modern compilers
482 * won't instantiate a method in a template class until it is used, and
483 * including the main header ensures all definitions are available
484 * before any possible use of this method.
486 template<typename XprT
> quaternion_type
& operator/=(QUATXPR_ARG_TYPE e
) {
487 return (*this = *this * cml::inverse(e
));
494 /** Overloaded function to assign the quaternion from 4 scalars. */
495 void assign(const value_type
& a
, const value_type
& b
,
496 const value_type
& c
, const value_type
& d
, scalar_first
)
498 m_q
[W
] = a
; m_q
[X
] = b
; m_q
[Y
] = c
; m_q
[Z
] = d
;
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
, vector_first
)
505 m_q
[X
] = a
; m_q
[Y
] = b
; m_q
[Z
] = c
; m_q
[W
] = d
;
518 // -------------------------------------------------------------------------