private void polarize(Svd<double> DG_SVD, out Vector3d RotationAxis, out double RotationAngle)
{
DenseMatrix RotationalMatrix = (DenseMatrix)DG_SVD.U.Multiply(DG_SVD.VT);
Quaterniond RotationQ = OpenTK_To_MathNET.MatrixToQuaternion(RotationalMatrix);
RotationQ.Normalize();
RotationQ.ToAxisAngle(out RotationAxis, out RotationAngle);
}
/// <summary>
/// Returns the rotation component of this instance. Quite slow.
/// </summary>
/// <param name="row_normalise">Whether the method should row-normalise (i.e. remove scale from) the Matrix. Pass false if you know it's already normalised.</param>
public Quaterniond ExtractRotation(bool row_normalise = true)
{
var row0 = Row0;
var row1 = Row1;
var row2 = Row2;
if (row_normalise)
{
row0 = row0.Normalized();
row1 = row1.Normalized();
row2 = row2.Normalized();
}
// code below adapted from Blender
Quaterniond q = new Quaterniond();
double trace = 0.25 * (row0[0] + row1[1] + row2[2] + 1.0);
if (trace > 0)
{
double sq = Math.Sqrt(trace);
q.W = sq;
sq = 1.0 / (4.0 * sq);
q.X = (row1[2] - row2[1]) * sq;
q.Y = (row2[0] - row0[2]) * sq;
q.Z = (row0[1] - row1[0]) * sq;
}
else if (row0[0] > row1[1] && row0[0] > row2[2])
{
double sq = 2.0 * Math.Sqrt(1.0 + row0[0] - row1[1] - row2[2]);
q.X = 0.25 * sq;
sq = 1.0 / sq;
q.W = (row2[1] - row1[2]) * sq;
q.Y = (row1[0] + row0[1]) * sq;
q.Z = (row2[0] + row0[2]) * sq;
}
else if (row1[1] > row2[2])
{
double sq = 2.0 * Math.Sqrt(1.0 + row1[1] - row0[0] - row2[2]);
q.Y = 0.25 * sq;
sq = 1.0 / sq;
q.W = (row2[0] - row0[2]) * sq;
q.X = (row1[0] + row0[1]) * sq;
q.Z = (row2[1] + row1[2]) * sq;
}
else
{
double sq = 2.0 * Math.Sqrt(1.0 + row2[2] - row0[0] - row1[1]);
q.Z = 0.25 * sq;
sq = 1.0 / sq;
q.W = (row1[0] - row0[1]) * sq;
q.X = (row2[0] + row0[2]) * sq;
q.Y = (row2[1] + row1[2]) * sq;
}
q.Normalize();
return(q);
}