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)); }