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

Subversion Repositories gelsvn

(root)/branches/vc2005beta-branch/src/CGLA/Quatd.h – Rev 89 → 119