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


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

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

Subversion Repositories gelsvn

(root)/trunk/src/CGLA/Quatd.h – Rev 49 → 51