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


#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 382 → 427

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