hi all,
quaternion multiplication is a non-commutative operator, i've changed
the source code and documentation accordingly.
best regards,
ir. m.j.baars
/* -*-c++-*- OpenSceneGraph - Copyright (C) 1998-2006 Robert Osfield
*
* This library is open source and may be redistributed and/or modified under
* the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or
* (at your option) any later version. The full license is in LICENSE file
* included with this distribution, and on the openscenegraph.org website.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* OpenSceneGraph Public License for more details.
*/
#ifndef OSG_QUAT
#define OSG_QUAT 1
#include <osg/Export>
#include <osg/Vec3f>
#include <osg/Vec4f>
#include <osg/Vec3d>
#include <osg/Vec4d>
namespace osg {
class Matrixf;
class Matrixd;
/** A quaternion class. It can be used to represent an orientation in 3D
space.*/
class OSG_EXPORT Quat
{
public:
typedef double value_type;
value_type _v[4]; // a four-vector
Quat();
Quat(value_type x, value_type y, value_type z, value_type w);
Quat(const Vec4f& v);
Quat(const Vec4d& v);
inline Quat( value_type angle, const Vec3f& axis)
{
makeRotate(angle,axis);
}
inline Quat( value_type angle, const Vec3d& axis)
{
makeRotate(angle,axis);
}
inline Quat( value_type angle1, const Vec3f& axis1,
value_type angle2, const Vec3f& axis2,
value_type angle3, const Vec3f& axis3)
{
makeRotate(angle1,axis1,angle2,axis2,angle3,axis3);
}
inline Quat( value_type angle1, const Vec3d& axis1,
value_type angle2, const Vec3d& axis2,
value_type angle3, const Vec3d& axis3)
{
makeRotate(angle1,axis1,angle2,axis2,angle3,axis3);
}
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; }
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]; }
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]; }
inline bool operator < (const Quat& v) const
{
if (_v[0]<v._v[0]) return true;
else if (_v[0]>v._v[0]) return false;
else if (_v[1]<v._v[1]) return true;
else if (_v[1]>v._v[1]) return false;
else if (_v[2]<v._v[2]) return true;
else if (_v[2]>v._v[2]) return false;
else return (_v[3]<v._v[3]);
}
/* ----------------------------------
Methods to access data members
---------------------------------- */
inline Vec4d asVec4() const
{
return Vec4d(_v[0], _v[1], _v[2], _v[3]);
}
inline Vec3d asVec3() const
{
return Vec3d(_v[0], _v[1], _v[2]);
}
inline void set(value_type x, value_type y, value_type z, value_type w)
{
_v[0]=x;
_v[1]=y;
_v[2]=z;
_v[3]=w;
}
inline void set(const osg::Vec4f& v)
{
_v[0]=v.x();
_v[1]=v.y();
_v[2]=v.z();
_v[3]=v.w();
}
inline void set(const osg::Vec4d& v)
{
_v[0]=v.x();
_v[1]=v.y();
_v[2]=v.z();
_v[3]=v.w();
}
void set(const Matrixf& matrix);
void set(const Matrixd& matrix);
void get(Matrixf& matrix) const;
void get(Matrixd& matrix) const;
inline value_type & operator [] (int i) { return _v[i]; }
inline value_type operator [] (int i) const { return _v[i]; }
inline value_type & x() { return _v[0]; }
inline value_type & y() { return _v[1]; }
inline value_type & z() { return _v[2]; }
inline value_type & w() { return _v[3]; }
inline value_type x() const { return _v[0]; }
inline value_type y() const { return _v[1]; }
inline value_type z() const { return _v[2]; }
inline value_type w() const { return _v[3]; }
/** return true if the Quat represents a zero rotation, and therefore
can be ignored in computations.*/
bool zeroRotation() const { return _v[0]==0.0 && _v[1]==0.0 &&
_v[2]==0.0 && _v[3]==1.0; }
/* -------------------------------------------------------------
BASIC ARITHMETIC METHODS
Implemented in terms of Vec4s. Some Vec4 operators, e.g.
operator* are not appropriate for quaternions (as
mathematical objects) so they are implemented differently.
Also define methods for conjugate and the multiplicative inverse.
------------------------------------------------------------- */
/// Multiply by scalar
inline const Quat operator * (value_type rhs) const
{
return Quat(_v[0]*rhs, _v[1]*rhs, _v[2]*rhs, _v[3]*rhs);
}
/// Unary multiply by scalar
inline Quat& operator *= (value_type rhs)
{
_v[0]*=rhs;
_v[1]*=rhs;
_v[2]*=rhs;
_v[3]*=rhs;
return *this; // enable nesting
}
const Quat operator*(const Quat& rhs) const;
/// Unary multiply
inline Quat& operator*=(const Quat& rhs)
{
value_type x = rhs._v[3]*_v[0] + rhs._v[0]*_v[3] + rhs._v[1]*_v[2]
- rhs._v[2]*_v[1];
value_type y = rhs._v[3]*_v[1] - rhs._v[0]*_v[2] + rhs._v[1]*_v[3]
+ rhs._v[2]*_v[0];
value_type z = rhs._v[3]*_v[2] + rhs._v[0]*_v[1] - rhs._v[1]*_v[0]
+ rhs._v[2]*_v[3];
_v[3] = rhs._v[3]*_v[3] - rhs._v[0]*_v[0] - rhs._v[1]*_v[1] -
rhs._v[2]*_v[2];
_v[2] = z;
_v[1] = y;
_v[0] = x;
return (*this); // enable nesting
}
/// Divide by scalar
inline Quat operator / (value_type rhs) const
{
value_type div = 1.0/rhs;
return Quat(_v[0]*div, _v[1]*div, _v[2]*div, _v[3]*div);
}
/// Unary divide by scalar
inline Quat& operator /= (value_type rhs)
{
value_type div = 1.0/rhs;
_v[0]*=div;
_v[1]*=div;
_v[2]*=div;
_v[3]*=div;
return *this;
}
/// Binary divide
inline const Quat operator/(const Quat& denom) const
{
return ( (*this) * denom.inverse() );
}
/// Unary divide
inline Quat& operator/=(const Quat& denom)
{
(*this) = (*this) * denom.inverse();
return (*this); // enable nesting
}
/// Binary addition
inline const Quat operator + (const Quat& rhs) const
{
return Quat(_v[0]+rhs._v[0], _v[1]+rhs._v[1],
_v[2]+rhs._v[2], _v[3]+rhs._v[3]);
}
/// Unary addition
inline Quat& operator += (const Quat& rhs)
{
_v[0] += rhs._v[0];
_v[1] += rhs._v[1];
_v[2] += rhs._v[2];
_v[3] += rhs._v[3];
return *this; // enable nesting
}
/// Binary subtraction
inline const Quat operator - (const Quat& rhs) const
{
return Quat(_v[0]-rhs._v[0], _v[1]-rhs._v[1],
_v[2]-rhs._v[2], _v[3]-rhs._v[3] );
}
/// Unary subtraction
inline Quat& operator -= (const Quat& rhs)
{
_v[0]-=rhs._v[0];
_v[1]-=rhs._v[1];
_v[2]-=rhs._v[2];
_v[3]-=rhs._v[3];
return *this; // enable nesting
}
/** Negation operator - returns the negative of the quaternion.
Basically just calls operator - () on the Vec4 */
inline const Quat operator - () const
{
return Quat (-_v[0], -_v[1], -_v[2], -_v[3]);
}
/// Length of the quaternion = sqrt( vec . vec )
value_type length() const
{
return sqrt( _v[0]*_v[0] + _v[1]*_v[1] + _v[2]*_v[2] + _v[3]*_v[3]);
}
/// Length of the quaternion = vec . vec
value_type length2() const
{
return _v[0]*_v[0] + _v[1]*_v[1] + _v[2]*_v[2] + _v[3]*_v[3];
}
/// Conjugate
inline Quat conj () const
{
return Quat( -_v[0], -_v[1], -_v[2], _v[3] );
}
/// Multiplicative inverse method: q^(-1) = q^*/(q.q^*)
inline const Quat inverse () const
{
return conj() / length2();
}
/* --------------------------------------------------------
METHODS RELATED TO ROTATIONS
Set a quaternion which will perform a rotation of an
angle around the axis given by the vector (x,y,z).
Should be written to also accept an angle and a Vec3?
Define Spherical Linear interpolation method also
Not inlined - see the Quat.cpp file for implementation
-------------------------------------------------------- */
void makeRotate( value_type angle,
value_type x, value_type y, value_type z );
void makeRotate ( value_type angle, const Vec3f& vec );
void makeRotate ( value_type angle, const Vec3d& vec );
void makeRotate ( value_type angle1, const Vec3f& axis1,
value_type angle2, const Vec3f& axis2,
value_type angle3, const Vec3f& axis3);
void makeRotate ( value_type angle1, const Vec3d& axis1,
value_type angle2, const Vec3d& axis2,
value_type angle3, const Vec3d& axis3);
/** Make a rotation Quat which will rotate vec1 to vec2.
Generally take a dot product to get the angle between these
and then use a cross product to get the rotation axis
Watch out for the two special cases when the vectors
are co-incident or opposite in direction.*/
void makeRotate( const Vec3f& vec1, const Vec3f& vec2 );
/** Make a rotation Quat which will rotate vec1 to vec2.
Generally take a dot product to get the angle between these
and then use a cross product to get the rotation axis
Watch out for the two special cases of when the vectors
are co-incident or opposite in direction.*/
void makeRotate( const Vec3d& vec1, const Vec3d& vec2 );
void makeRotate_original( const Vec3d& vec1, const Vec3d& vec2 );
/** Return the angle and vector components represented by the
quaternion.*/
void getRotate ( value_type & angle, value_type & x, value_type & y,
value_type & z ) const;
/** Return the angle and vector represented by the quaternion.*/
void getRotate ( value_type & angle, Vec3f& vec ) const;
/** Return the angle and vector represented by the quaternion.*/
void getRotate ( value_type & angle, Vec3d& vec ) const;
/** Spherical Linear Interpolation.
As t goes from 0 to 1, the Quat object goes from "from" to "to". */
void slerp ( value_type t, const Quat& from, const Quat& to);
/** Rotate a vector by this quaternion.*/
Vec3f operator* (const Vec3f& v) const
{
// nVidia SDK implementation
Vec3f uv, uuv;
Vec3f qvec(_v[0], _v[1], _v[2]);
uv = qvec ^ v;
uuv = qvec ^ uv;
uv *= ( 2.0f * _v[3] );
uuv *= 2.0f;
return v + uv + uuv;
}
/** Rotate a vector by this quaternion.*/
Vec3d operator* (const Vec3d& v) const
{
// nVidia SDK implementation
Vec3d uv, uuv;
Vec3d qvec(_v[0], _v[1], _v[2]);
uv = qvec ^ v;
uuv = qvec ^ uv;
uv *= ( 2.0f * _v[3] );
uuv *= 2.0f;
return v + uv + uuv;
}
protected:
}; // end of class prototype
} // end of namespace
#endif
/* -*-c++-*- OpenSceneGraph - Copyright (C) 1998-2006 Robert Osfield
*
* This library is open source and may be redistributed and/or modified under
* the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or
* (at your option) any later version. The full license is in LICENSE file
* included with this distribution, and on the openscenegraph.org website.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* OpenSceneGraph Public License for more details.
*/
#include <stdio.h>
#include <osg/Quat>
#include <osg/Matrixf>
#include <osg/Matrixd>
#include <osg/Notify>
#include <math.h>
/// Good introductions to Quaternions at:
/// http://www.gamasutra.com/features/programming/19980703/quaternions_01.htm
/// http://mathworld.wolfram.com/Quaternion.html
using namespace osg;
Quat::Quat()
{
_v[0]=0;
_v[1]=0;
_v[2]=0;
_v[3]=1;
}
Quat::Quat(value_type x, value_type y, value_type z, value_type w)
{
_v[0]=x;
_v[1]=y;
_v[2]=z;
_v[3]=w;
}
Quat::Quat(const Vec4f& v)
{
_v[0]=v.x();
_v[1]=v.y();
_v[2]=v.z();
_v[3]=v.w();
}
Quat::Quat( const Vec4d& v )
{
_v[0]=v.x();
_v[1]=v.y();
_v[2]=v.z();
_v[3]=v.w();
}
void Quat::set(const Matrixf& matrix)
{
*this = matrix.getRotate();
}
void Quat::set(const Matrixd& matrix)
{
*this = matrix.getRotate();
}
void Quat::get(Matrixf& matrix) const
{
matrix.makeRotate(*this);
}
void Quat::get(Matrixd& matrix) const
{
matrix.makeRotate(*this);
}
/*! \brief quaternion multiplication
* \f[
* \begin{array}{lll}
* q_1 q_2
* & = &
* \left( x_1, \mathbf{x}_1 \right)
* \left( x_2, \mathbf{x}_2 \right) \\
* & = &
* \left[
* \begin{array}{c}
* x_1 x_2 - \mathbf{x}_1 \cdot \mathbf{x}_2 \\
* x_1 \mathbf{x}_2 + x_2 \mathbf{x}_1 + \mathbf{x}_1 \times \mathbf{x}_2 \\
* \end{array}
* \right]
* \end{array}
* \f]
* \author Ir. M.J. Baars ([email protected]), 2009-01-26
*/
const Quat Quat::operator*(const Quat& rhs) const
{
osg::Quat q;
q._v[0] = _v[1] * rhs._v[2] - _v[2] * rhs._v[1] + _v[3] * rhs._v[0] + _v[0] * rhs._v[3];
q._v[1] = _v[2] * rhs._v[0] - _v[0] * rhs._v[2] + _v[3] * rhs._v[1] + _v[1] * rhs._v[3];
q._v[2] = _v[0] * rhs._v[1] - _v[1] * rhs._v[0] + _v[3] * rhs._v[2] + _v[2] * rhs._v[3];
q._v[3] = _v[3] * rhs._v[3] - _v[0] * rhs._v[0] - _v[1] * rhs._v[1] - _v[2] * rhs._v[2];
return q;
/*
return Quat(
rhs._v[3]*_v[0] + rhs._v[0]*_v[3] + rhs._v[1]*_v[2] - rhs._v[2]*_v[1],
rhs._v[3]*_v[1] - rhs._v[0]*_v[2] + rhs._v[1]*_v[3] + rhs._v[2]*_v[0],
rhs._v[3]*_v[2] + rhs._v[0]*_v[1] - rhs._v[1]*_v[0] + rhs._v[2]*_v[3],
rhs._v[3]*_v[3] - rhs._v[0]*_v[0] - rhs._v[1]*_v[1] - rhs._v[2]*_v[2]
);
*/
}
/// Set the elements of the Quat to represent a rotation of angle
/// (radians) around the axis (x,y,z)
void Quat::makeRotate( value_type angle, value_type x, value_type y, value_type z )
{
const value_type epsilon = 0.0000001;
value_type length = sqrt( x*x + y*y + z*z );
if (length < epsilon)
{
// ~zero length axis, so reset rotation to zero.
*this = Quat();
return;
}
value_type inversenorm = 1.0/length;
value_type coshalfangle = cos( 0.5*angle );
value_type sinhalfangle = sin( 0.5*angle );
_v[0] = x * sinhalfangle * inversenorm;
_v[1] = y * sinhalfangle * inversenorm;
_v[2] = z * sinhalfangle * inversenorm;
_v[3] = coshalfangle;
}
void Quat::makeRotate( value_type angle, const Vec3f& vec )
{
makeRotate( angle, vec[0], vec[1], vec[2] );
}
void Quat::makeRotate( value_type angle, const Vec3d& vec )
{
makeRotate( angle, vec[0], vec[1], vec[2] );
}
void Quat::makeRotate ( value_type angle1, const Vec3f& axis1,
value_type angle2, const Vec3f& axis2,
value_type angle3, const Vec3f& axis3)
{
makeRotate(angle1,Vec3d(axis1),
angle2,Vec3d(axis2),
angle3,Vec3d(axis3));
}
void Quat::makeRotate ( value_type angle1, const Vec3d& axis1,
value_type angle2, const Vec3d& axis2,
value_type angle3, const Vec3d& axis3)
{
Quat q1; q1.makeRotate(angle1,axis1);
Quat q2; q2.makeRotate(angle2,axis2);
Quat q3; q3.makeRotate(angle3,axis3);
*this = q1*q2*q3;
}
void Quat::makeRotate( const Vec3f& from, const Vec3f& to )
{
makeRotate( Vec3d(from), Vec3d(to) );
}
/** Make a rotation Quat which will rotate vec1 to vec2
This routine uses only fast geometric transforms, without costly acos/sin computations.
It's exact, fast, and with less degenerate cases than the acos/sin method.
For an explanation of the math used, you may see for example:
http://logiciels.cnes.fr/MARMOTTES/marmottes-mathematique.pdf
@note This is the rotation with shortest angle, which is the one equivalent to the
acos/sin transform method. Other rotations exists, for example to additionally keep
a local horizontal attitude.
@author Nicolas Brodu
*/
void Quat::makeRotate( const Vec3d& from, const Vec3d& to )
{
// This routine takes any vector as argument but normalized
// vectors are necessary, if only for computing the dot product.
// Too bad the API is that generic, it leads to performance loss.
// Even in the case the 2 vectors are not normalized but same length,
// the sqrt could be shared, but we have no way to know beforehand
// at this point, while the caller may know.
// So, we have to test... in the hope of saving at least a sqrt
Vec3d sourceVector = from;
Vec3d targetVector = to;
value_type fromLen2 = from.length2();
value_type fromLen;
// normalize only when necessary, epsilon test
if ((fromLen2 < 1.0-1e-7) || (fromLen2 > 1.0+1e-7)) {
fromLen = sqrt(fromLen2);
sourceVector /= fromLen;
} else fromLen = 1.0;
value_type toLen2 = to.length2();
// normalize only when necessary, epsilon test
if ((toLen2 < 1.0-1e-7) || (toLen2 > 1.0+1e-7)) {
value_type toLen;
// re-use fromLen for case of mapping 2 vectors of the same length
if ((toLen2 > fromLen2-1e-7) && (toLen2 < fromLen2+1e-7)) {
toLen = fromLen;
}
else toLen = sqrt(toLen2);
targetVector /= toLen;
}
// Now let's get into the real stuff
// Use "dot product plus one" as test as it can be re-used later on
double dotProdPlus1 = 1.0 + sourceVector * targetVector;
// Check for degenerate case of full u-turn. Use epsilon for detection
if (dotProdPlus1 < 1e-7) {
// Get an orthogonal vector of the given vector
// in a plane with maximum vector coordinates.
// Then use it as quaternion axis with pi angle
// Trick is to realize one value at least is >0.6 for a normalized vector.
if (fabs(sourceVector.x()) < 0.6) {
const double norm = sqrt(1.0 - sourceVector.x() * sourceVector.x());
_v[0] = 0.0;
_v[1] = sourceVector.z() / norm;
_v[2] = -sourceVector.y() / norm;
_v[3] = 0.0;
} else if (fabs(sourceVector.y()) < 0.6) {
const double norm = sqrt(1.0 - sourceVector.y() * sourceVector.y());
_v[0] = -sourceVector.z() / norm;
_v[1] = 0.0;
_v[2] = sourceVector.x() / norm;
_v[3] = 0.0;
} else {
const double norm = sqrt(1.0 - sourceVector.z() * sourceVector.z());
_v[0] = sourceVector.y() / norm;
_v[1] = -sourceVector.x() / norm;
_v[2] = 0.0;
_v[3] = 0.0;
}
}
else {
// Find the shortest angle quaternion that transforms normalized vectors
// into one other. Formula is still valid when vectors are colinear
const double s = sqrt(0.5 * dotProdPlus1);
const Vec3d tmp = sourceVector ^ targetVector / (2.0*s);
_v[0] = tmp.x();
_v[1] = tmp.y();
_v[2] = tmp.z();
_v[3] = s;
}
}
// Make a rotation Quat which will rotate vec1 to vec2
// Generally take adot product to get the angle between these
// and then use a cross product to get the rotation axis
// Watch out for the two special cases of when the vectors
// are co-incident or opposite in direction.
void Quat::makeRotate_original( const Vec3d& from, const Vec3d& to )
{
const value_type epsilon = 0.0000001;
value_type length1 = from.length();
value_type length2 = to.length();
// dot product vec1*vec2
value_type cosangle = from*to/(length1*length2);
if ( fabs(cosangle - 1) < epsilon )
{
osg::notify(osg::INFO)<<"*** Quat::makeRotate(from,to) with near co-linear vectors, epsilon= "<<fabs(cosangle-1)<<std::endl;
// cosangle is close to 1, so the vectors are close to being coincident
// Need to generate an angle of zero with any vector we like
// We'll choose (1,0,0)
makeRotate( 0.0, 0.0, 0.0, 1.0 );
}
else
if ( fabs(cosangle + 1.0) < epsilon )
{
// vectors are close to being opposite, so will need to find a
// vector orthongonal to from to rotate about.
Vec3d tmp;
if (fabs(from.x())<fabs(from.y()))
if (fabs(from.x())<fabs(from.z())) tmp.set(1.0,0.0,0.0); // use x axis.
else tmp.set(0.0,0.0,1.0);
else if (fabs(from.y())<fabs(from.z())) tmp.set(0.0,1.0,0.0);
else tmp.set(0.0,0.0,1.0);
Vec3d fromd(from.x(),from.y(),from.z());
// find orthogonal axis.
Vec3d axis(fromd^tmp);
axis.normalize();
_v[0] = axis[0]; // sin of half angle of PI is 1.0.
_v[1] = axis[1]; // sin of half angle of PI is 1.0.
_v[2] = axis[2]; // sin of half angle of PI is 1.0.
_v[3] = 0; // cos of half angle of PI is zero.
}
else
{
// This is the usual situation - take a cross-product of vec1 and vec2
// and that is the axis around which to rotate.
Vec3d axis(from^to);
value_type angle = acos( cosangle );
makeRotate( angle, axis );
}
}
void Quat::getRotate( value_type& angle, Vec3f& vec ) const
{
value_type x,y,z;
getRotate(angle,x,y,z);
vec[0]=x;
vec[1]=y;
vec[2]=z;
}
void Quat::getRotate( value_type& angle, Vec3d& vec ) const
{
value_type x,y,z;
getRotate(angle,x,y,z);
vec[0]=x;
vec[1]=y;
vec[2]=z;
}
// Get the angle of rotation and axis of this Quat object.
// Won't give very meaningful results if the Quat is not associated
// with a rotation!
void Quat::getRotate( value_type& angle, value_type& x, value_type& y, value_type& z ) const
{
value_type sinhalfangle = sqrt( _v[0]*_v[0] + _v[1]*_v[1] + _v[2]*_v[2] );
angle = 2.0 * atan2( sinhalfangle, _v[3] );
if(sinhalfangle)
{
x = _v[0] / sinhalfangle;
y = _v[1] / sinhalfangle;
z = _v[2] / sinhalfangle;
}
else
{
x = 0.0;
y = 0.0;
z = 1.0;
}
}
/// Spherical Linear Interpolation
/// As t goes from 0 to 1, the Quat object goes from "from" to "to"
/// Reference: Shoemake at SIGGRAPH 89
/// See also
/// http://www.gamasutra.com/features/programming/19980703/quaternions_01.htm
void Quat::slerp( value_type t, const Quat& from, const Quat& to )
{
const double epsilon = 0.00001;
double omega, cosomega, sinomega, scale_from, scale_to ;
osg::Quat quatTo(to);
// this is a dot product
cosomega = from.asVec4() * to.asVec4();
if ( cosomega <0.0 )
{
cosomega = -cosomega;
quatTo = -to;
}
if( (1.0 - cosomega) > epsilon )
{
omega= acos(cosomega) ; // 0 <= omega <= Pi (see man acos)
sinomega = sin(omega) ; // this sinomega should always be +ve so
// could try sinomega=sqrt(1-cosomega*cosomega) to avoid a sin()?
scale_from = sin((1.0-t)*omega)/sinomega ;
scale_to = sin(t*omega)/sinomega ;
}
else
{
/* --------------------------------------------------
The ends of the vectors are very close
we can use simple linear interpolation - no need
to worry about the "spherical" interpolation
-------------------------------------------------- */
scale_from = 1.0 - t ;
scale_to = t ;
}
*this = (from*scale_from) + (quatTo*scale_to);
// so that we get a Vec4
}
#define QX _v[0]
#define QY _v[1]
#define QZ _v[2]
#define QW _v[3]
#ifdef OSG_USE_UNIT_TESTS
void test_Quat_Eueler(value_type heading,value_type pitch,value_type roll)
{
osg::Quat q;
q.makeRotate(heading,pitch,roll);
osg::Matrix q_m;
q.get(q_m);
osg::Vec3 xAxis(1,0,0);
osg::Vec3 yAxis(0,1,0);
osg::Vec3 zAxis(0,0,1);
cout << "heading = "<<heading<<" pitch = "<<pitch<<" roll = "<<roll<<endl;
cout <<"q_m = "<<q_m;
cout <<"xAxis*q_m = "<<xAxis*q_m << endl;
cout <<"yAxis*q_m = "<<yAxis*q_m << endl;
cout <<"zAxis*q_m = "<<zAxis*q_m << endl;
osg::Matrix r_m = osg::Matrix::rotate(roll,0.0,1.0,0.0)*
osg::Matrix::rotate(pitch,1.0,0.0,0.0)*
osg::Matrix::rotate(-heading,0.0,0.0,1.0);
cout << "r_m = "<<r_m;
cout <<"xAxis*r_m = "<<xAxis*r_m << endl;
cout <<"yAxis*r_m = "<<yAxis*r_m << endl;
cout <<"zAxis*r_m = "<<zAxis*r_m << endl;
cout << endl<<"*****************************************" << endl<< endl;
}
void test_Quat()
{
test_Quat_Eueler(osg::DegreesToRadians(20),0,0);
test_Quat_Eueler(0,osg::DegreesToRadians(20),0);
test_Quat_Eueler(0,0,osg::DegreesToRadians(20));
test_Quat_Eueler(osg::DegreesToRadians(20),osg::DegreesToRadians(20),osg::DegreesToRadians(20));
return 0;
}
#endif
_______________________________________________
osg-users mailing list
[email protected]
http://lists.openscenegraph.org/listinfo.cgi/osg-users-openscenegraph.org
