public static void GetRefPoseTransform(int i, short[] hierarchy, teModelChunk_Skeleton skeleton,
Dictionary <int, teModelChunk_Cloth.ClothNode> clothNodeMap, out teVec3 scale, out teQuat quat,
out teVec3 translation)
{
if (clothNodeMap != null && clothNodeMap.ContainsKey(i))
{
Matrix thisMat = skeleton.GetWorldSpace(i);
Matrix parentMat = skeleton.GetWorldSpace(hierarchy[i]);
Matrix newParentMat = thisMat * Matrix.Invert(parentMat);
newParentMat.Decompose(out Vector3 scl2, out Quaternion rot2, out Vector3 pos2);
quat = new teQuat(rot2.X, rot2.Y, rot2.Z, rot2.W);
scale = new teVec3(scl2.X, scl2.Y, scl2.Z);
translation = new teVec3(pos2.X, pos2.Y, pos2.Z);
}
else
{
teMtx43 bone = skeleton.Matrices34Inverted[i];
scale = new teVec3(bone[1, 0], bone[1, 1], bone[1, 2]);
quat = new teQuat(bone[0, 0], bone[0, 1], bone[0, 2], bone[0, 3]);
translation = new teVec3(bone[2, 0], bone[2, 1], bone[2, 2]);
}
}
public void GetWorldSpace(int idx, out teVec3 scale, out teQuat rotation, out teVec3 translation)
{
teMtx43 parBoneMat = Matrices34[idx];
scale = new teVec3(parBoneMat[1, 0], parBoneMat[1, 1], parBoneMat[1, 2]);
rotation = new teQuat(parBoneMat[0, 0], parBoneMat[0, 1], parBoneMat[0, 2], parBoneMat[0, 3]);
translation = new teVec3(parBoneMat[2, 0], parBoneMat[2, 1], parBoneMat[2, 2]);
}
/// <summary>
/// Unpack rotation value
/// </summary>
/// <param name="a">Packed A component</param>
/// <param name="b">Packed B component</param>
/// <param name="c">Packed C component</param>
/// <returns>Unpacked rotation value</returns>
private static teQuat UnpackRotation(ushort a, ushort b, ushort c)
{
teQuat q = new teQuat();
int axis1 = a >> 15;
int axis2 = b >> 15;
int axis = axis2 << 1 | axis1;
a = (ushort)(a & 0x7FFF);
b = (ushort)(b & 0x7FFF);
double x = 1.41421 * (a - 0x4000) / 0x8000;
double y = 1.41421 * (b - 0x4000) / 0x8000;
double z = 1.41421 * (c - 0x8000) / 0x10000;
double w = System.Math.Pow(1.0 - x * x - y * y - z * z, 0.5);
// Console.Out.WriteLine("Unpack Values: X: {0}, Y: {1}, Z: {2}, W: {3}, Axis: {4}", x, y, z, w, axis);
if (axis == 0)
{
q = new teQuat(w, x, y, z);
}
else if (axis == 1)
{
q = new teQuat(x, w, y, z);
}
else if (axis == 2)
{
q = new teQuat(x, y, w, z);
}
else if (axis == 3)
{
q = new teQuat(x, y, z, w);
}
else
{
Console.Out.WriteLine($"Unknown Axis detected! Axis: {axis}");
}
return(q);
}
