MaxstARSDK  3.5.0
MatrixUtils.cs
1 /*==============================================================================
2 Copyright 2017 Maxst, Inc. All Rights Reserved.
3 ==============================================================================*/
4 
5 using System;
6 using UnityEngine;
7 using System.Runtime.InteropServices;
8 
9 namespace maxstAR
10 {
14  public static class MatrixUtils
15  {
16  #region Native To Unity Projection
17  private static float[] glProjection = new float[16];
18  private static Matrix4x4 unityProjection = new Matrix4x4();
19 
24  internal static Matrix4x4 getProjectionMatrix()
25  {
26  CameraDevice.GetInstance().GetProjectionMatrix(glProjection);
27  return ConvertGLProjectionToUnityProjection(glProjection);
28  }
29  #endregion
30 
31  internal static Matrix4x4 ConvertGLMatrixToUnityMatrix4x4(float[] glMatrix)
32  {
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];
38 
39  matrix[0, 1] = glMatrix[4];
40  matrix[1, 1] = glMatrix[5];
41  matrix[2, 1] = glMatrix[6];
42  matrix[3, 1] = glMatrix[7];
43 
44  matrix[0, 2] = glMatrix[8];
45  matrix[1, 2] = glMatrix[9];
46  matrix[2, 2] = glMatrix[10];
47  matrix[3, 2] = glMatrix[11];
48 
49  matrix[0, 3] = glMatrix[12];
50  matrix[1, 3] = glMatrix[13];
51  matrix[2, 3] = glMatrix[14];
52  matrix[3, 3] = glMatrix[15];
53  return matrix;
54  }
55 
56  internal static Matrix4x4 ConvertGLProjectionToUnityProjection(float[] projection)
57  {
58  unityProjection[0, 0] = projection[0]; // x
59  unityProjection[1, 0] = projection[1]; // x
60  unityProjection[2, 0] = projection[2];
61  unityProjection[3, 0] = projection[3];
62 
63  unityProjection[0, 1] = -projection[4]; // y
64  unityProjection[1, 1] = -projection[5]; // y
65  unityProjection[2, 1] = -projection[6];
66  unityProjection[3, 1] = -projection[7];
67 
68  unityProjection[0, 2] = -projection[8];
69  unityProjection[1, 2] = -projection[9];
70  unityProjection[2, 2] = -projection[10]; // z
71  unityProjection[3, 2] = -projection[11]; // z
72 
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;
78  }
79 
80  internal static Matrix4x4 GetUnityPoseMatrix(float[] glMatrix)
81  {
82  Matrix4x4 unityMatrix = ConvertGLMatrixToUnityMatrix4x4(glMatrix);
83  return GetUnityPoseMatrix(unityMatrix);
84  }
85 
86  internal static Matrix4x4 GetUnityPoseMatrix(Matrix4x4 matrix)
87  {
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)); //Translate + Quaternion + Scale = Matrix4x4
93  }
94 
95  internal static void ApplyLocalTransformFromMatrix(Transform transform, Matrix4x4 matrix)
96  {
97  transform.localScale = ScaleFromMatrix(matrix);
98  transform.localRotation = QuaternionFromMatrix(matrix);
99  transform.localPosition = PositionFromMatrix(matrix);
100  }
101 
102  internal static Quaternion InvQuaternionFromMatrix(Matrix4x4 input)
103  {
104  Quaternion q = QuaternionFromMatrix(input);
105  q.w = -q.w;
106  return q;
107  }
108 
114  public static Vector3 PositionFromMatrix(Matrix4x4 input)
115  {
116  return new Vector3(input.m03, input.m13, input.m23);
117  }
118 
124  public static Vector3 ScaleFromMatrix(Matrix4x4 input)
125  {
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);
130  }
131 
132  internal static Matrix4x4 MatrixFromQuaternionAndTranslate(Quaternion qua, Vector3 pos)
133  {
134  Matrix4x4 result = MatrixFromQuaternion(qua);
135  result[12] = pos.x;
136  result[13] = pos.y;
137  result[14] = pos.z;
138  return result;
139  }
140 
146  public static Matrix4x4 MatrixFromQuaternion(Quaternion input)
147  {
148  float qx = input.x;
149  float qy = input.y;
150  float qz = input.z;
151  float qw = input.w;
152 
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;
157  result[3] = 0;
158 
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;
162  result[7] = 0;
163 
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;
167  result[11] = 0;
168 
169  result[12] = 0;
170  result[13] = 0;
171  result[14] = 0;
172  result[15] = 1;
173  return result;
174  }
175 
181  public static Quaternion QuaternionFromMatrix(Matrix4x4 m)
182  {
183  return Quaternion.LookRotation(m.GetColumn(2), m.GetColumn(1));
184  }
185  }
186 }