WebSVN – gelsvn – Diff – /trunk/src/CGLA/ArithQuat.h


/* ----------------------------------------------------------------------- *
 * This file is part of GEL, http://www.imm.dtu.dk/GEL
 * Copyright (C) the authors and DTU Informatics
 * For license and list of authors, see ../../doc/intro.pdf
 * ----------------------------------------------------------------------- */
 
/** @file ArithQuat.h
 * Abstract quaternion class
 */
 
#ifndef __CGLA_ARITHQUAT_H__
#define __CGLA_ARITHQUAT_H__
 
#include "CGLA.h"
#include "ArithVecFloat.h"
#include "ArithSqMatFloat.h"
#include <cmath>
 
namespace CGLA {
 
  /** \brief A T based Quaterinion class. 

	Quaternions are algebraic entities useful for rotation. */
 
	template<class T, class V, class Q>
  class ArithQuat
  {
  public:
 
    /// Vector part of quaternion
    V qv;
 
    /// Scalar part of quaternion
    T qw;
 
    /// Construct undefined quaternion
#ifndef NDEBUG
    ArithQuat() : qw(CGLA_INIT_VALUE) {}
#else
    ArithQuat() {}
#endif
 
    /// Construct quaternion from vector and scalar
    ArithQuat(const V& imaginary, T real = 1.0f) : qv(imaginary) , qw(real) {}
 
    /// Construct quaternion from four scalars
    ArithQuat(T x, T y, T z, T _qw) : qv(x,y,z), qw(_qw) {}
 
   
    /// Assign values to a quaternion
    void set(const V& imaginary, T real=1.0f)
    {
      qv = imaginary;
      qw = real;
    }
 
    void set(T x, T y, T z, T _qw) 
    {
      qv.set(x,y,z);
      qw = _qw;
    }
 
    /*void set(const Vec4f& v)
    {
      qv.set(v[0], v[1], v[2]);
      qw = v[3];		  
    }*/
 
    /// Get values from a quaternion
    void get(T& x, T& y, T& z, T& _qw) const
    {
      x  = qv[0];
      y  = qv[1];
      z  = qv[2];
      _qw = qw;
    }
 
    /// Get imaginary part of a quaternion
    V get_imaginary_part() const { return qv; }
 
    /// Get real part of a quaternion
    T get_real_part() const { return qw; }
 
 
 
    /// Obtain angle of rotation and axis
    void get_rot(T& angle, V& v)
    {
      angle = 2*std::acos(qw);
 
      if(angle < TINY) 
				v = V(static_cast<T>(1.0), static_cast<T>(0.0), static_cast<T>(0.0));
      else 
				v = qv*(static_cast<T>(1.0)/std::sin(angle));
 
      if(angle > M_PI)
				v = -v;
      
      v.normalize();      
    }
 
    /// Construct a Quaternion from an angle and axis of rotation.
    void make_rot(T angle, const V& v)
    {
      angle /= static_cast<T>(2.0);
      qv = CGLA::normalize(v)*std::sin(angle);
      qw = std::cos(angle);
    }
 
    /** Construct a Quaternion rotating from the direction given
	by the first argument to the direction given by the second.*/
    void make_rot(const V& s,const V& t)
    {
      T tmp = std::sqrt(2*(1 + dot(s, t)));
      qv = cross(s, t)*(1.0/tmp);
      qw = tmp/2.0;    
    }
 
