Umwandlungsroutinen

Umwandlung zwischen Rotationsmatrizen und Eulerwinkeln

Umwandlung zwischen Quaternionen und Rotationsmatrizen

Um eine Rotationsmatrix aus einem Quaternion zu erhalten, wendet man z.B. folgende Funktion an:

Matrix3D Quaternion::RotMatrix3D(
) const {
  const double x2(x*x);
  const double y2(y*y);
  const double z2(z*z);
  const double w2(w*w);
  const double xy(x*y);
  const double yz(y*z);
  const double zx(z*x);
  const double wx(w*x);
  const double wy(w*y);
  const double wz(w*z);
  return Matrix3D(
    w2+x2-y2-z2, 2.0*(xy+wz), 2.0*(zx-wy),
    2.0*(xy-wz), w2-x2+y2-z2, 2.0*(yz+wx),
    2.0*(zx+wy), 2.0*(yz-wx), w2-x2-y2+z2
  );
}

Die Umkehrung, d.h. einen Quaternion aus einer Rotationsmatrix, ist aus numerischen Gründen etwas aufwendiger:

void Quaternion::RotMatrix3D(
  const Matrix3D& r
) {
  const double d0(r[0]);
  const double d1(r[4]);
  const double d2(r[8]);
  const double xx(1.0+d0-d1-d2);
  const double yy(1.0-d0+d1-d2);
  const double zz(1.0-d0-d1+d2);
  const double ww(1.0+d0+d1+d2);

  double max(ww);
  int i(0);
  if(xx>max) max=xx,i=1;
  if(yy>max) max=yy,i=2;
  if(zz>max) i=3;

  switch(i) {
    case 0: {
      const double ws(2.0*sqrt(ww));
      x=(r[7]-r[5])/ws;
      y=(r[2]-r[6])/ws;
      z=(r[3]-r[1])/ws;
      w=ws*0.25;
      break;
    }
    case 1: {
      const double xs(2.0*qrt(xx));
      x=xs*0.25;
      y=(r[3]+r[1])/xs;
      z=(r[6]+r[2])/xs;
      w=(r[7]-r[5])/xs;
      break;
    }
    case 2: {
      const double ys(2.0*sqrt(yy));
      x=(r[3]+r[1])/ys;
      y= ys*0.25;
      z=(r[7]+r[5])/ys;
      w=(r[2]-r[6])/ys;
      break;
    }
    case 3: {
      const double zs(2.0*sqrt(zz));
      x=(r[6]+r[2])/zs;
      y=(r[7]+r[5])/zs;
      z= zs*0.25;
      w=(r[3]-r[1])/zs;
      break;
    }
  }
}

Umwandlung zwischen Quaternionen und Eulerwinkeln

Um Eulerwinkel aus einem Quaternion zu erhalten, wendet man z.B. folgende Funktion an:

Vector3 Quaternion::Euler(
) const {
  double sin_a;
  double cos_a;
  sincos(w*0.5,&sin_a,&cos_a);
  const double x_(x*sin_a);
  const double y_(y*sin_a);
  const double z_(z*sin_a);
  const double sqw(cos_a*cos_a);
  const double sqx(x_*x_);
  const double sqy(y_*y_);
  const double sqz(z_*z_);
  return Vector3(
    atan2(2.0*(x_*y_+z_*cos_a),( sqx-sqy-sqz+sqw)),
    atan2(2.0*(y_*z_+x_*cos_a),(-sqx-sqy+sqz+sqw)),
    asin(-2.0*(x_*z_-y_*cos_a))
  );
}

Rotation mit Hamilton

Der Code zur Rotation eines Punktes $p$ mit einem entsprechenden Quaternion und der hamiltonschen Formel sieht wie folgt aus:

Vector3D Quaternion::Hamilton(
  const Vector3D& p
) const {
  const double Vx(p[0]);
  const double Vy(p[1]);
  const double Vz(p[2]);
  const double Q1( x*Vx+y*Vy+z*Vz);
  const double Q2( z*Vx+w*Vy-x*Vz);
  const double Q3( w*Vx-z*Vy+y*Vz);
  const double Q4(-y*Vx+x*Vy+w*Vz);
  return Vector3D(
    Q3*w+Q1*x+Q4*y-Q2*z,
    Q2*w-Q4*x+Q1*y+Q3*z,
    Q4*w+Q2*x-Q3*y+Q1*z
  );
}