public static fpquaternion FromToRotation(fp3 from, fp3 to) { var m = new fpmatrix3x3(); m.SetFromToRotation(from, to); var q = fpquaternion.zero; VecMath.MatrixToQuaternion(m, ref q); return(q); }
public static void MatrixToQuaternion(fpmatrix3x3 kRot, ref fpquaternion q) { // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes // article "Quaternionf Calculus and Fast Animation". var fTrace = kRot.Get(0, 0) + kRot.Get(1, 1) + kRot.Get(2, 2); fp fRoot; if (fTrace > 0.0f) { // |w| > 1/2, may as well choose w > 1/2 fRoot = fpmath.sqrt(fTrace + 1.0f); // 2w q.w = 0.5f * fRoot; fRoot = 0.5f / fRoot; // 1/(4w) q.x = (kRot.Get(2, 1) - kRot.Get(1, 2)) * fRoot; q.y = (kRot.Get(0, 2) - kRot.Get(2, 0)) * fRoot; q.z = (kRot.Get(1, 0) - kRot.Get(0, 1)) * fRoot; } else { // |w| <= 1/2 var sINext = new Vector3Int(1, 2, 0); var i = 0; if (kRot.Get(1, 1) > kRot.Get(0, 0)) { i = 1; } if (kRot.Get(2, 2) > kRot.Get(i, i)) { i = 2; } var j = sINext[i]; var k = sINext[j]; fRoot = fpmath.sqrt(kRot.Get(i, i) - kRot.Get(j, j) - kRot.Get(k, k) + 1.0f); var apkQuat = new fp3(q.x, q.y, q.z); apkQuat[i] = 0.5f * fRoot; fRoot = 0.5f / fRoot; q.w = (kRot.Get(k, j) - kRot.Get(j, k)) * fRoot; apkQuat[j] = (kRot.Get(j, i) + kRot.Get(i, j)) * fRoot; apkQuat[k] = (kRot.Get(k, i) + kRot.Get(i, k)) * fRoot; q.x = apkQuat[0]; q.y = apkQuat[1]; q.z = apkQuat[2]; } q = q.normalized; }