	/// Construct a Quaternion from a rotation matrix.
    template <class VT, class MT, unsigned int ROWS>
    void make_rot(ArithSqMatFloat<VT,MT,ROWS>& m)
    {
      assert(ROWS==3 || ROWS==4);
      T trace = m[0][0] + m[1][1] + m[2][2]  + static_cast<T>(1.0);
 
      //If the trace of the matrix is greater than zero, then
      //perform an "instant" calculation.
      if ( trace > TINY ) 
      {
        T S = sqrt(trace) * static_cast<T>(2.0);
        qv = V(m[2][1] - m[1][2], m[0][2] - m[2][0], m[1][0] - m[0][1] );
        qv /= S;
        qw = static_cast<T>(0.25) * S;
      }
      else
      {
        //If the trace of the matrix is equal to zero (or negative...) then identify
        //which major diagonal element has the greatest value.
        //Depending on this, calculate the following:
 
        if ( m[0][0] > m[1][1] && m[0][0] > m[2][2] )  {	// Column 0: 
          T S  = sqrt( static_cast<T>(1.0) + m[0][0] - m[1][1] - m[2][2] ) * static_cast<T>(2.0);
          qv[0] = 0.25f * S;
          qv[1] = (m[1][0] + m[0][1] ) / S;
          qv[2] = (m[0][2] + m[2][0] ) / S;
          qw = (m[2][1] - m[1][3] ) / S;
        } else if ( m[1][1] > m[2][2] ) {			// Column 1: 
          T S  = sqrt( 1.0 + m[1][1] - m[0][0] - m[2][2] ) * 2.0;
          qv[0] = (m[1][0] + m[0][1] ) / S;
          qv[1] = 0.25 * S;
          qv[2] = (m[2][1] + m[1][2] ) / S;
          qw = (m[0][2] - m[2][0] ) / S;
        } else {						// Column 2:
          T S  = sqrt( 1.0 + m[2][2] - m[0][0] - m[1][1] ) * 2.0;
          qv[0] = (m[0][2] + m[2][0] ) / S;
          qv[1] = (m[2][1] + m[1][2] ) / S;
          qv[2] = 0.25 * S;
          qw = (m[1][0] - m[0][1] ) / S;
        }
      }
      //The quaternion is then defined as:
      //  Q = | X Y Z W |
    }
 
    //----------------------------------------------------------------------
    // Binary operators
    //----------------------------------------------------------------------
    
    bool operator==(const ArithQuat<T,V,Q>& q) const
    {
      return qw == q.qw && qv == q.qv;
    }
 
    bool operator!=(const ArithQuat<T,V,Q>& q) const
    {
      return qw != q.qw || qv != q.qv;
    }
 
    /// Multiply two quaternions. (Combine their rotation)
    Q operator*(const ArithQuat<T,V,Q>& q) const
    {
      return Q(cross(qv, q.qv) + qv*q.qw + q.qv*qw, 
		         qw*q.qw - dot(qv, q.qv));      
    }
 
    /// Multiply scalar onto quaternion.
    Q operator*(T scalar) const
    {
      return Q(qv*scalar, qw*scalar);
    }
 
    /// Add two quaternions.
    Q operator+(const ArithQuat<T,V,Q>& q) const
    {
      return Q(qv + q.qv, qw + q.qw);
    }
 
    //----------------------------------------------------------------------
    // Unary operators
    //----------------------------------------------------------------------
 
    /// Compute the additive inverse of the quaternion
    Q operator-() const { return Q(-qv, -qw); }
 
    /// Compute norm of quaternion
    T norm() const { return dot(qv, qv) + qw*qw; }
 
    /// Return conjugate quaternion
    Q conjugate() const { return Q(-qv, qw); }
 
    /// Compute the multiplicative inverse of the quaternion
    Q inverse() const { return Q(conjugate()*(1/norm())); }
    
    /// Normalize quaternion.
    Q normalize() { return Q((*this)*(1/norm())); }
 
    //----------------------------------------------------------------------
    // Application
    //----------------------------------------------------------------------
 
    /// Rotate vector according to quaternion
    V apply(const V& vec) const 
    {
      return ((*this)*Q(vec)*inverse()).qv;
    }
 
    /// Rotate vector according to unit quaternion
    V apply_unit(const V& vec) const
    {
	  assert(fabs(norm() - 1.0) < SMALL);
      return ((*this)*Q(vec)*conjugate()).qv;
    }
  };
 
  template<class T, class V, class Q>
  inline Q operator*(T scalar, const ArithQuat<T,V,Q>& q)
  {
    return q*scalar;
  }
 
  /** Perform linear interpolation of two quaternions. 
      The last argument is the parameter used to interpolate
      between the two first. SLERP - invented by Shoemake -
      is a good way to interpolate because the interpolation
      is performed on the unit sphere. 	
  */
	template<class T, class V, class Q>
  inline Q slerp(const ArithQuat<T,V,Q>& q0, const ArithQuat<T,V,Q>& q1, T t)
  {
    T angle = std::acos(q0.qv[0]*q1.qv[0] + q0.qv[1]*q1.qv[1] 
			    + q0.qv[2]*q1.qv[2] + q0.qw*q1.qw);
    return (q0*std::sin((1 - t)*angle) + q1*std::sin(t*angle))*(1/std::sin(angle));
  }
 
