//! 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());
}
