ADN Open CIS
Сообщество программистов Autodesk в СНГ

28/10/2014

Получение угла поворота и оси из матрицы AcGeMatrix3d

Метод AcGeMatrix3d::rotation возвращает новую матрицу с определенным углом поворота. А как из существующей матрицы получить угол поворота? Следующий код это делает.

Код - C++: [Выделить]
  1. #define TWOPI 6.28318530718
  2. void getAngleAndAxis(
  3.            double& theta,
  4.            AcGeVector3d& axis,
  5.            const AcGeMatrix3d& m)
  6. {
  7.   // Контракт:  Матрица ‘m’ не должна содержать никакого масштабирования,
  8.   // сдвига и отзеркаливания (т.е., m.det() должен быть равен 1.0).
  9.   // Можете выполнить AcGeScale3d::removeScale( m ) чтобы обеспечить это условие
  10.   // Если axis передано и ненулевое,
  11.   // тогда этот метод пытается вернуть ось ориентированную также, как и переданная ось,
  12.   // т.е., возвращаемая ось должна быть в том же полупространстве, что и переданная
  13.   // (Заметим, что переданная ось будет перезаписана).
  14.   //
  15.   // Заметим что если матрица имеет зеркальный компонент,
  16.   // ( т.е. её определитель = -1 ),  это не допустимо по контракту.
  17.   // Чтобы понять, в чем проблема, представьте себе как при помощи только
  18.   // поворота совместить ваши левый и правый ботинки
  19.  
  20.   double trace, s;
  21.   int i, j, k;
  22.   double quat[4];
  23.  
  24.   assert(fabs(m.det() -1.0) <= AcGeContext::gTol.equalVector());
  25.  
  26.   trace = m(0, 0) + m(1, 1) + m(2, 2);
  27.  
  28.   if (trace > 0.0) {
  29.     s = sqrt(trace + 1.0);
  30.     quat[0] = s * 0.5;
  31.     s = 0.5 / s;
  32.     quat[1] = s * (m(2, 1) - m(1, 2));
  33.     quat[2] = s * (m(0, 2) - m(2, 0));
  34.     quat[3] = s * (m(1, 0) - m(0, 1));
  35.  
  36.   } else {
  37.     i = 0;
  38.     if (m(1, 1) > m(0, 0))
  39.       i = 1;
  40.     if (m(2, 2) > m(i, i))
  41.       i = 2;
  42.     j = (i + 1)%3;
  43.     k = (i + 2)%3;
  44.  
  45.     s = sqrt(m(i, i) - (m(j, j) + m(k, k)) + 1.0);
  46.     quat[i+1] = s * 0.5;
  47.     s = 0.5 / s;
  48.     quat[0] = s * (m(k, j) - m(j, k));
  49.     quat[j+1] = s * (m(j, i) + m(i, j));
  50.     quat[k+1] = s * (m(k, i) + m(i, k));
  51.   }
  52.  
  53.   theta = 2.0 * acos(quat[0]);
  54.   AcGeVector3d tempAxis;
  55.  
  56.   if (fabs(theta) >= AcGeContext::gTol.equalVector()) {
  57.     double factor = 1.0 / sin(theta * 0.5);
  58.     tempAxis.x = quat[1] * factor;
  59.     tempAxis.y = quat[2] * factor;
  60.     tempAxis.z = quat[3] * factor;
  61.  
  62.     // Нормируем направление и угол поворота при необходимости.
  63.     //
  64.     if (tempAxis.dotProduct(axis) < 0) {
  65.       tempAxis *= -1;
  66.       theta = TWOPI - theta;
  67.     }
  68.     axis = tempAxis;
  69.   } else {
  70.     theta = 0.0;
  71.   }
  72. }

 

Источник: http://adndevblog.typepad.com/autocad/2013/01/get-the-rotation-angle-and-axis-from-acgematrix3d.html

Обсуждение: http://adn-cis.org/forum/index.php?topic=1055

Опубликовано 28.10.2014