public void TestNormalize()
{
TK.Quaternion tkQ = TK.Quaternion.FromAxisAngle(new TK.Vector3(.25f, .5f, 0.0f), TK.MathHelper.PiOver2);
Quaternion q = new Quaternion(tkQ.W, tkQ.X, tkQ.Y, tkQ.Z);
tkQ.Normalize();
q.Normalize();
TestHelper.AssertEquals(tkQ.X, tkQ.Y, tkQ.Z, tkQ.W, q, "Testing normalize");
}
public Vector4 ToAxisAngle()
{
Quaternion q = this;
if (Math.Abs(q.W) > 1f)
{
q.Normalize();
}
Vector4 result = new Vector4
{
W = 2f * ((float)Math.Acos((double)q.W))
};
float den = (float)Math.Sqrt(1.0 - (q.W * q.W));
if (den > 0.0001f)
{
result.Xyz = (Vector3)(q.Xyz / den);
return(result);
}
result.Xyz = Vector3.UnitX;
return(result);
}
/// <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 Quaternion 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
Quaternion q = new Quaternion();
double trace = 0.25 * (row0[0] + row1[1] + row2[2] + 1.0);
if (trace > 0)
{
double sq = Math.Sqrt(trace);
q.W = (float)sq;
sq = 1.0 / (4.0 * sq);
q.X = (float)((row1[2] - row2[1]) * sq);
q.Y = (float)((row2[0] - row0[2]) * sq);
q.Z = (float)((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 = (float)(0.25 * sq);
sq = 1.0 / sq;
q.W = (float)((row2[1] - row1[2]) * sq);
q.Y = (float)((row1[0] + row0[1]) * sq);
q.Z = (float)((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 = (float)(0.25 * sq);
sq = 1.0 / sq;
q.W = (float)((row2[0] - row0[2]) * sq);
q.X = (float)((row1[0] + row0[1]) * sq);
q.Z = (float)((row2[1] + row1[2]) * sq);
}
else
{
double sq = 2.0 * Math.Sqrt(1.0 + row2[2] - row0[0] - row1[1]);
q.Z = (float)(0.25 * sq);
sq = 1.0 / sq;
q.W = (float)((row1[0] - row0[1]) * sq);
q.X = (float)((row2[0] + row0[2]) * sq);
q.Y = (float)((row2[1] + row1[2]) * sq);
}
q.Normalize();
return(q);
}
