예제 #1
0
 public Quaternion(Transform t)
 {
     Matrix4x4 m = t.M;
     float trace = m.m[0, 0] + m.m[1, 1] + m.m[2, 2];
     if (trace > 0.0f)
     {
         // Compute w from matrix trace, then xyz
         // 4w^2 = m[0,0] + m[1,1] + m[2,2] + m[3,3] (but m[3,3] == 1)
         float s = (float)Math.Sqrt(trace + 1.0f);
         w = s / 2.0f;
         s = 0.5f / s;
         v.x = (m.m[2, 1] - m.m[1, 2]) * s;
         v.y = (m.m[0, 2] - m.m[2, 0]) * s;
         v.z = (m.m[1, 0] - m.m[0, 1]) * s;
     }
     else
     {
         // Compute largest of $x$, $y$, or $z$, then remaining components
         int[] nxt = { 1, 2, 0 };
         float[] q = new float[3];
         int i = 0;
         if (m.m[1, 1] > m.m[0, 0]) i = 1;
         if (m.m[2, 2] > m.m[i, i]) i = 2;
         int j = nxt[i];
         int k = nxt[j];
         float s = (float)Math.Sqrt((m.m[i, i] - (m.m[j, j] + m.m[k, k])) + 1.0);
         q[i] = s * 0.5f;
         if (s != 0.0f) s = 0.5f / s;
         w = (m.m[k, j] - m.m[j, k]) * s;
         q[j] = (m.m[j, i] + m.m[i, j]) * s;
         q[k] = (m.m[k, i] + m.m[i, k]) * s;
         v.x = q[0];
         v.y = q[1];
         v.z = q[2];
     }
 }
예제 #2
0
 public virtual Transform Transpose(Transform t)
 {
     return new Transform(Transpose(t.m), Transpose(t.mInv));
 }
예제 #3
0
 public virtual Transform Inverse(Transform t)
 {
     return new Transform(t.mInv, t.m);
 }