/* -*-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 #include #include #include #include #include #include /// 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( float angle, float x, float y, float z ) { float inversenorm = 1.0/sqrt( x*x + y*y + z*z ); float coshalfangle = cos( 0.5*angle ); float sinhalfangle = sin( 0.5*angle ); _fv[0] = x * sinhalfangle * inversenorm; _fv[1] = y * sinhalfangle * inversenorm; _fv[2] = z * sinhalfangle * inversenorm; _fv[3] = coshalfangle; } void Quat::makeRotate( float angle, const Vec3& vec ) { makeRotate( angle, vec[0], vec[1], vec[2] ); } void Quat::makeRotate ( float angle1, const Vec3& axis1, float angle2, const Vec3& axis2, float angle3, const Vec3& axis3) { Quat q1; q1.makeRotate(angle1,axis1); Quat q2; q2.makeRotate(angle2,axis2); Quat q3; q3.makeRotate(angle3,axis3); *this = q1*q2*q3; } // 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( const Vec3& from, const Vec3& to ) { const float epsilon = 0.00001f; float length1 = from.length(); float length2 = to.length(); // dot product vec1*vec2 float cosangle = from*to/(length1*length2); if ( fabs(cosangle - 1) < epsilon ) { // 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, 1.0, 0.0, 0.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. osg::Vec3 tmp; if (fabs(from.x()) 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 ; } // use Vec4 arithmetic _fv = (from._fv*scale_from) + (quatTo._fv*scale_to); // so that we get a Vec4 } #define QX _fv[0] #define QY _fv[1] #define QZ _fv[2] #define QW _fv[3] #ifdef OSG_USE_UNIT_TESTS void test_Quat_Eueler(float heading,float pitch,float 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 = "<