  /// Print quaternion to stream.
  template<class T, class V, class Q>
  inline std::ostream& operator<<(std::ostream&os, const ArithQuat<T,V,Q>& v)
  {
    os << "[ ";
    for(unsigned int i=0;i<3;i++) os << v.qv[i] << " ";
    os << "~ " << v.qw << " ";
    os << "]";
 
    return os;
  }
}
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
#endif // __CGLA_ARITHQUAT_H__
 

Subversion Repositories gelsvn

(root)/trunk/src/CGLA/ArithQuat.h – Rev 427 → 595

Rev 427		Rev 595
-		1	`/* ----------------------------------------------------------------------- *`
-		2	`* This file is part of GEL, http://www.imm.dtu.dk/GEL`
-		3	`* Copyright (C) the authors and DTU Informatics`
-		4	`* For license and list of authors, see ../../doc/intro.pdf`
-		5	`* ----------------------------------------------------------------------- */`
-		6
-		7	`/** @file ArithQuat.h`
-		8	`* Abstract quaternion class`
-		9	`*/`
-		10
1	`#ifndef __CGLA_ARITHQUAT_H__`	11	`#ifndef __CGLA_ARITHQUAT_H__`
2	`#define __CGLA_ARITHQUAT_H__`	12	`#define __CGLA_ARITHQUAT_H__`
3		13
4	`#include "CGLA.h"`	14	`#include "CGLA.h"`
5	`#include "ArithVecFloat.h"`	15	`#include "ArithVecFloat.h"`
6	`#include "ArithSqMatFloat.h"`	16	`#include "ArithSqMatFloat.h"`
7	`#include <cmath>`	17	`#include <cmath>`
8		18
9	`namespace CGLA {`	19	`namespace CGLA {`
10		20
11	`/** \brief A T based Quaterinion class.`	21	`/** \brief A T based Quaterinion class.`
12		22
13	`Quaternions are algebraic entities useful for rotation. */`	23	`Quaternions are algebraic entities useful for rotation. */`
14		24
15	`template<class T, class V, class Q>`	25	`template<class T, class V, class Q>`
16	`class ArithQuat`	26	`class ArithQuat`
17	`{`	27	`{`
18	`public:`	28	`public:`
19		29
20	`/// Vector part of quaternion`	30	`/// Vector part of quaternion`
21	`V qv;`	31	`V qv;`
22		32
23	`/// Scalar part of quaternion`	33	`/// Scalar part of quaternion`
24	`T qw;`	34	`T qw;`
25		35
26	`/// Construct undefined quaternion`	36	`/// Construct undefined quaternion`
27	`#ifndef NDEBUG`	37	`#ifndef NDEBUG`
28	`ArithQuat() : qw(CGLA_INIT_VALUE) {}`	38	`ArithQuat() : qw(CGLA_INIT_VALUE) {}`
29	`#else`	39	`#else`
30	`ArithQuat() {}`	40	`ArithQuat() {}`
31	`#endif`	41	`#endif`
32		42
33	`/// Construct quaternion from vector and scalar`	43	`/// Construct quaternion from vector and scalar`
34	`ArithQuat(const V& imaginary, T real = 1.0f) : qv(imaginary) , qw(real) {}`	44	`ArithQuat(const V& imaginary, T real = 1.0f) : qv(imaginary) , qw(real) {}`
35		45
36	`/// Construct quaternion from four scalars`	46	`/// Construct quaternion from four scalars`
37	`ArithQuat(T x, T y, T z, T _qw) : qv(x,y,z), qw(_qw) {}`	47	`ArithQuat(T x, T y, T z, T _qw) : qv(x,y,z), qw(_qw) {}`
38		48
39		49
40	`/// Assign values to a quaternion`	50	`/// Assign values to a quaternion`
41	`void set(const V& imaginary, T real=1.0f)`	51	`void set(const V& imaginary, T real=1.0f)`
42	`{`	52	`{`
43	`qv = imaginary;`	53	`qv = imaginary;`
44	`qw = real;`	54	`qw = real;`
45	`}`	55	`}`
46		56
47	`void set(T x, T y, T z, T _qw)`	57	`void set(T x, T y, T z, T _qw)`
48	`{`	58	`{`
49	`qv.set(x,y,z);`	59	`qv.set(x,y,z);`
50	`qw = _qw;`	60	`qw = _qw;`
51	`}`	61	`}`
52		62
53	`/*void set(const Vec4f& v)`	63	`/*void set(const Vec4f& v)`
54	`{`	64	`{`
55	`qv.set(v[0], v[1], v[2]);`	65	`qv.set(v[0], v[1], v[2]);`
56	`qw = v[3];`	66	`qw = v[3];`
57	`}*/`	67	`}*/`
58		68
59	`/// Get values from a quaternion`	69	`/// Get values from a quaternion`
60	`void get(T& x, T& y, T& z, T& _qw) const`	70	`void get(T& x, T& y, T& z, T& _qw) const`
61	`{`	71	`{`
62	`x = qv[0];`	72	`x = qv[0];`
63	`y = qv[1];`	73	`y = qv[1];`
64	`z = qv[2];`	74	`z = qv[2];`
65	`_qw = qw;`	75	`_qw = qw;`
66	`}`	76	`}`
67		77
68	`/// Get imaginary part of a quaternion`	78	`/// Get imaginary part of a quaternion`
69	`V get_imaginary_part() const { return qv; }`	79	`V get_imaginary_part() const { return qv; }`
70		80
71	`/// Get real part of a quaternion`	81	`/// Get real part of a quaternion`
72	`T get_real_part() const { return qw; }`	82	`T get_real_part() const { return qw; }`
73		83
74		84
75		85
76	`/// Obtain angle of rotation and axis`	86	`/// Obtain angle of rotation and axis`
77	`void get_rot(T& angle, V& v)`	87	`void get_rot(T& angle, V& v)`
78	`{`	88	`{`
79	`angle = 2*std::acos(qw);`	89	`angle = 2*std::acos(qw);`
80		90
81	`if(angle < TINY)`	91	`if(angle < TINY)`
82	`v = V(static_cast<T>(1.0), static_cast<T>(0.0), static_cast<T>(0.0));`	92	`v = V(static_cast<T>(1.0), static_cast<T>(0.0), static_cast<T>(0.0));`
83	`else`	93	`else`
84	`v = qv*(static_cast<T>(1.0)/std::sin(angle));`	94	`v = qv*(static_cast<T>(1.0)/std::sin(angle));`
85		95
86	`if(angle > M_PI)`	96	`if(angle > M_PI)`
87	`v = -v;`	97	`v = -v;`
88		98
89	`v.normalize();`	99	`v.normalize();`
90	`}`	100	`}`
91		101
92	`/// Construct a Quaternion from an angle and axis of rotation.`	102	`/// Construct a Quaternion from an angle and axis of rotation.`
93	`void make_rot(T angle, const V& v)`	103	`void make_rot(T angle, const V& v)`
94	`{`	104	`{`
95	`angle /= static_cast<T>(2.0);`	105	`angle /= static_cast<T>(2.0);`
96	`qv = CGLA::normalize(v)*std::sin(angle);`	106	`qv = CGLA::normalize(v)*std::sin(angle);`
97	`qw = std::cos(angle);`	107	`qw = std::cos(angle);`
98	`}`	108	`}`
99		109
100	`/** Construct a Quaternion rotating from the direction given`	110	`/** Construct a Quaternion rotating from the direction given`
101	`by the first argument to the direction given by the second.*/`	111	`by the first argument to the direction given by the second.*/`
102	`void make_rot(const V& s,const V& t)`	112	`void make_rot(const V& s,const V& t)`
103	`{`	113	`{`
104	`T tmp = std::sqrt(2*(1 + dot(s, t)));`	114	`T tmp = std::sqrt(2*(1 + dot(s, t)));`
105	`qv = cross(s, t)*(1.0/tmp);`	115	`qv = cross(s, t)*(1.0/tmp);`
106	`qw = tmp/2.0;`	116	`qw = tmp/2.0;`
107	`}`	117	`}`
108		118
109	`/// Construct a Quaternion from a rotation matrix.`	119	`/// Construct a Quaternion from a rotation matrix.`
110	`template <class VT, class MT, unsigned int ROWS>`	120	`template <class VT, class MT, unsigned int ROWS>`
111	`void make_rot(ArithSqMatFloat<VT,MT,ROWS>& m)`	121	`void make_rot(ArithSqMatFloat<VT,MT,ROWS>& m)`
112	`{`	122	`{`
113	`assert(ROWS==3 \|\| ROWS==4);`	123	`assert(ROWS==3 \|\| ROWS==4);`
114	`T trace = m[0][0] + m[1][1] + m[2][2] + static_cast<T>(1.0);`	124	`T trace = m[0][0] + m[1][1] + m[2][2] + static_cast<T>(1.0);`
115		125
116	`//If the trace of the matrix is greater than zero, then`	126	`//If the trace of the matrix is greater than zero, then`
117	`//perform an "instant" calculation.`	127	`//perform an "instant" calculation.`
118	`if ( trace > TINY )`	128	`if ( trace > TINY )`
119	`{`	129	`{`
120	`T S = sqrt(trace) * static_cast<T>(2.0);`	130	`T S = sqrt(trace) * static_cast<T>(2.0);`
121	`qv = V(m[2][1] - m[1][2], m[0][2] - m[2][0], m[1][0] - m[0][1] );`	131	`qv = V(m[2][1] - m[1][2], m[0][2] - m[2][0], m[1][0] - m[0][1] );`
122	`qv /= S;`	132	`qv /= S;`
123	`qw = static_cast<T>(0.25) * S;`	133	`qw = static_cast<T>(0.25) * S;`
124	`}`	134	`}`
125	`else`	135	`else`
126	`{`	136	`{`
127	`//If the trace of the matrix is equal to zero (or negative...) then identify`	137	`//If the trace of the matrix is equal to zero (or negative...) then identify`
128	`//which major diagonal element has the greatest value.`	138	`//which major diagonal element has the greatest value.`
129	`//Depending on this, calculate the following:`	139	`//Depending on this, calculate the following:`
130		140
131	`if ( m[0][0] > m[1][1] && m[0][0] > m[2][2] ) { // Column 0:`	141	`if ( m[0][0] > m[1][1] && m[0][0] > m[2][2] ) { // Column 0:`
132	`T S = sqrt( static_cast<T>(1.0) + m[0][0] - m[1][1] - m[2][2] ) * static_cast<T>(2.0);`	142	`T S = sqrt( static_cast<T>(1.0) + m[0][0] - m[1][1] - m[2][2] ) * static_cast<T>(2.0);`
133	`qv[0] = 0.25f * S;`	143	`qv[0] = 0.25f * S;`
134	`qv[1] = (m[1][0] + m[0][1] ) / S;`	144	`qv[1] = (m[1][0] + m[0][1] ) / S;`
135	`qv[2] = (m[0][2] + m[2][0] ) / S;`	145	`qv[2] = (m[0][2] + m[2][0] ) / S;`
136	`qw = (m[2][1] - m[1][3] ) / S;`	146	`qw = (m[2][1] - m[1][3] ) / S;`
137	`} else if ( m[1][1] > m[2][2] ) { // Column 1:`	147	`} else if ( m[1][1] > m[2][2] ) { // Column 1:`
138	`T S = sqrt( 1.0 + m[1][1] - m[0][0] - m[2][2] ) * 2.0;`	148	`T S = sqrt( 1.0 + m[1][1] - m[0][0] - m[2][2] ) * 2.0;`
139	`qv[0] = (m[1][0] + m[0][1] ) / S;`	149	`qv[0] = (m[1][0] + m[0][1] ) / S;`
140	`qv[1] = 0.25 * S;`	150	`qv[1] = 0.25 * S;`
141	`qv[2] = (m[2][1] + m[1][2] ) / S;`	151	`qv[2] = (m[2][1] + m[1][2] ) / S;`
142	`qw = (m[0][2] - m[2][0] ) / S;`	152	`qw = (m[0][2] - m[2][0] ) / S;`
143	`} else { // Column 2:`	153	`} else { // Column 2:`
144	`T S = sqrt( 1.0 + m[2][2] - m[0][0] - m[1][1] ) * 2.0;`	154	`T S = sqrt( 1.0 + m[2][2] - m[0][0] - m[1][1] ) * 2.0;`
145	`qv[0] = (m[0][2] + m[2][0] ) / S;`	155	`qv[0] = (m[0][2] + m[2][0] ) / S;`
146	`qv[1] = (m[2][1] + m[1][2] ) / S;`	156	`qv[1] = (m[2][1] + m[1][2] ) / S;`
147	`qv[2] = 0.25 * S;`	157	`qv[2] = 0.25 * S;`
148	`qw = (m[1][0] - m[0][1] ) / S;`	158	`qw = (m[1][0] - m[0][1] ) / S;`
149	`}`	159	`}`
150	`}`	160	`}`
151	`//The quaternion is then defined as:`	161	`//The quaternion is then defined as:`
152	`// Q = \| X Y Z W \|`	162	`// Q = \| X Y Z W \|`
153	`}`	163	`}`
154		164
155	`//----------------------------------------------------------------------`	165	`//----------------------------------------------------------------------`
156	`// Binary operators`	166	`// Binary operators`
157	`//----------------------------------------------------------------------`	167	`//----------------------------------------------------------------------`
158		168
159	`bool operator==(const ArithQuat<T,V,Q>& q) const`	169	`bool operator==(const ArithQuat<T,V,Q>& q) const`
160	`{`	170	`{`
161	`return qw == q.qw && qv == q.qv;`	171	`return qw == q.qw && qv == q.qv;`
162	`}`	172	`}`
163		173
164	`bool operator!=(const ArithQuat<T,V,Q>& q) const`	174	`bool operator!=(const ArithQuat<T,V,Q>& q) const`
165	`{`	175	`{`
166	`return qw != q.qw \|\| qv != q.qv;`	176	`return qw != q.qw \|\| qv != q.qv;`
167	`}`	177	`}`
168		178
169	`/// Multiply two quaternions. (Combine their rotation)`	179	`/// Multiply two quaternions. (Combine their rotation)`
170	`Q operator*(const ArithQuat<T,V,Q>& q) const`	180	`Q operator*(const ArithQuat<T,V,Q>& q) const`
171	`{`	181	`{`
172	`return Q(cross(qv, q.qv) + qvq.qw + q.qvqw,`	182	`return Q(cross(qv, q.qv) + qvq.qw + q.qvqw,`
173	`qw*q.qw - dot(qv, q.qv));`	183	`qw*q.qw - dot(qv, q.qv));`
174	`}`	184	`}`
175		185
176	`/// Multiply scalar onto quaternion.`	186	`/// Multiply scalar onto quaternion.`
177	`Q operator*(T scalar) const`	187	`Q operator*(T scalar) const`
178	`{`	188	`{`
179	`return Q(qvscalar, qwscalar);`	189	`return Q(qvscalar, qwscalar);`
180	`}`	190	`}`
181		191
182	`/// Add two quaternions.`	192	`/// Add two quaternions.`
183	`Q operator+(const ArithQuat<T,V,Q>& q) const`	193	`Q operator+(const ArithQuat<T,V,Q>& q) const`
184	`{`	194	`{`
185	`return Q(qv + q.qv, qw + q.qw);`	195	`return Q(qv + q.qv, qw + q.qw);`
186	`}`	196	`}`
187		197
188	`//----------------------------------------------------------------------`	198	`//----------------------------------------------------------------------`
189	`// Unary operators`	199	`// Unary operators`
190	`//----------------------------------------------------------------------`	200	`//----------------------------------------------------------------------`
191		201
192	`/// Compute the additive inverse of the quaternion`	202	`/// Compute the additive inverse of the quaternion`
193	`Q operator-() const { return Q(-qv, -qw); }`	203	`Q operator-() const { return Q(-qv, -qw); }`
194		204
195	`/// Compute norm of quaternion`	205	`/// Compute norm of quaternion`
196	`T norm() const { return dot(qv, qv) + qw*qw; }`	206	`T norm() const { return dot(qv, qv) + qw*qw; }`
197		207
198	`/// Return conjugate quaternion`	208	`/// Return conjugate quaternion`
199	`Q conjugate() const { return Q(-qv, qw); }`	209	`Q conjugate() const { return Q(-qv, qw); }`
200		210
201	`/// Compute the multiplicative inverse of the quaternion`	211	`/// Compute the multiplicative inverse of the quaternion`
202	`Q inverse() const { return Q(conjugate()*(1/norm())); }`	212	`Q inverse() const { return Q(conjugate()*(1/norm())); }`
203		213
204	`/// Normalize quaternion.`	214	`/// Normalize quaternion.`
205	`Q normalize() { return Q((this)(1/norm())); }`	215	`Q normalize() { return Q((this)(1/norm())); }`
206		216
207	`//----------------------------------------------------------------------`	217	`//----------------------------------------------------------------------`
208	`// Application`	218	`// Application`
209	`//----------------------------------------------------------------------`	219	`//----------------------------------------------------------------------`
210		220
211	`/// Rotate vector according to quaternion`	221	`/// Rotate vector according to quaternion`
212	`V apply(const V& vec) const`	222	`V apply(const V& vec) const`
213	`{`	223	`{`
214	`return ((this)Q(vec)*inverse()).qv;`	224	`return ((this)Q(vec)*inverse()).qv;`
215	`}`	225	`}`
216		226
217	`/// Rotate vector according to unit quaternion`	227	`/// Rotate vector according to unit quaternion`
218	`V apply_unit(const V& vec) const`	228	`V apply_unit(const V& vec) const`
219	`{`	229	`{`
220	`assert(fabs(norm() - 1.0) < SMALL);`	230	`assert(fabs(norm() - 1.0) < SMALL);`
221	`return ((this)Q(vec)*conjugate()).qv;`	231	`return ((this)Q(vec)*conjugate()).qv;`
222	`}`	232	`}`
223	`};`	233	`};`
224		234
225	`template<class T, class V, class Q>`	235	`template<class T, class V, class Q>`
226	`inline Q operator*(T scalar, const ArithQuat<T,V,Q>& q)`	236	`inline Q operator*(T scalar, const ArithQuat<T,V,Q>& q)`
227	`{`	237	`{`
228	`return q*scalar;`	238	`return q*scalar;`
229	`}`	239	`}`
230		240
231	`/** Perform linear interpolation of two quaternions.`	241	`/** Perform linear interpolation of two quaternions.`
232	`The last argument is the parameter used to interpolate`	242	`The last argument is the parameter used to interpolate`
233	`between the two first. SLERP - invented by Shoemake -`	243	`between the two first. SLERP - invented by Shoemake -`
234	`is a good way to interpolate because the interpolation`	244	`is a good way to interpolate because the interpolation`
235	`is performed on the unit sphere.`	245	`is performed on the unit sphere.`
236	`*/`	246	`*/`
237	`template<class T, class V, class Q>`	247	`template<class T, class V, class Q>`
238	`inline Q slerp(const ArithQuat<T,V,Q>& q0, const ArithQuat<T,V,Q>& q1, T t)`	248	`inline Q slerp(const ArithQuat<T,V,Q>& q0, const ArithQuat<T,V,Q>& q1, T t)`
239	`{`	249	`{`
240	`T angle = std::acos(q0.qv[0]q1.qv[0] + q0.qv[1]q1.qv[1]`	250	`T angle = std::acos(q0.qv[0]q1.qv[0] + q0.qv[1]q1.qv[1]`
241	`+ q0.qv[2]q1.qv[2] + q0.qwq1.qw);`	251	`+ q0.qv[2]q1.qv[2] + q0.qwq1.qw);`
242	`return (q0std::sin((1 - t)angle) + q1std::sin(tangle))*(1/std::sin(angle));`	252	`return (q0std::sin((1 - t)angle) + q1std::sin(tangle))*(1/std::sin(angle));`
243	`}`	253	`}`
244		254
245	`/// Print quaternion to stream.`	255	`/// Print quaternion to stream.`
246	`template<class T, class V, class Q>`	256	`template<class T, class V, class Q>`
247	`inline std::ostream& operator<<(std::ostream&os, const ArithQuat<T,V,Q>& v)`	257	`inline std::ostream& operator<<(std::ostream&os, const ArithQuat<T,V,Q>& v)`
248	`{`	258	`{`
249	`os << "[ ";`	259	`os << "[ ";`
250	`for(unsigned int i=0;i<3;i++) os << v.qv[i] << " ";`	260	`for(unsigned int i=0;i<3;i++) os << v.qv[i] << " ";`
251	`os << "~ " << v.qw << " ";`	261	`os << "~ " << v.qw << " ";`
252	`os << "]";`	262	`os << "]";`
253		263
254	`return os;`	264	`return os;`
255	`}`	265	`}`
256	`}`	266	`}`
257		267
258		268
259		269
260		270
261		271
262		272
263		273
264		274
265		275
266		276
267		277
268		278
269		279
270		280
271		281
272		282
273		283
274	`#endif // __CGLA_ARITHQUAT_H__`	284	`#endif // __CGLA_ARITHQUAT_H__`
275		285