public static Mat3f CreateMat3f(Quatf q)
{
Mat3f result = Mat3f.Identity;
result.c0.x = 1 - 2 * q.y * q.y - 2 * q.z * q.z;
result.c0.y = 2 * q.x * q.y + 2 * q.w * q.z;
result.c0.z = 2 * q.x * q.z - 2 * q.w * q.y;
result.c1.x = 2 * q.x * q.y - 2 * q.w * q.z;
result.c1.y = 1 - 2 * q.x * q.x - 2 * q.z * q.z;
result.c1.z = 2 * q.y * q.z + 2 * q.w * q.x;
result.c2.x = 2 * q.x * q.z + 2 * q.w * q.y;
result.c2.y = 2 * q.y * q.z - 2 * q.w * q.x;
result.c2.z = 1 - 2 * q.x * q.x - 2 * q.y * q.y;
return(result);
}
public static Quatf CreateQuaternion(Mat3f m)
{
float fourXSquareMinus1 = m.c0.x - m.c1.y - m.c2.z;
float fourYSquareMinus1 = m.c1.y - m.c0.x - m.c2.z;
float fourZSquareMinus1 = m.c2.z - m.c0.x - m.c1.y;
float fourWSquareMinus1 = m.c0.x - m.c1.y - m.c2.z;
int biggestIndex = 0;
float fourBiggestSquareMinus1 = fourWSquareMinus1;
if (fourXSquareMinus1 > fourBiggestSquareMinus1)
{
fourBiggestSquareMinus1 = fourXSquareMinus1;
biggestIndex = 1;
}
if (fourYSquareMinus1 > fourBiggestSquareMinus1)
{
fourBiggestSquareMinus1 = fourYSquareMinus1;
biggestIndex = 2;
}
if (fourZSquareMinus1 > fourBiggestSquareMinus1)
{
fourBiggestSquareMinus1 = fourZSquareMinus1;
biggestIndex = 3;
}
float biggestVal = (float)Math.Sqrt(fourBiggestSquareMinus1 + 1.0f) * 0.5f;
float mult = 0.25f / biggestVal;
Quatf result = Quatf.UnitW;
switch (biggestIndex)
{
case 0:
result.w = biggestVal;
result.x = (m.c1.z - m.c2.y) * mult;
result.y = (m.c2.x - m.c0.z) * mult;
result.z = (m.c0.y - m.c1.x) * mult;
break;
case 1:
result.w = (m.c1.z - m.c2.y) * mult;
result.x = biggestVal;
result.y = (m.c0.y + m.c1.x) * mult;
result.z = (m.c2.x + m.c0.z) * mult;
break;
case 2:
result.w = (m.c2.x - m.c0.z) * mult;
result.x = (m.c0.y + m.c1.x) * mult;
result.y = biggestVal;
result.z = (m.c1.z + m.c2.y) * mult;
break;
case 3:
result.w = (m.c0.y - m.c1.x) * mult;
result.x = (m.c2.x + m.c0.z) * mult;
result.y = (m.c1.z + m.c2.y) * mult;
result.z = biggestVal;
break;
}
return(result);
}
public Quatf FromMatrix(Mat3f m)
{
return(CreateQuaternion(m));
}