Ugly problem in GB32 (Quaternions)

[Roger Cabo]

I have created a Unity3D Code to calculate with Quaternion = Rotations.

This is the Code in C# for Unity3d, works perfekt!. The C# code does all the terms manually.. not any use of system functions in Unity!


   // Struktur für eine 4x4-Matrix
   public struct Matrix
   {
       public float m0, m1, m2, m3;
       public float m4, m5, m6, m7;
       public float m8, m9, m10, m11;
       public float m12, m13, m14, m15;
   }

   Vector3 er = new Vector3(100,0,0); // Euler in Grad

   void Start()
   {
       Debug.Log("unity quaterions       " + transform.rotation.ToString("F10"));
       Debug.Log("WorldPos des Objektes: " + this.transform.position.ToString("F10"));
       Vector4 quat = new Vector4(0.4383555000f, 0.1923714000f, 0.2482790000f, 0.8421373000f);
       Debug.Log("QuaternionToVector3 Position : " + QuaternionToVector3(quat, er));
       //Debug.Log("-----------------------------");
   }

   Vector4 EulerToQuaternionD(Vector3 euler) // 100% correct!
   {
       // Umwandlung von Grad in Radiant
       euler *= Mathf.Deg2Rad * 0.5f;

       // Berechne Sinus und Kosinus nur einmal
       double cy = Math.Cos(euler.z);
       double sy = Math.Sin(euler.z);
       double cp = Math.Cos(euler.y);
       double sp = Math.Sin(euler.y);
       double cr = Math.Cos(euler.x);
       double sr = Math.Sin(euler.x);

       // Berechne Quaternion-Komponenten (vertauschte x und y)
       double qy = cr * sp * cy - sr * cp * sy;
       double qx = sr * cp * cy + cr * sp * sy;
       double qz = cr * cp * sy - sr * sp * cy;
       double qw = cr * cp * cy + sr * sp * sy;

       return new Vector4((float)qx, (float)qy, (float)qz, (float)qw);
   }

   public Matrix QuaternionToMatrix(Vector4 q)  // 100% correct!
   {
       Matrix m = new Matrix();
       
       m.m0 = 1 - 2 * q.y * q.y - 2 * q.z * q.z;
       m.m1 = 2 * q.x * q.y - 2 * q.z * q.w;
       m.m2 = 2 * q.x * q.z + 2 * q.y * q.w;
       m.m3 = 0;

       m.m4 = 2 * q.x * q.y + 2 * q.z * q.w;
       m.m5 = 1 - 2 * q.x * q.x - 2 * q.z * q.z;
       m.m6 = 2 * q.y * q.z - 2 * q.x * q.w;
       m.m7 = 0;

       m.m8 = 2 * q.x * q.z - 2 * q.y * q.w;
       m.m9 = 2 * q.y * q.z + 2 * q.x * q.w;
       m.m10 = 1 - 2 * q.x * q.x - 2 * q.y * q.y;
       m.m11 = 0;

       m.m12 = 0;
       m.m13 = 0;
       m.m14 = 0;
       m.m15 = 1;

       return m;
   }
   public Vector3 QuaternionToVector3(Vector4 q, Vector3 position)  // 100% correct!
   {
       Matrix m = QuaternionToMatrix(q);

       Vector3 result;
       result.x = m.m0 * position.x + m.m1 * position.y + m.m2 * position.z;
       result.y = m.m4 * position.x + m.m5 * position.y + m.m6 * position.z;
       result.z = m.m8 * position.x + m.m9 * position.y + m.m10 * position.z;

       return result;
   }

Unity Rotation (0.4383555000, 0.1923715000, 0.2482790000, 0.8421373000)
WorldPos des Objektes in UNITY: (80.2701600000, 58.6824200000, -10.6337500000)
C#Code Position result             : (80.2701600000, 58.6824100000, -10.6337300000)

___________________________

The gb32 code calculate this:
t1.rotation : (0.438355592215455 0.192371423458858, 0.248278949007736, 0.842137265348984)  // works perfekt!!
Code Position result                 : (-24.9515731810915, 49.2403876506104, 83.3834709028489)        // but the result  

So far I'm sure to converted the code from c# to gb32 correctly I hope.
Note: Quaternion is the same as Rotation in Unity!

