public static Matrix ToMatrix(this Quaternion q)
{
float x2 = q.X + q.X;
float y2 = q.Y + q.Y;
float z2 = q.Z + q.Z;
float xx = q.X * x2;
float xy = q.X * y2;
float xz = q.X * z2;
float yy = q.Y * y2;
float yz = q.Y * z2;
float zz = q.Z * z2;
float wx = q.W * x2;
float wy = q.W * y2;
float wz = q.W * z2;
return new Matrix(
1.0f - (yy + zz), xy - wz, xz + wy, 0,
xy + wz, 1.0f - (xx + zz), yz - wx, 0,
xz - wy, yz + wx, 1.0f - (xx + yy), 0,
0, 0, 0, 1);
}
#endregion