#include "osg/Quat" #include "osg/Vec4" #include "osg/Vec3" #include "osg/Types" #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; /// Default constructor is empty Quat::Quat( void ) { } /// Constructor with four floats - just call the constructor for the Vec4 Quat::Quat( const float x, const float y, const float z, const float w ) { _fv = Vec4( x, y, z, w ); } /// Constructor with a Vec4 Quat::Quat( const Vec4& vec ) { _fv = vec; } /// Set the elements of the Quat to represent a rotation of angle /// (radians) around the axis (x,y,z) void Quat::makeRot( const float angle, const float x, const float y, const 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::makeRot( const float angle, const Vec3& vec ) { makeRot( angle, vec[0], vec[1], vec[2] ); } // 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::makeRot( const Vec3& vec1, const Vec3& vec2 ) { const float epsilon = 0.00001f; float length1 = vec1.length(); float length2 = vec2.length(); float cosangle = vec1*vec2/(2*length1*length2); // dot product vec1*vec2 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) makeRot( 0.0, 1.0, 0.0, 0.0 ); } else if ( fabs(cosangle + 1) < epsilon ) { // cosangle is close to -1, so the vectors are close to being opposite // The angle of rotation is going to be Pi, but around which axis? // Basically, any one perpendicular to vec1 = (x,y,z) is going to work. // Choose a vector to cross product vec1 with. Find the biggest // in magnitude of x, y and z and then put a zero in that position. float biggest = fabs(vec1[0]); int bigposn = 0; if ( fabs(vec1[1]) > biggest ) { biggest=fabs(vec1[1]); bigposn = 1; } if ( fabs(vec1[2]) > biggest ) { biggest=fabs(vec1[2]); bigposn = 2; } Vec3 temp = Vec3( 1.0, 1.0, 1.0 ); temp[bigposn] = 0.0; Vec3 axis = vec1^temp; // this is a cross-product to generate the // axis around which to rotate makeRot( (float)M_PI, axis ); } else { // This is the usual situation - take a cross-product of vec1 and vec2 // and that is the axis around which to rotate. Vec3 axis = vec1^vec2; float angle = acos( cosangle ); makeRot( angle, axis ); } } // 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::getRot( float& angle, Vec3& vec ) const { float sinhalfangle = sqrt( _fv[0]*_fv[0] + _fv[1]*_fv[1] + _fv[2]*_fv[2] ); /// float coshalfangle = _fv[3]; /// These are not checked for performance reasons ? (cop out!) /// Point for discussion - how do one handle errors in the osg? /// if ( abs(sinhalfangle) > 1.0 ) { error }; /// if ( abs(coshalfangle) > 1.0 ) { error }; // *angle = atan2( sinhalfangle, coshalfangle ); // see man atan2 angle = 2 * atan2( sinhalfangle, _fv[3] ); // -pi < angle < pi vec = Vec3(_fv[0], _fv[1], _fv[2]) / sinhalfangle; } void Quat::getRot( float& angle, float& x, float& y, float& z ) const { float sinhalfangle = sqrt( _fv[0]*_fv[0] + _fv[1]*_fv[1] + _fv[2]*_fv[2] ); angle = 2 * atan2( sinhalfangle, _fv[3] ); x = _fv[0] / sinhalfangle; y = _fv[1] / sinhalfangle; z = _fv[2] / sinhalfangle; } /// 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( const float t, const Quat& from, const Quat& to ) { const double epsilon = 0.00001; double omega, cosomega, sinomega, scale_from, scale_to ; cosomega = from.asVec4() * to.asVec4() ; // this is a dot product 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 ; } _fv = (from._fv*scale_from) + (to._fv*scale_to); // use Vec4 arithmetic // so that we get a Vec4 } #define QX _fv[0] #define QY _fv[1] #define QZ _fv[2] #define QW _fv[3] void Quat::set( const Matrix& m ) { // Source: Gamasutra, Rotating Objects Using Quaternions // //http://www.gamasutra.com/features/programming/19980703/quaternions_01.htm float tr, s; float tq[4]; int i, j, k; int nxt[3] = {1, 2, 0}; tr = m._mat[0][0] + m._mat[1][1] + m._mat[2][2]; // check the diagonal if (tr > 0.0) { s = (float)sqrt (tr + 1.0); QW = s / 2.0f; s = 0.5f / s; QX = (m._mat[1][2] - m._mat[2][1]) * s; QY = (m._mat[2][0] - m._mat[0][2]) * s; QZ = (m._mat[0][1] - m._mat[1][0]) * s; } else { // diagonal is negative i = 0; if (m._mat[1][1] > m._mat[0][0]) i = 1; if (m._mat[2][2] > m._mat[i][i]) i = 2; j = nxt[i]; k = nxt[j]; s = (float)sqrt ((m._mat[i][i] - (m._mat[j][j] + m._mat[k][k])) + 1.0); tq[i] = s * 0.5f; if (s != 0.0f) s = 0.5f / s; tq[3] = (m._mat[j][k] - m._mat[k][j]) * s; tq[j] = (m._mat[i][j] + m._mat[j][i]) * s; tq[k] = (m._mat[i][k] + m._mat[k][i]) * s; QX = tq[0]; QY = tq[1]; QZ = tq[2]; QW = tq[3]; } } void Quat::get( Matrix& m ) const { // Source: Gamasutra, Rotating Objects Using Quaternions // //http://www.gamasutra.com/features/programming/19980703/quaternions_01.htm float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2; // calculate coefficients x2 = QX + QX; y2 = QY + QY; z2 = QZ + QZ; xx = QX * x2; xy = QX * y2; xz = QX * z2; yy = QY * y2; yz = QY * z2; zz = QZ * z2; wx = QW * x2; wy = QW * y2; wz = QW * z2; m._mat[0][0] = 1.0f - (yy + zz); m._mat[0][1] = xy - wz; m._mat[0][2] = xz + wy; m._mat[0][3] = 0.0f; m._mat[1][0] = xy + wz; m._mat[1][1] = 1.0f - (xx + zz); m._mat[1][2] = yz - wx; m._mat[1][3] = 0.0f; m._mat[2][0] = xz - wy; m._mat[2][1] = yz + wx; m._mat[2][2] = 1.0f - (xx + yy); m._mat[2][3] = 0.0f; m._mat[3][0] = 0; m._mat[3][1] = 0; m._mat[3][2] = 0; m._mat[3][3] = 1; }