Ejemplos de Quatd::magnitude en C++ (Cpp)

Ejemplo n.º 1

Archivo: Operatorsd.cpp Proyecto: PositronicsLab/Ravelin

Matrix3d to_Matrix3d(const Quatd& q)
{
  const unsigned X = 0, Y = 1, Z = 2;
 
  // verify that the quaternion is normalized
  assert(std::fabs(q.magnitude()) - 1 < EPS_FLOAT);

  // setup repeated products
  const double xx = q.x*q.x;
  const double xy = q.x*q.y;
  const double xz = q.x*q.z;
  const double xw = q.x*q.w;
  const double yy = q.y*q.y;
  const double yz = q.y*q.z;
  const double yw = q.y*q.w;
  const double zz = q.z*q.z;
  const double zw = q.z*q.w; 
  const double ww = q.w*q.w;

  Matrix3d m;
  m(X,X) = 2*(xx + ww) - 1;
  m(X,Y) = 2*(xy - zw);
  m(X,Z) = 2*(xz + yw);
  m(Y,X) = 2*(xy + zw);
  m(Y,Y) = 2*(yy + ww) - 1;
  m(Y,Z) = 2*(yz - xw);
  m(Z,X) = 2*(xz - yw);
  m(Z,Y) = 2*(yz + xw);
  m(Z,Z) = 2*(zz + ww) - 1;
  return m;
}