|
public static Normalize ( q ) : |
||
| q | The Quaterniond to normalize | |
| return | ||
/// <summary>Constructs left matrix from the given quaternion.</summary>
/// <param name="quaternion">The quaternion to use to construct the martix.</param>
public Matrix3d(Quaterniond quaternion)
{
quaternion.Normalize();
double xx = quaternion.X * quaternion.X;
double yy = quaternion.Y * quaternion.Y;
double zz = quaternion.Z * quaternion.Z;
double xy = quaternion.X * quaternion.Y;
double xz = quaternion.X * quaternion.Z;
double yz = quaternion.Y * quaternion.Z;
double wx = quaternion.W * quaternion.X;
double wy = quaternion.W * quaternion.Y;
double wz = quaternion.W * quaternion.Z;
R0C0 = 1 - 2 * (yy + zz);
R0C1 = 2 * (xy - wz);
R0C2 = 2 * (xz + wy);
R1C0 = 2 * (xy + wz);
R1C1 = 1 - 2 * (xx + zz);
R1C2 = 2 * (yz - wx);
R2C0 = 2 * (xz - wy);
R2C1 = 2 * (yz + wx);
R2C2 = 1 - 2 * (xx + yy);
}
/// <summary>Constructs left matrix from the given quaternion.</summary>
/// <param name="quaternion">The quaternion to use to construct the martix.</param>
public Matrix3(Quaterniond quaternion)
{
quaternion.Normalize();
float xx = (float)(quaternion.X * quaternion.X);
float yy = (float)(quaternion.Y * quaternion.Y);
float zz = (float)(quaternion.Z * quaternion.Z);
float xy = (float)(quaternion.X * quaternion.Y);
float xz = (float)(quaternion.X * quaternion.Z);
float yz = (float)(quaternion.Y * quaternion.Z);
float wx = (float)(quaternion.W * quaternion.X);
float wy = (float)(quaternion.W * quaternion.Y);
float wz = (float)(quaternion.W * quaternion.Z);
R0C0 = 1 - 2 * (yy + zz);
R0C1 = 2 * (xy - wz);
R0C2 = 2 * (xz + wy);
R1C0 = 2 * (xy + wz);
R1C1 = 1 - 2 * (xx + zz);
R1C2 = 2 * (yz - wx);
R2C0 = 2 * (xz - wy);
R2C1 = 2 * (yz + wx);
R2C2 = 1 - 2 * (xx + yy);
}
/// <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);
}