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..."
This commit is contained in:
Robert Osfield
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user