1 /*
2  * Copyright (C) 2015 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.example.android.rs.vr.engine;
17 
18 import android.util.Log;
19 
20 import java.text.*;
21 import java.util.*;
22 
23 /**
24  * Generic matrix code
25  * Written for maximum portability between desktop and Android
26  * Not in performance critical sections
27  */
28 public class Matrix {
29 
30     private static final String LOGTAG = "Matrix";
31     public double[] m;
32 
makeRotation()33     public void makeRotation() {
34         {
35             double[] v = {m[0], m[4], m[8]};
36             VectorUtil.normalize(v);
37             m[0] = v[0];
38             m[4] = v[1];
39             m[8] = v[2];
40         }
41         {
42             double[] v = {m[1], m[5], m[9]};
43             VectorUtil.normalize(v);
44             m[1] = v[0];
45             m[5] = v[1];
46             m[9] = v[2];
47         }
48         {
49             double[] v = {m[2], m[6], m[10]};
50             VectorUtil.normalize(v);
51             m[2] = v[0];
52             m[6] = v[1];
53             m[10] = v[2];
54         }
55 
56     }
57 
trim(String s)58     private static String trim(String s) {
59         return s.substring(s.length() - 7);
60     }
61 
print()62     public void print() {
63         DecimalFormat df = new DecimalFormat("      ##0.000");
64         for (int i = 0; i < 4; i++) {
65             String s="";
66             for (int j = 0; j < 4; j++) {
67                s+= ((j == 0) ? "[ " : " , ") + trim("       "+df.format(m[i * 4 + j]));
68             }
69             Log.v(LOGTAG, s+"]");
70         }
71     }
72 
Matrix()73     public Matrix() {
74         m = new double[4 * 4];
75         setToUnit();
76     }
77 
Matrix(Matrix matrix)78     public Matrix(Matrix matrix) {
79         this(Arrays.copyOf(matrix.m, matrix.m.length));
80     }
81 
Matrix(double[] m)82     protected Matrix(double[] m) {
83         this.m = m;
84     }
85 
setToUnit()86     public void setToUnit() {
87         for (int i = 1; i < m.length; i++) {
88             m[i] = 0;
89         }
90         m[0] = 1;
91         m[5] = 1;
92         m[10] = 1;
93         m[15] = 1;
94     }
95 
mult4(float[] src, float[] dest)96     public void mult4(float[] src, float[] dest) {
97         for (int i = 0; i < 4; i++) {
98             int col = i * 4;
99             double sum = 0;
100             for (int j = 0; j < 4; j++) {
101                 sum += m[col + j] * src[j];
102             }
103             dest[i] = (float) sum;
104         }
105     }
106 
mult3(float[] src, float[] dest)107     public void mult3(float[] src, float[] dest) {
108         for (int i = 0; i < 3; i++) {
109             int col = i * 4;
110             double sum = m[col + 3];
111             for (int j = 0; j < 3; j++) {
112                 sum += m[col + j] * src[j];
113             }
114             dest[i] = (float) sum;
115         }
116     }
117 
mult3v(float[] src, float[] dest)118     public void mult3v(float[] src, float[] dest) {
119         for (int i = 0; i < 3; i++) {
120             int col = i * 4;
121             double sum = 0;
122             for (int j = 0; j < 3; j++) {
123                 sum += m[col + j] * src[j];
124             }
125             dest[i] = (float) sum;
126         }
127     }
128 
mult4(double[] src, double[] dest)129     public void mult4(double[] src, double[] dest) {
130         for (int i = 0; i < 4; i++) {
131             int col = i * 4;
132             double sum = 0;
133             for (int j = 0; j < 4; j++) {
134                 sum += m[col + j] * src[j];
135             }
136             dest[i] = (float) sum;
137         }
138     }
139 
mult3(double[] src, double[] dest)140     public void mult3(double[] src, double[] dest) {
141         for (int i = 0; i < 3; i++) {
142             int col = i * 4;
143             double sum = m[col + 3];
144             for (int j = 0; j < 3; j++) {
145                 sum += m[col + j] * src[j];
146             }
147             dest[i] = (float) sum;
148         }
149     }
150 
mult3v(double[] src, double[] dest)151     public void mult3v(double[] src, double[] dest) {
152         for (int i = 0; i < 3; i++) {
153             int col = i * 4;
154             double sum = 0;
155             for (int j = 0; j < 3; j++) {
156                 sum += m[col + j] * src[j];
157             }
158             dest[i] = (float) sum;
159         }
160     }
161 
vecmult(double[] src)162     public double[] vecmult(double[] src) {
163         double[] ret = new double[3];
164         mult3v(src, ret);
165         return ret;
166     }
167 
mult3(float[] src, int off1, float[] dest, int off2)168     public void mult3(float[] src, int off1, float[] dest, int off2) {
169 
170         int col = 0 * 4;
171         double sum = m[col + 3];
172         for (int j = 0; j < 3; j++) {
173             sum += m[col + j] * src[j + off1];
174         }
175         float v0 = (float) sum;
176 
177 
178         col = 1 * 4;
179         sum = m[col + 3];
180         for (int j = 0; j < 3; j++) {
181             sum += m[col + j] * src[j + off1];
182         }
183 
184         float v1 = (float) sum;
185 
186 
187         col = 2 * 4;
188         sum = m[col + 3];
189         for (int j = 0; j < 3; j++) {
190             sum += m[col + j] * src[j + off1];
191         }
192         float v2 = (float) sum;
193 
194         dest[off2] = v0;
195         dest[1 + off2] = v1;
196         dest[2 + off2] = v2;
197 
198     }
199 
invers()200     public Matrix invers() {
201         double[] inv = new double[16];
202 
203         inv[0] = m[5] * m[10] * m[15] -
204                 m[5] * m[11] * m[14] -
205                 m[9] * m[6] * m[15] +
206                 m[9] * m[7] * m[14] +
207                 m[13] * m[6] * m[11] -
208                 m[13] * m[7] * m[10];
209 
210         inv[4] = -m[4] * m[10] * m[15] +
211                 m[4] * m[11] * m[14] +
212                 m[8] * m[6] * m[15] -
213                 m[8] * m[7] * m[14] -
214                 m[12] * m[6] * m[11] +
215                 m[12] * m[7] * m[10];
216 
217         inv[8] = m[4] * m[9] * m[15] -
218                 m[4] * m[11] * m[13] -
219                 m[8] * m[5] * m[15] +
220                 m[8] * m[7] * m[13] +
221                 m[12] * m[5] * m[11] -
222                 m[12] * m[7] * m[9];
223 
224         inv[12] = -m[4] * m[9] * m[14] +
225                 m[4] * m[10] * m[13] +
226                 m[8] * m[5] * m[14] -
227                 m[8] * m[6] * m[13] -
228                 m[12] * m[5] * m[10] +
229                 m[12] * m[6] * m[9];
230 
231         inv[1] = -m[1] * m[10] * m[15] +
232                 m[1] * m[11] * m[14] +
233                 m[9] * m[2] * m[15] -
234                 m[9] * m[3] * m[14] -
235                 m[13] * m[2] * m[11] +
236                 m[13] * m[3] * m[10];
237 
238         inv[5] = m[0] * m[10] * m[15] -
239                 m[0] * m[11] * m[14] -
240                 m[8] * m[2] * m[15] +
241                 m[8] * m[3] * m[14] +
242                 m[12] * m[2] * m[11] -
243                 m[12] * m[3] * m[10];
244 
245         inv[9] = -m[0] * m[9] * m[15] +
246                 m[0] * m[11] * m[13] +
247                 m[8] * m[1] * m[15] -
248                 m[8] * m[3] * m[13] -
249                 m[12] * m[1] * m[11] +
250                 m[12] * m[3] * m[9];
251 
252         inv[13] = m[0] * m[9] * m[14] -
253                 m[0] * m[10] * m[13] -
254                 m[8] * m[1] * m[14] +
255                 m[8] * m[2] * m[13] +
256                 m[12] * m[1] * m[10] -
257                 m[12] * m[2] * m[9];
258 
259         inv[2] = m[1] * m[6] * m[15] -
260                 m[1] * m[7] * m[14] -
261                 m[5] * m[2] * m[15] +
262                 m[5] * m[3] * m[14] +
263                 m[13] * m[2] * m[7] -
264                 m[13] * m[3] * m[6];
265 
266         inv[6] = -m[0] * m[6] * m[15] +
267                 m[0] * m[7] * m[14] +
268                 m[4] * m[2] * m[15] -
269                 m[4] * m[3] * m[14] -
270                 m[12] * m[2] * m[7] +
271                 m[12] * m[3] * m[6];
272 
273         inv[10] = m[0] * m[5] * m[15] -
274                 m[0] * m[7] * m[13] -
275                 m[4] * m[1] * m[15] +
276                 m[4] * m[3] * m[13] +
277                 m[12] * m[1] * m[7] -
278                 m[12] * m[3] * m[5];
279 
280         inv[14] = -m[0] * m[5] * m[14] +
281                 m[0] * m[6] * m[13] +
282                 m[4] * m[1] * m[14] -
283                 m[4] * m[2] * m[13] -
284                 m[12] * m[1] * m[6] +
285                 m[12] * m[2] * m[5];
286 
287         inv[3] = -m[1] * m[6] * m[11] +
288                 m[1] * m[7] * m[10] +
289                 m[5] * m[2] * m[11] -
290                 m[5] * m[3] * m[10] -
291                 m[9] * m[2] * m[7] +
292                 m[9] * m[3] * m[6];
293 
294         inv[7] = m[0] * m[6] * m[11] -
295                 m[0] * m[7] * m[10] -
296                 m[4] * m[2] * m[11] +
297                 m[4] * m[3] * m[10] +
298                 m[8] * m[2] * m[7] -
299                 m[8] * m[3] * m[6];
300 
301         inv[11] = -m[0] * m[5] * m[11] +
302                 m[0] * m[7] * m[9] +
303                 m[4] * m[1] * m[11] -
304                 m[4] * m[3] * m[9] -
305                 m[8] * m[1] * m[7] +
306                 m[8] * m[3] * m[5];
307 
308         inv[15] = m[0] * m[5] * m[10] -
309                 m[0] * m[6] * m[9] -
310                 m[4] * m[1] * m[10] +
311                 m[4] * m[2] * m[9] +
312                 m[8] * m[1] * m[6] -
313                 m[8] * m[2] * m[5];
314 
315 
316         double det;
317         det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12];
318 
319         if (det == 0) {
320             return null;
321         }
322 
323         det = 1.0 / det;
324 
325         double[] out = new double[16];
326 
327         for (int i = 0; i < 16; i++) {
328             out[i] = inv[i] * det;
329         }
330 
331         Matrix ret = new Matrix(out);
332         return ret;
333     }
334 
getAsFloats(float[] fm)335     public void getAsFloats(float[] fm) {
336         for (int y = 0; y < 4; y++) {
337             int col = y * 4;
338 
339             for (int x = 0; x < 4; x++)
340                 fm[y + x * 4] = (float) m[x + y * 4];
341         }
342     }
343 
mult(Matrix b)344     Matrix mult(Matrix b) {
345         return new Matrix(multiply(this.m, b.m));
346     }
347 
premult(Matrix b)348     Matrix premult(Matrix b) {
349         return new Matrix(multiply(b.m, this.m));
350     }
351 
multiply(double a[], double b[])352     private static double[] multiply(double a[], double b[]) {
353         double[] resultant = new double[16];
354         for (int i = 0; i < 4; i++) {
355             for (int j = 0; j < 4; j++) {
356                 for (int k = 0; k < 4; k++) {
357                     resultant[i + 4 * j] += a[i + 4 * k] * b[k + 4 * j];
358                 }
359             }
360         }
361 
362         return resultant;
363     }
364 
clone(Matrix src)365     public void clone(Matrix src) {
366         System.arraycopy(src.m, 0, m, 0, m.length);
367     }
368 }
369