1 /*
2  * Copyright (C) 2023 The Android Open Source Project
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  *      http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 package com.android.server.uwb.correction.math;
17 
18 import androidx.annotation.NonNull;
19 
20 import java.util.Locale;
21 
22 /**
23  * 4x4 Matrix representing translation, scale, and rotation. Column major, right handed:
24  *
25  * <pre>
26  *  [0, 4,  8, 12]
27  *  [1, 5,  9, 13]
28  *  [2, 6, 10, 14]
29  *  [3, 7, 11, 15]
30  * </pre>
31  */
32 public class Matrix {
33     private static final float[] IDENTITY_DATA =
34             new float[] {
35                     1.0f, 0.0f, 0.0f, 0.0f,
36                     0.0f, 1.0f, 0.0f, 0.0f,
37                     0.0f, 0.0f, 1.0f, 0.0f,
38                     0.0f, 0.0f, 0.0f, 1.0f
39             };
40 
41     public final float[] data = new float[16];
42 
Matrix()43     private Matrix() {}
44 
45     /**
46      * Creates a new instance of the Matrix class.
47      * @param data A 16-element array of the matrix contents.
48      */
Matrix(float[] data)49     public Matrix(float[] data) {
50         set(data);
51     }
52 
set(float[] data)53     private void set(float[] data) {
54         if (data.length != 16) {
55             throw new IllegalArgumentException("Cannot set Matrix, invalid data.");
56         }
57 
58         System.arraycopy(data, 0, this.data, 0, 16);
59     }
60 
61     /**
62      * Gets the identity matrix.
63      *
64      * @return The identity matrix.
65      */
identity()66     public static Matrix identity() {
67         return new Matrix(IDENTITY_DATA);
68     }
69 
70     /** Transform a Vector3 with this Matrix. */
transformPoint(Vector3 vector)71     public Vector3 transformPoint(Vector3 vector) {
72         float vx = vector.x;
73         float vy = vector.y;
74         float vz = vector.z;
75 
76         float x, y, z;
77         x = data[0] * vx;
78         x += data[4] * vy;
79         x += data[8] * vz;
80         x += data[12]; // *1
81 
82         y = data[1] * vx;
83         y += data[5] * vy;
84         y += data[9] * vz;
85         y += data[13]; // *1
86 
87         z = data[2] * vx;
88         z += data[6] * vy;
89         z += data[10] * vz;
90         z += data[14]; // *1
91         return new Vector3(x, y, z);
92     }
93 
94     /** Multiply two Matrices together and store the result in a third Matrix. */
multiply(Matrix lhs, Matrix rhs)95     public static Matrix multiply(Matrix lhs, Matrix rhs) {
96         float[] result = new float[16];
97 
98         for (int row = 0; row < 4; row++) {
99             for (int col = 0; col < 4; col++) {
100                 float sum = 0;
101                 for (int k = 0; k < 4; k++) {
102                     sum += lhs.data[row * 4 + k] * rhs.data[k * 4 + col];
103                 }
104                 result[row * 4 + col] = sum;
105             }
106         }
107         return new Matrix(result);
108     }
109 
110     /** Returns a matrix that is an inversion of this one. */
invert()111     public Matrix invert() {
112         float a00 = data[0], a01 = data[1], a02 = data[2], a03 = data[3];
113         float a10 = data[4], a11 = data[5], a12 = data[6], a13 = data[7];
114         float a20 = data[8], a21 = data[9], a22 = data[10], a23 = data[11];
115         float a30 = data[12], a31 = data[13], a32 = data[14], a33 = data[15];
116 
117         float b00 = a00 * a11 - a01 * a10;
118         float b01 = a00 * a12 - a02 * a10;
119         float b02 = a00 * a13 - a03 * a10;
120         float b03 = a01 * a12 - a02 * a11;
121         float b04 = a01 * a13 - a03 * a11;
122         float b05 = a02 * a13 - a03 * a12;
123         float b06 = a20 * a31 - a21 * a30;
124         float b07 = a20 * a32 - a22 * a30;
125         float b08 = a20 * a33 - a23 * a30;
126         float b09 = a21 * a32 - a22 * a31;
127         float b10 = a21 * a33 - a23 * a31;
128         float b11 = a22 * a33 - a23 * a32;
129 
130         float det = b00 * b11 - b01 * b10 + b02 * b09 + b03 * b08 - b04 * b07 + b05 * b06;
131         if (det == 0) {
132             return null;
133         }
134 
135         float[] m = new float[16];
136 
137         float invDet = 1.0f / det;
138         m[0] = (a11 * b11 - a12 * b10 + a13 * b09) * invDet;
139         m[1] = (-a01 * b11 + a02 * b10 - a03 * b09) * invDet;
140         m[2] = (a31 * b05 - a32 * b04 + a33 * b03) * invDet;
141         m[3] = (-a21 * b05 + a22 * b04 - a23 * b03) * invDet;
142         m[4] = (-a10 * b11 + a12 * b08 - a13 * b07) * invDet;
143         m[5] = (a00 * b11 - a02 * b08 + a03 * b07) * invDet;
144         m[6] = (-a30 * b05 + a32 * b02 - a33 * b01) * invDet;
145         m[7] = (a20 * b05 - a22 * b02 + a23 * b01) * invDet;
146         m[8] = (a10 * b10 - a11 * b08 + a13 * b06) * invDet;
147         m[9] = (-a00 * b10 + a01 * b08 - a03 * b06) * invDet;
148         m[10] = (a30 * b04 - a31 * b02 + a33 * b00) * invDet;
149         m[11] = (-a20 * b04 + a21 * b02 - a23 * b00) * invDet;
150         m[12] = (-a10 * b09 + a11 * b07 - a12 * b06) * invDet;
151         m[13] = (a00 * b09 - a01 * b07 + a02 * b06) * invDet;
152         m[14] = (-a30 * b03 + a31 * b01 - a32 * b00) * invDet;
153         m[15] = (a20 * b03 - a21 * b01 + a22 * b00) * invDet;
154         return new Matrix(m);
155     }
156 
157     /** Set the translation component of this Matrix. */
setTranslation(Vector3 translation)158     public void setTranslation(Vector3 translation) {
159         data[12] = translation.x;
160         data[13] = translation.y;
161         data[14] = translation.z;
162     }
163 
164     /** Make this a translation Matrix. */
makeTranslation(Vector3 translation)165     public void makeTranslation(Vector3 translation) {
166         set(IDENTITY_DATA);
167         setTranslation(translation);
168     }
169 
170     /** Make this a rotation Matrix. */
makeRotation(Quaternion q)171     public void makeRotation(Quaternion q) {
172         set(IDENTITY_DATA);
173 
174         float tx = 2 * q.x;
175         float ty = 2 * q.y;
176         float tz = 2 * q.z;
177 
178         float mdsqx = 1 - tx * q.x;
179         float dqxy = ty * q.x;
180         float dsqy = ty * q.y;
181         float dqyz = tz * q.y;
182         float dsqz = tz * q.z;
183         float dqxz = tx * q.z;
184         float dqxw = tx * q.w;
185         float dqyw = ty * q.w;
186         float dqzw = tz * q.w;
187 
188         data[0] = 1 - dsqy - dsqz;
189         data[4] = dqxy - dqzw;
190         data[8] = dqxz + dqyw;
191 
192         data[1] = dqxy + dqzw;
193         data[5] = mdsqx - dsqz;
194         data[9] = dqyz - dqxw;
195 
196         data[2] = dqxz - dqyw;
197         data[6] = dqyz + dqxw;
198         data[10] = mdsqx - dsqy;
199     }
200 
201     /** Make this a rigid transform Matrix. */
makeRigidTransform(Pose pose)202     public void makeRigidTransform(Pose pose) {
203         makeRotation(pose.rotation);
204         setTranslation(pose.translation);
205     }
206 
207     /** Make this a uniform scale Matrix. */
makeScale(float scale)208     public void makeScale(float scale) {
209         set(IDENTITY_DATA);
210 
211         data[0] = scale;
212         data[5] = scale;
213         data[10] = scale;
214     }
215 
216     /** Make this a scale Matrix. */
makeScale(Vector3 scale)217     public void makeScale(Vector3 scale) {
218         set(IDENTITY_DATA);
219 
220         data[0] = scale.x;
221         data[5] = scale.y;
222         data[10] = scale.z;
223     }
224 
225     /**
226      * Make this a translation, rotation and scale Matrix. See {@link
227      * #newTrs(Vector3,Quaternion,Vector3 )}.
228      */
makeTrs(Vector3 translation, Quaternion q, Vector3 scale)229     public void makeTrs(Vector3 translation, Quaternion q, Vector3 scale) {
230         float tx = 2 * q.x;
231         float ty = 2 * q.y;
232         float tz = 2 * q.z;
233 
234         float mdsqx = 1 - tx * q.x;
235         float dqxy = ty * q.x;
236         float dsqy = ty * q.y;
237         float dqyz = tz * q.y;
238         float dsqz = tz * q.z;
239         float dqxz = tx * q.z;
240         float dqxw = tx * q.w;
241         float dqyw = ty * q.w;
242         float dqzw = tz * q.w;
243 
244         data[0] = (1 - dsqy - dsqz) * scale.x;
245         data[4] = (dqxy - dqzw) * scale.y;
246         data[8] = (dqxz + dqyw) * scale.z;
247 
248         data[1] = (dqxy + dqzw) * scale.x;
249         data[5] = (mdsqx - dsqz) * scale.y;
250         data[9] = (dqyz - dqxw) * scale.z;
251 
252         data[2] = (dqxz - dqyw) * scale.x;
253         data[6] = (dqyz + dqxw) * scale.y;
254         data[10] = (mdsqx - dsqy) * scale.z;
255 
256         data[12] = translation.x;
257         data[13] = translation.y;
258         data[14] = translation.z;
259 
260         data[3] = 0.0f;
261         data[7] = 0.0f;
262         data[11] = 0.0f;
263         data[15] = 1.0f;
264     }
265 
266     /** Create a new translation Matrix. */
newTranslation(Vector3 translation)267     public static Matrix newTranslation(Vector3 translation) {
268         Matrix m = new Matrix();
269         m.makeTranslation(translation);
270         return m;
271     }
272 
273     /** Create a new rotation Matrix. */
newRotation(Quaternion rotation)274     public static Matrix newRotation(Quaternion rotation) {
275         Matrix m = new Matrix();
276         m.makeRotation(rotation);
277         return m;
278     }
279 
280     /** Create a new rigid transform Matrix. */
newRigidTransform(Pose pose)281     public static Matrix newRigidTransform(Pose pose) {
282         Matrix m = new Matrix();
283         m.makeRigidTransform(pose);
284         return m;
285     }
286 
287     /** Create a new uniform scale Matrix. */
newScale(float scale)288     public static Matrix newScale(float scale) {
289         Matrix m = new Matrix();
290         m.makeScale(scale);
291         return m;
292     }
293 
294     /** Create a new scale Matrix. */
newScale(Vector3 scale)295     public static Matrix newScale(Vector3 scale) {
296         Matrix m = new Matrix();
297         m.makeScale(scale);
298         return m;
299     }
300 
301     /**
302      * Creates a new translation, rotation and scale Matrix. The returned matrix is such that it
303      * first scales objects, then rotates them, and finally translates them.
304      *
305      * <p>See
306      * https://www.opengl-tutorial.org/beginners-tutorials/tutorial-3-matrices/#cumulating-transformations
307      */
newTrs(Vector3 translation, Quaternion rotation, Vector3 scale)308     public static Matrix newTrs(Vector3 translation, Quaternion rotation, Vector3 scale) {
309         Matrix m = new Matrix();
310         m.makeTrs(translation, rotation, scale);
311         return m;
312     }
313 
314     @NonNull
315     @Override
toString()316     public String toString() {
317         return String.format(Locale.getDefault(),
318             "% 5.1f, % 5.1f, % 5.1f, % 5.1f / "
319                 + "% 5.1f, % 5.1f, % 5.1f, % 5.1f / "
320                 + "% 5.1f, % 5.1f, % 5.1f, % 5.1f / "
321                 + "% 5.1f, % 5.1f, % 5.1f, % 5.1f",
322             data[0], data[1], data[2], data[3],
323             data[4], data[5], data[6], data[7],
324             data[8], data[9], data[10], data[11],
325             data[12], data[13], data[14], data[15]
326             );
327     }
328 }
329