Помогаем работать и общаться.

Euler <=> Quaternion

Публично отправил Влад в 01:30 23-08-2012 с типом csharp и размером 3.25 Kb
Хранить: Вечно, просмотров: 345
В буфер! | Скачать!
  1. using UnityEngine;
  2. using System;
  3. using System.Collections;
  4.  
  5. public class eulerToquat : MonoBehaviour {
  6.  
  7.         void Start () {
  8.                 Vector3 eulers = new Vector3(100,55,-11);
  9.                 Debug.Log(Quaternion.Euler(eulers)); // (0.6, 0.4, -0.4, 0.5)
  10.                 Debug.Log(ToQ(eulers));              // (0.5, -0.4, 0.2, 0.7)
  11.                 Debug.Log(FromQ(ToQ(eulers)));       // (55.0, 100.0, -11.0)
  12.                 Debug.Log(FromQ2(ToQ(eulers)));      // (-55.5, -6.3, 71.0)
  13.                 Debug.Log(Quaternion.Euler(eulers).eulerAngles); // (80.0, 235.0, 169.0)
  14.                 Debug.Log(FromQ2(Quaternion.Euler(eulers)));     // (65.8, 1.9, 99.8)
  15.                 Debug.Log(ToQ(eulers).eulerAngles);              // (70.0, 286.9, 341.4)
  16.                 Debug.Log(FromQ(Quaternion.Euler(eulers)));      // (-65.8, 76.0, 4.6) 
  17.                
  18.         }
  19.        
  20.         public static Vector3 FromQ(Quaternion q)
  21.         {
  22.                 Vector3 pitchYawRoll;
  23.                 pitchYawRoll.y = (float)Math.Atan2(2f * q.x * q.w + 2f * q.y * q.z, 1 - 2f * (q.z*q.z  + q.w*q.w));     // Yaw
  24.                 pitchYawRoll.x = (float)Math.Asin(2f * ( q.x * q.z - q.w * q.y ) );                             // Pitch
  25.                 pitchYawRoll.z = (float)Math.Atan2(2f * q.x * q.y + 2f * q.z * q.w, 1 - 2f * (q.y*q.y + q.z*q.z));      // Roll
  26.                 return new Vector3(pitchYawRoll.x*Mathf.Rad2Deg,pitchYawRoll.y*Mathf.Rad2Deg,pitchYawRoll.z*Mathf.Rad2Deg);
  27.         }
  28.                
  29.         public static Quaternion ToQ(Vector3 v)
  30.         {
  31.                 return ToQ(v.x,v.y,v.z);
  32.         }
  33.        
  34.         public static Quaternion ToQ(float yaw, float pitch, float roll)
  35.         {
  36.                 yaw*=Mathf.Deg2Rad;
  37.                 pitch*=Mathf.Deg2Rad;
  38.                 roll*=Mathf.Deg2Rad;
  39.             float rollOver2 = roll * 0.5f;
  40.             float sinRollOver2 = (float)Math.Sin((double)rollOver2);
  41.             float cosRollOver2 = (float)Math.Cos((double)rollOver2);
  42.             float pitchOver2 = pitch * 0.5f;
  43.             float sinPitchOver2 = (float)Math.Sin((double)pitchOver2);
  44.             float cosPitchOver2 = (float)Math.Cos((double)pitchOver2);
  45.             float yawOver2 = yaw * 0.5f;
  46.             float sinYawOver2 = (float)Math.Sin((double)yawOver2);
  47.             float cosYawOver2 = (float)Math.Cos((double)yawOver2);
  48.             Quaternion result;
  49.             result.x = cosYawOver2 * cosPitchOver2 * cosRollOver2 + sinYawOver2 * sinPitchOver2 * sinRollOver2;
  50.             result.y = cosYawOver2 * cosPitchOver2 * sinRollOver2 - sinYawOver2 * sinPitchOver2 * cosRollOver2;
  51.             result.z = cosYawOver2 * sinPitchOver2 * cosRollOver2 + sinYawOver2 * cosPitchOver2 * sinRollOver2;
  52.             result.w = sinYawOver2 * cosPitchOver2 * cosRollOver2 - cosYawOver2 * sinPitchOver2 * sinRollOver2;
  53.             return result;
  54.         }
  55.        
  56.                
  57.         public static Vector3 FromQ2(Quaternion q1) {
  58.             float sqw = q1.w*q1.w;
  59.             float sqx = q1.x*q1.x;
  60.             float sqy = q1.y*q1.y;
  61.             float sqz = q1.z*q1.z;
  62.                 float unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
  63.                 float test = q1.x*q1.y + q1.z*q1.w;
  64.                 Vector3 v;
  65.                 if (test > 0.499*unit) { // singularity at north pole
  66.                         v.x = 2 * Mathf.Atan2(q1.x,q1.w);
  67.                         v.y = Mathf.PI/2;
  68.                         v.z = 0;
  69.                         return v*Mathf.Rad2Deg;
  70.                 }
  71.                 if (test < -0.499*unit) { // singularity at south pole
  72.                         v.x = -2 * Mathf.Atan2(q1.x,q1.w);
  73.                         v.y = -Mathf.PI/2;
  74.                         v.z = 0;
  75.                         return v*Mathf.Rad2Deg;
  76.                 }
  77.             v.x = Mathf.Atan2(2*q1.y*q1.w-2*q1.x*q1.z , sqx - sqy - sqz + sqw);
  78.                 v.y = Mathf.Asin(2*test/unit);
  79.                 v.z = Mathf.Atan2(2*q1.x*q1.w-2*q1.y*q1.z , -sqx + sqy - sqz + sqw);
  80.                 return v*Mathf.Rad2Deg;
  81.         }
  82. }
  83.  
  84.  

Комментируй
Исходный текст