I really appreciate help!

Function I_EulerToQuaternionD(ByRef euler As Vector3) As Quaternion
 
 // Umwandlung von Grad in Radiant
 
 //euler.x *= (Mathf.PI / 180) * 0.5;
 //euler.y *= (Mathf.PI / 180) * 0.5;
 //euler.z *= (Mathf.PI / 180) * 0.5;
 
 Dim e As Vector3
 e = euler
 
 e.x *=  0.00872664625997165
 e.y *=  0.00872664625997165
 e.z *=  0.00872664625997165
 
 // Berechne Sinus und Kosinus nur einmal
 Dim cy# = Cos(e.z)
 Dim sy# = Sin(e.z)
 Dim cp# = Cos(e.y)
 Dim sp# = Sin(e.y)
 Dim cr# = Cos(e.x)
 Dim sr# = Sin(e.x)
 
 // Berechne Quaternion-Komponenten (vertauschte x und y)
 Dim q As Quaternion
 
 q.y = (cr * sp * cy) - (sr * cp * sy)
 q.x = (sr * cp * cy) + (cr * sp * sy)
 q.z = (cr * cp * sy) - (sr * sp * cy)
 q.w = (cr * cp * cy) + (sr * sp * sy)
 
 Return q
End Function

Function I_QuaternionToMatrix(ByRef q As Quaternion) As m_Matrix
 Dim m As m_Matrix
 
 m.m0 = 1 - (2 * q.y * q.y) - (2 * q.z * q.z)
 m.m1 = (2 * q.x * q.y) - (2 * q.z * q.w)
 m.m2 = (2 * q.x * q.z) + (2 * q.y * q.w)
 m.m3 = 0
 
 m.m4 = (2 * q.x * q.y) + (2 * q.z * q.w)
 m.m5 = 1 - (2 * q.x * q.x) - (2 * q.z * q.z)
 m.m6 = (2 * q.y * q.z) - (2 * q.x * q.w)
 m.m7 = 0
 
 m.m8 = (2 * q.x * q.z) - (2 * q.y * q.w)
 m.m9 = (2 * q.y * q.z) + (2 * q.x * q.w)
 m.m10 = 1 - (2 * q.x * q.x) - (2 * q.y * q.y)
 m.m11 = 0
 
 m.m12 = 0
 m.m13 = 0
 m.m14 = 0
 m.m15 = 1
 
 Return m
End Function

Function I_QuaternionToVector3(ByRef q As Quaternion, ByRef v As Vector3) As Vector3
 
 Dim result As Vector3
 
 ' Umwandeln des Quaternions in eine Rotationsmatrix
 Dim m As m_Matrix
 m = I_QuaternionToMatrix(q)
 
 ' Anwenden der Rotationsmatrix auf den Vektor v
 result.x = (m.m0 * v.x) + (m.m1 * v.y) + (m.m2 * v.z)
 result.y = (m.m4 * v.x) + (m.m5 * v.y) + (m.m6 * v.z)
 result.z = (m.m8 * v.x) + (m.m9 * v.y) + (m.m10 * v.z)
 
 Return result
End Function

Perhaps anyone have a hint what happen here!
Thank you!

Quaternion_2.G32 (5.05 KB)

[(X)]

A 'slip' of the keyboard?

In original code:

Vector3 er = new Vector3(100,0,0); // Euler in Grad

In GFA code, seems to have been transcribed as:

t2.position = NEW_Vector3(0, 100, 0)

Debug result with (100,0,0)...

t1.rotation = I_EulerToQuaternionD(t1.eulerAngles)

t1.position        :  ( 0 0 0)
t1.EulerAngles     :  ( 40 45 50)
t1.rotation        :  ( 0.438355592215455 0.192371423458858 0.248278949007736 0.842137265348984)

t2 = I_QuaternionToVector3m(t1.rotation, t2)

t2.position        :  ( 80.2701597832053 58.6824088833465 -10.6337357512034)
t2.EulerAngles     :  ( 0 0 0)
t2.rotation        :  ( 0 0 0 1)

        Should be!!: (80.2701600000,     58.6824200000, -10.6337500000)

[Roger Cabo]

Yes!!!!   I think I become old!

Thanks a lot!