15b539461dcc159bd89297443780d635ccc5e3564John Hoford/* 25b539461dcc159bd89297443780d635ccc5e3564John Hoford * Copyright (C) 2015 The Android Open Source Project 35b539461dcc159bd89297443780d635ccc5e3564John Hoford * 45b539461dcc159bd89297443780d635ccc5e3564John Hoford * Licensed under the Apache License, Version 2.0 (the "License"); 55b539461dcc159bd89297443780d635ccc5e3564John Hoford * you may not use this file except in compliance with the License. 65b539461dcc159bd89297443780d635ccc5e3564John Hoford * You may obtain a copy of the License at 75b539461dcc159bd89297443780d635ccc5e3564John Hoford * 85b539461dcc159bd89297443780d635ccc5e3564John Hoford * http://www.apache.org/licenses/LICENSE-2.0 95b539461dcc159bd89297443780d635ccc5e3564John Hoford * 105b539461dcc159bd89297443780d635ccc5e3564John Hoford * Unless required by applicable law or agreed to in writing, software 115b539461dcc159bd89297443780d635ccc5e3564John Hoford * distributed under the License is distributed on an "AS IS" BASIS, 125b539461dcc159bd89297443780d635ccc5e3564John Hoford * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 135b539461dcc159bd89297443780d635ccc5e3564John Hoford * See the License for the specific language governing permissions and 145b539461dcc159bd89297443780d635ccc5e3564John Hoford * limitations under the License. 155b539461dcc159bd89297443780d635ccc5e3564John Hoford */ 165b539461dcc159bd89297443780d635ccc5e3564John Hofordpackage com.example.android.rs.vr.engine; 175b539461dcc159bd89297443780d635ccc5e3564John Hoford 185b539461dcc159bd89297443780d635ccc5e3564John Hofordimport android.util.Log; 195b539461dcc159bd89297443780d635ccc5e3564John Hoford 205b539461dcc159bd89297443780d635ccc5e3564John Hofordimport java.text.*; 215b539461dcc159bd89297443780d635ccc5e3564John Hofordimport java.util.*; 225b539461dcc159bd89297443780d635ccc5e3564John Hoford 235b539461dcc159bd89297443780d635ccc5e3564John Hoford/** 245b539461dcc159bd89297443780d635ccc5e3564John Hoford * Generic matrix code 255b539461dcc159bd89297443780d635ccc5e3564John Hoford * Written for maximum portability between desktop and Android 265b539461dcc159bd89297443780d635ccc5e3564John Hoford * Not in performance critical sections 275b539461dcc159bd89297443780d635ccc5e3564John Hoford */ 285b539461dcc159bd89297443780d635ccc5e3564John Hofordpublic class Matrix { 295b539461dcc159bd89297443780d635ccc5e3564John Hoford 305b539461dcc159bd89297443780d635ccc5e3564John Hoford private static final String LOGTAG = "Matrix"; 315b539461dcc159bd89297443780d635ccc5e3564John Hoford public double[] m; 325b539461dcc159bd89297443780d635ccc5e3564John Hoford 335b539461dcc159bd89297443780d635ccc5e3564John Hoford public void makeRotation() { 345b539461dcc159bd89297443780d635ccc5e3564John Hoford { 355b539461dcc159bd89297443780d635ccc5e3564John Hoford double[] v = {m[0], m[4], m[8]}; 365b539461dcc159bd89297443780d635ccc5e3564John Hoford VectorUtil.normalize(v); 375b539461dcc159bd89297443780d635ccc5e3564John Hoford m[0] = v[0]; 385b539461dcc159bd89297443780d635ccc5e3564John Hoford m[4] = v[1]; 395b539461dcc159bd89297443780d635ccc5e3564John Hoford m[8] = v[2]; 405b539461dcc159bd89297443780d635ccc5e3564John Hoford } 415b539461dcc159bd89297443780d635ccc5e3564John Hoford { 425b539461dcc159bd89297443780d635ccc5e3564John Hoford double[] v = {m[1], m[5], m[9]}; 435b539461dcc159bd89297443780d635ccc5e3564John Hoford VectorUtil.normalize(v); 445b539461dcc159bd89297443780d635ccc5e3564John Hoford m[1] = v[0]; 455b539461dcc159bd89297443780d635ccc5e3564John Hoford m[5] = v[1]; 465b539461dcc159bd89297443780d635ccc5e3564John Hoford m[9] = v[2]; 475b539461dcc159bd89297443780d635ccc5e3564John Hoford } 485b539461dcc159bd89297443780d635ccc5e3564John Hoford { 495b539461dcc159bd89297443780d635ccc5e3564John Hoford double[] v = {m[2], m[6], m[10]}; 505b539461dcc159bd89297443780d635ccc5e3564John Hoford VectorUtil.normalize(v); 515b539461dcc159bd89297443780d635ccc5e3564John Hoford m[2] = v[0]; 525b539461dcc159bd89297443780d635ccc5e3564John Hoford m[6] = v[1]; 535b539461dcc159bd89297443780d635ccc5e3564John Hoford m[10] = v[2]; 545b539461dcc159bd89297443780d635ccc5e3564John Hoford } 555b539461dcc159bd89297443780d635ccc5e3564John Hoford 565b539461dcc159bd89297443780d635ccc5e3564John Hoford } 575b539461dcc159bd89297443780d635ccc5e3564John Hoford 585b539461dcc159bd89297443780d635ccc5e3564John Hoford private static String trim(String s) { 595b539461dcc159bd89297443780d635ccc5e3564John Hoford return s.substring(s.length() - 7); 605b539461dcc159bd89297443780d635ccc5e3564John Hoford } 615b539461dcc159bd89297443780d635ccc5e3564John Hoford 625b539461dcc159bd89297443780d635ccc5e3564John Hoford public void print() { 635b539461dcc159bd89297443780d635ccc5e3564John Hoford DecimalFormat df = new DecimalFormat(" ##0.000"); 645b539461dcc159bd89297443780d635ccc5e3564John Hoford for (int i = 0; i < 4; i++) { 655b539461dcc159bd89297443780d635ccc5e3564John Hoford String s=""; 665b539461dcc159bd89297443780d635ccc5e3564John Hoford for (int j = 0; j < 4; j++) { 675b539461dcc159bd89297443780d635ccc5e3564John Hoford s+= ((j == 0) ? "[ " : " , ") + trim(" "+df.format(m[i * 4 + j])); 685b539461dcc159bd89297443780d635ccc5e3564John Hoford } 695b539461dcc159bd89297443780d635ccc5e3564John Hoford Log.v(LOGTAG, s+"]"); 705b539461dcc159bd89297443780d635ccc5e3564John Hoford } 715b539461dcc159bd89297443780d635ccc5e3564John Hoford } 725b539461dcc159bd89297443780d635ccc5e3564John Hoford 735b539461dcc159bd89297443780d635ccc5e3564John Hoford public Matrix() { 745b539461dcc159bd89297443780d635ccc5e3564John Hoford m = new double[4 * 4]; 755b539461dcc159bd89297443780d635ccc5e3564John Hoford setToUnit(); 765b539461dcc159bd89297443780d635ccc5e3564John Hoford } 775b539461dcc159bd89297443780d635ccc5e3564John Hoford 785b539461dcc159bd89297443780d635ccc5e3564John Hoford public Matrix(Matrix matrix) { 795b539461dcc159bd89297443780d635ccc5e3564John Hoford this(Arrays.copyOf(matrix.m, matrix.m.length)); 805b539461dcc159bd89297443780d635ccc5e3564John Hoford } 815b539461dcc159bd89297443780d635ccc5e3564John Hoford 825b539461dcc159bd89297443780d635ccc5e3564John Hoford protected Matrix(double[] m) { 835b539461dcc159bd89297443780d635ccc5e3564John Hoford this.m = m; 845b539461dcc159bd89297443780d635ccc5e3564John Hoford } 855b539461dcc159bd89297443780d635ccc5e3564John Hoford 865b539461dcc159bd89297443780d635ccc5e3564John Hoford public void setToUnit() { 875b539461dcc159bd89297443780d635ccc5e3564John Hoford for (int i = 1; i < m.length; i++) { 885b539461dcc159bd89297443780d635ccc5e3564John Hoford m[i] = 0; 895b539461dcc159bd89297443780d635ccc5e3564John Hoford } 905b539461dcc159bd89297443780d635ccc5e3564John Hoford m[0] = 1; 915b539461dcc159bd89297443780d635ccc5e3564John Hoford m[5] = 1; 925b539461dcc159bd89297443780d635ccc5e3564John Hoford m[10] = 1; 935b539461dcc159bd89297443780d635ccc5e3564John Hoford m[15] = 1; 945b539461dcc159bd89297443780d635ccc5e3564John Hoford } 955b539461dcc159bd89297443780d635ccc5e3564John Hoford 965b539461dcc159bd89297443780d635ccc5e3564John Hoford public void mult4(float[] src, float[] dest) { 975b539461dcc159bd89297443780d635ccc5e3564John Hoford for (int i = 0; i < 4; i++) { 985b539461dcc159bd89297443780d635ccc5e3564John Hoford int col = i * 4; 995b539461dcc159bd89297443780d635ccc5e3564John Hoford double sum = 0; 1005b539461dcc159bd89297443780d635ccc5e3564John Hoford for (int j = 0; j < 4; j++) { 1015b539461dcc159bd89297443780d635ccc5e3564John Hoford sum += m[col + j] * src[j]; 1025b539461dcc159bd89297443780d635ccc5e3564John Hoford } 1035b539461dcc159bd89297443780d635ccc5e3564John Hoford dest[i] = (float) sum; 1045b539461dcc159bd89297443780d635ccc5e3564John Hoford } 1055b539461dcc159bd89297443780d635ccc5e3564John Hoford } 1065b539461dcc159bd89297443780d635ccc5e3564John Hoford 1075b539461dcc159bd89297443780d635ccc5e3564John Hoford public void mult3(float[] src, float[] dest) { 1085b539461dcc159bd89297443780d635ccc5e3564John Hoford for (int i = 0; i < 3; i++) { 1095b539461dcc159bd89297443780d635ccc5e3564John Hoford int col = i * 4; 1105b539461dcc159bd89297443780d635ccc5e3564John Hoford double sum = m[col + 3]; 1115b539461dcc159bd89297443780d635ccc5e3564John Hoford for (int j = 0; j < 3; j++) { 1125b539461dcc159bd89297443780d635ccc5e3564John Hoford sum += m[col + j] * src[j]; 1135b539461dcc159bd89297443780d635ccc5e3564John Hoford } 1145b539461dcc159bd89297443780d635ccc5e3564John Hoford dest[i] = (float) sum; 1155b539461dcc159bd89297443780d635ccc5e3564John Hoford } 1165b539461dcc159bd89297443780d635ccc5e3564John Hoford } 1175b539461dcc159bd89297443780d635ccc5e3564John Hoford 1185b539461dcc159bd89297443780d635ccc5e3564John Hoford public void mult3v(float[] src, float[] dest) { 1195b539461dcc159bd89297443780d635ccc5e3564John Hoford for (int i = 0; i < 3; i++) { 1205b539461dcc159bd89297443780d635ccc5e3564John Hoford int col = i * 4; 1215b539461dcc159bd89297443780d635ccc5e3564John Hoford double sum = 0; 1225b539461dcc159bd89297443780d635ccc5e3564John Hoford for (int j = 0; j < 3; j++) { 1235b539461dcc159bd89297443780d635ccc5e3564John Hoford sum += m[col + j] * src[j]; 1245b539461dcc159bd89297443780d635ccc5e3564John Hoford } 1255b539461dcc159bd89297443780d635ccc5e3564John Hoford dest[i] = (float) sum; 1265b539461dcc159bd89297443780d635ccc5e3564John Hoford } 1275b539461dcc159bd89297443780d635ccc5e3564John Hoford } 1285b539461dcc159bd89297443780d635ccc5e3564John Hoford 1295b539461dcc159bd89297443780d635ccc5e3564John Hoford public void mult4(double[] src, double[] dest) { 1305b539461dcc159bd89297443780d635ccc5e3564John Hoford for (int i = 0; i < 4; i++) { 1315b539461dcc159bd89297443780d635ccc5e3564John Hoford int col = i * 4; 1325b539461dcc159bd89297443780d635ccc5e3564John Hoford double sum = 0; 1335b539461dcc159bd89297443780d635ccc5e3564John Hoford for (int j = 0; j < 4; j++) { 1345b539461dcc159bd89297443780d635ccc5e3564John Hoford sum += m[col + j] * src[j]; 1355b539461dcc159bd89297443780d635ccc5e3564John Hoford } 1365b539461dcc159bd89297443780d635ccc5e3564John Hoford dest[i] = (float) sum; 1375b539461dcc159bd89297443780d635ccc5e3564John Hoford } 1385b539461dcc159bd89297443780d635ccc5e3564John Hoford } 1395b539461dcc159bd89297443780d635ccc5e3564John Hoford 1405b539461dcc159bd89297443780d635ccc5e3564John Hoford public void mult3(double[] src, double[] dest) { 1415b539461dcc159bd89297443780d635ccc5e3564John Hoford for (int i = 0; i < 3; i++) { 1425b539461dcc159bd89297443780d635ccc5e3564John Hoford int col = i * 4; 1435b539461dcc159bd89297443780d635ccc5e3564John Hoford double sum = m[col + 3]; 1445b539461dcc159bd89297443780d635ccc5e3564John Hoford for (int j = 0; j < 3; j++) { 1455b539461dcc159bd89297443780d635ccc5e3564John Hoford sum += m[col + j] * src[j]; 1465b539461dcc159bd89297443780d635ccc5e3564John Hoford } 1475b539461dcc159bd89297443780d635ccc5e3564John Hoford dest[i] = (float) sum; 1485b539461dcc159bd89297443780d635ccc5e3564John Hoford } 1495b539461dcc159bd89297443780d635ccc5e3564John Hoford } 1505b539461dcc159bd89297443780d635ccc5e3564John Hoford 1515b539461dcc159bd89297443780d635ccc5e3564John Hoford public void mult3v(double[] src, double[] dest) { 1525b539461dcc159bd89297443780d635ccc5e3564John Hoford for (int i = 0; i < 3; i++) { 1535b539461dcc159bd89297443780d635ccc5e3564John Hoford int col = i * 4; 1545b539461dcc159bd89297443780d635ccc5e3564John Hoford double sum = 0; 1555b539461dcc159bd89297443780d635ccc5e3564John Hoford for (int j = 0; j < 3; j++) { 1565b539461dcc159bd89297443780d635ccc5e3564John Hoford sum += m[col + j] * src[j]; 1575b539461dcc159bd89297443780d635ccc5e3564John Hoford } 1585b539461dcc159bd89297443780d635ccc5e3564John Hoford dest[i] = (float) sum; 1595b539461dcc159bd89297443780d635ccc5e3564John Hoford } 1605b539461dcc159bd89297443780d635ccc5e3564John Hoford } 1615b539461dcc159bd89297443780d635ccc5e3564John Hoford 1625b539461dcc159bd89297443780d635ccc5e3564John Hoford public double[] vecmult(double[] src) { 1635b539461dcc159bd89297443780d635ccc5e3564John Hoford double[] ret = new double[3]; 1645b539461dcc159bd89297443780d635ccc5e3564John Hoford mult3v(src, ret); 1655b539461dcc159bd89297443780d635ccc5e3564John Hoford return ret; 1665b539461dcc159bd89297443780d635ccc5e3564John Hoford } 1675b539461dcc159bd89297443780d635ccc5e3564John Hoford 1685b539461dcc159bd89297443780d635ccc5e3564John Hoford public void mult3(float[] src, int off1, float[] dest, int off2) { 1695b539461dcc159bd89297443780d635ccc5e3564John Hoford 1705b539461dcc159bd89297443780d635ccc5e3564John Hoford int col = 0 * 4; 1715b539461dcc159bd89297443780d635ccc5e3564John Hoford double sum = m[col + 3]; 1725b539461dcc159bd89297443780d635ccc5e3564John Hoford for (int j = 0; j < 3; j++) { 1735b539461dcc159bd89297443780d635ccc5e3564John Hoford sum += m[col + j] * src[j + off1]; 1745b539461dcc159bd89297443780d635ccc5e3564John Hoford } 1755b539461dcc159bd89297443780d635ccc5e3564John Hoford float v0 = (float) sum; 1765b539461dcc159bd89297443780d635ccc5e3564John Hoford 1775b539461dcc159bd89297443780d635ccc5e3564John Hoford 1785b539461dcc159bd89297443780d635ccc5e3564John Hoford col = 1 * 4; 1795b539461dcc159bd89297443780d635ccc5e3564John Hoford sum = m[col + 3]; 1805b539461dcc159bd89297443780d635ccc5e3564John Hoford for (int j = 0; j < 3; j++) { 1815b539461dcc159bd89297443780d635ccc5e3564John Hoford sum += m[col + j] * src[j + off1]; 1825b539461dcc159bd89297443780d635ccc5e3564John Hoford } 1835b539461dcc159bd89297443780d635ccc5e3564John Hoford 1845b539461dcc159bd89297443780d635ccc5e3564John Hoford float v1 = (float) sum; 1855b539461dcc159bd89297443780d635ccc5e3564John Hoford 1865b539461dcc159bd89297443780d635ccc5e3564John Hoford 1875b539461dcc159bd89297443780d635ccc5e3564John Hoford col = 2 * 4; 1885b539461dcc159bd89297443780d635ccc5e3564John Hoford sum = m[col + 3]; 1895b539461dcc159bd89297443780d635ccc5e3564John Hoford for (int j = 0; j < 3; j++) { 1905b539461dcc159bd89297443780d635ccc5e3564John Hoford sum += m[col + j] * src[j + off1]; 1915b539461dcc159bd89297443780d635ccc5e3564John Hoford } 1925b539461dcc159bd89297443780d635ccc5e3564John Hoford float v2 = (float) sum; 1935b539461dcc159bd89297443780d635ccc5e3564John Hoford 1945b539461dcc159bd89297443780d635ccc5e3564John Hoford dest[off2] = v0; 1955b539461dcc159bd89297443780d635ccc5e3564John Hoford dest[1 + off2] = v1; 1965b539461dcc159bd89297443780d635ccc5e3564John Hoford dest[2 + off2] = v2; 1975b539461dcc159bd89297443780d635ccc5e3564John Hoford 1985b539461dcc159bd89297443780d635ccc5e3564John Hoford } 1995b539461dcc159bd89297443780d635ccc5e3564John Hoford 2005b539461dcc159bd89297443780d635ccc5e3564John Hoford public Matrix invers() { 2015b539461dcc159bd89297443780d635ccc5e3564John Hoford double[] inv = new double[16]; 2025b539461dcc159bd89297443780d635ccc5e3564John Hoford 2035b539461dcc159bd89297443780d635ccc5e3564John Hoford inv[0] = m[5] * m[10] * m[15] - 2045b539461dcc159bd89297443780d635ccc5e3564John Hoford m[5] * m[11] * m[14] - 2055b539461dcc159bd89297443780d635ccc5e3564John Hoford m[9] * m[6] * m[15] + 2065b539461dcc159bd89297443780d635ccc5e3564John Hoford m[9] * m[7] * m[14] + 2075b539461dcc159bd89297443780d635ccc5e3564John Hoford m[13] * m[6] * m[11] - 2085b539461dcc159bd89297443780d635ccc5e3564John Hoford m[13] * m[7] * m[10]; 2095b539461dcc159bd89297443780d635ccc5e3564John Hoford 2105b539461dcc159bd89297443780d635ccc5e3564John Hoford inv[4] = -m[4] * m[10] * m[15] + 2115b539461dcc159bd89297443780d635ccc5e3564John Hoford m[4] * m[11] * m[14] + 2125b539461dcc159bd89297443780d635ccc5e3564John Hoford m[8] * m[6] * m[15] - 2135b539461dcc159bd89297443780d635ccc5e3564John Hoford m[8] * m[7] * m[14] - 2145b539461dcc159bd89297443780d635ccc5e3564John Hoford m[12] * m[6] * m[11] + 2155b539461dcc159bd89297443780d635ccc5e3564John Hoford m[12] * m[7] * m[10]; 2165b539461dcc159bd89297443780d635ccc5e3564John Hoford 2175b539461dcc159bd89297443780d635ccc5e3564John Hoford inv[8] = m[4] * m[9] * m[15] - 2185b539461dcc159bd89297443780d635ccc5e3564John Hoford m[4] * m[11] * m[13] - 2195b539461dcc159bd89297443780d635ccc5e3564John Hoford m[8] * m[5] * m[15] + 2205b539461dcc159bd89297443780d635ccc5e3564John Hoford m[8] * m[7] * m[13] + 2215b539461dcc159bd89297443780d635ccc5e3564John Hoford m[12] * m[5] * m[11] - 2225b539461dcc159bd89297443780d635ccc5e3564John Hoford m[12] * m[7] * m[9]; 2235b539461dcc159bd89297443780d635ccc5e3564John Hoford 2245b539461dcc159bd89297443780d635ccc5e3564John Hoford inv[12] = -m[4] * m[9] * m[14] + 2255b539461dcc159bd89297443780d635ccc5e3564John Hoford m[4] * m[10] * m[13] + 2265b539461dcc159bd89297443780d635ccc5e3564John Hoford m[8] * m[5] * m[14] - 2275b539461dcc159bd89297443780d635ccc5e3564John Hoford m[8] * m[6] * m[13] - 2285b539461dcc159bd89297443780d635ccc5e3564John Hoford m[12] * m[5] * m[10] + 2295b539461dcc159bd89297443780d635ccc5e3564John Hoford m[12] * m[6] * m[9]; 2305b539461dcc159bd89297443780d635ccc5e3564John Hoford 2315b539461dcc159bd89297443780d635ccc5e3564John Hoford inv[1] = -m[1] * m[10] * m[15] + 2325b539461dcc159bd89297443780d635ccc5e3564John Hoford m[1] * m[11] * m[14] + 2335b539461dcc159bd89297443780d635ccc5e3564John Hoford m[9] * m[2] * m[15] - 2345b539461dcc159bd89297443780d635ccc5e3564John Hoford m[9] * m[3] * m[14] - 2355b539461dcc159bd89297443780d635ccc5e3564John Hoford m[13] * m[2] * m[11] + 2365b539461dcc159bd89297443780d635ccc5e3564John Hoford m[13] * m[3] * m[10]; 2375b539461dcc159bd89297443780d635ccc5e3564John Hoford 2385b539461dcc159bd89297443780d635ccc5e3564John Hoford inv[5] = m[0] * m[10] * m[15] - 2395b539461dcc159bd89297443780d635ccc5e3564John Hoford m[0] * m[11] * m[14] - 2405b539461dcc159bd89297443780d635ccc5e3564John Hoford m[8] * m[2] * m[15] + 2415b539461dcc159bd89297443780d635ccc5e3564John Hoford m[8] * m[3] * m[14] + 2425b539461dcc159bd89297443780d635ccc5e3564John Hoford m[12] * m[2] * m[11] - 2435b539461dcc159bd89297443780d635ccc5e3564John Hoford m[12] * m[3] * m[10]; 2445b539461dcc159bd89297443780d635ccc5e3564John Hoford 2455b539461dcc159bd89297443780d635ccc5e3564John Hoford inv[9] = -m[0] * m[9] * m[15] + 2465b539461dcc159bd89297443780d635ccc5e3564John Hoford m[0] * m[11] * m[13] + 2475b539461dcc159bd89297443780d635ccc5e3564John Hoford m[8] * m[1] * m[15] - 2485b539461dcc159bd89297443780d635ccc5e3564John Hoford m[8] * m[3] * m[13] - 2495b539461dcc159bd89297443780d635ccc5e3564John Hoford m[12] * m[1] * m[11] + 2505b539461dcc159bd89297443780d635ccc5e3564John Hoford m[12] * m[3] * m[9]; 2515b539461dcc159bd89297443780d635ccc5e3564John Hoford 2525b539461dcc159bd89297443780d635ccc5e3564John Hoford inv[13] = m[0] * m[9] * m[14] - 2535b539461dcc159bd89297443780d635ccc5e3564John Hoford m[0] * m[10] * m[13] - 2545b539461dcc159bd89297443780d635ccc5e3564John Hoford m[8] * m[1] * m[14] + 2555b539461dcc159bd89297443780d635ccc5e3564John Hoford m[8] * m[2] * m[13] + 2565b539461dcc159bd89297443780d635ccc5e3564John Hoford m[12] * m[1] * m[10] - 2575b539461dcc159bd89297443780d635ccc5e3564John Hoford m[12] * m[2] * m[9]; 2585b539461dcc159bd89297443780d635ccc5e3564John Hoford 2595b539461dcc159bd89297443780d635ccc5e3564John Hoford inv[2] = m[1] * m[6] * m[15] - 2605b539461dcc159bd89297443780d635ccc5e3564John Hoford m[1] * m[7] * m[14] - 2615b539461dcc159bd89297443780d635ccc5e3564John Hoford m[5] * m[2] * m[15] + 2625b539461dcc159bd89297443780d635ccc5e3564John Hoford m[5] * m[3] * m[14] + 2635b539461dcc159bd89297443780d635ccc5e3564John Hoford m[13] * m[2] * m[7] - 2645b539461dcc159bd89297443780d635ccc5e3564John Hoford m[13] * m[3] * m[6]; 2655b539461dcc159bd89297443780d635ccc5e3564John Hoford 2665b539461dcc159bd89297443780d635ccc5e3564John Hoford inv[6] = -m[0] * m[6] * m[15] + 2675b539461dcc159bd89297443780d635ccc5e3564John Hoford m[0] * m[7] * m[14] + 2685b539461dcc159bd89297443780d635ccc5e3564John Hoford m[4] * m[2] * m[15] - 2695b539461dcc159bd89297443780d635ccc5e3564John Hoford m[4] * m[3] * m[14] - 2705b539461dcc159bd89297443780d635ccc5e3564John Hoford m[12] * m[2] * m[7] + 2715b539461dcc159bd89297443780d635ccc5e3564John Hoford m[12] * m[3] * m[6]; 2725b539461dcc159bd89297443780d635ccc5e3564John Hoford 2735b539461dcc159bd89297443780d635ccc5e3564John Hoford inv[10] = m[0] * m[5] * m[15] - 2745b539461dcc159bd89297443780d635ccc5e3564John Hoford m[0] * m[7] * m[13] - 2755b539461dcc159bd89297443780d635ccc5e3564John Hoford m[4] * m[1] * m[15] + 2765b539461dcc159bd89297443780d635ccc5e3564John Hoford m[4] * m[3] * m[13] + 2775b539461dcc159bd89297443780d635ccc5e3564John Hoford m[12] * m[1] * m[7] - 2785b539461dcc159bd89297443780d635ccc5e3564John Hoford m[12] * m[3] * m[5]; 2795b539461dcc159bd89297443780d635ccc5e3564John Hoford 2805b539461dcc159bd89297443780d635ccc5e3564John Hoford inv[14] = -m[0] * m[5] * m[14] + 2815b539461dcc159bd89297443780d635ccc5e3564John Hoford m[0] * m[6] * m[13] + 2825b539461dcc159bd89297443780d635ccc5e3564John Hoford m[4] * m[1] * m[14] - 2835b539461dcc159bd89297443780d635ccc5e3564John Hoford m[4] * m[2] * m[13] - 2845b539461dcc159bd89297443780d635ccc5e3564John Hoford m[12] * m[1] * m[6] + 2855b539461dcc159bd89297443780d635ccc5e3564John Hoford m[12] * m[2] * m[5]; 2865b539461dcc159bd89297443780d635ccc5e3564John Hoford 2875b539461dcc159bd89297443780d635ccc5e3564John Hoford inv[3] = -m[1] * m[6] * m[11] + 2885b539461dcc159bd89297443780d635ccc5e3564John Hoford m[1] * m[7] * m[10] + 2895b539461dcc159bd89297443780d635ccc5e3564John Hoford m[5] * m[2] * m[11] - 2905b539461dcc159bd89297443780d635ccc5e3564John Hoford m[5] * m[3] * m[10] - 2915b539461dcc159bd89297443780d635ccc5e3564John Hoford m[9] * m[2] * m[7] + 2925b539461dcc159bd89297443780d635ccc5e3564John Hoford m[9] * m[3] * m[6]; 2935b539461dcc159bd89297443780d635ccc5e3564John Hoford 2945b539461dcc159bd89297443780d635ccc5e3564John Hoford inv[7] = m[0] * m[6] * m[11] - 2955b539461dcc159bd89297443780d635ccc5e3564John Hoford m[0] * m[7] * m[10] - 2965b539461dcc159bd89297443780d635ccc5e3564John Hoford m[4] * m[2] * m[11] + 2975b539461dcc159bd89297443780d635ccc5e3564John Hoford m[4] * m[3] * m[10] + 2985b539461dcc159bd89297443780d635ccc5e3564John Hoford m[8] * m[2] * m[7] - 2995b539461dcc159bd89297443780d635ccc5e3564John Hoford m[8] * m[3] * m[6]; 3005b539461dcc159bd89297443780d635ccc5e3564John Hoford 3015b539461dcc159bd89297443780d635ccc5e3564John Hoford inv[11] = -m[0] * m[5] * m[11] + 3025b539461dcc159bd89297443780d635ccc5e3564John Hoford m[0] * m[7] * m[9] + 3035b539461dcc159bd89297443780d635ccc5e3564John Hoford m[4] * m[1] * m[11] - 3045b539461dcc159bd89297443780d635ccc5e3564John Hoford m[4] * m[3] * m[9] - 3055b539461dcc159bd89297443780d635ccc5e3564John Hoford m[8] * m[1] * m[7] + 3065b539461dcc159bd89297443780d635ccc5e3564John Hoford m[8] * m[3] * m[5]; 3075b539461dcc159bd89297443780d635ccc5e3564John Hoford 3085b539461dcc159bd89297443780d635ccc5e3564John Hoford inv[15] = m[0] * m[5] * m[10] - 3095b539461dcc159bd89297443780d635ccc5e3564John Hoford m[0] * m[6] * m[9] - 3105b539461dcc159bd89297443780d635ccc5e3564John Hoford m[4] * m[1] * m[10] + 3115b539461dcc159bd89297443780d635ccc5e3564John Hoford m[4] * m[2] * m[9] + 3125b539461dcc159bd89297443780d635ccc5e3564John Hoford m[8] * m[1] * m[6] - 3135b539461dcc159bd89297443780d635ccc5e3564John Hoford m[8] * m[2] * m[5]; 3145b539461dcc159bd89297443780d635ccc5e3564John Hoford 3155b539461dcc159bd89297443780d635ccc5e3564John Hoford 3165b539461dcc159bd89297443780d635ccc5e3564John Hoford double det; 3175b539461dcc159bd89297443780d635ccc5e3564John Hoford det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12]; 3185b539461dcc159bd89297443780d635ccc5e3564John Hoford 3195b539461dcc159bd89297443780d635ccc5e3564John Hoford if (det == 0) { 3205b539461dcc159bd89297443780d635ccc5e3564John Hoford return null; 3215b539461dcc159bd89297443780d635ccc5e3564John Hoford } 3225b539461dcc159bd89297443780d635ccc5e3564John Hoford 3235b539461dcc159bd89297443780d635ccc5e3564John Hoford det = 1.0 / det; 3245b539461dcc159bd89297443780d635ccc5e3564John Hoford 3255b539461dcc159bd89297443780d635ccc5e3564John Hoford double[] out = new double[16]; 3265b539461dcc159bd89297443780d635ccc5e3564John Hoford 3275b539461dcc159bd89297443780d635ccc5e3564John Hoford for (int i = 0; i < 16; i++) { 3285b539461dcc159bd89297443780d635ccc5e3564John Hoford out[i] = inv[i] * det; 3295b539461dcc159bd89297443780d635ccc5e3564John Hoford } 3305b539461dcc159bd89297443780d635ccc5e3564John Hoford 3315b539461dcc159bd89297443780d635ccc5e3564John Hoford Matrix ret = new Matrix(out); 3325b539461dcc159bd89297443780d635ccc5e3564John Hoford return ret; 3335b539461dcc159bd89297443780d635ccc5e3564John Hoford } 3345b539461dcc159bd89297443780d635ccc5e3564John Hoford 3355b539461dcc159bd89297443780d635ccc5e3564John Hoford public void getAsFloats(float[] fm) { 3365b539461dcc159bd89297443780d635ccc5e3564John Hoford for (int y = 0; y < 4; y++) { 3375b539461dcc159bd89297443780d635ccc5e3564John Hoford int col = y * 4; 3385b539461dcc159bd89297443780d635ccc5e3564John Hoford 3395b539461dcc159bd89297443780d635ccc5e3564John Hoford for (int x = 0; x < 4; x++) 3405b539461dcc159bd89297443780d635ccc5e3564John Hoford fm[y + x * 4] = (float) m[x + y * 4]; 3415b539461dcc159bd89297443780d635ccc5e3564John Hoford } 3425b539461dcc159bd89297443780d635ccc5e3564John Hoford } 3435b539461dcc159bd89297443780d635ccc5e3564John Hoford 3445b539461dcc159bd89297443780d635ccc5e3564John Hoford Matrix mult(Matrix b) { 3455b539461dcc159bd89297443780d635ccc5e3564John Hoford return new Matrix(multiply(this.m, b.m)); 3465b539461dcc159bd89297443780d635ccc5e3564John Hoford } 3475b539461dcc159bd89297443780d635ccc5e3564John Hoford 3485b539461dcc159bd89297443780d635ccc5e3564John Hoford Matrix premult(Matrix b) { 3495b539461dcc159bd89297443780d635ccc5e3564John Hoford return new Matrix(multiply(b.m, this.m)); 3505b539461dcc159bd89297443780d635ccc5e3564John Hoford } 3515b539461dcc159bd89297443780d635ccc5e3564John Hoford 3525b539461dcc159bd89297443780d635ccc5e3564John Hoford private static double[] multiply(double a[], double b[]) { 3535b539461dcc159bd89297443780d635ccc5e3564John Hoford double[] resultant = new double[16]; 3545b539461dcc159bd89297443780d635ccc5e3564John Hoford for (int i = 0; i < 4; i++) { 3555b539461dcc159bd89297443780d635ccc5e3564John Hoford for (int j = 0; j < 4; j++) { 3565b539461dcc159bd89297443780d635ccc5e3564John Hoford for (int k = 0; k < 4; k++) { 3575b539461dcc159bd89297443780d635ccc5e3564John Hoford resultant[i + 4 * j] += a[i + 4 * k] * b[k + 4 * j]; 3585b539461dcc159bd89297443780d635ccc5e3564John Hoford } 3595b539461dcc159bd89297443780d635ccc5e3564John Hoford } 3605b539461dcc159bd89297443780d635ccc5e3564John Hoford } 3615b539461dcc159bd89297443780d635ccc5e3564John Hoford 3625b539461dcc159bd89297443780d635ccc5e3564John Hoford return resultant; 3635b539461dcc159bd89297443780d635ccc5e3564John Hoford } 3645b539461dcc159bd89297443780d635ccc5e3564John Hoford 3655b539461dcc159bd89297443780d635ccc5e3564John Hoford public void clone(Matrix src) { 3665b539461dcc159bd89297443780d635ccc5e3564John Hoford System.arraycopy(src.m, 0, m, 0, m.length); 3675b539461dcc159bd89297443780d635ccc5e3564John Hoford } 3685b539461dcc159bd89297443780d635ccc5e3564John Hoford} 369