262 lines
9.4 KiB
C#
262 lines
9.4 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
using System.Text;
|
|
using System.IO;
|
|
using System.Numerics;
|
|
|
|
namespace Toolbox.Library.IO
|
|
{
|
|
public static class MatrixExenstion
|
|
{
|
|
public static float Deg2Rad = (float)(System.Math.PI * 2) / 360;
|
|
public static float Rad2Deg = (float)(360 / (System.Math.PI * 2));
|
|
|
|
public static OpenTK.Vector3 QuaternionToEuler(OpenTK.Quaternion q1)
|
|
{
|
|
float sqw = q1.W * q1.W;
|
|
float sqx = q1.X * q1.X;
|
|
float sqy = q1.Y * q1.Y;
|
|
float sqz = q1.Z * q1.Z;
|
|
float unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
|
|
float test = q1.X * q1.W - q1.Y * q1.Z;
|
|
OpenTK.Vector3 v;
|
|
|
|
|
|
|
|
if (test > 0.4995f * unit)
|
|
{ // singularity at north pole
|
|
v.Y = 2f * (float)System.Math.Atan2(q1.X, q1.Y);
|
|
v.X = (float)System.Math.PI / 2;
|
|
v.Z = 0;
|
|
return NormalizeAngles(v * Rad2Deg);
|
|
}
|
|
if (test < -0.4995f * unit)
|
|
{ // singularity at south pole
|
|
v.Y = -2f * (float)System.Math.Atan2(q1.Y, q1.X);
|
|
v.X = (float)-System.Math.PI / 2;
|
|
v.Z = 0;
|
|
return NormalizeAngles(v * Rad2Deg);
|
|
}
|
|
Quaternion q = new Quaternion(q1.W, q1.Z, q1.X, q1.Y);
|
|
v.Y = (float)Math.Atan2(2f * q.X * q.W + 2f * q.Y * q.Z, 1 - 2f * (q.Z * q.Z + q.W * q.W)); // Yaw
|
|
v.X = (float)Math.Asin(2f * (q.X * q.Z - q.W * q.Y)); // Pitch
|
|
v.Z = (float)Math.Atan2(2f * q.X * q.Y + 2f * q.Z * q.W, 1 - 2f * (q.Y * q.Y + q.Z * q.Z)); // Roll
|
|
return NormalizeAngles(v * Rad2Deg);
|
|
}
|
|
|
|
static OpenTK.Vector3 NormalizeAngles(OpenTK.Vector3 angles)
|
|
{
|
|
angles.X = NormalizeAngle(angles.X);
|
|
angles.Y = NormalizeAngle(angles.Y);
|
|
angles.Z = NormalizeAngle(angles.Z);
|
|
return angles;
|
|
}
|
|
|
|
static float NormalizeAngle(float angle)
|
|
{
|
|
while (angle > 360)
|
|
angle -= 360;
|
|
while (angle < 0)
|
|
angle += 360;
|
|
return angle;
|
|
}
|
|
|
|
public static OpenTK.Quaternion EulerToQuaternion(float yaw, float pitch, float roll)
|
|
{
|
|
yaw *= Deg2Rad;
|
|
pitch *= Deg2Rad;
|
|
roll *= Deg2Rad;
|
|
float rollOver2 = roll * 0.5f;
|
|
float sinRollOver2 = (float)Math.Sin((double)rollOver2);
|
|
float cosRollOver2 = (float)Math.Cos((double)rollOver2);
|
|
float pitchOver2 = pitch * 0.5f;
|
|
float sinPitchOver2 = (float)Math.Sin((double)pitchOver2);
|
|
float cosPitchOver2 = (float)Math.Cos((double)pitchOver2);
|
|
float yawOver2 = yaw * 0.5f;
|
|
float sinYawOver2 = (float)Math.Sin((double)yawOver2);
|
|
float cosYawOver2 = (float)Math.Cos((double)yawOver2);
|
|
OpenTK.Quaternion result = OpenTK.Quaternion.Identity;
|
|
result.W = cosYawOver2 * cosPitchOver2 * cosRollOver2 + sinYawOver2 * sinPitchOver2 * sinRollOver2;
|
|
result.X = cosYawOver2 * sinPitchOver2 * cosRollOver2 + sinYawOver2 * cosPitchOver2 * sinRollOver2;
|
|
result.Y = sinYawOver2 * cosPitchOver2 * cosRollOver2 - cosYawOver2 * sinPitchOver2 * sinRollOver2;
|
|
result.Z = cosYawOver2 * cosPitchOver2 * sinRollOver2 - sinYawOver2 * sinPitchOver2 * cosRollOver2;
|
|
|
|
return result;
|
|
}
|
|
|
|
public static OpenTK.Matrix4 CreateRotation(OpenTK.Vector3 Normal, OpenTK.Vector3 Tangent)
|
|
{
|
|
var mat4 = OpenTK.Matrix4.Identity;
|
|
var vec3 = OpenTK.Vector3.Cross(Normal, Tangent);
|
|
|
|
mat4.M11 = Tangent.X;
|
|
mat4.M21 = Tangent.Y;
|
|
mat4.M31 = Tangent.Z;
|
|
mat4.M12 = Normal.X;
|
|
mat4.M22 = Normal.Y;
|
|
mat4.M32 = Normal.Z;
|
|
mat4.M13 = vec3.X;
|
|
mat4.M23 = vec3.Y;
|
|
mat4.M33 = vec3.Z;
|
|
|
|
return mat4;
|
|
}
|
|
|
|
public static Syroot.Maths.Matrix3x4 GetMatrixInverted(STBone bone)
|
|
{
|
|
return ToMatrix3x4(CalculateInverseMatrix(bone).inverse);
|
|
}
|
|
|
|
public class Matrices
|
|
{
|
|
public Matrix4x4 transform = Matrix4x4.Identity;
|
|
public Matrix4x4 inverse = Matrix4x4.Identity;
|
|
}
|
|
|
|
public static Matrix4x4 CalculateTransformMatrix(STBone bone)
|
|
{
|
|
var trans = Matrix4x4.CreateTranslation(new Vector3(bone.position[0], bone.position[1], bone.position[2]));
|
|
var scale = Matrix4x4.CreateScale(new Vector3(bone.scale[0], bone.scale[1], bone.scale[2]));
|
|
|
|
Matrix4x4 quat = Matrix4x4.Identity;
|
|
|
|
if (bone.RotationType == STBone.BoneRotationType.Euler)
|
|
quat = Matrix4x4.CreateFromQuaternion(QuatFromEular(bone.rotation[0], bone.rotation[1], bone.rotation[2]));
|
|
else
|
|
quat = Matrix4x4.CreateFromQuaternion(QuatFromQuat(bone.rotation[0], bone.rotation[1], bone.rotation[2], bone.rotation[3]));
|
|
|
|
return Matrix4x4.Multiply(quat, trans);
|
|
}
|
|
public static Matrices CalculateInverseMatrix(STBone bone)
|
|
{
|
|
var matrices = new Matrices();
|
|
|
|
//Get parent transform for a smooth matrix
|
|
if (bone.Parent != null && bone.Parent is STBone)
|
|
matrices.transform *= CalculateInverseMatrix((STBone)bone.Parent).transform;
|
|
else
|
|
matrices.transform = Matrix4x4.Identity;
|
|
|
|
//Now calculate the matrix with TK matrices
|
|
var trans = Matrix4x4.CreateTranslation(new Vector3(bone.position[0], bone.position[1], bone.position[2]));
|
|
var scale = Matrix4x4.CreateScale(new Vector3(bone.scale[0], bone.scale[1], bone.scale[2]));
|
|
|
|
Matrix4x4 quat = Matrix4x4.Identity;
|
|
|
|
if (bone.RotationType == STBone.BoneRotationType.Euler)
|
|
quat = Matrix4x4.CreateFromQuaternion(QuatFromEular(bone.rotation[0], bone.rotation[1], bone.rotation[2]));
|
|
else
|
|
quat = Matrix4x4.CreateFromQuaternion(QuatFromQuat(bone.rotation[0], bone.rotation[1], bone.rotation[2], bone.rotation[3]));
|
|
|
|
matrices.transform = Matrix4x4.Multiply(Matrix4x4.Multiply(quat, trans), matrices.transform);
|
|
|
|
Matrix4x4 Inverse;
|
|
Matrix4x4.Invert(matrices.transform, out Inverse);
|
|
|
|
matrices.inverse = Inverse;
|
|
|
|
return matrices;
|
|
}
|
|
|
|
public static Quaternion QuatFromQuat(float x, float y, float z, float w)
|
|
{
|
|
Quaternion q = new Quaternion();
|
|
q.X = x;
|
|
q.Y = y;
|
|
q.Z = z;
|
|
q.W = w;
|
|
|
|
if (q.W < 0)
|
|
q *= -1;
|
|
|
|
return q;
|
|
}
|
|
|
|
public static Quaternion QuatFromEular(float x, float y, float z)
|
|
{
|
|
Quaternion xRotation = Quaternion.CreateFromAxisAngle(Vector3.UnitX, x);
|
|
Quaternion yRotation = Quaternion.CreateFromAxisAngle(Vector3.UnitY, y);
|
|
Quaternion zRotation = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, z);
|
|
|
|
Quaternion q = (zRotation * yRotation * xRotation);
|
|
|
|
if (q.W < 0)
|
|
q *= -1;
|
|
|
|
//return xRotation * yRotation * zRotation;
|
|
return q;
|
|
}
|
|
|
|
//Left-Handed
|
|
public static Matrix4x4 ToMatrix4x4(this Syroot.Maths.Matrix3x4 mat)
|
|
{
|
|
return new Matrix4x4()
|
|
{
|
|
M11 = mat.M11,
|
|
M21 = mat.M12,
|
|
M31 = mat.M13,
|
|
M41 = mat.M14,
|
|
M12 = mat.M21,
|
|
M22 = mat.M22,
|
|
M32 = mat.M23,
|
|
M42 = mat.M24,
|
|
M13 = mat.M31,
|
|
M23 = mat.M32,
|
|
M33 = mat.M33,
|
|
M43 = mat.M34,
|
|
M14 = 0,
|
|
M24 = 0,
|
|
M34 = 0,
|
|
M44 = 0
|
|
};
|
|
}
|
|
|
|
//Left-Handed
|
|
public static Syroot.Maths.Matrix3x4 ToMatrix3x4(this Matrix4x4 mat)
|
|
{
|
|
if (mat.M11 == -0) mat.M11 = 0;
|
|
if (mat.M12 == -0) mat.M12 = 0;
|
|
if (mat.M13 == -0) mat.M13 = 0;
|
|
if (mat.M14 == -0) mat.M14 = 0;
|
|
if (mat.M21 == -0) mat.M21 = 0;
|
|
if (mat.M22 == -0) mat.M22 = 0;
|
|
if (mat.M23 == -0) mat.M23 = 0;
|
|
if (mat.M24 == -0) mat.M24 = 0;
|
|
if (mat.M31 == -0) mat.M31 = 0;
|
|
if (mat.M32 == -0) mat.M32 = 0;
|
|
if (mat.M33 == -0) mat.M33 = 0;
|
|
if (mat.M34 == -0) mat.M34 = 0;
|
|
|
|
return new Syroot.Maths.Matrix3x4()
|
|
{
|
|
M11 = mat.M11,
|
|
M12 = mat.M21,
|
|
M13 = mat.M31,
|
|
M14 = mat.M41,
|
|
M21 = mat.M12,
|
|
M22 = mat.M22,
|
|
M23 = mat.M32,
|
|
M24 = mat.M42,
|
|
M31 = mat.M13,
|
|
M32 = mat.M23,
|
|
M33 = mat.M33,
|
|
M34 = mat.M43,
|
|
|
|
/* M11 = mat.M11,
|
|
M12 = mat.M12,
|
|
M13 = mat.M13,
|
|
M14 = mat.M14,
|
|
M21 = mat.M21,
|
|
M22 = mat.M22,
|
|
M23 = mat.M23,
|
|
M24 = mat.M24,
|
|
M31 = mat.M31,
|
|
M32 = mat.M32,
|
|
M33 = mat.M33,
|
|
M34 = mat.M34,*/
|
|
};
|
|
}
|
|
}
|
|
}
|