1
0
mirror of synced 2024-12-13 16:21:10 +01:00
Switch-Toolbox/Switch_Toolbox_Library/IO/Matrix4x4.cs

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,*/
};
}
}
}