Сообщество программистов Autodesk в СНГ :: Получение угла поворота и оси из матрицы AcGeMatrix3d

Сообщество программистов Autodesk в СНГ
ObjectARX
Получение угла поворота и оси из матрицы AcGeMatrix3d

28/10/2014

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

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

Код - C++: [Выделить]

#define TWOPI 6.28318530718
void getAngleAndAxis(
           double& theta,
           AcGeVector3d& axis,
           const AcGeMatrix3d& m)
{
  // Контракт:  Матрица ‘m’ не должна содержать никакого масштабирования,
  // сдвига и отзеркаливания (т.е., m.det() должен быть равен 1.0).
  // Можете выполнить AcGeScale3d::removeScale( m ) чтобы обеспечить это условие
  // Если axis передано и ненулевое,
  // тогда этот метод пытается вернуть ось ориентированную также, как и переданная ось,
  // т.е., возвращаемая ось должна быть в том же полупространстве, что и переданная
  // (Заметим, что переданная ось будет перезаписана).
  //
  // Заметим что если матрица имеет зеркальный компонент,
  // ( т.е. её определитель = -1 ),  это не допустимо по контракту.
  // Чтобы понять, в чем проблема, представьте себе как при помощи только
  // поворота совместить ваши левый и правый ботинки
 
  double trace, s;
  int i, j, k;
  double quat[4];
 
  assert(fabs(m.det() -1.0) <= AcGeContext::gTol.equalVector());
 
  trace = m(0, 0) + m(1, 1) + m(2, 2);
 
  if (trace > 0.0) {
    s = sqrt(trace + 1.0);
    quat[0] = s * 0.5;
    s = 0.5 / s;
    quat[1] = s * (m(2, 1) - m(1, 2));
    quat[2] = s * (m(0, 2) - m(2, 0));
    quat[3] = s * (m(1, 0) - m(0, 1));
 
  } else {
    i = 0;
    if (m(1, 1) > m(0, 0))
      i = 1;
    if (m(2, 2) > m(i, i))
      i = 2;
    j = (i + 1)%3;
    k = (i + 2)%3;
 
    s = sqrt(m(i, i) - (m(j, j) + m(k, k)) + 1.0);
    quat[i+1] = s * 0.5;
    s = 0.5 / s;
    quat[0] = s * (m(k, j) - m(j, k));
    quat[j+1] = s * (m(j, i) + m(i, j));
    quat[k+1] = s * (m(k, i) + m(i, k));
  }
 
  theta = 2.0 * acos(quat[0]);
  AcGeVector3d tempAxis;
 
  if (fabs(theta) >= AcGeContext::gTol.equalVector()) {
    double factor = 1.0 / sin(theta * 0.5);
    tempAxis.x = quat[1] * factor;
    tempAxis.y = quat[2] * factor;
    tempAxis.z = quat[3] * factor;
 
    // Нормируем направление и угол поворота при необходимости.
    //
    if (tempAxis.dotProduct(axis) < 0) {
      tempAxis *= -1;
      theta = TWOPI - theta;
    }
    axis = tempAxis;
  } else {
    theta = 0.0;
  }
}