Advertisement
Guest User

Untitled

a guest
Feb 10th, 2016
61
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 0.85 KB | None | 0 0
  1. Matrix3 ComputeJacobiRotation(const Matrix3& matrix)
  2. {
  3.     std::vector<int> pq;
  4.  
  5.     if (abs(matrix[0][1]) > abs(matrix[0][2]))
  6.     {
  7.         if (abs(matrix[0][1]) > abs(matrix[1][2]))
  8.             pq = { 0, 1 };
  9.         else
  10.             pq = { 1, 2 };
  11.     }
  12.     else
  13.     {
  14.         if (abs(matrix[0][2]) > abs(matrix[1][2]))
  15.             pq = { 0, 2 };
  16.         else
  17.             pq = { 1, 2 };
  18.     }
  19.  
  20.     // [c, s]
  21.     // [-s, c]
  22.    
  23.     float beta = (matrix[pq[1]][pq[1]] - matrix[pq[0]][pq[0]]) /
  24.                  (2 * matrix[pq[0]][pq[1]]);
  25.  
  26.     float tan = (beta > 0.0f ? 1.0f : -1.0f) / (abs(beta) + sqrt((beta * beta) + 1.0f));
  27.  
  28.     float cos2 = 1.0f / ((tan * tan) + 1);
  29.     float sin2 = 1.0f - cos2;
  30.  
  31.     float cos = sqrt(cos2);
  32.     float sin = sqrt(sin2);
  33.  
  34.     Matrix3 result;
  35.  
  36.     result.SetIdentity( );
  37.  
  38.     result[pq[0]][pq[0]] = cos;
  39.     result[pq[0]][pq[1]] = sin;
  40.     result[pq[1]][pq[1]] = cos;
  41.     result[pq[1]][pq[0]] = -sin;
  42.  
  43.     return result;
  44. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement