7 using System.Runtime.InteropServices;
14 public static class MatrixUtils
16 #region Native To Unity Projection 17 private static float[] glProjection =
new float[16];
18 private static Matrix4x4 unityProjection =
new Matrix4x4();
24 internal static Matrix4x4 getProjectionMatrix()
26 CameraDevice.GetInstance().GetProjectionMatrix(glProjection);
27 return ConvertGLProjectionToUnityProjection(glProjection);
31 internal static Matrix4x4 ConvertGLMatrixToUnityMatrix4x4(
float[] glMatrix)
33 Matrix4x4 matrix = Matrix4x4.zero;
34 matrix[0, 0] = glMatrix[0];
35 matrix[1, 0] = glMatrix[1];
36 matrix[2, 0] = glMatrix[2];
37 matrix[3, 0] = glMatrix[3];
39 matrix[0, 1] = glMatrix[4];
40 matrix[1, 1] = glMatrix[5];
41 matrix[2, 1] = glMatrix[6];
42 matrix[3, 1] = glMatrix[7];
44 matrix[0, 2] = glMatrix[8];
45 matrix[1, 2] = glMatrix[9];
46 matrix[2, 2] = glMatrix[10];
47 matrix[3, 2] = glMatrix[11];
49 matrix[0, 3] = glMatrix[12];
50 matrix[1, 3] = glMatrix[13];
51 matrix[2, 3] = glMatrix[14];
52 matrix[3, 3] = glMatrix[15];
56 internal static Matrix4x4 ConvertGLProjectionToUnityProjection(
float[] projection)
58 unityProjection[0, 0] = projection[0];
59 unityProjection[1, 0] = projection[1];
60 unityProjection[2, 0] = projection[2];
61 unityProjection[3, 0] = projection[3];
63 unityProjection[0, 1] = -projection[4];
64 unityProjection[1, 1] = -projection[5];
65 unityProjection[2, 1] = -projection[6];
66 unityProjection[3, 1] = -projection[7];
68 unityProjection[0, 2] = -projection[8];
69 unityProjection[1, 2] = -projection[9];
70 unityProjection[2, 2] = -projection[10];
71 unityProjection[3, 2] = -projection[11];
73 unityProjection[0, 3] = projection[12];
74 unityProjection[1, 3] = projection[13];
75 unityProjection[2, 3] = projection[14];
76 unityProjection[3, 3] = projection[15];
77 return unityProjection;
80 internal static Matrix4x4 GetUnityPoseMatrix(
float[] glMatrix)
82 Matrix4x4 unityMatrix = ConvertGLMatrixToUnityMatrix4x4(glMatrix);
83 return GetUnityPoseMatrix(unityMatrix);
86 internal static Matrix4x4 GetUnityPoseMatrix(Matrix4x4 matrix)
88 Quaternion q = QuaternionFromMatrix(matrix);
89 Vector3 tempEuler = q.eulerAngles;
90 tempEuler.x = -tempEuler.x;
91 tempEuler.z = -tempEuler.z;
92 return Matrix4x4.TRS(
new Vector3(matrix.m03, -matrix.m13, matrix.m23), Quaternion.Euler(tempEuler),
new Vector3(1.0f, 1.0f, 1.0f));
95 internal static void ApplyLocalTransformFromMatrix(Transform transform, Matrix4x4 matrix)
97 transform.localScale = ScaleFromMatrix(matrix);
98 transform.localRotation = QuaternionFromMatrix(matrix);
99 transform.localPosition = PositionFromMatrix(matrix);
102 internal static Quaternion InvQuaternionFromMatrix(Matrix4x4 input)
104 Quaternion q = QuaternionFromMatrix(input);
114 public static Vector3 PositionFromMatrix(Matrix4x4 input)
116 return new Vector3(input.m03, input.m13, input.m23);
124 public static Vector3 ScaleFromMatrix(Matrix4x4 input)
126 float x = Mathf.Sqrt(input.m00 * input.m00 + input.m01 * input.m01 + input.m02 * input.m02);
127 float y = Mathf.Sqrt(input.m10 * input.m10 + input.m11 * input.m11 + input.m12 * input.m12);
128 float z = Mathf.Sqrt(input.m20 * input.m20 + input.m21 * input.m21 + input.m22 * input.m22);
129 return new Vector3(x, y, z);
132 internal static Matrix4x4 MatrixFromQuaternionAndTranslate(Quaternion qua, Vector3 pos)
134 Matrix4x4 result = MatrixFromQuaternion(qua);
146 public static Matrix4x4 MatrixFromQuaternion(Quaternion input)
153 Matrix4x4 result =
new Matrix4x4();
154 result[0] = 1 - 2 * qy * qy - 2 * qz * qz;
155 result[1] = 2 * qx * qy + 2 * qz * qw;
156 result[2] = 2 * qx * qz - 2 * qy * qw;
159 result[4] = 2 * qx * qy - 2 * qz * qw;
160 result[5] = 1 - 2 * qx * qx - 2 * qz * qz;
161 result[6] = 2 * qy * qz + 2 * qx * qw;
164 result[8] = 2 * qx * qz + 2 * qy * qw;
165 result[9] = 2 * qy * qz - 2 * qx * qw;
166 result[10] = 1 - 2 * qx * qx - 2 * qy * qy;
181 public static Quaternion QuaternionFromMatrix(Matrix4x4 m)
183 return Quaternion.LookRotation(m.GetColumn(2), m.GetColumn(1));