355650ac1d
From Robert Osfield, modes to osg::Quat to keep the original implmentation around as makeRotate_original(,) and added tests into osgunittest to test the new methods provide equivilant results to the original implemementation. The orignal implementation will be removed once the new method is more widely tested.
398 lines
12 KiB
C++
398 lines
12 KiB
C++
/* -*-c++-*- OpenSceneGraph - Copyright (C) 1998-2003 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;
|
|
|
|
|
|
void Quat::set(const Matrixf& matrix)
|
|
{
|
|
matrix.get(*this);
|
|
}
|
|
|
|
void Quat::set(const Matrixd& matrix)
|
|
{
|
|
matrix.get(*this);
|
|
}
|
|
|
|
void Quat::get(Matrixf& matrix) const
|
|
{
|
|
matrix.set(*this);
|
|
}
|
|
|
|
void Quat::get(Matrixd& matrix) const
|
|
{
|
|
matrix.set(*this);
|
|
}
|
|
|
|
|
|
/// 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
|