static Vector3RightHand NormalizeAngles(Vector3RightHand angles)
{
angles.x = NormalizeAngle(angles.x);
angles.y = NormalizeAngle(angles.y);
angles.z = NormalizeAngle(angles.z);
return(angles);
}
public static QuaternionRightHand GetRotation(Matrix4x4RightHand m)
{
Vector3RightHand s = GetScale(m);
// Normalize Scale from Matrix4x4
float m00 = m[0, 0] / s.x;
float m01 = m[0, 1] / s.y;
float m02 = m[0, 2] / s.z;
float m10 = m[1, 0] / s.x;
float m11 = m[1, 1] / s.y;
float m12 = m[1, 2] / s.z;
float m20 = m[2, 0] / s.x;
float m21 = m[2, 1] / s.y;
float m22 = m[2, 2] / s.z;
QuaternionRightHand q = new QuaternionRightHand();
q.w = Mathf.Sqrt(Mathf.Max(0, 1 + m00 + m11 + m22)) / 2;
q.x = Mathf.Sqrt(Mathf.Max(0, 1 + m00 - m11 - m22)) / 2;
q.y = Mathf.Sqrt(Mathf.Max(0, 1 - m00 + m11 - m22)) / 2;
q.z = Mathf.Sqrt(Mathf.Max(0, 1 - m00 - m11 + m22)) / 2;
q.x *= Mathf.Sign(q.x * (m21 - m12));
q.y *= Mathf.Sign(q.y * (m02 - m20));
q.z *= Mathf.Sign(q.z * (m10 - m01));
// q.Normalize()
float qMagnitude = Mathf.Sqrt(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z);
q.w /= qMagnitude;
q.x /= qMagnitude;
q.y /= qMagnitude;
q.z /= qMagnitude;
return(q);
}
static void Main(string[] args)
{
//UnityEngine.Matrix4x4RightHand.Test();
//Vector3RightHand euler0 = new Vector3RightHand(-86, 0.000000f, 0.000000f);
//QuaternionRightHand qua0 = QuaternionRightHand.Euler(euler0);
//Vector3RightHand euler = new Vector3RightHand(-88, 0.000000f, 0.000000f);
//QuaternionRightHand qua = QuaternionRightHand.Euler(euler);
//Vector3RightHand euler4 = qua0.eulerAngles;
//Vector3RightHand euler3 = qua.eulerAngles;
//Debug.Log(NormalizeAngles(euler3));
//Matrix4x4RightHand trs = Matrix4x4RightHand.TRS();
Matrix4x4RightHand mat = new Matrix4x4RightHand();
mat.m00 = 10.121946f;
mat.m01 = -0.070887f;
mat.m02 = 16.107624f;
mat.m03 = 16.404133f;
mat.m10 = 16.107441f;
mat.m11 = 0.167915f;
mat.m12 = -10.121094f;
mat.m13 = -1.901794f;
mat.m20 = -0.104461f;
mat.m21 = 19.023176f;
mat.m22 = 0.149360f;
mat.m23 = -0.155247f;
mat.m30 = 0.000000f;
mat.m31 = 0.000000f;
mat.m32 = 0.000000f;
mat.m33 = 1.000000f;
Vector3RightHand pos = MathHelper.GetPosition(mat);
Vector3RightHand euler = MathHelper.GetRotation(mat).eulerAngles;
Vector3RightHand scale = MathHelper.GetScale(mat);
Console.WriteLine(pos);
Console.WriteLine(euler);
Console.WriteLine(scale);
Console.WriteLine("right hand math end");
Console.ReadLine();
}