public static Quaternion GetQuaternion(MatrixByArr mat, double scale)
{
// [a b c] [q0*q0 + q1*q1 - q2*q2 - q3*q3, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2) ]
// s*[d e f] = [2*(q1*q2 + q0*q3), q0*q0 + q2*q2 - q1*q1 - q3*q3, 2*(q2*q3 - q0*q1) ]
// [g h i] [2*(q1*q3 - q0*q2), 2*(q2*q3 + q0*q1), q0*q0 + q3*q3 - q1*q1 - q2*q2]
//
// q0*q0 + q1*q1 + q2*q2 + q3*q3 = 1
// s(a+e) = 2*(q0*q0 - q3*q3)
// s(a+i) = 2*(q0*q0 - q2*q2)
// s(e+i) = 2*(q0*q0 - q1*q1)
// s(d+b) = 4 * q1*q2
// s(d-b) = 4 * q0*q3
// s(c+g) = 4 * q1*q3
// s(c-g) = 4 * q0*q2
// s(h+f) = 4 * q2*q3
// s(h-f) = 4 * q0*q1
//
// if we assume 's == 1',
// then (a+e+i) = 3*q0*q0 - (q1*q1 + q2*q2 + q3*q3) = 3*q0*q0 - (1-q0*q0) = 4*q0*q0 - 1
// q0 = sqrt((a + e + i + 4)/4)
// q1 = (h - f)/(4 * q0)
// q2 = (c - g)/(4 * q0)
// q3 = (d - b)/(4 * q0)
HDebug.Assert(scale == 1);
HDebug.Assert(mat.ColSize == 3, mat.RowSize == 3);
double q0 = Math.Sqrt((mat[0, 0] + mat[1, 1] + mat[2, 2] + 4) / 4);
HDebug.Assert(double.IsNaN(q0) == false);
HDebug.Assert(q0 != 0);
double q1 = (mat[2, 1] - mat[1, 2]) / (4 * q0);
double q2 = (mat[0, 2] - mat[2, 0]) / (4 * q0);
double q3 = (mat[1, 0] - mat[0, 1]) / (4 * q0);
HDebug.AssertSimilar((mat[0, 0] + mat[1, 1]), 2 * (q0 * q0 - q3 * q3), 0.001);
HDebug.AssertSimilar((mat[0, 0] + mat[2, 2]), 2 * (q0 * q0 - q2 * q2), 0.001);
HDebug.AssertSimilar((mat[1, 1] + mat[2, 2]), 2 * (q0 * q0 - q1 * q1), 0.001);
HDebug.AssertSimilar((mat[1, 0] + mat[0, 1]), (4 * q1 * q2), 0.001);
HDebug.AssertSimilar((mat[1, 0] - mat[0, 1]), (4 * q0 * q3), 0.001);
HDebug.AssertSimilar((mat[0, 2] + mat[2, 0]), (4 * q1 * q3), 0.001);
HDebug.AssertSimilar((mat[0, 2] - mat[2, 0]), (4 * q0 * q2), 0.001);
HDebug.AssertSimilar((mat[2, 1] + mat[1, 2]), (4 * q2 * q3), 0.001);
HDebug.AssertSimilar((mat[2, 1] - mat[1, 2]), (4 * q0 * q1), 0.001);
Quaternion quater = new Quaternion(q0, q1, q2, q3);
if (HDebug.IsDebuggerAttached)
{
MatrixByArr rotmat = quater.RotationMatrix;
const double tolerance = 0.001;
HDebug.Assert(Math.Abs(rotmat[0, 0] - mat[0, 0]) < tolerance);
HDebug.Assert(Math.Abs(rotmat[0, 1] - mat[0, 1]) < tolerance);
HDebug.Assert(Math.Abs(rotmat[0, 2] - mat[0, 2]) < tolerance);
HDebug.Assert(Math.Abs(rotmat[1, 0] - mat[1, 0]) < tolerance);
HDebug.Assert(Math.Abs(rotmat[1, 1] - mat[1, 1]) < tolerance);
HDebug.Assert(Math.Abs(rotmat[1, 2] - mat[1, 2]) < tolerance);
HDebug.Assert(Math.Abs(rotmat[2, 0] - mat[2, 0]) < tolerance);
HDebug.Assert(Math.Abs(rotmat[2, 1] - mat[2, 1]) < tolerance);
HDebug.Assert(Math.Abs(rotmat[2, 2] - mat[2, 2]) < tolerance);
}
return(quater);
//////////////////////////////////////////////////////////////////////////
// (sqrt(3)*q0 + q1 + q2 + q3) * (sqrt(3)*q0 - q1 - q2 - q3)
// = (3*q0*q0 - q1*q1 - q2*q2 - q3*q3) - 2*(q1*q2 + q2*q3 + q1*q3)
// = (a+e+i) - (d+b + c+g + h+f)/2
//
// (q0 - q1 + q2 - q3) * (a0 - q1 - q2 + q3)
// = (q0*q0 + q1*q1 - q2*q2 - q3*q3) + 2*(q0*q1 + q2*q3)
// = a + h/2
}
