convert roation matrix to quat

最新推荐文章于 2022-10-29 15:16:45 发布

fu_shuwu

最新推荐文章于 2022-10-29 15:16:45 发布

阅读量1.6k

点赞数

分类专栏： Android Opengl Linux 文章标签： matrix glm quat rotation pose information

本文链接：https://blog.csdn.net/fu_shuwu/article/details/73555510

版权

Android 同时被 3 个专栏收录

202 篇文章 2 订阅

订阅专栏

Linux

114 篇文章 1 订阅

订阅专栏

Opengl

25 篇文章 0 订阅

订阅专栏

//rotation_mat get from the decompose matrix's output of rotation

glm::quat *rotation = glm::quat_cast(rotation_mat);

template <typename T, precision P>
GLM_FUNC_QUALIFIER detail::tquat<T, P> quat_cast
(
   detail::tmat4x4<T, P> const & m4
)
{
   return quat_cast(detail::tmat3x3<T, P>(m4));
}

template <typename T, precision P>
GLM_FUNC_QUALIFIER detail::tquat<T, P> quat_cast
(
   detail::tmat3x3<T, P> const & m
)
{
   T fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2];
   T fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2];
   T fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1];
   T fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2];

   int biggestIndex = 0;
   T fourBiggestSquaredMinus1 = fourWSquaredMinus1;
   if(fourXSquaredMinus1 > fourBiggestSquaredMinus1)
   {
      fourBiggestSquaredMinus1 = fourXSquaredMinus1;
      biggestIndex = 1;
   }
   if(fourYSquaredMinus1 > fourBiggestSquaredMinus1)
   {
      fourBiggestSquaredMinus1 = fourYSquaredMinus1;
      biggestIndex = 2;
   }
   if(fourZSquaredMinus1 > fourBiggestSquaredMinus1)
   {
      fourBiggestSquaredMinus1 = fourZSquaredMinus1;
      biggestIndex = 3;
   }

   T biggestVal = sqrt(fourBiggestSquaredMinus1 + T(1)) * T(0.5);
   T mult = static_cast<T>(0.25) / biggestVal;

   detail::tquat<T, P> Result;
   switch(biggestIndex)
   {
   case 0:
      Result.w = biggestVal;
      Result.x = (m[1][2] - m[2][1]) * mult;
      Result.y = (m[2][0] - m[0][2]) * mult;
      Result.z = (m[0][1] - m[1][0]) * mult;
      break;
   case 1:
      Result.w = (m[1][2] - m[2][1]) * mult;
      Result.x = biggestVal;
      Result.y = (m[0][1] + m[1][0]) * mult;
      Result.z = (m[2][0] + m[0][2]) * mult;
      break;
   case 2:
      Result.w = (m[2][0] - m[0][2]) * mult;
      Result.x = (m[0][1] + m[1][0]) * mult;
      Result.y = biggestVal;
      Result.z = (m[1][2] + m[2][1]) * mult;
      break;
   case 3:
      Result.w = (m[0][1] - m[1][0]) * mult;
      Result.x = (m[2][0] + m[0][2]) * mult;
      Result.y = (m[1][2] + m[2][1]) * mult;
      Result.z = biggestVal;
      break;
      
   default:               // Silence a -Wswitch-default warning in GCC. Should never actually get here. Assert is just for sanity.
      assert(false);
      break;
   }
   return Result;
}