public Matrix44 GetTransformFromParts(Vector3 localTranslation, Matrix33 localRotation, Vector3 localScale)
{
Matrix44 transform = new Matrix44
{
// For Node Chunks, the translation appears to be along the bottom of the matrix, and scale on right side.
// Translation part
m41 = localTranslation.x,
m42 = localTranslation.y,
m43 = localTranslation.z,
// Rotation part. Invert this matrix, which results in proper rotation in Blender.
m11 = localRotation.m11,
m12 = localRotation.m21,
m13 = localRotation.m31,
m21 = localRotation.m12,
m22 = localRotation.m22,
m23 = localRotation.m32,
m31 = localRotation.m13,
m32 = localRotation.m23,
m33 = localRotation.m33,
// Scale part
m14 = localScale.x,
m24 = localScale.y,
m34 = localScale.z,
// Set final row
m44 = 1
};
return(transform);
}
public Matrix33 ConvertToRotationalMatrix()
{
// https://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/index.htm
var rotationalMatrix = new Matrix33();
double sqw = w * w;
double sqx = x * x;
double sqy = y * y;
double sqz = z * z;
// invs (inverse square length) is only required if quaternion is not already normalised
double invs = 1 / (sqx + sqy + sqz + sqw);
rotationalMatrix.m11 = (sqx - sqy - sqz + sqw) * invs; // since sqw + sqx + sqy + sqz =1/invs*invs
rotationalMatrix.m22 = (-sqx + sqy - sqz + sqw) * invs;
rotationalMatrix.m33 = (-sqx - sqy + sqz + sqw) * invs;
double tmp1 = x * y;
double tmp2 = z * w;
rotationalMatrix.m21 = 2.0 * (tmp1 + tmp2) * invs;
rotationalMatrix.m12 = 2.0 * (tmp1 - tmp2) * invs;
tmp1 = x * z;
tmp2 = y * w;
rotationalMatrix.m31 = 2.0 * (tmp1 - tmp2) * invs;
rotationalMatrix.m13 = 2.0 * (tmp1 + tmp2) * invs;
tmp1 = y * z;
tmp2 = x * w;
rotationalMatrix.m32 = 2.0 * (tmp1 + tmp2) * invs;
rotationalMatrix.m23 = 2.0 * (tmp1 - tmp2) * invs;
return(rotationalMatrix);
}
public Matrix33 ConjugateTransposeThisAndMultiply(Matrix33 inputMatrix)
{
Matrix <double> matrix = Matrix <double> .Build.Dense(3, 3);
Matrix <double> matrix2 = Matrix <double> .Build.Dense(3, 3);
matrix2 = inputMatrix.ToMathMatrix();
matrix = ToMathMatrix().ConjugateTransposeThisAndMultiply(matrix2);
return(GetMatrix33(matrix));
}
public Matrix44 GetTransformFromParts(Vector3 localTranslation, Matrix33 localRotation)
{
var defaultScale = new Vector3
{
x = 0.0f,
y = 0.0f,
z = 0.0f
};
return(GetTransformFromParts(localTranslation, localRotation, defaultScale));
}
public Matrix33 To3x3()
{
Matrix33 result = new Matrix33();
result.m11 = m11;
result.m12 = m12;
result.m13 = m13;
result.m21 = m21;
result.m22 = m22;
result.m23 = m23;
result.m31 = m31;
result.m32 = m32;
result.m33 = m33;
return(result);
}
public Matrix33 Mult(Matrix33 mat)
{
Matrix33 mat2 = new Matrix33();
mat2.m11 = (m11 * mat.m11) + (m12 * mat.m21) + (m13 * mat.m31);
mat2.m12 = (m11 * mat.m12) + (m12 * mat.m22) + (m13 * mat.m32);
mat2.m13 = (m11 * mat.m13) + (m12 * mat.m23) + (m13 * mat.m33);
mat2.m21 = (m21 * mat.m11) + (m22 * mat.m21) + (m23 * mat.m31);
mat2.m22 = (m21 * mat.m12) + (m22 * mat.m22) + (m23 * mat.m32);
mat2.m23 = (m21 * mat.m13) + (m22 * mat.m23) + (m23 * mat.m33);
mat2.m31 = (m31 * mat.m11) + (m32 * mat.m21) + (m33 * mat.m31);
mat2.m32 = (m31 * mat.m12) + (m32 * mat.m22) + (m33 * mat.m32);
mat2.m33 = (m31 * mat.m13) + (m32 * mat.m23) + (m33 * mat.m33);
return(mat2);
}
public Matrix33 Mult(Matrix33 mat)
{
Matrix33 mat2 = new Matrix33();
mat2.m11 = this.m11 * mat.m11 + this.m12 * mat.m21 + this.m13 * mat.m31;
mat2.m12 = this.m11 * mat.m12 + this.m12 * mat.m22 + this.m13 * mat.m32;
mat2.m13 = this.m11 * mat.m13 + this.m12 * mat.m23 + this.m13 * mat.m33;
mat2.m21 = this.m21 * mat.m11 + this.m22 * mat.m21 + this.m23 * mat.m31;
mat2.m22 = this.m21 * mat.m12 + this.m22 * mat.m22 + this.m23 * mat.m32;
mat2.m23 = this.m21 * mat.m13 + this.m22 * mat.m23 + this.m23 * mat.m33;
mat2.m31 = this.m31 * mat.m11 + this.m32 * mat.m21 + this.m33 * mat.m31;
mat2.m32 = this.m31 * mat.m12 + this.m32 * mat.m22 + this.m33 * mat.m32;
mat2.m33 = this.m31 * mat.m13 + this.m32 * mat.m23 + this.m33 * mat.m33;
return(mat2);
}
public Matrix33 Get_Transpose() // returns a copy of the matrix33
{
Matrix33 mat = new Matrix33();
mat.m11 = m11;
mat.m12 = m21;
mat.m13 = m31;
mat.m21 = m12;
mat.m22 = m22;
mat.m23 = m32;
mat.m31 = m13;
mat.m32 = m23;
mat.m33 = m33;
return(mat);
}
public bool Is_Scale_Rotation() // Returns true if the matrix decomposes nicely into scale * rotation\
{
Matrix33 self_transpose, mat = new Matrix33();
self_transpose = this.Get_Transpose();
mat = this.Mult(self_transpose);
if (System.Math.Abs(mat.m12) + System.Math.Abs(mat.m13)
+ System.Math.Abs(mat.m21) + System.Math.Abs(mat.m23)
+ System.Math.Abs(mat.m31) + System.Math.Abs(mat.m32) > 0.01)
{
Utils.Log(LogLevelEnum.Debug, " is a Scale_Rot matrix");
return(false);
}
Utils.Log(LogLevelEnum.Debug, " is not a Scale_Rot matrix");
return(true);
}
public bool Is_Scale_Rotation() // Returns true if the matrix decomposes nicely into scale * rotation\
{
Matrix33 self_transpose, mat = new Matrix33();
self_transpose = this.Get_Transpose();
mat = this.Mult(self_transpose);
if (System.Math.Abs(mat.m12) + System.Math.Abs(mat.m13)
+ System.Math.Abs(mat.m21) + System.Math.Abs(mat.m23)
+ System.Math.Abs(mat.m31) + System.Math.Abs(mat.m32) > 0.01)
{
Console.WriteLine(" is a Scale_Rot matrix");
return(false);
}
Console.WriteLine(" is not a Scale_Rot matrix");
return(true);
}
public double[,] worldToBone; // 4x3 structure
public WORLDTOBONE(Matrix33 worldRotation, Vector3 worldTransform) : this()
{
worldToBone = new double[3, 4];
worldToBone[0, 0] = worldRotation.m11;
worldToBone[0, 1] = worldRotation.m12;
worldToBone[0, 2] = worldRotation.m13;
worldToBone[1, 0] = worldRotation.m21;
worldToBone[1, 1] = worldRotation.m22;
worldToBone[1, 2] = worldRotation.m23;
worldToBone[2, 0] = worldRotation.m31;
worldToBone[2, 1] = worldRotation.m32;
worldToBone[2, 2] = worldRotation.m33;
worldToBone[0, 3] = worldTransform.x;
worldToBone[1, 3] = worldTransform.y;
worldToBone[2, 3] = worldTransform.z;
}
public double[,] boneToWorld; // 4x3 structure
public BONETOWORLD(Matrix33 matrix33, Vector3 relativeTransform) : this()
{
boneToWorld = new double[3, 4];
boneToWorld[0, 0] = matrix33.m11;
boneToWorld[0, 1] = matrix33.m12;
boneToWorld[0, 2] = matrix33.m13;
boneToWorld[1, 0] = matrix33.m21;
boneToWorld[1, 1] = matrix33.m22;
boneToWorld[1, 2] = matrix33.m23;
boneToWorld[2, 0] = matrix33.m31;
boneToWorld[2, 1] = matrix33.m32;
boneToWorld[2, 2] = matrix33.m33;
boneToWorld[0, 3] = relativeTransform.x;
boneToWorld[1, 3] = relativeTransform.y;
boneToWorld[2, 3] = relativeTransform.z;
}
internal Matrix33 GetWorldToBoneRotationMatrix()
{
Matrix33 result = new Matrix33
{
m11 = worldToBone[0, 0],
m12 = worldToBone[0, 1],
m13 = worldToBone[0, 2],
m21 = worldToBone[1, 0],
m22 = worldToBone[1, 1],
m23 = worldToBone[1, 2],
m31 = worldToBone[2, 0],
m32 = worldToBone[2, 1],
m33 = worldToBone[2, 2]
};
return(result);
}
/// <summary> Returns the world space rotational matrix in a Math.net 3x3 matrix. </summary>
/// <returns>Matrix33</returns>
public Matrix33 GetBoneToWorldRotationMatrix()
{
Matrix33 result = new Matrix33
{
m11 = boneToWorld[0, 0],
m12 = boneToWorld[0, 1],
m13 = boneToWorld[0, 2],
m21 = boneToWorld[1, 0],
m22 = boneToWorld[1, 1],
m23 = boneToWorld[1, 2],
m31 = boneToWorld[2, 0],
m32 = boneToWorld[2, 1],
m33 = boneToWorld[2, 2]
};
return(result);
}
public Matrix33 GetTranspose() // returns a copy of the matrix33
{
Matrix33 mat = new Matrix33
{
m11 = m11,
m12 = m21,
m13 = m31,
m21 = m12,
m22 = m22,
m23 = m32,
m31 = m13,
m32 = m23,
m33 = m33
};
return(mat);
}
public Matrix33 GetMatrix33(Matrix <double> matrix)
{
Matrix33 result = new Matrix33
{
m11 = matrix[0, 0],
m12 = matrix[0, 1],
m13 = matrix[0, 2],
m21 = matrix[1, 0],
m22 = matrix[1, 1],
m23 = matrix[1, 2],
m31 = matrix[2, 0],
m32 = matrix[2, 1],
m33 = matrix[2, 2]
};
return(result);
}
public bool Equals(Matrix33 matrix)
{
if (
matrix.m11 == m11 &&
matrix.m12 == m12 &&
matrix.m13 == m13 &&
matrix.m21 == m21 &&
matrix.m22 == m22 &&
matrix.m23 == m23 &&
matrix.m31 == m31 &&
matrix.m32 == m32 &&
matrix.m33 == m33)
{
return(true);
}
else
{
return(false);
}
}
public Vector3 Get_Scale()
{
// Get the scale, assuming is_scale_rotation is true
Matrix33 mat = this.Mult(this.Get_Transpose());
Vector3 scale = new Vector3();
scale.x = (Double)System.Math.Pow(mat.m11, 0.5);
scale.y = (Double)System.Math.Pow(mat.m22, 0.5);
scale.z = (Double)System.Math.Pow(mat.m33, 0.5);
if (this.Get_Determinant() < 0)
{
scale.x = 0 - scale.x;
scale.y = 0 - scale.y;
scale.z = 0 - scale.z;
return(scale);
}
else
{
return(scale);
}
}
