//! Quaternion creation from matrix public Quaternion FromMatrix(Matrix4 m) { float diag = m.GetM(0, 0) + m.GetM(1, 1) + m.GetM(2, 2) + 1; float scale; if (diag > 0.0f) { scale = (float)Math.Sqrt(diag) * 2.0f; // get scale from diagonal // TODO: speed this up m_x = (m.GetM(2, 1) - m.GetM(1, 2)) / scale; m_y = (m.GetM(0, 2) - m.GetM(2, 0)) / scale; m_z = (m.GetM(1, 0) - m.GetM(0, 1)) / scale; m_w = 0.25f * scale; } else { if (m.GetM(0, 0) > m.GetM(1, 1) && m.GetM(0, 0) > m.GetM(2, 2)) { // 1st element of diag is greatest value // find scale according to 1st element, and double it scale = (float)Math.Sqrt(1.0f + m.GetM(0, 0) - m.GetM(1, 1) - m.GetM(2, 2)) * 2.0f; // TODO: speed this up m_x = 0.25f * scale; m_y = (m.GetM(0, 1) + m.GetM(1, 0)) / scale; m_z = (m.GetM(2, 0) + m.GetM(0, 2)) / scale; m_w = (m.GetM(2, 1) - m.GetM(1, 2)) / scale; } else if (m.GetM(1, 1) > m.GetM(2, 2)) { // 2nd element of diag is greatest value // find scale according to 2nd element, and double it scale = (float)Math.Sqrt(1.0f + m.GetM(1, 1) - m.GetM(0, 0) - m.GetM(2, 2)) * 2.0f; // TODO: speed this up m_x = (m.GetM(0, 1) + m.GetM(1, 0)) / scale; m_y = 0.25f * scale; m_z = (m.GetM(1, 2) + m.GetM(2, 1)) / scale; m_w = (m.GetM(0, 2) - m.GetM(2, 0)) / scale; } else { // 3rd element of diag is greatest value // find scale according to 3rd element, and double it scale = (float)Math.Sqrt(1.0f + m.GetM(2, 2) - m.GetM(0, 0) - m.GetM(1, 1)) * 2.0f; // TODO: speed this up m_x = (m.GetM(0, 2) + m.GetM(2, 0)) / scale; m_y = (m.GetM(1, 2) + m.GetM(2, 1)) / scale; m_z = 0.25f * scale; m_w = (m.GetM(1, 0) - m.GetM(0, 1)) / scale; } } Normalize(); return this; }
//! Constructor which converts a matrix to a Quaternion public Quaternion(Matrix4 mat) { this.FromMatrix(mat); }
public void Transform(Matrix4 mat) { VF_Transform(_raw, mat.ToUnmanaged()); }
public bool GetInverse(out Matrix4 outM) { outM = new Matrix4(); float d = (GetMInsecure(0, 0) * GetMInsecure(1, 1) - GetMInsecure(1, 0) * GetMInsecure(0, 1)) * (GetMInsecure(2, 2) * GetMInsecure(3, 3) - GetMInsecure(3, 2) * GetMInsecure(2, 3)) - (GetMInsecure(0, 0) * GetMInsecure(2, 1) - GetMInsecure(2, 0) * GetMInsecure(0, 1)) * (GetMInsecure(1, 2) * GetMInsecure(3, 3) - GetMInsecure(3, 2) * GetMInsecure(1, 3)) + (GetMInsecure(0, 0) * GetMInsecure(3, 1) - GetMInsecure(3, 0) * GetMInsecure(0, 1)) * (GetMInsecure(1, 2) * GetMInsecure(2, 3) - GetMInsecure(2, 2) * GetMInsecure(1, 3)) + (GetMInsecure(1, 0) * GetMInsecure(2, 1) - GetMInsecure(2, 0) * GetMInsecure(1, 1)) * (GetMInsecure(0, 2) * GetMInsecure(3, 3) - GetMInsecure(3, 2) * GetMInsecure(0, 3)) - (GetMInsecure(1, 0) * GetMInsecure(3, 1) - GetMInsecure(3, 0) * GetMInsecure(1, 1)) * (GetMInsecure(0, 2) * GetMInsecure(2, 3) - GetMInsecure(2, 2) * GetMInsecure(0, 3)) + (GetMInsecure(2, 0) * GetMInsecure(3, 1) - GetMInsecure(3, 0) * GetMInsecure(2, 1)) * (GetMInsecure(0, 2) * GetMInsecure(1, 3) - GetMInsecure(1, 2) * GetMInsecure(0, 3)); if (d == 0f) return false; d = 1f / d; outM.SetMInsecure(0, 0, d * (GetMInsecure(1, 1) * (GetMInsecure(2, 2) * GetMInsecure(3, 3) - GetMInsecure(3, 2) * GetMInsecure(2, 3)) + GetMInsecure(2, 1) * (GetMInsecure(3, 2) * GetMInsecure(1, 3) - GetMInsecure(1, 2) * GetMInsecure(3, 3)) + GetMInsecure(3, 1) * (GetMInsecure(1, 2) * GetMInsecure(2, 3) - GetMInsecure(2, 2) * GetMInsecure(1, 3)))); outM.SetMInsecure(1, 0, d * (GetMInsecure(1, 2) * (GetMInsecure(2, 0) * GetMInsecure(3, 3) - GetMInsecure(3, 0) * GetMInsecure(2, 3)) + GetMInsecure(2, 2) * (GetMInsecure(3, 0) * GetMInsecure(1, 3) - GetMInsecure(1, 0) * GetMInsecure(3, 3)) + GetMInsecure(3, 2) * (GetMInsecure(1, 0) * GetMInsecure(2, 3) - GetMInsecure(2, 0) * GetMInsecure(1, 3)))); outM.SetMInsecure(2, 0, d * (GetMInsecure(1, 3) * (GetMInsecure(2, 0) * GetMInsecure(3, 1) - GetMInsecure(3, 0) * GetMInsecure(2, 1)) + GetMInsecure(2, 3) * (GetMInsecure(3, 0) * GetMInsecure(1, 1) - GetMInsecure(1, 0) * GetMInsecure(3, 1)) + GetMInsecure(3, 3) * (GetMInsecure(1, 0) * GetMInsecure(2, 1) - GetMInsecure(2, 0) * GetMInsecure(1, 1)))); outM.SetMInsecure(3, 0, d * (GetMInsecure(1, 0) * (GetMInsecure(3, 1) * GetMInsecure(2, 2) - GetMInsecure(2, 1) * GetMInsecure(3, 2)) + GetMInsecure(2, 0) * (GetMInsecure(1, 1) * GetMInsecure(3, 2) - GetMInsecure(3, 1) * GetMInsecure(1, 2)) + GetMInsecure(3, 0) * (GetMInsecure(2, 1) * GetMInsecure(1, 2) - GetMInsecure(1, 1) * GetMInsecure(2, 2)))); outM.SetMInsecure(0, 1, d * (GetMInsecure(2, 1) * (GetMInsecure(0, 2) * GetMInsecure(3, 3) - GetMInsecure(3, 2) * GetMInsecure(0, 3)) + GetMInsecure(3, 1) * (GetMInsecure(2, 2) * GetMInsecure(0, 3) - GetMInsecure(0, 2) * GetMInsecure(2, 3)) + GetMInsecure(0, 1) * (GetMInsecure(3, 2) * GetMInsecure(2, 3) - GetMInsecure(2, 2) * GetMInsecure(3, 3)))); outM.SetMInsecure(1, 1, d * (GetMInsecure(2, 2) * (GetMInsecure(0, 0) * GetMInsecure(3, 3) - GetMInsecure(3, 0) * GetMInsecure(0, 3)) + GetMInsecure(3, 2) * (GetMInsecure(2, 0) * GetMInsecure(0, 3) - GetMInsecure(0, 0) * GetMInsecure(2, 3)) + GetMInsecure(0, 2) * (GetMInsecure(3, 0) * GetMInsecure(2, 3) - GetMInsecure(2, 0) * GetMInsecure(3, 3)))); outM.SetMInsecure(2, 1, d * (GetMInsecure(2, 3) * (GetMInsecure(0, 0) * GetMInsecure(3, 1) - GetMInsecure(3, 0) * GetMInsecure(0, 1)) + GetMInsecure(3, 3) * (GetMInsecure(2, 0) * GetMInsecure(0, 1) - GetMInsecure(0, 0) * GetMInsecure(2, 1)) + GetMInsecure(0, 3) * (GetMInsecure(3, 0) * GetMInsecure(2, 1) - GetMInsecure(2, 0) * GetMInsecure(3, 1)))); outM.SetMInsecure(3, 1, d * (GetMInsecure(2, 0) * (GetMInsecure(3, 1) * GetMInsecure(0, 2) - GetMInsecure(0, 1) * GetMInsecure(3, 2)) + GetMInsecure(3, 0) * (GetMInsecure(0, 1) * GetMInsecure(2, 2) - GetMInsecure(2, 1) * GetMInsecure(0, 2)) + GetMInsecure(0, 0) * (GetMInsecure(2, 1) * GetMInsecure(3, 2) - GetMInsecure(3, 1) * GetMInsecure(2, 2)))); outM.SetMInsecure(0, 2, d * (GetMInsecure(3, 1) * (GetMInsecure(0, 2) * GetMInsecure(1, 3) - GetMInsecure(1, 2) * GetMInsecure(0, 3)) + GetMInsecure(0, 1) * (GetMInsecure(1, 2) * GetMInsecure(3, 3) - GetMInsecure(3, 2) * GetMInsecure(1, 3)) + GetMInsecure(1, 1) * (GetMInsecure(3, 2) * GetMInsecure(0, 3) - GetMInsecure(0, 2) * GetMInsecure(3, 3)))); outM.SetMInsecure(1, 2, d * (GetMInsecure(3, 2) * (GetMInsecure(0, 0) * GetMInsecure(1, 3) - GetMInsecure(1, 0) * GetMInsecure(0, 3)) + GetMInsecure(0, 2) * (GetMInsecure(1, 0) * GetMInsecure(3, 3) - GetMInsecure(3, 0) * GetMInsecure(1, 3)) + GetMInsecure(1, 2) * (GetMInsecure(3, 0) * GetMInsecure(0, 3) - GetMInsecure(0, 0) * GetMInsecure(3, 3)))); outM.SetMInsecure(2, 2, d * (GetMInsecure(3, 3) * (GetMInsecure(0, 0) * GetMInsecure(1, 1) - GetMInsecure(1, 0) * GetMInsecure(0, 1)) + GetMInsecure(0, 3) * (GetMInsecure(1, 0) * GetMInsecure(3, 1) - GetMInsecure(3, 0) * GetMInsecure(1, 1)) + GetMInsecure(1, 3) * (GetMInsecure(3, 0) * GetMInsecure(0, 1) - GetMInsecure(0, 0) * GetMInsecure(3, 1)))); outM.SetMInsecure(3, 2, d * (GetMInsecure(3, 0) * (GetMInsecure(1, 1) * GetMInsecure(0, 2) - GetMInsecure(0, 1) * GetMInsecure(1, 2)) + GetMInsecure(0, 0) * (GetMInsecure(3, 1) * GetMInsecure(1, 2) - GetMInsecure(1, 1) * GetMInsecure(3, 2)) + GetMInsecure(1, 0) * (GetMInsecure(0, 1) * GetMInsecure(3, 2) - GetMInsecure(3, 1) * GetMInsecure(0, 2)))); outM.SetMInsecure(0, 3, d * (GetMInsecure(0, 1) * (GetMInsecure(2, 2) * GetMInsecure(1, 3) - GetMInsecure(1, 2) * GetMInsecure(2, 3)) + GetMInsecure(1, 1) * (GetMInsecure(0, 2) * GetMInsecure(2, 3) - GetMInsecure(2, 2) * GetMInsecure(0, 3)) + GetMInsecure(2, 1) * (GetMInsecure(1, 2) * GetMInsecure(0, 3) - GetMInsecure(0, 2) * GetMInsecure(1, 3)))); outM.SetMInsecure(1, 3, d * (GetMInsecure(0, 2) * (GetMInsecure(2, 0) * GetMInsecure(1, 3) - GetMInsecure(1, 0) * GetMInsecure(2, 3)) + GetMInsecure(1, 2) * (GetMInsecure(0, 0) * GetMInsecure(2, 3) - GetMInsecure(2, 0) * GetMInsecure(0, 3)) + GetMInsecure(2, 2) * (GetMInsecure(1, 0) * GetMInsecure(0, 3) - GetMInsecure(0, 0) * GetMInsecure(1, 3)))); outM.SetMInsecure(2, 3, d * (GetMInsecure(0, 3) * (GetMInsecure(2, 0) * GetMInsecure(1, 1) - GetMInsecure(1, 0) * GetMInsecure(2, 1)) + GetMInsecure(1, 3) * (GetMInsecure(0, 0) * GetMInsecure(2, 1) - GetMInsecure(2, 0) * GetMInsecure(0, 1)) + GetMInsecure(2, 3) * (GetMInsecure(1, 0) * GetMInsecure(0, 1) - GetMInsecure(0, 0) * GetMInsecure(1, 1)))); outM.SetMInsecure(3, 3, d * (GetMInsecure(0, 0) * (GetMInsecure(1, 1) * GetMInsecure(2, 2) - GetMInsecure(2, 1) * GetMInsecure(1, 2)) + GetMInsecure(1, 0) * (GetMInsecure(2, 1) * GetMInsecure(0, 2) - GetMInsecure(0, 1) * GetMInsecure(2, 2)) + GetMInsecure(2, 0) * (GetMInsecure(0, 1) * GetMInsecure(1, 2) - GetMInsecure(1, 1) * GetMInsecure(0, 2)))); return true; }
public static Matrix4 operator *(Matrix4 a, Matrix4 b) { Matrix4 tmtrx = new Matrix4(); tmtrx.M[0] = a.M[0] * b.M[0] + a.M[4] * b.M[1] + a.M[8] * b.M[2] + a.M[12] * b.M[3]; tmtrx.M[1] = a.M[1] * b.M[0] + a.M[5] * b.M[1] + a.M[9] * b.M[2] + a.M[13] * b.M[3]; tmtrx.M[2] = a.M[2] * b.M[0] + a.M[6] * b.M[1] + a.M[10] * b.M[2] + a.M[14] * b.M[3]; tmtrx.M[3] = a.M[3] * b.M[0] + a.M[7] * b.M[1] + a.M[11] * b.M[2] + a.M[15] * b.M[3]; tmtrx.M[4] = a.M[0] * b.M[4] + a.M[4] * b.M[5] + a.M[8] * b.M[6] + a.M[12] * b.M[7]; tmtrx.M[5] = a.M[1] * b.M[4] + a.M[5] * b.M[5] + a.M[9] * b.M[6] + a.M[13] * b.M[7]; tmtrx.M[6] = a.M[2] * b.M[4] + a.M[6] * b.M[5] + a.M[10] * b.M[6] + a.M[14] * b.M[7]; tmtrx.M[7] = a.M[3] * b.M[4] + a.M[7] * b.M[5] + a.M[11] * b.M[6] + a.M[15] * b.M[7]; tmtrx.M[8] = a.M[0] * b.M[8] + a.M[4] * b.M[9] + a.M[8] * b.M[10] + a.M[12] * b.M[11]; tmtrx.M[9] = a.M[1] * b.M[8] + a.M[5] * b.M[9] + a.M[9] * b.M[10] + a.M[13] * b.M[11]; tmtrx.M[10] = a.M[2] * b.M[8] + a.M[6] * b.M[9] + a.M[10] * b.M[10] + a.M[14] * b.M[11]; tmtrx.M[11] = a.M[3] * b.M[8] + a.M[7] * b.M[9] + a.M[11] * b.M[10] + a.M[15] * b.M[11]; tmtrx.M[12] = a.M[0] * b.M[12] + a.M[4] * b.M[13] + a.M[8] * b.M[14] + a.M[12] * b.M[15]; tmtrx.M[13] = a.M[1] * b.M[12] + a.M[5] * b.M[13] + a.M[9] * b.M[14] + a.M[13] * b.M[15]; tmtrx.M[14] = a.M[2] * b.M[12] + a.M[6] * b.M[13] + a.M[10] * b.M[14] + a.M[14] * b.M[15]; tmtrx.M[15] = a.M[3] * b.M[12] + a.M[7] * b.M[13] + a.M[11] * b.M[14] + a.M[15] * b.M[15]; return tmtrx; }
/// <summary> /// Applies a transformation. /// </summary> /// <param name="mesh"> /// A mesh to be transformed <see cref="Mesh"/> /// </param> /// <param name="mat"> /// A transform matrix <see cref="Matrix4"/> /// </param> public void TransformMesh(Mesh mesh, Matrix4 mat) { MeshManipulator_TransformMesh(_raw, mesh.Raw, mat.ToUnmanaged()); }
public void SetTransform(TransformationState state, Matrix4 mat) { VideoDriver_SetTransform(_raw, state, mat.ToUnmanaged()); }