From a5ea4e0b33296dfc01a10eb0d7855f411f0277f3 Mon Sep 17 00:00:00 2001 From: Robert Osfield Date: Thu, 27 Jul 2006 15:23:50 +0000 Subject: [PATCH] From David Spilling, "Matrix_implementation.cpp modified as requested. I ran a version of it through a local version of osgunittests.cpp and it passes. Note that firstly it always returns the positive real quaternion (positive w) Note also that it will sometimes slightly differ from the results of the other methods because it assumes that the input matrix really is a rotation matrix - if it isn't, e.g. because of rounding error, then the output quaternion will be very slightly different. For example, the test matrix 0 1 0 0 1 0 0 0 0 0 0.999999 0 0 0 0 1 will return 0.707107 0.707107 0.0005033 0.0005033 whereas the previous methods return 0.707107 0.707107 0.0 0.0 However, since quaternions are rotations, the meaning of how to convert a matrix that isn't a rotation is a little unclear..." --- src/osg/Matrix_implementation.cpp | 205 ++++++++++++++++-------------- 1 file changed, 112 insertions(+), 93 deletions(-) diff --git a/src/osg/Matrix_implementation.cpp b/src/osg/Matrix_implementation.cpp index 1eb18d459..04c3bcb84 100644 --- a/src/osg/Matrix_implementation.cpp +++ b/src/osg/Matrix_implementation.cpp @@ -122,115 +122,134 @@ void Matrix_implementation::set(const Quat& q_in) } #if 1 -// David Spillings implementation + +// David Spillings implementation Mk 2 void Matrix_implementation::get( Quat& q ) const { - value_type s; - value_type tq[4]; - int i, j; - - // Use tq to store the largest trace - tq[0] = 1 + _mat[0][0]+_mat[1][1]+_mat[2][2]; - tq[1] = 1 + _mat[0][0]-_mat[1][1]-_mat[2][2]; - tq[2] = 1 - _mat[0][0]+_mat[1][1]-_mat[2][2]; - tq[3] = 1 - _mat[0][0]-_mat[1][1]+_mat[2][2]; - - // Find the maximum (could also use stacked if's later) - j = 0; - for(i=1;i<4;i++) j = (tq[i]>tq[j])? i : j; - - // check the diagonal - if (j==0) - { - /* perform instant calculation */ - QW = tq[0]; - QX = _mat[1][2]-_mat[2][1]; - QY = _mat[2][0]-_mat[0][2]; - QZ = _mat[0][1]-_mat[1][0]; - } - else if (j==1) - { - QW = _mat[1][2]-_mat[2][1]; - QX = tq[1]; - QY = _mat[0][1]+_mat[1][0]; - QZ = _mat[2][0]+_mat[0][2]; - } - else if (j==2) - { - QW = _mat[2][0]-_mat[0][2]; - QX = _mat[0][1]+_mat[1][0]; - QY = tq[2]; - QZ = _mat[1][2]+_mat[2][1]; - } - else /* if (j==3) */ - { - QW = _mat[0][1]-_mat[1][0]; - QX = _mat[2][0]+_mat[0][2]; - QY = _mat[1][2]+_mat[2][1]; - QZ = tq[3]; - } - - s = sqrt(0.25/tq[j]); - QW *= s; - QX *= s; - QY *= s; - QZ *= s; + // From http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm + QW = 0.5 * sqrt( osg::maximum( 0.0, 1.0 + _mat[0][0] + _mat[1][1] + _mat[2][2] ) ); + QX = 0.5 * sqrt( osg::maximum( 0.0, 1.0 + _mat[0][0] - _mat[1][1] - _mat[2][2] ) ); + QY = 0.5 * sqrt( osg::maximum( 0.0, 1.0 - _mat[0][0] + _mat[1][1] - _mat[2][2] ) ); + QZ = 0.5 * sqrt( osg::maximum( 0.0, 1.0 - _mat[0][0] - _mat[1][1] + _mat[2][2] ) ); + QX = QX * osg::sign( _mat[1][2] - _mat[2][1]) ; + QY = QY * osg::sign( _mat[2][0] - _mat[0][2]) ; + QZ = QZ * osg::sign( _mat[0][1] - _mat[1][0]) ; } + #else -// Original implementation -void Matrix_implementation::get( Quat& q ) const -{ - // Source: Gamasutra, Rotating Objects Using Quaternions - // - //http://www.gamasutra.com/features/programming/19980703/quaternions_01.htm - value_type tr, s; - value_type tq[4]; - int i, j, k; - - int nxt[3] = {1, 2, 0}; - - tr = _mat[0][0] + _mat[1][1] + _mat[2][2]+1.0; - - // check the diagonal - if (tr > 0.0) + #if 1 + // David Spillings implementation Mk 1 + void Matrix_implementation::get( Quat& q ) const { - s = (value_type)sqrt (tr); - QW = s / 2.0; - s = 0.5 / s; - QX = (_mat[1][2] - _mat[2][1]) * s; - QY = (_mat[2][0] - _mat[0][2]) * s; - QZ = (_mat[0][1] - _mat[1][0]) * s; + value_type s; + value_type tq[4]; + int i, j; + + // Use tq to store the largest trace + tq[0] = 1 + _mat[0][0]+_mat[1][1]+_mat[2][2]; + tq[1] = 1 + _mat[0][0]-_mat[1][1]-_mat[2][2]; + tq[2] = 1 - _mat[0][0]+_mat[1][1]-_mat[2][2]; + tq[3] = 1 - _mat[0][0]-_mat[1][1]+_mat[2][2]; + + // Find the maximum (could also use stacked if's later) + j = 0; + for(i=1;i<4;i++) j = (tq[i]>tq[j])? i : j; + + // check the diagonal + if (j==0) + { + /* perform instant calculation */ + QW = tq[0]; + QX = _mat[1][2]-_mat[2][1]; + QY = _mat[2][0]-_mat[0][2]; + QZ = _mat[0][1]-_mat[1][0]; + } + else if (j==1) + { + QW = _mat[1][2]-_mat[2][1]; + QX = tq[1]; + QY = _mat[0][1]+_mat[1][0]; + QZ = _mat[2][0]+_mat[0][2]; + } + else if (j==2) + { + QW = _mat[2][0]-_mat[0][2]; + QX = _mat[0][1]+_mat[1][0]; + QY = tq[2]; + QZ = _mat[1][2]+_mat[2][1]; + } + else /* if (j==3) */ + { + QW = _mat[0][1]-_mat[1][0]; + QX = _mat[2][0]+_mat[0][2]; + QY = _mat[1][2]+_mat[2][1]; + QZ = tq[3]; + } + + s = sqrt(0.25/tq[j]); + QW *= s; + QX *= s; + QY *= s; + QZ *= s; + } - else + #else + // Original implementation + void Matrix_implementation::get( Quat& q ) const { - // diagonal is negative - i = 0; - if (_mat[1][1] > _mat[0][0]) - i = 1; - if (_mat[2][2] > _mat[i][i]) - i = 2; - j = nxt[i]; - k = nxt[j]; + // Source: Gamasutra, Rotating Objects Using Quaternions + // + //http://www.gamasutra.com/features/programming/19980703/quaternions_01.htm - s = (value_type)sqrt ((_mat[i][i] - (_mat[j][j] + _mat[k][k])) + 1.0); + value_type tr, s; + value_type tq[4]; + int i, j, k; - tq[i] = s * 0.5; + int nxt[3] = {1, 2, 0}; - if (s != 0.0) + tr = _mat[0][0] + _mat[1][1] + _mat[2][2]+1.0; + + // check the diagonal + if (tr > 0.0) + { + s = (value_type)sqrt (tr); + QW = s / 2.0; s = 0.5 / s; + QX = (_mat[1][2] - _mat[2][1]) * s; + QY = (_mat[2][0] - _mat[0][2]) * s; + QZ = (_mat[0][1] - _mat[1][0]) * s; + } + else + { + // diagonal is negative + i = 0; + if (_mat[1][1] > _mat[0][0]) + i = 1; + if (_mat[2][2] > _mat[i][i]) + i = 2; + j = nxt[i]; + k = nxt[j]; - tq[3] = (_mat[j][k] - _mat[k][j]) * s; - tq[j] = (_mat[i][j] + _mat[j][i]) * s; - tq[k] = (_mat[i][k] + _mat[k][i]) * s; + s = (value_type)sqrt ((_mat[i][i] - (_mat[j][j] + _mat[k][k])) + 1.0); - QX = tq[0]; - QY = tq[1]; - QZ = tq[2]; - QW = tq[3]; + tq[i] = s * 0.5; + + if (s != 0.0) + s = 0.5 / s; + + tq[3] = (_mat[j][k] - _mat[k][j]) * s; + tq[j] = (_mat[i][j] + _mat[j][i]) * s; + tq[k] = (_mat[i][k] + _mat[k][i]) * s; + + QX = tq[0]; + QY = tq[1]; + QZ = tq[2]; + QW = tq[3]; + } } -} + #endif #endif int Matrix_implementation::compare(const Matrix_implementation& m) const