1/* -*-c++-*- OpenSceneGraph - Copyright (C) 1998-2006 Robert Osfield
3 * This library is open source and may be redistributed and/or modified under
4 * the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or
5 * (at your option) any later version. The full license is in LICENSE file
6 * included with this distribution, and on the openscenegraph.org website.
8 * This library is distributed in the hope that it will be useful,
9 * but WITHOUT ANY WARRANTY; without even the implied warranty of
10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11 * OpenSceneGraph Public License for more details.
28/** A quaternion class. It can be used to represent an orientation in 3D space.*/
34 /** Data type of vector components.*/
35 #ifdef OSG_USE_FLOAT_QUAT
36 typedef float value_type;
38 typedef double value_type;
41 /** Number of vector components. */
42 enum { num_components = 4 };
44 value_type _v[4]; // a four-vector
46 inline Quat() { _v[0]=0.0; _v[1]=0.0; _v[2]=0.0; _v[3]=1.0; }
48 inline Quat( value_type x, value_type y, value_type z, value_type w )
56 inline Quat( const Vec4f& v )
64 inline Quat( const Vec4d& v )
72 inline Quat( value_type angle, const Vec3f& axis)
74 makeRotate(angle,axis);
76 inline Quat( value_type angle, const Vec3d& axis)
78 makeRotate(angle,axis);
81 inline Quat( value_type angle1, const Vec3f& axis1,
82 value_type angle2, const Vec3f& axis2,
83 value_type angle3, const Vec3f& axis3)
85 makeRotate(angle1,axis1,angle2,axis2,angle3,axis3);
88 inline Quat( value_type angle1, const Vec3d& axis1,
89 value_type angle2, const Vec3d& axis2,
90 value_type angle3, const Vec3d& axis3)
92 makeRotate(angle1,axis1,angle2,axis2,angle3,axis3);
95 Quat(const Quat& q) { _v[0]=q._v[0]; _v[1]=q._v[1]; _v[2]=q._v[2]; _v[3]=q._v[3]; }
97 inline Quat& operator = (const Quat& v) { _v[0]=v._v[0]; _v[1]=v._v[1]; _v[2]=v._v[2]; _v[3]=v._v[3]; return *this; }
99 inline bool operator == (const Quat& v) const { return _v[0]==v._v[0] && _v[1]==v._v[1] && _v[2]==v._v[2] && _v[3]==v._v[3]; }
101 inline bool operator != (const Quat& v) const { return _v[0]!=v._v[0] || _v[1]!=v._v[1] || _v[2]!=v._v[2] || _v[3]!=v._v[3]; }
103 inline bool operator < (const Quat& v) const
105 if (_v[0]<v._v[0]) return true;
106 else if (_v[0]>v._v[0]) return false;
107 else if (_v[1]<v._v[1]) return true;
108 else if (_v[1]>v._v[1]) return false;
109 else if (_v[2]<v._v[2]) return true;
110 else if (_v[2]>v._v[2]) return false;
111 else return (_v[3]<v._v[3]);
114 /* ----------------------------------
115 Methods to access data members
116 ---------------------------------- */
118 inline Vec4d asVec4() const
120 return Vec4d(_v[0], _v[1], _v[2], _v[3]);
123 inline Vec3d asVec3() const
125 return Vec3d(_v[0], _v[1], _v[2]);
128 inline void set(value_type x, value_type y, value_type z, value_type w)
136 inline void set(const osg::Vec4f& v)
144 inline void set(const osg::Vec4d& v)
152 void set(const Matrixf& matrix);
154 void set(const Matrixd& matrix);
156 void get(Matrixf& matrix) const;
158 void get(Matrixd& matrix) const;
161 inline value_type & operator [] (int i) { return _v[i]; }
162 inline value_type operator [] (int i) const { return _v[i]; }
164 inline value_type & x() { return _v[0]; }
165 inline value_type & y() { return _v[1]; }
166 inline value_type & z() { return _v[2]; }
167 inline value_type & w() { return _v[3]; }
169 inline value_type x() const { return _v[0]; }
170 inline value_type y() const { return _v[1]; }
171 inline value_type z() const { return _v[2]; }
172 inline value_type w() const { return _v[3]; }
174 /** return true if the Quat represents a zero rotation, and therefore can be ignored in computations.*/
175 bool zeroRotation() const { return _v[0]==0.0 && _v[1]==0.0 && _v[2]==0.0 && _v[3]==1.0; }
178 /* -------------------------------------------------------------
179 BASIC ARITHMETIC METHODS
180 Implemented in terms of Vec4s. Some Vec4 operators, e.g.
181 operator* are not appropriate for quaternions (as
182 mathematical objects) so they are implemented differently.
183 Also define methods for conjugate and the multiplicative inverse.
184 ------------------------------------------------------------- */
185 /// Multiply by scalar
186 inline const Quat operator * (value_type rhs) const
188 return Quat(_v[0]*rhs, _v[1]*rhs, _v[2]*rhs, _v[3]*rhs);
191 /// Unary multiply by scalar
192 inline Quat& operator *= (value_type rhs)
198 return *this; // enable nesting
202 inline const Quat operator*(const Quat& rhs) const
204 return Quat( rhs._v[3]*_v[0] + rhs._v[0]*_v[3] + rhs._v[1]*_v[2] - rhs._v[2]*_v[1],
205 rhs._v[3]*_v[1] - rhs._v[0]*_v[2] + rhs._v[1]*_v[3] + rhs._v[2]*_v[0],
206 rhs._v[3]*_v[2] + rhs._v[0]*_v[1] - rhs._v[1]*_v[0] + rhs._v[2]*_v[3],
207 rhs._v[3]*_v[3] - rhs._v[0]*_v[0] - rhs._v[1]*_v[1] - rhs._v[2]*_v[2] );
211 inline Quat& operator*=(const Quat& rhs)
213 value_type x = rhs._v[3]*_v[0] + rhs._v[0]*_v[3] + rhs._v[1]*_v[2] - rhs._v[2]*_v[1];
214 value_type y = rhs._v[3]*_v[1] - rhs._v[0]*_v[2] + rhs._v[1]*_v[3] + rhs._v[2]*_v[0];
215 value_type z = rhs._v[3]*_v[2] + rhs._v[0]*_v[1] - rhs._v[1]*_v[0] + rhs._v[2]*_v[3];
216 _v[3] = rhs._v[3]*_v[3] - rhs._v[0]*_v[0] - rhs._v[1]*_v[1] - rhs._v[2]*_v[2];
222 return (*this); // enable nesting
226 inline Quat operator / (value_type rhs) const
228 value_type div = 1.0/rhs;
229 return Quat(_v[0]*div, _v[1]*div, _v[2]*div, _v[3]*div);
232 /// Unary divide by scalar
233 inline Quat& operator /= (value_type rhs)
235 value_type div = 1.0/rhs;
244 inline const Quat operator/(const Quat& denom) const
246 return ( (*this) * denom.inverse() );
250 inline Quat& operator/=(const Quat& denom)
252 (*this) = (*this) * denom.inverse();
253 return (*this); // enable nesting
257 inline const Quat operator + (const Quat& rhs) const
259 return Quat(_v[0]+rhs._v[0], _v[1]+rhs._v[1],
260 _v[2]+rhs._v[2], _v[3]+rhs._v[3]);
264 inline Quat& operator += (const Quat& rhs)
270 return *this; // enable nesting
273 /// Binary subtraction
274 inline const Quat operator - (const Quat& rhs) const
276 return Quat(_v[0]-rhs._v[0], _v[1]-rhs._v[1],
277 _v[2]-rhs._v[2], _v[3]-rhs._v[3] );
280 /// Unary subtraction
281 inline Quat& operator -= (const Quat& rhs)
287 return *this; // enable nesting
290 /** Negation operator - returns the negative of the quaternion.
291 Basically just calls operator - () on the Vec4 */
292 inline const Quat operator - () const
294 return Quat (-_v[0], -_v[1], -_v[2], -_v[3]);
297 /// Length of the quaternion = sqrt( vec . vec )
298 value_type length() const
300 return sqrt( _v[0]*_v[0] + _v[1]*_v[1] + _v[2]*_v[2] + _v[3]*_v[3]);
303 /// Length of the quaternion = vec . vec
304 value_type length2() const
306 return _v[0]*_v[0] + _v[1]*_v[1] + _v[2]*_v[2] + _v[3]*_v[3];
310 inline Quat conj () const
312 return Quat( -_v[0], -_v[1], -_v[2], _v[3] );
315 /// Multiplicative inverse method: q^(-1) = q^*/(q.q^*)
316 inline const Quat inverse () const
318 return conj() / length2();
321 /* --------------------------------------------------------
322 METHODS RELATED TO ROTATIONS
323 Set a quaternion which will perform a rotation of an
324 angle around the axis given by the vector (x,y,z).
325 Should be written to also accept an angle and a Vec3?
327 Define Spherical Linear interpolation method also
329 Not inlined - see the Quat.cpp file for implementation
330 -------------------------------------------------------- */
331 void makeRotate( value_type angle,
332 value_type x, value_type y, value_type z );
333 void makeRotate ( value_type angle, const Vec3f& vec );
334 void makeRotate ( value_type angle, const Vec3d& vec );
336 void makeRotate ( value_type angle1, const Vec3f& axis1,
337 value_type angle2, const Vec3f& axis2,
338 value_type angle3, const Vec3f& axis3);
339 void makeRotate ( value_type angle1, const Vec3d& axis1,
340 value_type angle2, const Vec3d& axis2,
341 value_type angle3, const Vec3d& axis3);
343 /** Make a rotation Quat which will rotate vec1 to vec2.
344 Generally take a dot product to get the angle between these
345 and then use a cross product to get the rotation axis
346 Watch out for the two special cases when the vectors
347 are co-incident or opposite in direction.*/
348 void makeRotate( const Vec3f& vec1, const Vec3f& vec2 );
349 /** Make a rotation Quat which will rotate vec1 to vec2.
350 Generally take a dot product to get the angle between these
351 and then use a cross product to get the rotation axis
352 Watch out for the two special cases of when the vectors
353 are co-incident or opposite in direction.*/
354 void makeRotate( const Vec3d& vec1, const Vec3d& vec2 );
356 void makeRotate_original( const Vec3d& vec1, const Vec3d& vec2 );
358 /** Return the angle and vector components represented by the quaternion.*/
359 void getRotate ( value_type & angle, value_type & x, value_type & y, value_type & z ) const;
361 /** Return the angle and vector represented by the quaternion.*/
362 void getRotate ( value_type & angle, Vec3f& vec ) const;
364 /** Return the angle and vector represented by the quaternion.*/
365 void getRotate ( value_type & angle, Vec3d& vec ) const;
367 /** Spherical Linear Interpolation.
368 As t goes from 0 to 1, the Quat object goes from "from" to "to". */
369 void slerp ( value_type t, const Quat& from, const Quat& to);
371 /** Rotate a vector by this quaternion.*/
372 Vec3f operator* (const Vec3f& v) const
374 // nVidia SDK implementation
376 Vec3f qvec(_v[0], _v[1], _v[2]);
379 uv *= ( 2.0f * _v[3] );
384 /** Rotate a vector by this quaternion.*/
385 Vec3d operator* (const Vec3d& v) const
387 // nVidia SDK implementation
389 Vec3d qvec(_v[0], _v[1], _v[2]);
392 uv *= ( 2.0f * _v[3] );
399}; // end of class prototype