diff --git a/miscmaths.cc b/miscmaths.cc index 9c2688ccdce6fe2b6f40c64a46bee211cb59a6d7..c93eef2f72565851f684b9dfa0e34715a4cd4ef2 100644 --- a/miscmaths.cc +++ b/miscmaths.cc @@ -929,38 +929,34 @@ namespace MISCMATHS { int rotmat2quat(ColumnVector& quaternion, const Matrix& rotmat) - { - Tracer tr("rotmat2quat"); - - float trace = rotmat.SubMatrix(1,3,1,3).Trace(); - - if (trace > 0) { - float w = std::sqrt((trace + 1.0)/4.0); - quaternion(1) = (rotmat(3,2) - rotmat(2,3))/(4.0*w); - quaternion(2) = (rotmat(1,3) - rotmat(3,1))/(4.0*w); - quaternion(3) = (rotmat(2,1) - rotmat(1,2))/(4.0*w); - } else if ((rotmat(1,1) > rotmat(2,2)) && (rotmat(1,1) > rotmat(3,3))) { - // first col case - float s = std::sqrt(1.0 + rotmat(1,1) - rotmat(2,2) - rotmat(3,3)) * 2.0; - quaternion(1) = 0.5 / s; - quaternion(2) = (-rotmat(1,2) - rotmat(1,2)) / s; - quaternion(3) = (-rotmat(1,3) - rotmat(3,1)) / s; - } else if ((rotmat(2,2) > rotmat(1,1)) && (rotmat(2,2) > rotmat(3,3))) { - // 2nd col case - float s = std::sqrt(1.0 + rotmat(2,2) - rotmat(1,1) - rotmat(3,3)) * 2.0; - quaternion(1) = (-rotmat(1,2) - rotmat(2,1)) / s; - quaternion(2) = 0.5 / s; - quaternion(3) = (-rotmat(2,3) - rotmat(3,2)) / s; - } else if ((rotmat(3,3) > rotmat(1,1)) && (rotmat(3,3) > rotmat(2,2))) { - // 3rd col case - float s = std::sqrt(1.0 + rotmat(3,3) - rotmat(1,1) - rotmat(2,2)) * 2.0; - quaternion(1) = (-rotmat(1,3) - rotmat(3,1)) / s; - quaternion(2) = (-rotmat(2,3) - rotmat(3,2)) / s; - quaternion(3) = 0.5 / s; - } - return 0; + { + Tracer tr("rotmat2quat"); + + // Variable d is a dummy, used to soak + // up outputs from nifti_mat44_to_quatern + // that we are not interested in. + mat44 rot; + double x, y, z, w, d; + + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + rot.m[i][j] = rotmat(i + 1, j + 1); + }} + + legacy::nifti_mat44_to_quatern(rot, x, y, z, d, d, d, d, d, d, w); + + if (quaternion.size() < 4) { + quaternion.ReSize(4); } + quaternion(1) = x; + quaternion(2) = y; + quaternion(3) = z; + quaternion(4) = w; + + return 0; + } + int decompose_aff(ColumnVector& params, const Matrix& affmat, const ColumnVector& centre,