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


#ifndef __CGLA_ARITHQUAT_H__
#define __CGLA_ARITHQUAT_H__
 
#include "CGLA.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;    
    }
 
    //----------------------------------------------------------------------
    // 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 303 → 306

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