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;
}
