I have a 4x4 matrix (row-major) class and a quaternion class and I'm attempting to provide conversion methods that convert between the two representations for rotations.
This is my conversion function for converting from a quaternion to a matrix, where _2 is System.Math.Pow:
/// <summary>
/// Converts the quaternion to it's matrix representation.
/// </summary>
/// <returns>A matrix representing the quaternion.</returns>
public Matrix ToMatrix()
{
return new Matrix(new double[,] {
{
_2(W) + _2(X) - _2(Y) - _2(Z),
(2 * X * Y) - (2 * W * Z),
(2 * X * Z) + (2 * W * Y),
0
},
{
(2 * X * Y) + (2 * W * Z),
_2(W) - _2(X) + _2(Y) - _2(Z),
(2 * Y * Z) + (2 * W * X),
0
},
{
(2 * X * Z) - (2 * W * Y),
(2 * Y * Z) - (2 * W * X),
_2(W) - _2(X) - _2(Y) + _2(Z),
0
},
{
0,
0,
0,
1
}
});
}
These are my two conversion functions for converting from a matrix to a quaternion. Note that they both don't work when considering X rotation.
/// <summary>
/// Converts the matrix to a quaternion assuming the matrix purely
/// represents rotation (any translation or scaling information will
/// result in an invalid quaternion).
/// </summary>
/// <returns>A quaternion representing the rotation.</returns>
public Quaternion ToQuaternion()
{
/* http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
*/
double tr = this.m_Data[0, 0] + this.m_Data[1, 1] + this.m_Data[2, 2];
if (tr > 0)
{
double s = _N2(tr + 1) * 2;
return new Quaternion(
(this.m_Data[2, 1] - this.m_Data[1, 2]) / s,
(this.m_Data[0, 2] - this.m_Data[2, 0]) / s,
(this.m_Data[1, 0] - this.m_Data[0, 1]) / s,
0.25 * s
);
}
else if ((this.m_Data[0, 0] > this.m_Data[1, 1]) && (this.m_Data[0, 0] > this.m_Data[2, 2]))
{
double s = _N2(1 + this.m_Data[0, 0] - this.m_Data[1, 1] - this.m_Data[2, 2]) * 2;
return new Quaternion(
0.25 * s,
(this.m_Data[0, 1] + this.m_Data[1, 0]) / s,
(this.m_Data[0, 2] + this.m_Data[2, 0]) / s,
(this.m_Data[2, 1] - this.m_Data[1, 2]) / s
);
}
else if (this.m_Data[1, 1] > this.m_Data[2, 2])
{
double s = _N2(1 + this.m_Data[1, 1] - this.m_Data[0, 0] - this.m_Data[2, 2]) * 2;
return new Quaternion(
(this.m_Data[0, 1] + this.m_Data[1, 0]) / s,
0.25 * s,
(this.m_Data[1, 2] + this.m_Data[2, 1]) / s,
(this.m_Data[0, 2] - this.m_Data[2, 0]) / s
);
}
else
{
double s = _N2(1 + this.m_Data[2, 2] - this.m_Data[0, 0] - this.m_Data[1, 1]) * 2;
return new Quaternion(
(this.m_Data[0, 2] + this.m_Data[2, 0]) / s,
(this.m_Data[1, 2] + this.m_Data[2, 1]) / s,
0.25 * s,
(this.m_Data[1, 0] - this.m_Data[0, 1]) / s
);
}
}
/// <summary>
/// This is a simpler form than above, but doesn't work for all values. It exhibits the
/// *same results* as ToQuaternion for X rotation however (i.e. both are invalid).
/// </summary>
public Quaternion ToQuaternionAlt()
{
double w = System.Math.Sqrt(1 + this.m_Data[0, 0] + this.m_Data[1, 1] + this.m_Data[2, 2]) / 2;
return new Quaternion(
(this.m_Data[2, 1] - this.m_Data[1, 2]) / (4 * w),
(this.m_Data[0, 2] - this.m_Data[2, 0]) / (4 * w),
(this.m_Data[1, 0] - this.m_Data[0, 1]) / (4 * w),
w
);
}
Now my test suite has a simple test like so:
[TestMethod]
public void TestMatrixXA()
{
Matrix m = Matrix.CreateRotationX(45 / (180 / System.Math.PI));
Assert.AreEqual<Matrix>(m, m.ToQuaternion().ToMatrix(), "Quaternion conversion was not completed successfully.");
}
This is the result I get from the test suite:
Expected:
{ 1, 0, 0, 0 }
{ 0, 0.707106781186548, -0.707106781186547, 0 }
{ 0, 0.707106781186547, 0.707106781186548, 0 }
{ 0, 0, 0, 1 }
Actual:
{ 1, 0, 0, 0 }
{ 0, 0.707106781186547, 0.707106781186547, 0 }
{ 0, -0.707106781186547, 0.707106781186547, 0 }
{ 0, 0, 0, 1 }
You will note that the two values in the matrix are inverted. I've tested it and upon each conversion back and forth (so .ToQuaternion().ToMatrix()) these fields are inverted. i.e. If I do the quaternion / matrix conversion twice I get the correct matrix.
Since the difference between the correct value and the result is so simple, I'm assuming it's something simple like a negative sign being in the wrong place, but as I'm not an expert at matrix and quaternion math, I'm having trouble finding where the problem is.
Does anyone know what's wrong with the math?
One of the other solutions for converting quaternions to matrixes seems to work:
/// <summary>
/// Converts the quaternion to it's matrix representation.
/// </summary>
/// <returns>A matrix representing the quaternion.</returns>
public Matrix ToMatrix()
{
if (!this.Normalized)
return this.Normalize().ToMatrix();
double xx = X * X;
double xy = X * Y;
double xz = X * Z;
double xw = X * W;
double yy = Y * Y;
double yz = Y * Z;
double yw = Y * W;
double zz = Z * Z;
double zw = Z * W;
return new Matrix(new double[,]
{
{ 1 - 2 * (yy + zz), 2 * (xy - zw), 2 * (xz + yw), 0 },
{ 2 * (xy + zw), 1 - 2 * (xx + zz), 2 * (yz - xw), 0 },
{ 2 * (xz - yw), 2 * (yz + xw), 1 - 2 * (xx + yy), 0 },
{ 0, 0, 0, 1 }
});
}
I'm sure there's a subtle mathematical difference in there that can probably be found if you rearrange each of the values to get it into the first form, but for me this works and I'm happy with that.