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(abs(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 323 → 382

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