quaternion <-> Euler

我正在使用C＃，在我的程序中我收到四元數的ridigbody的四元數，但軸不對應於我使用的軸的方向，所以我想旋轉四元數。爲了做到這一點，我將四元數轉換爲歐拉角，切換音高，偏航和滾動，使其與我的座標系相關，將其轉換回四元數，然後生成一個旋轉矩陣來轉換位置。quaternion <-> Euler

但不順心的事，我只是轉換了相同的輸入，從四元數和歐拉回我再弄四元......的功能，這樣一個做錯了事，我不知道在哪裏...

Quaternion q = new Quaternion(-0.4, -0.7, -0.8, 0.5); 

double yaw = 0, pitch = 0, roll = 0; 
toEuler(q, ref yaw, ref pitch, ref roll); 

Quaternion quat = ToQ((float)(yaw), (float)(pitch), (float)(roll)); 





private void toEuler(Quaternion q, ref double x, ref double y, ref double z) 
     { 
      double test = q.X * q.Y + q.Z * q.W; 
      if (test > 0.499)       // singularity at north pole 
      { 
       y = 2.0F * System.Math.Atan2(q.X, q.W); 
       z = Math.PI/2.0F; 
       x = 0.0F; 
       return; 
      } 

      if (test < -0.499)       // singularity at south pole 
      { 
       y = -2.0F * System.Math.Atan2(q.X, q.W); 
       z = -Math.PI/2.0F; 
       x = 0.0F; 
       return; 
      } 

      double sqx = q.X * q.X; 
      double sqy = q.Y * q.Y; 
      double sqz = q.Z * q.Z; 
      y = System.Math.Atan2(2.0F * q.Y * q.W - 2.0 * q.X * q.Z, 1.0F - 2.0 * sqy - 2.0 * sqz); 
      z = System.Math.Asin(2.0F * test); 
      x = System.Math.Atan2(2.0 * q.X * q.W - 2.0 * q.Y * q.Z, 1.0F - 2.0 * sqx - 2.0 * sqz); 
     } 

public Quaternion ToQ(float yaw, float pitch, float roll) 
     { 
      float rollOver2 = roll * 0.5f; 
      float sinRollOver2 = (float)Math.Sin((double)rollOver2); 
      float cosRollOver2 = (float)Math.Cos((double)rollOver2); 
      float pitchOver2 = pitch * 0.5f; 
      float sinPitchOver2 = (float)Math.Sin((double)pitchOver2); 
      float cosPitchOver2 = (float)Math.Cos((double)pitchOver2); 
      float yawOver2 = yaw * 0.5f; 
      float sinYawOver2 = (float)Math.Sin((double)yawOver2); 
      float cosYawOver2 = (float)Math.Cos((double)yawOver2); 
      Quaternion result = new Quaternion(); 
      result.W = cosYawOver2 * cosPitchOver2 * cosRollOver2 + sinYawOver2 * sinPitchOver2 * sinRollOver2; 
      result.X = cosYawOver2 * sinPitchOver2 * cosRollOver2 + sinYawOver2 * cosPitchOver2 * sinRollOver2; 
      result.Y = sinYawOver2 * cosPitchOver2 * cosRollOver2 - cosYawOver2 * sinPitchOver2 * sinRollOver2; 
      result.Z = cosYawOver2 * cosPitchOver2 * sinRollOver2 - sinYawOver2 * sinPitchOver2 * cosRollOver2; 

      return result; 
     }

來源

2013-03-03 sven