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


#ifndef __CGLA_QUATF_H__
#define __CGLA_QUATF_H__
 
#include "Vec3f.h"
#include "Vec4f.h"
#include "Mat3x3f.h"
#include "Mat4x4f.h"
#include <cmath>
 
namespace CGLA {
 
  /** \brief A float based Quaterinion class. 
 
	Quaternions are algebraic entities useful for rotation. */
 
  class Quatf
  {
  public:
 
    /// Vector part of quaternion
    Vec3f qv;
 
    /// Scalar part of quaternion
    float qw;
 
    /// Construct undefined quaternion
#ifndef NDEBUG
    Quatf() : qw(CGLA_INIT_VALUE) {}
#else
    Quatf() {}
#endif
 
    /// Construct quaternion from vector and scalar
    Quatf(const Vec3f& imaginary, float real = 1.0f) : qv(imaginary) , qw(real) {}
 
    /// Construct quaternion from four scalars
    Quatf(float x, float y, float z, float _qw) : qv(x,y,z), qw(_qw) {}
 
    /// Construct quaternion from a 4D vector
    explicit Quatf(const Vec4f& v) : qv(v[0], v[1], v[2]), qw(v[3]) {}
 
    /// Assign values to a quaternion
    void set(const Vec3f& imaginary, float real=1.0f)
    {
      qv = imaginary;
      qw = real;
    }
 
    void set(float x, float y, float z, float _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(float& x, float& y, float& z, float& _qw) const
    {
      x  = qv[0];
      y  = qv[1];
      z  = qv[2];
      _qw = qw;
    }
 
    /// Get imaginary part of a quaternion
    Vec3f get_imaginary_part() const { return qv; }
 
    /// Get real part of a quaternion
    float get_real_part() const { return qw; }
 
    /// Get a 3x3 rotation matrix from a quaternion
    Mat3x3f get_mat3x3f() const
    {
      float s = 2/norm();
      // note that the all q_*q_ are used twice (optimize)
      return Mat3x3f(Vec3f(1.0 - s*(qv[1]*qv[1] + qv[2]*qv[2]),
			         s*(qv[0]*qv[1] - qw*qv[2]),
			         s*(qv[0]*qv[2] + qw*qv[1])),
		     Vec3f(      s*(qv[0]*qv[1] + qw*qv[2]),
  			   1.0 - s*(qv[0]*qv[0] + qv[2]*qv[2]),
			         s*(qv[1]*qv[2] - qw*qv[0])),
		     Vec3f(      s*(qv[0]*qv[2] - qw*qv[1]),
			         s*(qv[1]*qv[2] + qw*qv[0]),
			   1.0 - s*(qv[0]*qv[0] + qv[1]*qv[1])));
    }
 
    /// Get a 4x4 rotation matrix from a quaternion
    Mat4x4f get_mat4x4f() const
    {
      float s = 2/norm();
      // note that the all q_*q_ are used twice (optimize?)
      return Mat4x4f(Vec4f(1.0 - s*(qv[1]*qv[1] + qv[2]*qv[2]),
			         s*(qv[0]*qv[1] - qw*qv[2]),
			         s*(qv[0]*qv[2] + qw*qv[1]),
		           0.0),
		     Vec4f(      s*(qv[0]*qv[1] + qw*qv[2]),
			   1.0 - s*(qv[0]*qv[0] + qv[2]*qv[2]),
			         s*(qv[1]*qv[2] - qw*qv[0]),
			   0.0),
		     Vec4f(      s*(qv[0]*qv[2] - qw*qv[1]),
			         s*(qv[1]*qv[2] + qw*qv[0]),
			   1.0 - s*(qv[0]*qv[0] + qv[1]*qv[1]),
			   0.0),
		     Vec4f(0.0, 0.0, 0.0, 1.0));
    }
 
    /// Obtain angle of rotation and axis
    void get_rot(float& angle, Vec3f& v)
    {
      angle = 2*std::acos(qw);
 
      if(angle < TINY) 
	v = Vec3f(1.0, 0.0, 0.0);
      else 
	v = qv*(1/std::sin(angle));
 
      if(angle > M_PI)
	v = -v;
      
      v.normalize();      
    }
 
    /// Construct a Quaternion from an angle and axis of rotation.
    void make_rot(float angle, const Vec3f& v)
    {
      angle /= 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 Vec3f& s,const Vec3f& t)
    {
      float tmp = std::sqrt(2*(1 + dot(s, t)));
      qv = cross(s, t)*(1.0/tmp);
      qw = tmp/2.0;    
    }
 
    //----------------------------------------------------------------------
    // Binary operators
    //----------------------------------------------------------------------
    
    bool operator==(const Quatf& q) const
    {
      return qw == q.qw && qv == q.qv;
    }
 
    bool operator!=(const Quatf& q) const
    {
      return qw != q.qw || qv != q.qv;
    }
 
    /// Multiply two quaternions. (Combine their rotation)
    Quatf operator*(const Quatf& q) const
    {
      return Quatf(cross(qv, q.qv) + qv*q.qw + q.qv*qw, 
		         qw*q.qw - dot(qv, q.qv));      
    }
 
    /// Multiply scalar onto quaternion.
    Quatf operator*(float scalar) const
    {
      return Quatf(qv*scalar, qw*scalar);
    }
 
    /// Add two quaternions.
    Quatf operator+(const Quatf& q) const
    {
      return Quatf(qv + q.qv, qw + q.qw);
    }
 
    //----------------------------------------------------------------------
    // Unary operators
    //----------------------------------------------------------------------
 
    /// Compute the additive inverse of the quaternion
    Quatf operator-() const { return Quatf(-qv, -qw); }
 
    /// Compute norm of quaternion
    float norm() const { return dot(qv, qv) + qw*qw; }
 
    /// Return conjugate quaternion
    Quatf conjugate() const { return Quatf(-qv, qw); }
 
    /// Compute the multiplicative inverse of the quaternion
    Quatf inverse() const { return Quatf(conjugate()*(1/norm())); }
    
    /// Normalize quaternion.
    Quatf normalize() { return Quatf((*this)*(1/norm())); }
 
    //----------------------------------------------------------------------
    // Application
    //----------------------------------------------------------------------
 
    /// Rotate vector according to quaternion
    Vec3f apply(const Vec3f& vec) const 
    {
      return ((*this)*Quatf(vec)*inverse()).qv;
    }
 
    /// Rotate vector according to unit quaternion
    Vec3f apply_unit(const Vec3f& vec) const
    {
      return ((*this)*Quatf(vec)*conjugate()).qv;
    }
  };
 
  inline Quatf operator*(float scalar, const Quatf& 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. 	
  */
  inline Quatf slerp(Quatf q0, Quatf q1, float t)
  {
    float 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));
  }
 
  /// Create an identity quaternion
  inline Quatf identity_Quatf()
  {
    return Quatf(Vec3f(0.0));
  }
 
  /// Print quaternion to stream.
  inline std::ostream& operator<<(std::ostream&os, const Quatf v)
  {
    os << "[ ";
    for(unsigned int i=0;i<3;i++) os << v.qv[i] << " ";
    os << "~ " << v.qw << " ";
    os << "]";
 
    return os;
  }
}
#endif
 

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

Subversion Repositories gelsvn

(root)/trunk/src/CGLA/Quatf.h – Rev 49 → 89