Want more features on Pastebin? Sign Up, it's FREE!
Guest

Untitled

By: a guest on Aug 25th, 2012  |  syntax: C#  |  size: 4.57 KB  |  views: 255  |  expires: Never
download  |  raw  |  embed  |  report abuse  |  print
Text below is selected. Please press Ctrl+C to copy to your clipboard. (⌘+C on Mac)
  1. using UnityEngine;
  2. using System.Collections;
  3. using System;
  4.  
  5. public class NewBehaviourScript : MonoBehaviour
  6. {
  7.         public Transform t1, t2, t3;
  8.         // Use this for initialization
  9.         void Start ()
  10.         {
  11.                 Do (new Vector3 (50, 0, 90));
  12.                
  13.                 Do (new Vector3 (0, 50, 90));
  14.                
  15.                 Do (new Vector3 (90, 0, 50));
  16.                
  17.                 Do (new Vector3 (90, 50, 0));
  18.                
  19.                 Do (new Vector3 (50, 90, 0));
  20.                 Do (new Vector3 (0, 90, 50));
  21.         }
  22.        
  23.         void Do (Vector3 eulers)
  24.         {
  25.                 Debug.Log ("Eulers: " + eulers);
  26.                 //ChangeQ(ToQ (eulers), (q)=>Debug.Log(q + ": " + FromQ(q)));
  27.                 //return;// (w0.5, z-0.4, x0.6, y0.4):
  28.                 Debug.Log (".Euler: " + Quaternion.Euler (eulers)); // (0.6, 0.4, -0.4, 0.5)
  29.                 Debug.Log ("ToQ: " + ToQ (eulers));              // (0.5, -0.4, 0.2, 0.7)
  30.                 Debug.Log ("FromQ: " + FromQ (ToQ (eulers)));       // (55.0, 100.0, -11.0)
  31.                 Debug.Log ("FromQ2: " + FromQ2 (ToQ (eulers)));      // (-55.5, -6.3, 71.0)
  32.                 Debug.Log (".Euler.eulerAngles: " + Quaternion.Euler (eulers).eulerAngles); // (80.0, 235.0, 169.0)
  33.                 Debug.Log ("FromQ2(,Euler): " + FromQ2 (Quaternion.Euler (eulers)));     // (65.8, 1.9, 99.8)
  34.                 Debug.Log ("ToQ.eulerAngles: " + ToQ (eulers).eulerAngles);              // (70.0, 286.9, 341.4)
  35.                 Debug.Log ("FromQ(,Euler): " + FromQ (Quaternion.Euler (eulers)));      // (-65.8, 76.0, 4.6)  
  36.                 Debug.Log ("--");
  37.                
  38.         }
  39.        
  40.         void ChangeQ (Quaternion q, Action<Quaternion> f)
  41.         {
  42.                 for (int i1=0; i1<=3; i1++) {
  43.                         for (int i2=0; i2<=3; i2++) {
  44.                                 for (int i3=0; i3<=3; i3++) {
  45.                                         for (int i4=0; i4<=3; i4++) {
  46.                                                 f (new Quaternion (q [i1], q [i2], q [i3], q [i4]));
  47.                                         }      
  48.                                 }      
  49.                         }
  50.                 }
  51.         }
  52.        
  53.         public static Quaternion ToQ (Vector3 v)
  54.         {
  55.                 return ToQ (v.y, v.x, v.z);
  56.         }
  57.  
  58.         public static Vector3 FromQ (Quaternion q2)
  59.         {
  60.                 Quaternion q = new Quaternion (q2.w, q2.z, q2.x, q2.y);
  61.                 Vector3 pitchYawRoll;
  62.                 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
  63.                 pitchYawRoll.x = (float)Math.Asin (2f * (q.x * q.z - q.w * q.y));                             // Pitch
  64.                 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
  65.                 return new Vector3 (pitchYawRoll.x * Mathf.Rad2Deg, pitchYawRoll.y * Mathf.Rad2Deg, pitchYawRoll.z * Mathf.Rad2Deg);
  66.         }
  67.  
  68.         public static Quaternion ToQ (float yaw, float pitch, float roll)
  69.         {
  70.                 yaw *= Mathf.Deg2Rad;
  71.                 pitch *= Mathf.Deg2Rad;
  72.                 roll *= Mathf.Deg2Rad;
  73.                 float rollOver2 = roll * 0.5f;
  74.                 float sinRollOver2 = (float)Math.Sin ((double)rollOver2);
  75.                 float cosRollOver2 = (float)Math.Cos ((double)rollOver2);
  76.                 float pitchOver2 = pitch * 0.5f;
  77.                 float sinPitchOver2 = (float)Math.Sin ((double)pitchOver2);
  78.                 float cosPitchOver2 = (float)Math.Cos ((double)pitchOver2);
  79.                 float yawOver2 = yaw * 0.5f;
  80.                 float sinYawOver2 = (float)Math.Sin ((double)yawOver2);
  81.                 float cosYawOver2 = (float)Math.Cos ((double)yawOver2);
  82.                 Quaternion result;
  83.                 result.w = cosYawOver2 * cosPitchOver2 * cosRollOver2 + sinYawOver2 * sinPitchOver2 * sinRollOver2;
  84.                 result.x = cosYawOver2 * sinPitchOver2 * cosRollOver2 + sinYawOver2 * cosPitchOver2 * sinRollOver2;
  85.                 result.y = sinYawOver2 * cosPitchOver2 * cosRollOver2 - cosYawOver2 * sinPitchOver2 * sinRollOver2;
  86.                 result.z = cosYawOver2 * cosPitchOver2 * sinRollOver2 - sinYawOver2 * sinPitchOver2 * cosRollOver2;
  87.  
  88.                 return result;
  89.         }
  90.  
  91.         public static Vector3 FromQ2 (Quaternion q1)
  92.         {
  93.                 float sqw = q1.w * q1.w;
  94.                 float sqx = q1.x * q1.x;
  95.                 float sqy = q1.y * q1.y;
  96.                 float sqz = q1.z * q1.z;
  97.                 float unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
  98.                 float test = q1.x * q1.y + q1.z * q1.w;
  99.                 Vector3 v;
  100.                 Debug.Log ("u: " + unit + ", test: " + test + ", test2: " + (q1.x * q1.z + q1.y * q1.w)
  101.                         + ", test3: " + (q1.x * q1.w + q1.z * q1.y));
  102.                 if (test > 0.4999 * unit) { // singularity at north pole
  103.                         Debug.Log ("singularity1");            
  104.                         v.y = 2 * Mathf.Atan2 (q1.x, q1.w);
  105.                         v.x = Mathf.PI / 2;
  106.                         v.z = 0;
  107.                         return v * Mathf.Rad2Deg;
  108.                 }
  109.                 if (test < -0.4999 * unit) { // singularity at south pole
  110.                         Debug.Log ("singularity2");
  111.                         v.y = -2 * Mathf.Atan2 (q1.x, q1.w);
  112.                         v.x = -Mathf.PI / 2;
  113.                         v.z = 0;
  114.                         return v * Mathf.Rad2Deg;
  115.                 }
  116.                 Quaternion q = new Quaternion (q1.w, q1.z, q1.x, q1.y);
  117.                 v.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
  118.                 v.x = (float)Math.Asin (2f * (q.x * q.z - q.w * q.y));                             // Pitch
  119.                 v.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
  120.                 return v * Mathf.Rad2Deg;
  121.         }
  122.        
  123. }
clone this paste RAW Paste Data