/// <summary>
///
/// </summary>
/// <param name="matrix"></param>
public static Quaternion FromRotationMatrix(Matrix3 matrix)
{
// Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
// article "Quaternion Calculus and Fast Animation".
Quaternion result = Quaternion.Zero;
Real trace = matrix.m00 + matrix.m11 + matrix.m22;
Real root = 0.0f;
if (trace > 0.0f)
{
// |this.w| > 1/2, may as well choose this.w > 1/2
root = Utility.Sqrt(trace + 1.0f); // 2w
result.w = 0.5f * root;
root = 0.5f / root; // 1/(4w)
result.x = (matrix.m21 - matrix.m12) * root;
result.y = (matrix.m02 - matrix.m20) * root;
result.z = (matrix.m10 - matrix.m01) * root;
}
else
{
// |result.w| <= 1/2
int i = 0;
if (matrix.m11 > matrix.m00)
{
i = 1;
}
if (matrix.m22 > matrix[i, i])
{
i = 2;
}
int j = next[i];
int k = next[j];
root = Utility.Sqrt(matrix[i, i] - matrix[j, j] - matrix[k, k] + 1.0f);
unsafe
{
Real *apkQuat = &result.x;
apkQuat[i] = 0.5f * root;
root = 0.5f / root;
result.w = (matrix[k, j] - matrix[j, k]) * root;
apkQuat[j] = (matrix[j, i] + matrix[i, j]) * root;
apkQuat[k] = (matrix[k, i] + matrix[i, k]) * root;
}
}
return(result);
}
internal JavaVM(Real* real)
{
this.real = real;
functions = *(*real).functions;
}
internal JNIEnv(Real* real)
{
this.real = real;
functions = *(*real).functions;
}
