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 {
 
  /** \brief A double precision 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 51	Rev 89
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	`/** \brief A double precision Quaterinion class.`
-		13
13	`useful for rotation. */`	14	`Quaternions are algebraic entities useful for rotation. */`
14		15
15	`class Quatd`	16	`class Quatd`
16	`{`	17	`{`
17	`public:`	18	`public:`
18		19
19	`/// Vector part of quaternion`	20	`/// Vector part of quaternion`
20	`Vec3d qv;`	21	`Vec3d qv;`
21		22
22	`/// Scalar part of quaternion`	23	`/// Scalar part of quaternion`
23	`double qw;`	24	`double qw;`
24		25
25	`/// Construct undefined quaternion`	26	`/// Construct undefined quaternion`
26	`#ifndef NDEBUG`	27	`#ifndef NDEBUG`
27	`Quatd() : qw(CGLA_INIT_VALUE) {}`	28	`Quatd() : qw(CGLA_INIT_VALUE) {}`
28	`#else`	29	`#else`
29	`Quatd() {}`	30	`Quatd() {}`
30	`#endif`	31	`#endif`
31		32
32	`/// Construct quaternion from vector and scalar`	33	`/// Construct quaternion from vector and scalar`
33	`Quatd(const Vec3d& imaginary, double real = 1.0f) : qv(imaginary) , qw(real) {}`	34	`Quatd(const Vec3d& imaginary, double real = 1.0f) : qv(imaginary) , qw(real) {}`
34		35
35	`/// Construct quaternion from four scalars`	36	`/// Construct quaternion from four scalars`
36	`Quatd(double x, double y, double z, double _qw) : qv(x,y,z), qw(_qw) {}`	37	`Quatd(double x, double y, double z, double _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 Quatd(const Vec4d& v) : qv(v[0], v[1], v[2]), qw(v[3]) {}`	40	`explicit Quatd(const Vec4d& 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 Vec3d& imaginary, double real=1.0f)`	43	`void set(const Vec3d& imaginary, double real=1.0f)`
43	`{`	44	`{`
44	`qv = imaginary;`	45	`qv = imaginary;`
45	`qw = real;`	46	`qw = real;`
46	`}`	47	`}`
47		48
48	`void set(double x, double y, double z, double _qw)`	49	`void set(double x, double y, double z, double _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(double& x, double& y, double& z, double& _qw) const`	62	`void get(double& x, double& y, double& z, double& _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	`Vec3d get_imaginary_part() const { return qv; }`	71	`Vec3d get_imaginary_part() const { return qv; }`
71		72
72	`/// Get real part of a quaternion`	73	`/// Get real part of a quaternion`
73	`double get_real_part() const { return qw; }`	74	`double 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	`Mat3x3d get_mat3x3d() const`	77	`Mat3x3d get_mat3x3d() const`
77	`{`	78	`{`
78	`double s = 2/norm();`	79	`double 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 Mat3x3d(Vec3d(1.0 - s(qv[1]qv[1] + qv[2]*qv[2]),`	81	`return Mat3x3d(Vec3d(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	`Vec3d( s(qv[0]qv[1] + qw*qv[2]),`	84	`Vec3d( 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	`Vec3d( s(qv[0]qv[2] - qw*qv[1]),`	87	`Vec3d( 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	`Mat4x4d get_mat4x4d() const`	93	`Mat4x4d get_mat4x4d() const`
93	`{`	94	`{`
94	`double s = 2/norm();`	95	`double 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 Mat4x4d(Vec4d(1.0 - s(qv[1]qv[1] + qv[2]*qv[2]),`	97	`return Mat4x4d(Vec4d(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	`Vec4d( s(qv[0]qv[1] + qw*qv[2]),`	101	`Vec4d( 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	`Vec4d( s(qv[0]qv[2] - qw*qv[1]),`	105	`Vec4d( 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	`Vec4d(0.0, 0.0, 0.0, 1.0));`	109	`Vec4d(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(double& angle, Vec3d& v)`	113	`void get_rot(double& angle, Vec3d& 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 = Vec3d(1.0, 0.0, 0.0);`	118	`v = Vec3d(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(double angle, const Vec3d& v)`	129	`void make_rot(double angle, const Vec3d& 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 Vec3d& s,const Vec3d& t)`	138	`void make_rot(const Vec3d& s,const Vec3d& t)`
138	`{`	139	`{`
139	`double tmp = std::sqrt(2*(1 + dot(s, t)));`	140	`double 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 Quatd& q) const`	149	`bool operator==(const Quatd& 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 Quatd& q) const`	154	`bool operator!=(const Quatd& 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	`Quatd operator*(const Quatd& q) const`	160	`Quatd operator*(const Quatd& q) const`
160	`{`	161	`{`
161	`return Quatd(cross(qv, q.qv) + qvq.qw + q.qvqw,`	162	`return Quatd(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	`Quatd operator*(double scalar) const`	167	`Quatd operator*(double scalar) const`
167	`{`	168	`{`
168	`return Quatd(qvscalar, qwscalar);`	169	`return Quatd(qvscalar, qwscalar);`
169	`}`	170	`}`
170		171
171	`/// Add two quaternions.`	172	`/// Add two quaternions.`
172	`Quatd operator+(const Quatd& q) const`	173	`Quatd operator+(const Quatd& q) const`
173	`{`	174	`{`
174	`return Quatd(qv + q.qv, qw + q.qw);`	175	`return Quatd(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	`Quatd operator-() const { return Quatd(-qv, -qw); }`	183	`Quatd operator-() const { return Quatd(-qv, -qw); }`
183		184
184	`/// Compute norm of quaternion`	185	`/// Compute norm of quaternion`
185	`double norm() const { return dot(qv, qv) + qw*qw; }`	186	`double norm() const { return dot(qv, qv) + qw*qw; }`
186		187
187	`/// Return conjugate quaternion`	188	`/// Return conjugate quaternion`
188	`Quatd conjugate() const { return Quatd(-qv, qw); }`	189	`Quatd conjugate() const { return Quatd(-qv, qw); }`
189		190
190	`/// Compute the multiplicative inverse of the quaternion`	191	`/// Compute the multiplicative inverse of the quaternion`
191	`Quatd inverse() const { return Quatd(conjugate()*(1/norm())); }`	192	`Quatd inverse() const { return Quatd(conjugate()*(1/norm())); }`
192		193
193	`/// Normalize quaternion.`	194	`/// Normalize quaternion.`
194	`Quatd normalize() { return Quatd((this)(1/norm())); }`	195	`Quatd normalize() { return Quatd((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	`Vec3d apply(const Vec3d& vec) const`	202	`Vec3d apply(const Vec3d& vec) const`
202	`{`	203	`{`
203	`return ((this)Quatd(vec)*inverse()).qv;`	204	`return ((this)Quatd(vec)*inverse()).qv;`
204	`}`	205	`}`
205		206
206	`/// Rotate vector according to unit quaternion`	207	`/// Rotate vector according to unit quaternion`
207	`Vec3d apply_unit(const Vec3d& vec) const`	208	`Vec3d apply_unit(const Vec3d& vec) const`
208	`{`	209	`{`
209	`return ((this)Quatd(vec)*conjugate()).qv;`	210	`return ((this)Quatd(vec)*conjugate()).qv;`
210	`}`	211	`}`
211	`};`	212	`};`
212		213
213	`inline Quatd operator*(double scalar, const Quatd& q)`	214	`inline Quatd operator*(double scalar, const Quatd& 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 Quatd slerp(Quatd q0, Quatd q1, double t)`	225	`inline Quatd slerp(Quatd q0, Quatd q1, double t)`
225	`{`	226	`{`
226	`double angle = std::acos(q0.qv[0]q1.qv[0] + q0.qv[1]q1.qv[1]`	227	`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);`	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 Quatd identity_Quatd()`	233	`inline Quatd identity_Quatd()`
233	`{`	234	`{`
234	`return Quatd(Vec3d(0.0));`	235	`return Quatd(Vec3d(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 Quatd v)`	239	`inline std::ostream& operator<<(std::ostream&os, const Quatd 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/Quatd.h – Rev 51 → 89