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];
}
}
public virtual Transform Transpose(Transform t)
{
return new Transform(Transpose(t.m), Transpose(t.mInv));
}
public virtual Transform Inverse(Transform t)
{
return new Transform(t.mInv, t.m);
}